针对Tmini Plus增加获取俯仰角

hannesduske · Dec 11, 2024 · afbd189 · afbd189
1 parent 6bdbe78
commit afbd189
Show file tree

Hide file tree

Showing 8 changed files with 129 additions and 187 deletions.
diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h
@@ -502,6 +502,12 @@ namespace ydlidar
           return RESULT_OK; 
         }
 
+        //获取俯仰角值
+        virtual bool getPitchAngle(float& pitch) {
+          UNUSED(pitch);
+          return false;
+        }
+
         // 开始OTA升级
         virtual bool ota() {
           return false;
@@ -552,6 +558,7 @@ namespace ydlidar
           YDLIDAR_Tmini = 140, /**< Tmini LiDAR Model. */
           YDLIDAR_TminiPro = 150, /**< Tmini Pro LiDAR Model. */
           YDLIDAR_TminiPlus = 151, /**< Tmini Plus LiDAR Model. */
+          YDLIDAR_TminiPlusSH = 152, //Tmini Plus 森合
 
           YDLIDAR_SDM15 = 160, //SDM15单点雷达
           YDLIDAR_SDM18, //DTS单点雷达

diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h
@@ -80,6 +80,12 @@ namespace common {
     localTime->tm_hour, \
     localTime->tm_min, \
     localTime->tm_sec);
+#define WIN_PRINT_TIME UNIX_PRINT_TIME
+#ifdef _WIN32
+  #define PRINT_TIME WIN_PRINT_TIME
+#else
+  #define PRINT_TIME UNIX_PRINT_TIME
+#endif
 //格式化字符串
 #define FORMAT_STDOUT \
   char buff[1024] = {0}; \
@@ -94,10 +100,7 @@ namespace common {
 inline void debug(char* fmt, ...)
 {
   printf(GREEN); //设置绿色
-#ifdef _WIN32
-#else
-  UNIX_PRINT_TIME
-#endif
+  PRINT_TIME
   printf("[debug] ");
   FORMAT_STDOUT
   printf(COLOFF); //恢复默认颜色
@@ -107,10 +110,7 @@ inline void debug(char* fmt, ...)
 //常规
 inline void info(char* fmt, ...)
 {
-#ifdef _WIN32
-#else
-  UNIX_PRINT_TIME
-#endif
+  PRINT_TIME
   printf("[info] ");
   FORMAT_STDOUT
   fflush(stdout);
@@ -120,10 +120,7 @@ inline void info(char* fmt, ...)
 inline void warn(char* fmt, ...)
 {
   printf(YELLOW); //设置黄色
-#ifdef _WIN32
-#else
-  UNIX_PRINT_TIME
-#endif
+  PRINT_TIME
   printf("[warn] ");
   FORMAT_STDOUT
   printf(COLOFF); //恢复默认颜色
@@ -134,10 +131,7 @@ inline void warn(char* fmt, ...)
 inline void error(char* fmt, ...)
 {
   printf(RED); //设置红色
-#ifdef _WIN32
-#else
-  UNIX_PRINT_TIME
-#endif
+  PRINT_TIME
   printf("[error] ");
   FORMAT_STDOUT
   printf(COLOFF); //恢复默认颜色
@@ -150,10 +144,7 @@ inline void debugh(const uint8_t *data, int size)
   if (!data || !size)
     return;
   printf(GREEN); //设置绿色
-#ifdef _WIN32
-#else
-  UNIX_PRINT_TIME
-#endif
+  PRINT_TIME
   printf("[debug] ");
   for (int i=0; i<size; ++i)
       printf("%02X", data[i]);
@@ -165,17 +156,14 @@ inline void debugh(const uint8_t *data, int size)
 //常规（16进制）
 inline void infoh(const uint8_t *data, int size)
 {
-    if (!data || !size)
-        return;
-#ifdef _WIN32
-#else
-  UNIX_PRINT_TIME
-#endif
-    printf("[info] ");
-    for (int i=0; i<size; ++i)
-        printf("%02X", data[i]);
-    printf("\n");
-    fflush(stdout);
+  if (!data || !size)
+      return;
+  PRINT_TIME
+  printf("[info] ");
+  for (int i=0; i<size; ++i)
+      printf("%02X", data[i]);
+  printf("\n");
+  fflush(stdout);
 }
 
 /*!
@@ -295,6 +283,9 @@ inline std::string lidarModelToString(int model)
   case DriverInterface::YDLIDAR_TminiPlus:
     name = "Tmini Plus";
     break;
+  case DriverInterface::YDLIDAR_TminiPlusSH:
+    name = "Tmini Plus SH";
+    break;
   case DriverInterface::YDLIDAR_T15:
     name = "T15";
     break;

diff --git a/core/common/ydlidar_protocol.h b/core/common/ydlidar_protocol.h
@@ -60,20 +60,22 @@
 #define LIDAR_CMD_GET_EAI                   0x55
 #define LIDAR_CMD_GET_DEVICE_INFO           0x90
 #define LIDAR_CMD_GET_DEVICE_HEALTH         0x92
-#define LIDAR_ANS_TYPE_DEVINFO              0x4
-#define LIDAR_ANS_TYPE_DEVHEALTH            0x6
 #define LIDAR_CMD_SYNC_BYTE                 0xA5
 #define LIDAR_CMDFLAG_HAS_PAYLOAD           0x80
-#define LIDAR_ANS_SYNC_BYTE1                0xA5
-#define LIDAR_ANS_SYNC_BYTE2                0x5A
-#define LIDAR_ANS_TYPE_MEASUREMENT          0x81
 #define LIDAR_RESP_SYNCBIT        (0x1<<0)
 #define LIDAR_RESP_QUALITY_SHIFT  2
 #define LIDAR_RESP_CHECKBIT       (0x1<<0)
 #define LIDAR_RESP_ANGLE_SHIFT    1
 #define LIDAR_RESP_DIST_SHIFT  2
 #define LIDAR_RESP_ANGLE_SAMPLE_SHIFT 8
 
+#define LIDAR_ANS_SYNC_BYTE1                0xA5
+#define LIDAR_ANS_SYNC_BYTE2                0x5A
+#define LIDAR_ANS_TYPE_DEVINFO              0x04
+#define LIDAR_ANS_TYPE_DEVHEALTH            0x06
+#define LIDAR_ANS_TYPE_PITCH                0x10
+#define LIDAR_ANS_TYPE_MEASUREMENT          0x81
+
 #define LIDAR_CMD_RUN_POSITIVE             0x06
 #define LIDAR_CMD_RUN_INVERSION            0x07
 #define LIDAR_CMD_SET_AIMSPEED_ADDMIC      0x09
@@ -101,6 +103,7 @@
 #define LIDAR_CMD_DIS_EXPOSURE       	    0x97
 
 #define LIDAR_CMD_SET_HEART_BEAT            0xD9
+#define LIDAR_CMD_GETPITCH 0x98 //获取俯仰角
 
 //GS命令
 #define GS_LIDAR_CMD_GET_ADDRESS               0x60