修改GS可打印帧率

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 4)
+set(YDLIDAR_SDK_VERSION_PATCH 5)
 set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
 
 ##########################################################

examples/gs_test.cpp

@@ -269,9 +269,10 @@ int main(int argc, char *argv[])
   {
     if (laser.doProcessSimple(scan))
     {
-      printf("Module [%d] [%d] points\n",
+      printf("Module [%d] [%d] points in [%.02f]Hz\n",
         scan.moduleNum,
-        int(scan.points.size()));
+        int(scan.points.size()),
+        scan.scanFreq);
 
       //打印帧间隔
       // uint32_t t = getms();

src/CYdLidar.cpp

@@ -731,7 +731,8 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
             scanfrequency = global_nodes[i].scanFreq / 10.0 + 3.0;
           }
         }
-        else if (isTEALidar(lidar_model)) //TEA雷达转速范围10~30，无缩放
+        else if (isTEALidar(lidar_model) ||
+          isGSLidar(m_LidarType)) //TEA雷达转速范围10~30，无缩放
         {
           scanfrequency = global_nodes[i].scanFreq; 
         }

src/GSLidarDriver.cpp

@@ -596,6 +596,7 @@ int GSLidarDriver::cacheScanData()
 
     int timeout_count = 0;
     retryCount = 0;
+    lastStamp = 0;
 
     m_isScanning = true;
 
@@ -822,6 +823,11 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout)
                             else
                             {
                                 CheckSumResult = true;
+                                if (lastStamp > 0)
+                                {
+                                    m_ScanFreq = 1000.0 / (getms() - lastStamp);
+                                }
+                                lastStamp = getms();
                             }
                             break;
                         }
@@ -848,7 +854,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout)
         stamp = (*node).stamp;
 
         (*node).index = 0x03 & (moduleNum >> 1); //模组地址转编号: 1, 2, 4
-        (*node).scanFreq = 0;
+        (*node).scanFreq = m_ScanFreq;
         (*node).qual = 0;
         (*node).sync = Node_NotSync;
 

src/GSLidarDriver.h

@@ -509,6 +509,7 @@ namespace ydlidar
     uint64_t stamp = 0; //时间戳
     std::list<gs_module_nodes> datas; //各模组数据
     double m_pitchAngle = Angle_PAngle;
+    uint32_t lastStamp = 0; //上一次时间
   };
 
 } // namespace ydlidar