增加支持GS2雷达

CMakeLists.txt

@@ -4,8 +4,8 @@ project(ydlidar_sdk C CXX)
 #########################################################
 # version
 set(YDLIDAR_SDK_VERSION_MAJOR 1)
-set(YDLIDAR_SDK_VERSION_MINOR 0)
-set(YDLIDAR_SDK_VERSION_PATCH 6)
+set(YDLIDAR_SDK_VERSION_MINOR 1)
+set(YDLIDAR_SDK_VERSION_PATCH 0)
 set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
 
 ##########################################################

README.md

@@ -2,15 +2,16 @@
 
 # Table of Contents
 
-1. [Introduction](#introduction)
+1. [Table of Contents](#table-of-contents)
+2. [Introduction](#introduction)
     - [Prerequisites](#prerequisites)
     - [Supported Languages](#supported-languages)
-2. [YDLidar SDK Communication Protocol](#ydlidar-sdk-communication-protocol)
-3. [Architecture](#architecture)
-4. [Installation](#installation)
-5. [Documents](#documents)
-6. [Support](#support)
-7. [Contact EAI](#contact-eai)
+3. [YDLidar SDK Communication Protocol](#ydlidar-sdk-communication-protocol)
+4. [Architecture](#architecture)
+5. [Installation](#installation)
+6. [Documents](#documents)
+7. [Support](#support)
+8. [Contact EAI](#contact-eai)
 
 # Introduction
 

core/common/DriverInterface.h

@@ -435,7 +435,7 @@ class DriverInterface {
   }
 
  public:
-  enum {
+  enum YDLIDAR_MODLES {
     YDLIDAR_F4      = 1,/**< F4 LiDAR Model. */
     YDLIDAR_T1      = 2,/**< T1 LiDAR Model. */
     YDLIDAR_F2      = 3,/**< F2 LiDAR Model. */
@@ -458,6 +458,8 @@ class DriverInterface {
     YDLIDAR_G5      = 20,/**< G5 LiDAR Model. */
     YDLIDAR_G7      = 21,/**< G7 LiDAR Model. */
 
+    YDLIDAR_GS2     = 51, //GS2雷达
+
     YDLIDAR_TG15    = 100,/**< TG15 LiDAR Model. */
     YDLIDAR_TG30    = 101,/**< T30 LiDAR Model. */
     YDLIDAR_TG50    = 102,/**< TG50 LiDAR Model. */
@@ -470,7 +472,7 @@ class DriverInterface {
     YDLIDAR_Tail,
   };
 
-  enum {
+  enum YDLIDAR_RATE {
     YDLIDAR_RATE_4K = 0,/**< 4K sample rate code */
     YDLIDAR_RATE_8K = 1,/**< 8K sample rate code */
     YDLIDAR_RATE_9K = 2,/**< 9K sample rate code */
@@ -488,9 +490,9 @@ class DriverInterface {
  protected:
   /* Variable for LIDAR compatibility */
   /// LiDAR Scanning state
-  bool            m_isScanning;
+  bool            m_isScanning = false;
   /// LiDAR connected state
-  bool            m_isConnected;
+  bool            m_isConnected = false;
   /// Scan Data Event
   Event           _dataEvent;
   /// Data Locker

core/common/ydlidar_datatype.h

@@ -110,4 +110,5 @@ typedef struct {
   std::vector<LaserPoint> points;
   /// Configuration of scan
   LaserConfig config;
+  int moduleNum;
 } LaserScan;

core/common/ydlidar_def.h

@@ -45,6 +45,7 @@ typedef enum {
   TYPE_TOF = 0,/**< TG TX LiDAR.*/
   TYPE_TRIANGLE  = 1,/**< G4. G6. G2 LiDAR.*/
   TYPE_TOF_NET = 2,/**< T15 LiDAR.*/
+  TYPE_GS = 3, //GS2
   TYPE_Tail,
 } LidarTypeID;
 
@@ -191,7 +192,7 @@ typedef struct {
   /// System time when first range was measured in nanoseconds
   uint64_t stamp;///< ns
   /// Array of lidar points
-  uint32_t    npoints;
+  uint32_t npoints;
   LaserPoint *points;
   /// Configuration of scan
   LaserConfig config;

core/common/ydlidar_help.h

@@ -168,6 +168,10 @@ inline std::string lidarModelToString(int model) {
       name = "G7";
       break;
 
+    case DriverInterface::YDLIDAR_GS2:
+      name = "GS2";
+      break;
+
     case DriverInterface::YDLIDAR_TG15:
       name = "TG15";
       break;
@@ -359,7 +363,8 @@ inline bool hasScanFrequencyCtrl(int model) {
   if (model == DriverInterface::YDLIDAR_S4 ||
       model == DriverInterface::YDLIDAR_S4B ||
       model == DriverInterface::YDLIDAR_S2 ||
-      model == DriverInterface::YDLIDAR_X4) {
+      model == DriverInterface::YDLIDAR_X4 ||
+      model == DriverInterface::YDLIDAR_GS2) {
     ret = false;
   }
 
@@ -373,16 +378,16 @@ inline bool hasScanFrequencyCtrl(int model) {
  */
 inline bool isSupportLidar(int model)
 {
-    if (model < DriverInterface::YDLIDAR_F4 ||
-            (model > DriverInterface::YDLIDAR_G7 &&
-             model < DriverInterface::YDLIDAR_TG15) ||
-            (model > DriverInterface::YDLIDAR_Tmini &&
-             model < DriverInterface::YDLIDAR_T15))
-    {
-        return false;
-    }
+  if (model < DriverInterface::YDLIDAR_F4 ||
+      (model > DriverInterface::YDLIDAR_G7 &&
+       model < DriverInterface::YDLIDAR_GS2) ||
+      (model > DriverInterface::YDLIDAR_Tmini &&
+       model < DriverInterface::YDLIDAR_T15))
+  {
+    return false;
+  }
 
-    return true;
+  return true;
 }
 
 /*!
@@ -395,7 +400,8 @@ inline bool hasIntensity(int model) {
 
   if (model == DriverInterface::YDLIDAR_G2B ||
       model == DriverInterface::YDLIDAR_G4B ||
-      model == DriverInterface::YDLIDAR_S4B) {
+      model == DriverInterface::YDLIDAR_S4B ||
+      model == DriverInterface::YDLIDAR_GS2) {
     ret = true;
   }
 
@@ -525,6 +531,21 @@ inline bool isTriangleLidar(int type) {
   return ret;
 }
 
+/**
+ * @brief Whether it is a GS type LiDAR
+ * @param type  LiDAR type
+ * @return true if it is a Triangle type, otherwise false.
+ */
+inline bool isGSLidar(int type) {
+  bool ret = false;
+
+  if (type == TYPE_GS) {
+    ret = true;
+  }
+
+  return ret;
+}
+
 /**
  * @brief Whether it is Old Version protocol TOF LiDAR
  * @param model     lidar model
@@ -976,6 +997,16 @@ inline bool isV1Protocol(uint8_t protocol) {
   return false;
 }
 
+//以16进制打印数据
+inline void printHex(const uint8_t *data, int size)
+{
+    if (!data)
+        return;
+    for (int i=0; i<size; ++i)
+        printf("%02X", data[i]);
+    printf("\n");
+}
+
 }//common
 }//core
 }//ydlidar

core/common/ydlidar_protocol.h

@@ -121,10 +121,31 @@
 
 #define LIDAR_CMD_SET_HEART_BEAT            0xD9
 
+//gs2
+#define GS_LIDAR_CMD_GET_ADDRESS               0x60
+#define GS_LIDAR_CMD_GET_PARAMETER             0x61
+#define GS_LIDAR_CMD_GET_VERSION               0x62
+#define GS_LIDAR_CMD_SCAN                      0x63
+#define GS_LIDAR_ANS_SCAN                      0x63
+#define GS_LIDAR_CMD_STOP                      0x64
+#define GS_LIDAR_CMD_RESET                     0x67
+#define GS_LIDAR_CMD_SET_BIAS                  0xD9
+#define GS_LIDAR_CMD_SET_DEBUG_MODE            0xF0
+
 /** @} LIDAR CMD Protocol */
 
+//GS2
+#define Angle_Px   1.22
+#define Angle_Py   5.315
+#define Angle_PAngle   22.5
+#define PackageMaxModuleNums  0x03
+#define PackageSampleMaxLngth_GS 0xA0  //160*2
+#define PackagePaidBytes_GS 8
+#define NORMAL_PACKAGE_SIZE 331
+
 /// Maximuum number of samples in a packet
 #define PackageSampleMaxLngth 0x100
+#define MaximumNumberOfPackages 765
 
 /// CT Package Type
 typedef enum {
@@ -158,19 +179,19 @@ typedef enum {
 #pragma pack(1)
 #endif
 
-/// LiDAR Node info
+//雷达节点信息
 struct node_info {
-  uint8_t    sync_flag;  ///< sync flag
-  uint16_t   sync_quality;///< intensity
-  uint16_t   angle_q6_checkbit; ///< angle
-  uint16_t   distance_q2; ///< range
-  uint64_t   stamp; ///< time stamp
-  uint32_t   delay_time; ///< delay time
-  uint8_t    scan_frequence;///< scan frequency. invalid: 0
-  uint8_t    debugInfo;///< debug information
-  uint8_t    index;///< package index
-  uint8_t    error_package;///< error package state
-} __attribute__((packed)) ;
+  uint8_t sync_flag; //首包标记
+  uint16_t sync_quality; //信号强度
+  uint16_t angle_q6_checkbit; //角度值（°）
+  uint16_t distance_q2; //距离值
+  uint64_t stamp; //时间戳
+  uint32_t delay_time; ///< delay time
+  uint8_t scan_frequence; //扫描频率
+  uint8_t debugInfo; ///< debug information
+  uint8_t index; //包序号
+  uint8_t error_package; ///< error package state
+} __attribute__((packed));
 
 /// package node info
 struct PackageNode {
@@ -285,6 +306,65 @@ struct lidar_ans_header {
   uint8_t  type;
 } __attribute__((packed));
 
+//GS2
+struct GS2_Multi_Package {
+    int frameNum;
+    int moduleNum;
+    bool left = false;
+    bool right = false;
+    node_info  all_points[160];
+} __attribute__((packed)) ;
+
+struct GS2PackageNode {
+  uint16_t PakageSampleDistance:9;
+  uint16_t PakageSampleQuality:7;
+} __attribute__((packed));
+
+struct gs2_node_package {
+  uint32_t  package_Head;
+  uint8_t   address;
+  uint8_t   package_CT;
+  uint16_t  size;
+  uint16_t  BackgroudLight;
+  GS2PackageNode  packageSample[PackageSampleMaxLngth_GS];
+  uint8_t  checkSum;
+} __attribute__((packed)) ;
+
+struct gs_lidar_ans_header {
+    uint8_t  syncByte0;
+    uint8_t  syncByte1;
+    uint8_t  syncByte2;
+    uint8_t  syncByte3;
+    uint8_t  address;
+    uint8_t  type;
+    uint16_t size;
+} __attribute__((packed));
+
+struct gs_device_para {
+    uint16_t u_compensateK0;
+    uint16_t u_compensateB0;
+    uint16_t u_compensateK1;
+    uint16_t u_compensateB1;
+    int8_t  bias;
+    uint8_t  crc;
+} __attribute__((packed));
+
+struct cmd_packet_gs {
+    uint8_t syncByte0;
+    uint8_t syncByte1;
+    uint8_t syncByte2;
+    uint8_t syncByte3;
+    uint8_t address;
+    uint8_t cmd_flag;
+    uint16_t size;
+} __attribute__((packed));
+//GS系列设备信息
+struct gs_device_info {
+    uint8_t hardware_version; //硬件版本号
+    uint16_t firmware_version; //固件版本号
+    uint8_t serialnum[16]; //序列号
+} __attribute__((packed));
+
 #if defined(_WIN32)
 #pragma pack()
 #endif

samples/etlidar_test.cpp

@@ -175,7 +175,7 @@ int main(int argc, char *argv[]) {
 
   while (ret && ydlidar::os_isOk()) {
     if (laser.doProcessSimple(scan)) {
-      fprintf(stdout, "Scan received[%llu]: %u ranges is [%f]Hz\n",
+      fprintf(stdout, "Scan received[%lu]: %u ranges is [%f]Hz\n",
               scan.stamp,
               (unsigned int)scan.points.size(), 1.0 / scan.config.scan_time);
       fflush(stdout);