diff --git a/CMakeLists.txt b/CMakeLists.txt index 681e373..1678b50 100644 --- 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 index 88627cc..7f6649d 100644 --- a/core/common/ydlidar_protocol.h +++ b/core/common/ydlidar_protocol.h @@ -55,7 +55,7 @@ * @name sun noise flag constant * @{ */ -#define SUNNOISEINTENSITY 0xff +#define SUNNOISEINTENSITY 0x03 /** @} */ @@ -63,7 +63,7 @@ * @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 index 094a80d..c797635 100644 --- 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(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(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 index 3b2ab85..9a5e47d 100644 --- 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 index b836939..b1ad5d5 100644 --- 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 index 0c77864..5ac85ca 100644 --- 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)