增加获取上下板设备信息接口

CMakeLists.txt

@@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
 # version
 set(YDLIDAR_SDK_VERSION_MAJOR 1)
 set(YDLIDAR_SDK_VERSION_MINOR 1)
-set(YDLIDAR_SDK_VERSION_PATCH 8)
+set(YDLIDAR_SDK_VERSION_PATCH 9)
 set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
 
 ##########################################################

core/common/DriverInterface.h

@@ -103,6 +103,8 @@ class DriverInterface {
 
   //是否底板优先
   PropertyBuilderByName(bool, Bottom, protected);
+  //是否已获取到设备信息
+  PropertyBuilderByName(bool, HasDeviceInfo, protected);
 
   /**
    * @par Constructor
@@ -134,6 +136,8 @@ class DriverInterface {
     m_BufferSize          = 0;
     m_Debug = false;
     m_ScanFreq = 0;
+    m_Bottom = true;
+    m_HasDeviceInfo = false;
   }
 
   virtual ~DriverInterface() {}
@@ -289,6 +293,12 @@ class DriverInterface {
    */
   virtual result_t getDeviceInfo(device_info &info,
                                  uint32_t timeout = DEFAULT_TIMEOUT) = 0;
+
+  //获取设备信息
+  virtual bool getDeviceInfoEx(device_info &info) {
+    UNUSED(info); 
+    return false;
+  }
 
   /**
    * @brief Turn on scanning \n

core/common/ydlidar_help.h

@@ -930,13 +930,12 @@ inline bool printfVersionInfo(const device_info &info,
 
   uint8_t Major = (uint8_t)(info.firmware_version >> 8);
   uint8_t Minjor = (uint8_t)(info.firmware_version & 0xff);
-  printf("[YDLIDAR] Connection established in [%s][%d]\n"
+
+  printf("[YDLIDAR] Device info\n"
          "Firmware version: %u.%u\n"
          "Hardware version: %u\n"
          "Model: %s\n"
          "Serial: ",
-         port.c_str(),
-         baudrate,
          Major,
          Minjor,
          (unsigned int)info.hardware_version,

samples/tri_test.cpp

@@ -37,6 +37,7 @@
 #include <algorithm>
 #include <cctype>
 #include <core/base/timer.h>
+#include <core/common/ydlidar_help.h>
 
 using namespace std;
 using namespace ydlidar;
@@ -275,6 +276,9 @@ int main(int argc, char *argv[])
   laser.enableGlassNoise(false);
   laser.enableSunNoise(false);
 
+  //设置是否底板优先
+  laser.setBottomPriority(true);
+
   bool ret = laser.initialize();
   if (!ret)
   {
@@ -301,6 +305,19 @@ int main(int argc, char *argv[])
     }
   }
 
+  //获取设备信息
+  if (ret)
+  {
+    device_info di;
+    memset(&di, 0, DEVICEINFOSIZE);
+    if (!laser.getDeviceInfo(di)) {
+      ydlidar::core::common::printfVersionInfo(di, "", 0);
+    }
+    else {
+      printf("Fail to get device info\n");
+    }
+  }
+
   LaserScan scan;
   while (ydlidar::os_isOk())
   {

src/CYdLidar.cpp

@@ -83,7 +83,6 @@ CYdLidar::CYdLidar() : lidarPtr(nullptr)
   m_DeviceType = YDLIDAR_TYPE_SERIAL;
   m_SupportMotorDtrCtrl = true;
   m_SupportHearBeat = false;
-  m_parsingCompleted = false;
   m_isAngleOffsetCorrected = false;
   m_field_of_view = 360.f;
   memset(&m_LidarVersion, 0, sizeof(LidarVersion));
@@ -921,10 +920,15 @@ bool CYdLidar::getUserVersion(std::string &version)
     return false;
 }
 
+void CYdLidar::setBottomPriority(bool yes)
+{
+  m_Bottom = yes;
+}
+
 bool CYdLidar::getDeviceInfo(device_info& di)
 {
   if (lidarPtr)
-    return lidarPtr->getDeviceInfo(di);
+    return IS_OK(lidarPtr->getDeviceInfoEx(di));
 
   return false;
 }
@@ -967,7 +971,9 @@ bool CYdLidar::isRangeIgnore(double angle) const
 -------------------------------------------------------------*/
 void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug)
 {
-  if (m_parsingCompleted)
+  if (!lidarPtr || 
+    lidarPtr->getBottom() || //如果是底板优先
+    lidarPtr->getHasDeviceInfo()) //如果已获取设备信息
   {
     return;
   }
@@ -994,7 +1000,7 @@ void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug)
       }
 
       m_SerialNumber = serial_number;
-      m_parsingCompleted = true;
+      lidarPtr->setHasDeviceInfo(true);
     }
   }
 }
@@ -1422,7 +1428,6 @@ bool CYdLidar::getDeviceInfo()
     }
 
     m_SerialNumber = serial_number;
-    m_parsingCompleted = true;
     zero_offset_angle_scale = lidarZeroOffsetAngleScale(
       di.model, di.firmware_version >> 8, di.firmware_version & 0x00ff);
   }
@@ -1473,7 +1478,9 @@ bool CYdLidar::getDeviceInfo()
 -------------------------------------------------------------*/
 void CYdLidar::handleSingleChannelDevice()
 {
-  if (!lidarPtr || !lidarPtr->getSingleChannel())
+  if (!lidarPtr || 
+    lidarPtr->getBottom() ||
+    !lidarPtr->getSingleChannel())
   {
     return;
   }
@@ -1489,7 +1496,6 @@ void CYdLidar::handleSingleChannelDevice()
 
   if (printfVersionInfo(devinfo, m_SerialPort, m_SerialBaudrate))
   {
-    m_parsingCompleted = true;
     m_LidarVersion.hardware = devinfo.hardware_version;
     m_LidarVersion.soft_major = Major;
     m_LidarVersion.soft_minor = Minjor / 10;
@@ -1738,7 +1744,6 @@ bool CYdLidar::checkCOMMs()
     printf("[YDLIDAR] SDK has been initialized\n");
     printf("[YDLIDAR] SDK Version: %s\n", lidarPtr->getSDKVersion().c_str());
     fflush(stdout);
-    lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl);
   }
 
   if (lidarPtr->isconnected())
@@ -1784,6 +1789,8 @@ bool CYdLidar::checkCOMMs()
   lidarPtr->setSingleChannel(m_SingleChannel);
   lidarPtr->setLidarType(m_LidarType);
   lidarPtr->setScanFreq(m_ScanFrequency);
+  lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl);
+  lidarPtr->setBottom(m_Bottom);
 
   printf("[YDLIDAR] Lidar successfully connected %s[%d]\n", 
     m_SerialPort.c_str(), m_SerialBaudrate);

src/CYdLidar.h

@@ -946,6 +946,8 @@ class YDLIDAR_API CYdLidar {
    */
   bool getUserVersion(std::string &version);
 
+  //设置是否优先获取底板设备信息
+  void setBottomPriority(bool yes=true);
   //获取设备信息
   bool getDeviceInfo(device_info& di);
 
@@ -1089,7 +1091,6 @@ class YDLIDAR_API CYdLidar {
   std::string m_SerialNumber;       ///< LiDAR serial number
   // int defalutSampleRate;            ///< LiDAR Default Sampling Rate
   std::vector<int> defalutSampleRate; //默认采样率可能是多个值
-  bool m_parsingCompleted;          ///< LiDAR Version Information is successfully parsed
   float m_field_of_view;            ///< LiDAR Field of View Angle.
   LidarVersion m_LidarVersion;      ///< LiDAR Version information
   float zero_offset_angle_scale;   ///< LiDAR Zero Offset Angle
@@ -1121,6 +1122,7 @@ class YDLIDAR_API CYdLidar {
   float m_MaxRange;                 ///< LiDAR maximum range
   float m_MinRange;                 ///< LiDAR minimum range
   float m_ScanFrequency;            ///< LiDAR scanning frequency
+  bool m_Bottom = true; //是否底板优先
 
   bool m_SunNoise = false; //阳光噪点过滤标识
   bool m_GlassNoise = false; //玻璃噪点过滤标识

src/GS2LidarDriver.cpp

@@ -906,7 +906,9 @@ result_t GS2LidarDriver::waitPackage(node_info *node, uint32_t timeout)
         else if (1 == package_Sample_Index)
             (*node).is = package.env >> 8;
 
-        // printf("%u %u %u\n", package_Sample_Index, node->angle, node->dist);
+        // printf("%u 0x%X %.02f %.02f\n", package_Sample_Index, 
+        //     package.nodes[package_Sample_Index].dist,
+        //     sampleAngle, node->dist/1.0);
     }
     else
     {

src/YDlidarDriver.cpp

@@ -86,7 +86,7 @@ YDlidarDriver::YDlidarDriver(uint8_t type):
   has_package_error     = false;
 
   get_device_health_success         = false;
-  get_device_info_success           = false;
+  m_HasDeviceInfo           = false;
   IntervalSampleAngle_LastPackage   = 0.0;
   m_heartbeat_ts = getms();
   m_BlockRevSize = 0;
@@ -537,8 +537,7 @@ result_t YDlidarDriver::checkAutoConnecting(bool serialError)
       if (!m_SingleChannel && m_driverErrno != BlockError)
       {
         device_info devinfo;
-        ans = getDeviceInfo(devinfo);
-
+        ans = getDeviceInfo(devinfo, 1000);
         if (!IS_OK(ans))
         {
             stopScan();
@@ -554,11 +553,8 @@ result_t YDlidarDriver::checkAutoConnecting(bool serialError)
 
       {
         ans = startAutoScan();
-
         if (!IS_OK(ans))
-        {
             ans = startAutoScan();
-        }
       }
 
       if (IS_OK(ans))
@@ -628,10 +624,6 @@ int YDlidarDriver::cacheScanData()
     result_t       ans = RESULT_FAIL;
     memset(local_scan, 0, sizeof(local_scan));
 
-    // if (m_SingleChannel) {
-    //     waitDevicePackage(1000);
-    // }
-
     int timeout_count = 0;
     retryCount = 0;
     m_BufferSize = 0;
@@ -663,7 +655,6 @@ int YDlidarDriver::cacheScanData()
                     }
 
                     ans = checkAutoConnecting(IS_FAIL(ans));
-
                     if (IS_OK(ans)) {
                         timeout_count = 0;
                         local_scan[0].sync = Node_NotSync;
@@ -797,7 +788,7 @@ result_t YDlidarDriver::checkDeviceInfo(uint8_t *recvBuffer, uint8_t byte,
             if (async_size == sizeof(info_)) {
               asyncRecvPos = 0;
               async_size = 0;
-              get_device_info_success = true;
+              m_HasDeviceInfo = true;
 
               last_device_byte = byte;
               return RESULT_OK;
@@ -906,7 +897,7 @@ result_t YDlidarDriver::waitDevicePackage(uint32_t timeout) {
       }
     }
 
-    if (get_device_info_success) {
+    if (m_HasDeviceInfo) {
       ans = RESULT_OK;
       break;
     }
@@ -1727,7 +1718,7 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout)
   if (m_SingleChannel)
   {
     //获取启动时抛出的设备信息或每帧数据中的设备信息
-    if (get_device_info_success)
+    if (m_HasDeviceInfo)
     {
       info = this->info_;
       return RESULT_OK;
@@ -1742,9 +1733,9 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout)
   //双通，此处仅能获取到底板设备信息
   else
   {
-    flushSerial();
     if (m_Bottom)
     {
+      flushSerial();
       ScopedLocker l(_cmd_lock);
       if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != RESULT_OK)
         return ans;
@@ -1761,13 +1752,24 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout)
 
       getData(reinterpret_cast<uint8_t *>(&info), sizeof(info));
       model = info.model;
-      get_device_info_success = true;
+      m_HasDeviceInfo = true;
       info_ = info;
     }
     return ans;
   }
 }
 
+bool YDlidarDriver::getDeviceInfoEx(device_info &info)
+{
+  if (m_HasDeviceInfo)
+  {
+    info = info_;
+    return true;
+  }
+
+  return false;
+}
+
 /************************************************************************/
 /* the set to signal quality                                            */
 /************************************************************************/
@@ -1888,7 +1890,6 @@ result_t YDlidarDriver::startScan(bool force, uint32_t timeout)
     if (!m_SingleChannel)
     {
       lidar_ans_header response_header;
-
       if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK)
       {
         return ans;
@@ -1897,13 +1898,16 @@ result_t YDlidarDriver::startScan(bool force, uint32_t timeout)
       {
         return RESULT_FAIL;
       }
-
       if (response_header.size < 5)
       {
         return RESULT_FAIL;
       }
     }
 
+    //此处仅获取模组设备信息
+    if (!m_Bottom) {
+        waitDevicePackage(1000);
+    }
     //非Tmini系列雷达才自动获取强度标识
     if (!isTminiLidar(model))
     {

src/YDlidarDriver.h

@@ -171,6 +171,9 @@ class YDlidarDriver : public DriverInterface {
    */
   virtual result_t getDeviceInfo(device_info &info,
                                  uint32_t timeout = DEFAULT_TIMEOUT);
+
+  //获取设备信息
+  virtual bool getDeviceInfoEx(device_info &info);
 
   /**
    * @brief Turn on scanning \n
@@ -648,8 +651,7 @@ class YDlidarDriver : public DriverInterface {
   uint8_t  *headerBuffer;
   uint8_t  *infoBuffer;
   uint8_t  *healthBuffer;
-  bool     get_device_info_success;
-  bool     get_device_health_success;
+  bool get_device_health_success;
 
   int package_index;
   bool has_package_error;