[A]add sdk_core.

2019-01-18 23:07:03 +08:00
parent e0b8bd9dd2
commit 4b677d5d64
47 changed files with 5922 additions and 0 deletions
@@ -0,0 +1,164 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "comm/comm_port.h"
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+
+#include "sdk_protocol.h"
+
+namespace livox {
+const uint32_t kSearchPacketPreamble = 0;
+const uint32_t kGetPacketData = 1;
+
+CommPort::CommPort() {
+  protocol_ = new SdkProtocol(0x4c49, 0x564f580a);
+  cache_.wr_idx = 0;
+  cache_.rd_idx = 0;
+  cache_.size = kCacheSize;
+  parse_step_ = kSearchPacketPreamble;
+  seq_num_ = 0;
+}
+
+CommPort::~CommPort() {
+  if (protocol_) {
+    delete protocol_;
+  }
+}
+
+uint8_t *CommPort::FetchCacheFreeSpace(uint32_t *o_len) {
+  UpdateCache();
+
+  if (cache_.wr_idx < cache_.size) {
+    *o_len = cache_.size - cache_.wr_idx;
+    return &cache_.buf[cache_.wr_idx];
+  } else {
+    *o_len = 0;
+    return NULL;
+  }
+}
+
+int32_t CommPort::UpdateCacheWrIdx(uint32_t used_size) {
+  if ((cache_.wr_idx + used_size) < cache_.size) {
+    cache_.wr_idx += used_size;
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+uint32_t CommPort::GetCacheTailSize() {
+  if (cache_.wr_idx < cache_.size) {
+    return cache_.size - cache_.wr_idx;
+  } else {
+    return 0;
+  }
+}
+
+uint32_t CommPort::GetValidDataSize() {
+  if (cache_.wr_idx > cache_.rd_idx) {
+    return cache_.wr_idx - cache_.rd_idx;
+  } else {
+    return 0;
+  }
+}
+
+void CommPort::UpdateCache(void) {
+  if (GetCacheTailSize() < kMoveCacheLimit) {  // move unused data to cache head
+    uint32_t valid_data_size = GetValidDataSize();
+
+    if (valid_data_size) {
+      memmove(cache_.buf, &cache_.buf[cache_.rd_idx], valid_data_size);
+      cache_.rd_idx = 0;
+      cache_.wr_idx = valid_data_size;
+    } else if (cache_.rd_idx) {
+      cache_.rd_idx = 0;
+      cache_.wr_idx = 0;
+    }
+  }
+}
+
+int32_t CommPort::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) {
+  return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet);
+}
+
+int32_t CommPort::ParseCommStream(CommPacket *o_pack) {
+#define CUR_PACK_LEN (protocol_->GetPacketLen(&cache_.buf[cache_.rd_idx]))
+#define CUR_CACHE_POS (&cache_.buf[cache_.rd_idx])
+
+  while (GetValidDataSize() > protocol_->GetPreambleLen()) {
+    // printf("unpack step : %d %d %d\r\n", parse_step_, cache_.wr_idx, cache_.rd_idx);
+    if (kSearchPacketPreamble == parse_step_) {
+      if (!protocol_->CheckPreamble(CUR_CACHE_POS)) {
+        parse_step_ = kGetPacketData;
+      } else {
+        ++cache_.rd_idx;
+      }
+    } else if (kGetPacketData == parse_step_) {
+      if (GetValidDataSize() >= CUR_PACK_LEN && CUR_PACK_LEN) {
+        parse_step_ = kSearchPacketPreamble;
+        if (!protocol_->CheckPacket(CUR_CACHE_POS)) {
+          int32_t ret = protocol_->ParsePacket(CUR_CACHE_POS, GetValidDataSize(), o_pack);
+          cache_.rd_idx += CUR_PACK_LEN;
+          if (kParseSuccess == ret) {
+            return ret;
+          }
+        } else {
+          cache_.rd_idx += protocol_->GetPreambleLen();
+        }
+      } else {
+        if (CUR_PACK_LEN > cache_.size) {  // always > cache_.size,
+          cache_.rd_idx = 0;
+          cache_.wr_idx = 0;
+          parse_step_ = kSearchPacketPreamble;
+        }
+
+        break;
+      }
+    } else {
+      parse_step_ = kSearchPacketPreamble;
+    }
+  }
+
+  return kParseFail;
+}
+
+uint16_t CommPort::GetAndUpdateSeqNum() {
+  // add lock here for thread safe
+  uint16_t seq = seq_num_;
+  seq_num_++;
+
+  return seq;
+}
+
+// only for pack function test use
+// uint8_t test_buf[1024];
+// uint32_t o_len = 0;
+// comm_port_->Pack(test_buf, sizeof(test_buf), &o_len, packet);
+// for (uint32_t print_i = 0; print_i < o_len; print_i++) {
+//  printf("%.2x_", test_buf[print_i]);
+//}
+// printf("\r\n");
+}  // namespace livox
@@ -0,0 +1,128 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "sdk_protocol.h"
+
+#include <stdio.h>
+#include <string.h>
+namespace livox {
+const uint8_t kSdkProtocolSof = 0xAA;
+const uint32_t kSdkPacketCrcSize = 4;          // crc32
+const uint32_t kSdkPacketPreambleCrcSize = 2;  // crc16
+
+SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32) : crc16_(seed16), crc32_(seed32) {}
+
+SdkProtocol::~SdkProtocol() {}
+
+int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) {
+  SdkPacket *sdk_packet = (SdkPacket *)o_buf;
+
+  if (kLidarSdk != i_packet.protocol) {
+    return -1;
+  }
+
+  sdk_packet->sof = kSdkProtocolSof;
+  sdk_packet->length = i_packet.data_len + GetPacketWrapperLen();
+
+  if (sdk_packet->length > o_buf_size) {
+    return -1;
+  }
+
+  sdk_packet->version = kSdkVer0;
+  sdk_packet->packet_type = i_packet.packet_type;
+  sdk_packet->seq_num = i_packet.seq_num & 0xFFFF;
+  sdk_packet->preamble_crc = crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - kSdkPacketPreambleCrcSize);
+  sdk_packet->cmd_set = i_packet.cmd_set;
+  sdk_packet->cmd_id = i_packet.cmd_code;
+
+  memcpy(sdk_packet->data, i_packet.data, i_packet.data_len);
+
+  uint32_t crc = crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize);
+  o_buf[sdk_packet->length - 4] = crc & 0xFF;
+  o_buf[sdk_packet->length - 3] = (crc >> 8) & 0xFF;
+  o_buf[sdk_packet->length - 2] = (crc >> 16) & 0xFF;
+  o_buf[sdk_packet->length - 1] = (crc >> 24) & 0xFF;
+
+  *o_len = sdk_packet->length;
+
+  return 0;
+}
+
+int32_t SdkProtocol::ParsePacket(uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) {
+  SdkPacket *sdk_packet = (SdkPacket *)i_buf;
+
+  if (i_len < GetPacketWrapperLen()) {
+    return -1;  // packet lenth error
+  }
+
+  memset((void *)o_packet, 0, sizeof(CommPacket));
+
+  o_packet->packet_type = sdk_packet->packet_type;
+  o_packet->protocol = kLidarSdk;
+  o_packet->protocol_version = sdk_packet->version;
+
+  o_packet->seq_num = sdk_packet->seq_num;
+  o_packet->cmd_set = sdk_packet->cmd_set;
+  o_packet->cmd_code = sdk_packet->cmd_id;
+  o_packet->data_len = sdk_packet->length - GetPacketWrapperLen();
+  o_packet->data = sdk_packet->data;
+
+  return 0;
+}
+
+uint32_t SdkProtocol::GetPreambleLen() {
+  return sizeof(SdkPreamble);
+}
+
+uint32_t SdkProtocol::GetPacketWrapperLen() {
+  return sizeof(SdkPacket) - 1 + kSdkPacketCrcSize;
+}
+
+uint32_t SdkProtocol::GetPacketLen(uint8_t *buf) {
+  SdkPreamble *preamble = (SdkPreamble *)buf;
+  return preamble->length;
+}
+
+int32_t SdkProtocol::CheckPreamble(uint8_t *buf) {
+  SdkPreamble *preamble = (SdkPreamble *)buf;
+
+  if ((preamble->sof == kSdkProtocolSof) && (crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) {
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+int32_t SdkProtocol::CheckPacket(uint8_t *buf) {
+  SdkPacket *sdk_packet = (SdkPacket *)buf;
+  uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) | (((uint32_t)(buf[sdk_packet->length - 3])) << 8) |
+                 (((uint32_t)(buf[sdk_packet->length - 2])) << 16) | (((uint32_t)(buf[sdk_packet->length - 1])) << 24);
+
+  if (crc32_.crc32_calc(buf, sdk_packet->length - kSdkPacketCrcSize) == crc) {
+    return 0;
+  } else {
+    return -1;
+  }
+}
+}  // namespace livox
@@ -0,0 +1,84 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_SDK_PROTOCOL_H_
+#define LIVOX_SDK_PROTOCOL_H_
+
+#include <stdint.h>
+#include "../include/comm/protocol.h"
+#include "../include/third_party/FastCRC/FastCRC.h"
+namespace livox {
+typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;
+
+#pragma pack(1)
+
+typedef struct {
+  uint8_t sof;
+  uint8_t version;
+  uint16_t length;
+  uint8_t packet_type;
+  uint16_t seq_num;
+  uint16_t preamble_crc;
+} SdkPreamble;
+
+typedef struct {
+  uint8_t sof;
+  uint8_t version;
+  uint16_t length;
+  uint8_t packet_type;
+  uint16_t seq_num;
+  uint16_t preamble_crc;
+  uint8_t cmd_set;
+  uint8_t cmd_id;
+  uint8_t data[1];
+} SdkPacket;
+
+#pragma pack()
+
+class SdkProtocol : public Protocol {
+ public:
+  SdkProtocol(uint16_t seed16, uint32_t seed32);
+
+  ~SdkProtocol();
+
+  int32_t ParsePacket(uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet);
+
+  int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet);
+
+  uint32_t GetPreambleLen();
+
+  uint32_t GetPacketWrapperLen();
+
+  uint32_t GetPacketLen(uint8_t *buf);
+
+  int32_t CheckPreamble(uint8_t *buf);
+
+  int32_t CheckPacket(uint8_t *buf);
+
+ private:
+  FastCRC16 crc16_;
+  FastCRC32 crc32_;
+};
+}  // namespace livox
+#endif  // LIVOX_SDK_PROTOCOL_H_