针对TG系列转速虚高0.4Hz修改

examples/gs_test2.cpp

@@ -46,7 +46,7 @@ struct YdGsOutParam
     memset(items, 0, sizeof(items));
   }
 };
-//3D点
+//2D点
 struct Yd2DPoint
 {
   float x = .0; //x值，单位mm；弧度值，单位弧度
@@ -249,9 +249,9 @@ int main(int argc, char *argv[])
     return -1;
   }
   //设置雷达工作模式（0表示避障模式，1表示沿边模式）
-  ret &= laser.setWorkMode(0, 0x01);
-  ret &= laser.setWorkMode(0, 0x02);
-  ret &= laser.setWorkMode(1, 0x04);
+  ret &= laser.setWorkMode(0, LIDAR_MODULE_1);
+  ret &= laser.setWorkMode(0, LIDAR_MODULE_2);
+  ret &= laser.setWorkMode(1, LIDAR_MODULE_3);
   if (!ret)
   {
     fprintf(stderr, "Fail to set work mode %s\n", laser.DescribeError());
@@ -279,9 +279,9 @@ int main(int argc, char *argv[])
   ops[LIDAR2].rp.laserPitch = 17.0;
   ops[LIDAR2].rp.modulePitch = 15.0;
   //从文件中解析外参
-  if (!parseCsv("../examples/data/lidar0.csv", ops[0]) ||
-    !parseCsv("../examples/data/lidar1.csv", ops[1]) ||
-    !parseCsv("../examples/data/lidar2.csv", ops[2]))
+  if (!parseCsv("../examples/data/lidar0.csv", ops[LIDAR0]) ||
+    !parseCsv("../examples/data/lidar1.csv", ops[LIDAR1]) ||
+    !parseCsv("../examples/data/lidar2.csv", ops[LIDAR2]))
   {
     return -1;
   }
@@ -351,10 +351,11 @@ bool parseCsv(const std::string& name, YdGsOutParam& op)
       &op.items[index].k0,
       &op.items[index].k1) == 4) //解析成功
     {
-      std::cout << op.items[index].b0 << " "
-        << op.items[index].b1 << " "
-        << op.items[index].k0 << " "
-        << op.items[index].k1 << std::endl;
+      //打印参数
+      // std::cout << op.items[index].b0 << " "
+      //   << op.items[index].b1 << " "
+      //   << op.items[index].k0 << " "
+      //   << op.items[index].k1 << std::endl;
       index ++;
       if (index >= GS_MAXPOINTSIZE)
         break;

examples/tof_test.cpp

@@ -213,11 +213,13 @@ int main(int argc, char *argv[]) {
 
   //////////////////////bool property/////////////////
   /// fixed angle resolution
-  bool b_optvalue = false;
+  bool b_optvalue = true;
   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));
@@ -266,8 +268,8 @@ int main(int argc, char *argv[]) {
     /// Turn On success and loop
     if (laser.doProcessSimple(scan))
     {
-      fprintf(stdout, "Scan received at [%lu] %u points is [%f]s\n",
-              scan.stamp / 100000,
+      fprintf(stdout, "Scan received at [%f]Hz %u points is [%f]s\n",
+              scan.scanFreq,
               (unsigned int)scan.points.size(),
               scan.config.scan_time);
 

src/CYdLidar.cpp

@@ -1587,8 +1587,10 @@ bool CYdLidar::checkScanFrequency()
     if (IS_OK(ans))
     {
       frequency = _scan_frequency.frequency / 100.f;
+      if (isTOFLidar(m_LidarType)) //TG雷达转速虚高0.4需要减去还原真实转速
+        frequency -= 0.4;
       hz = m_ScanFrequency - frequency;
-
+      printf("[YDLIDAR] Current scan frequency: %.02fHz\n", frequency);
       if (hz > 0)
       {
         //大调速
@@ -1635,6 +1637,8 @@ bool CYdLidar::checkScanFrequency()
   if (IS_OK(ans))
   {
     frequency = _scan_frequency.frequency / 100.0f;
+    if (isTOFLidar(m_LidarType)) //TG雷达转速虚高0.4需要减去还原真实转速
+        frequency -= 0.4;
     m_ScanFrequency = frequency;
   }
 

src/YDlidarDriver.cpp

@@ -308,7 +308,7 @@ result_t YDlidarDriver::sendData(const uint8_t *data, size_t size) {
       return RESULT_FAIL;
     }
 
-    if (m_Debug)
+    // if (m_Debug)
     {
       printf("send: ");
       printHex(data, r);