优化三角雷达启动时间

YDLIDAR · Nov 11, 2024 · c8faaca · c8faaca
1 parent ad8e30f
commit c8faaca
Show file tree

Hide file tree

Showing 6 changed files with 74 additions and 39 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
 # version
 set(YDLIDAR_SDK_VERSION_MAJOR 1)
 set(YDLIDAR_SDK_VERSION_MINOR 2)
-set(YDLIDAR_SDK_VERSION_PATCH 7)
+set(YDLIDAR_SDK_VERSION_PATCH 8)
 set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
 
 ##########################################################

diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h
@@ -55,6 +55,8 @@ namespace ydlidar
         PropertyBuilderByName(std::string, OtaName, protected);
         //OTA文件加密
         PropertyBuilderByName(bool, OtaEncode, protected);
+        //是否启用自动强度判断
+        PropertyBuilderByName(bool, AutoIntensity, protected);
 
         /**
          * @par Constructor
@@ -90,6 +92,7 @@ namespace ydlidar
           m_ScanFreq = 0;
           m_Bottom = true;
           m_HasDeviceInfo = EPT_None;
+          m_AutoIntensity = true;
         }
 
         virtual ~DriverInterface() {}

diff --git a/examples/scl_test.cpp b/examples/scl_test.cpp
@@ -239,11 +239,13 @@ int main(int argc, char *argv[])
 
   //////////////////////bool property/////////////////
   /// fixed angle resolution
-  bool b_optvalue = true;
+  bool b_optvalue = false;
   laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
   /// rotate 180
+  b_optvalue = false;
   laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
   /// Counterclockwise
+  b_optvalue = false;
   laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
   b_optvalue = true;
   laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
@@ -278,7 +280,12 @@ int main(int argc, char *argv[])
   laser.enableSunNoise(false);
 
   //设置是否底板优先
-  laser.setBottomPriority(true);
+  laser.setBottomPriority(false);
+
+  //启用调试
+  laser.setEnableDebug(false);
+
+  laser.setAutoIntensity(false);
 
   uint32_t t = getms(); //时间
   int c = 0; //计数

diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp
@@ -437,7 +437,6 @@ bool CYdLidar::initialize()
 
   printf("[YDLIDAR] Lidar init success, Elapsed time %u ms\n", getms() - t);
   fflush(stdout);
-
   return true;
 }
 
@@ -975,6 +974,11 @@ bool CYdLidar::getDeviceInfo(std::vector<device_info_ex>& dis)
   return false;
 }
 
+void CYdLidar::setAutoIntensity(bool yes)
+{
+  m_AutoIntensity = yes;
+}
+
 bool CYdLidar::ota()
 {
   if (lidarPtr)
@@ -1345,8 +1349,6 @@ bool CYdLidar::getDeviceHealth()
     return false;
   }
 
-  lidarPtr->stop();
-
   result_t op_result;
   device_health healthinfo;
   memset(&healthinfo, 0, sizeof(device_health));
@@ -1429,14 +1431,15 @@ bool CYdLidar::getDeviceInfo()
   frequencyOffset = 0.4;
   lidar_model = di.model;
   printf("Current Lidar Model Code %d\n", lidar_model);
-  bool intensity = hasIntensity(di.model);
+  // bool intensity = hasIntensity(di.model);
+    // intensity = m_Intensity;
+      // lidarPtr->setIntensities(intensity);
+  //  printf("Set Lidar Intensity Bit count %d\n", m_IntensityBit);
+  // lidarPtr->setIntensityBit(m_IntensityBit);
   defalutSampleRate = getDefaultSampleRate(di.model);
   // printf("getDefaultSampleRate %d\n", defalutSampleRate.size());
-  intensity = m_Intensity;
+
   std::string serial_number;
-  lidarPtr->setIntensities(intensity);
-  //  printf("Set Lidar Intensity Bit count %d\n", m_IntensityBit);
-  lidarPtr->setIntensityBit(m_IntensityBit);
   ret = true;
 
   if (printfDeviceInfo(di, EPT_Base))
@@ -1798,10 +1801,26 @@ bool CYdLidar::checkCOMMs()
     return true;
   }
 
+  //初始化
+  lidarPtr->setSingleChannel(m_SingleChannel);
+  lidarPtr->setLidarType(m_LidarType);
+  lidarPtr->setScanFreq(m_ScanFrequency);
+  lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl);
+  lidarPtr->setBottom(m_Bottom);
+  lidarPtr->setDebug(m_Debug);
+  lidarPtr->setOtaName(otaName);
+  lidarPtr->setOtaEncode(otaEncode);
+  lidarPtr->setIntensities(m_Intensity);
+  lidarPtr->setIntensityBit(m_IntensityBit);
+  lidarPtr->setAutoIntensity(m_AutoIntensity);
+
+  uint32_t t = getms();
+
   // Is it COMX, X>4? ->  "\\.\COMX"
   if (m_SerialPort.size() >= 3)
   {
-    if (tolower(m_SerialPort[0]) == 'c' && tolower(m_SerialPort[1]) == 'o' &&
+    if (tolower(m_SerialPort[0]) == 'c' && 
+        tolower(m_SerialPort[1]) == 'o' &&
         tolower(m_SerialPort[2]) == 'm')
     {
       // Need to add "\\.\"?
@@ -1811,8 +1830,7 @@ bool CYdLidar::checkCOMMs()
       }
     }
   }
-
-  // make connection...
+  //连接
   result_t op_result = lidarPtr->connect(m_SerialPort.c_str(), m_SerialBaudrate);
   if (!IS_OK(op_result))
   {
@@ -1833,14 +1851,8 @@ bool CYdLidar::checkCOMMs()
     return false;
   }
 
-  lidarPtr->setSingleChannel(m_SingleChannel);
-  lidarPtr->setLidarType(m_LidarType);
-  lidarPtr->setScanFreq(m_ScanFrequency);
-  lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl);
-  lidarPtr->setBottom(m_Bottom);
-  lidarPtr->setDebug(m_Debug);
-  lidarPtr->setOtaName(otaName);
-  lidarPtr->setOtaEncode(otaEncode);
+  printf("[YDLIDAR] connect, Elapsed time %u ms\n", getms() - t);
+  fflush(stdout);
 
   printf("[YDLIDAR] Lidar successfully connected [%s:%d]\n", 
     m_SerialPort.c_str(), m_SerialBaudrate);
@@ -1852,9 +1864,11 @@ bool CYdLidar::checkCOMMs()
 -------------------------------------------------------------*/
 bool CYdLidar::checkStatus()
 {
+  uint32_t t = getms();
   getDeviceHealth();
-
   getDeviceInfo();
+  printf("[YDLIDAR] Check status, Elapsed time %u ms\n", getms() - t);
+  fflush(stdout);
 
   return true;
 }

diff --git a/src/CYdLidar.h b/src/CYdLidar.h
@@ -176,6 +176,8 @@ class YDLIDAR_API CYdLidar {
   bool getDeviceInfo(device_info& di, int type);
   //获取级联设备信息
   bool getDeviceInfo(std::vector<device_info_ex>& dis);
+  //设置是否自动识别强度（启用时会占用一定时间）
+  void setAutoIntensity(bool yes=false);
 
   //启用调试
   void setEnableDebug(bool yes) {m_Debug = yes;}
@@ -284,10 +286,9 @@ class YDLIDAR_API CYdLidar {
     */
   void handleSingleChannelDevice();
 
-
   /**
     * @brief Parse Version by Package Information
-    * @param debug  LiDAR Point CT Pakcage Information
+    * @param debug LiDAR Point CT Pakcage Information
     */
   bool getDeviceInfoByPackage(const LaserDebug &debug);
 
@@ -348,6 +349,7 @@ class YDLIDAR_API CYdLidar {
   bool m_SingleChannel;             ///< LiDAR single channel
   bool m_Intensity;                 ///< LiDAR Intensity
   int m_IntensityBit;               ///< LiDAR Intensity bit
+  bool m_AutoIntensity; //自动识别强度
   bool m_SupportMotorDtrCtrl;       ///< LiDAR Motor DTR
   bool m_SupportHearBeat;           ///< LiDAR HeartBeat
 

diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp
@@ -144,8 +144,9 @@ namespace ydlidar
         }
         else
         {
-          _serial = new serial::Serial(m_port, m_baudrate,
-                                       serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
+          _serial = new serial::Serial(
+            m_port, m_baudrate,
+            serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT/2));
         }
         _serial->bindport(port_path, baudrate);
       }
@@ -157,9 +158,12 @@ namespace ydlidar
       m_isConnected = true;
     }
 
-    stopScan();
-    delay(100);
-    clearDTR();
+    //如果是双通雷达，需要先停止
+    if (!m_SingleChannel)
+    {
+      printf("[YDLIDAR] Stop Lidar\n");
+      stop();
+    }
 
     return RESULT_OK;
   }
@@ -1924,8 +1928,9 @@ namespace ydlidar
       if (!IS_OK(ret))
         return ret;
 
-      if (!m_SingleChannel)
+      if (!m_SingleChannel) //双通雷达
       {
+        //双通雷达需要等待响应
         lidar_ans_header response_header;
         if ((ret = waitResponseHeader(&response_header, timeout)) != RESULT_OK)
         {
@@ -1939,17 +1944,21 @@ namespace ydlidar
         {
           return RESULT_FAIL;
         }
-      }
 
-      // 此处仅获取模组设备信息
-      {
-        waitDevicePackage(1000);
+        //此处仅获取模组设备信息
+        {
+          waitDevicePackage(1000);
+        }
       }
-      // 非Tmini系列雷达才自动获取强度标识
-      if (!isTminiLidar(model))
+
+      //非Tmini系列雷达才自动获取强度标识
+      if (m_AutoIntensity)
       {
-        // 获取强度标识
-        getIntensityFlag();
+        if (!isTminiLidar(model))
+        {
+          // 获取强度标识
+          getIntensityFlag();
+        }
       }
 
       // 创建数据解析线程