增加S2-PRO抗阳光玻璃干扰

YDLIDAR · Jun 24, 2022 · 04f737f · 04f737f
1 parent c5c38b7
commit 04f737f
Show file tree

Hide file tree

Showing 6 changed files with 55 additions and 27 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 1)
-set(YDLIDAR_SDK_VERSION_PATCH 0)
+set(YDLIDAR_SDK_VERSION_PATCH 1)
 set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})
 
 ##########################################################

diff --git a/core/common/ydlidar_protocol.h b/core/common/ydlidar_protocol.h
@@ -55,15 +55,15 @@
  * @name sun noise flag constant
  * @{
  */
-#define SUNNOISEINTENSITY 0xff
+#define SUNNOISEINTENSITY 0x03
 /** @}
 */
 
 /**
  * @name glass noise flag constant
  * @{
  */
-#define GLASSNOISEINTENSITY 0xfe
+#define GLASSNOISEINTENSITY 0x02
 /** @}
 */
 
@@ -183,6 +183,7 @@ typedef enum {
 //雷达节点信息
 struct node_info {
   uint8_t sync_flag; //首包标记
+  uint8_t is; //抗干扰标志
   uint16_t sync_quality; //信号强度
   uint16_t angle_q6_checkbit; //角度值（°）
   uint16_t distance_q2; //距离值

diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp
@@ -649,8 +649,12 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
     float angle = 0.0;
     debug.MaxDebugIndex = 0;
 
+
+    //遍历一圈点
     for (int i = 0; i < count; i++)
     {
+      const node_info& node = global_nodes[i];
+
       if (isNetTOFLidar(m_LidarType))
       {
         angle = static_cast<float>(global_nodes[i].angle_q6_checkbit / 100.0f) +
@@ -683,8 +687,6 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
         }
       }
 
-      // printf("%d %.03f\n", i, range);
-
       intensity = static_cast<float>(global_nodes[i].sync_quality);
 
       angle = math::from_degrees(angle);
@@ -722,10 +724,12 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan)
         range = 0.0;
       }
 
-      // valid range
-      if (!isRangeValid(range))
+      //过滤点
+      if (!isRangeValid(range) ||
+        (m_SunNoise && node.is == SUNNOISEINTENSITY) ||
+        (m_GlassNoise && node.is == GLASSNOISEINTENSITY))
       {
-        range = 0.0;
+        range = .0;
       }
 
       if (angle >= outscan.config.min_angle &&
@@ -867,6 +871,16 @@ bool CYdLidar::setWorkMode(int mode, uint8_t addr)
     return false;
 }
 
+void CYdLidar::enableSunNoise(bool e)
+{
+  m_SunNoise = e;
+}
+
+void CYdLidar::enableGlassNoise(bool e)
+{
+  m_GlassNoise = e;
+}
+
 /*-------------------------------------------------------------
                     isRangeValid
 -------------------------------------------------------------*/

diff --git a/src/CYdLidar.h b/src/CYdLidar.h
@@ -924,6 +924,20 @@ class YDLIDAR_API CYdLidar {
    * @return 成功返回true，否则返回false
    */
   bool setWorkMode(int mode, uint8_t addr=0x00);
+
+  /**
+   * @brief 是否开启阳光噪点过滤功能
+   * @param[in] e true开启，false关闭
+   * @return 无
+   */
+  void enableSunNoise(bool e=true);
+
+  /**
+   * @brief 是否开启玻璃噪点过滤功能
+   * @param[in] e true开启，false关闭
+   * @return 无
+   */
+  void enableGlassNoise(bool e=true);
 
  private:
   /**
@@ -1099,7 +1113,8 @@ class YDLIDAR_API CYdLidar {
   float m_MinRange;                 ///< LiDAR minimum range
   float m_ScanFrequency;            ///< LiDAR scanning frequency
 
-
+  bool m_SunNoise = true; //阳光噪点过滤标识
+  bool m_GlassNoise = true; //玻璃噪点过滤标识
 };	// End of class
 #endif // CYDLIDAR_H
 

diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp
@@ -1208,10 +1208,11 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
     (*node).delay_time = 0;
     (*node).stamp = getTime();
     (*node).scan_frequence = scan_frequence;
+    (*node).is = 0;
 
     if (CheckSumResult)
     {
-        if (m_intensities)
+        if (m_intensities) //如果带强度信息
         {
             if (isTriangleLidar(m_LidarType))
             {
@@ -1222,17 +1223,13 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
                 else
                 {
                     (*node).sync_quality = ((uint16_t)((package.packageSample[package_Sample_Index].PakageSampleDistance & 0x03) <<
-                                                       LIDAR_RESP_MEASUREMENT_ANGLE_SAMPLE_SHIFT) |
-                                            (package.packageSample[package_Sample_Index].PakageSampleQuality));
+                        LIDAR_RESP_MEASUREMENT_ANGLE_SAMPLE_SHIFT) |
+                        (package.packageSample[package_Sample_Index].PakageSampleQuality));
                 }
-//                printf("intensity(%d): %u 0x%x 0x%x\n",
-//                       m_intensityBit,
-//                       (*node).sync_quality,
-//                       uint8_t(package.packageSample[package_Sample_Index].PakageSampleDistance & 0x03),
-//                       uint8_t(package.packageSample[package_Sample_Index].PakageSampleQuality));
 
                 (*node).distance_q2 =
                         package.packageSample[package_Sample_Index].PakageSampleDistance & 0xfffc;
+                (*node).is = package.packageSample[package_Sample_Index].PakageSampleDistance & 0x0003;
             }
             else
             {
@@ -1242,14 +1239,15 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
                         tof_package.packageSample[package_Sample_Index].PakageSampleDistance;
             }
         }
-        else
+        else //如果不带强度信息
         {
             (*node).distance_q2 = packages.packageSampleDistance[package_Sample_Index];
 
-            if (isTriangleLidar(m_LidarType)) {
+            if (isTriangleLidar(m_LidarType)) 
+            {
                 (*node).sync_quality = ((uint16_t)(0xfc |
-                                                   packages.packageSampleDistance[package_Sample_Index] &
-                                                   0x0003)) << LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;
+                    packages.packageSampleDistance[package_Sample_Index] &
+                    0x0003)) << LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;
             }
         }
 
@@ -1296,14 +1294,14 @@ void YDlidarDriver::parseNodeFromeBuffer(node_info *node)
             }
         }
     } else {
-        (*node).sync_flag       = Node_NotSync;
-        (*node).sync_quality    = Node_Default_Quality;
+        (*node).sync_flag = Node_NotSync;
+        (*node).sync_quality = Node_Default_Quality;
         (*node).angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT;
-        (*node).distance_q2      = 0;
-        (*node).scan_frequence  = 0;
+        (*node).distance_q2 = 0;
+        (*node).scan_frequence = 0;
     }
 
-    package_Sample_Index++;
+    package_Sample_Index ++;
 
     if (package_Sample_Index >= nowPackageNum) {
         package_Sample_Index = 0;

diff --git a/src/filters/NoiseFilter.cpp b/src/filters/NoiseFilter.cpp
@@ -576,7 +576,7 @@ void NoiseFilter::filter_tail2(
             noiseCount ++;
         }
     }
-    printf("Noise count %u\n", noiseCount);
+    printf("Noise count %lu\n", noiseCount);
 }
 
 void NoiseFilter::setStrategy(int value)