From 9177c43913cf8ebbdc59971fc63b2cf8c2ebe43d Mon Sep 17 00:00:00 2001 From: eaibot Date: Fri, 11 Feb 2022 17:11:28 +0800 Subject: [PATCH] The problem of intensity bit resolution of S2 lidar is solved --- cmake/script_show_final_summary.cmake | 0 core/common/DriverInterface.h | 4 + core/common/ydlidar_def.h | 1 + doc/Dataset.md | 0 doc/Diagram.md | 0 doc/FAQs/General_FAQs.md | 0 doc/FAQs/General_FAQs_cn.md | 0 doc/FAQs/Hardware_FAQs.md | 0 doc/FAQs/Hardware_FAQs_cn.md | 0 doc/FAQs/README.md | 0 doc/FAQs/Software_FAQs.md | 0 doc/FAQs/Software_FAQs_cn.md | 0 doc/README.md | 0 doc/Tutorials.md | 0 doc/YDLIDAR_SDK_API_for_Developers.md | 0 doc/howto/README.md | 0 .../how_to_build_and_debug_using_vscode.md | 0 doc/howto/how_to_build_and_install.md | 0 doc/howto/how_to_create_a_csharp_project.md | 0 doc/howto/how_to_create_a_pull.md | 0 doc/howto/how_to_create_a_udev_rules.md | 0 .../how_to_gerenrate_vs_project_by_cmake.md | 0 doc/howto/how_to_solve_slow_pull_from_cn.md | 0 doc/quickstart/README.md | 0 ...ydlidar_sdk_software_installation_guide.md | 0 python/examples/test.py | 4 +- python/examples/ydlidar_test.py | 3 +- samples/tof_test.cpp | 2 +- samples/ydlidar_test.cpp | 28 +-- src/CYdLidar.cpp | 12 ++ src/CYdLidar.h | 1 + src/ydlidar_driver.cpp | 159 ++++++++++-------- startup/initenv.sh | 0 33 files changed, 127 insertions(+), 87 deletions(-) mode change 100755 => 100644 cmake/script_show_final_summary.cmake mode change 100755 => 100644 doc/Dataset.md mode change 100755 => 100644 doc/Diagram.md mode change 100755 => 100644 doc/FAQs/General_FAQs.md mode change 100755 => 100644 doc/FAQs/General_FAQs_cn.md mode change 100755 => 100644 doc/FAQs/Hardware_FAQs.md mode change 100755 => 100644 doc/FAQs/Hardware_FAQs_cn.md mode change 100755 => 100644 doc/FAQs/README.md mode change 100755 => 100644 doc/FAQs/Software_FAQs.md mode change 100755 => 100644 doc/FAQs/Software_FAQs_cn.md mode change 100755 => 100644 doc/README.md mode change 100755 => 100644 doc/Tutorials.md mode change 100755 => 100644 doc/YDLIDAR_SDK_API_for_Developers.md mode change 100755 => 100644 doc/howto/README.md mode change 100755 => 100644 doc/howto/how_to_build_and_debug_using_vscode.md mode change 100755 => 100644 doc/howto/how_to_build_and_install.md mode change 100755 => 100644 doc/howto/how_to_create_a_csharp_project.md mode change 100755 => 100644 doc/howto/how_to_create_a_pull.md mode change 100755 => 100644 doc/howto/how_to_create_a_udev_rules.md mode change 100755 => 100644 doc/howto/how_to_gerenrate_vs_project_by_cmake.md mode change 100755 => 100644 doc/howto/how_to_solve_slow_pull_from_cn.md mode change 100755 => 100644 doc/quickstart/README.md mode change 100755 => 100644 doc/quickstart/ydlidar_sdk_software_installation_guide.md mode change 100755 => 100644 startup/initenv.sh diff --git a/cmake/script_show_final_summary.cmake b/cmake/script_show_final_summary.cmake old mode 100755 new mode 100644 diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h index 1991d2d..6e77203 100644 --- a/core/common/DriverInterface.h +++ b/core/common/DriverInterface.h @@ -100,6 +100,7 @@ class DriverInterface { DriverInterface() : serial_port(""), m_baudrate(8000), m_intensities(false), + m_intensityBit(10), scan_node_buf(NULL), scan_node_count(0), package_Sample_Index(0), @@ -234,6 +235,7 @@ class DriverInterface { * false no intensity */ virtual void setIntensities(const bool &isintensities) = 0; + virtual void setIntensityBit(int bit) {m_intensityBit = bit;} /** * @brief whether to support hot plug \n @@ -503,6 +505,8 @@ class DriverInterface { uint32_t m_baudrate; /// LiDAR intensity bool m_intensities; + /// LiDAR intensity bit + int m_intensityBit; /// LiDAR Point pointer node_info *scan_node_buf; diff --git a/core/common/ydlidar_def.h b/core/common/ydlidar_def.h index 41c1e14..ab45ee5 100644 --- a/core/common/ydlidar_def.h +++ b/core/common/ydlidar_def.h @@ -62,6 +62,7 @@ typedef enum { LidarPropDeviceType,/**< lidar connection type code */ LidarPropSampleRate,/**< lidar sample rate */ LidarPropAbnormalCheckCount,/**< abnormal maximum check times */ + LidarPropIntenstiyBit,/**< lidar intensity bit count */ /* float properties */ LidarPropMaxRange = 20,/**< lidar maximum range */ LidarPropMinRange,/**< lidar minimum range */ diff --git a/doc/Dataset.md b/doc/Dataset.md old mode 100755 new mode 100644 diff --git a/doc/Diagram.md b/doc/Diagram.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/General_FAQs.md b/doc/FAQs/General_FAQs.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/General_FAQs_cn.md b/doc/FAQs/General_FAQs_cn.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/Hardware_FAQs.md b/doc/FAQs/Hardware_FAQs.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/Hardware_FAQs_cn.md b/doc/FAQs/Hardware_FAQs_cn.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/README.md b/doc/FAQs/README.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/Software_FAQs.md b/doc/FAQs/Software_FAQs.md old mode 100755 new mode 100644 diff --git a/doc/FAQs/Software_FAQs_cn.md b/doc/FAQs/Software_FAQs_cn.md old mode 100755 new mode 100644 diff --git a/doc/README.md b/doc/README.md old mode 100755 new mode 100644 diff --git a/doc/Tutorials.md b/doc/Tutorials.md old mode 100755 new mode 100644 diff --git a/doc/YDLIDAR_SDK_API_for_Developers.md b/doc/YDLIDAR_SDK_API_for_Developers.md old mode 100755 new mode 100644 diff --git a/doc/howto/README.md b/doc/howto/README.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_build_and_debug_using_vscode.md b/doc/howto/how_to_build_and_debug_using_vscode.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_build_and_install.md b/doc/howto/how_to_build_and_install.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_create_a_csharp_project.md b/doc/howto/how_to_create_a_csharp_project.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_create_a_pull.md b/doc/howto/how_to_create_a_pull.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_create_a_udev_rules.md b/doc/howto/how_to_create_a_udev_rules.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_gerenrate_vs_project_by_cmake.md b/doc/howto/how_to_gerenrate_vs_project_by_cmake.md old mode 100755 new mode 100644 diff --git a/doc/howto/how_to_solve_slow_pull_from_cn.md b/doc/howto/how_to_solve_slow_pull_from_cn.md old mode 100755 new mode 100644 diff --git a/doc/quickstart/README.md b/doc/quickstart/README.md old mode 100755 new mode 100644 diff --git a/doc/quickstart/ydlidar_sdk_software_installation_guide.md b/doc/quickstart/ydlidar_sdk_software_installation_guide.md old mode 100755 new mode 100644 diff --git a/python/examples/test.py b/python/examples/test.py index b1b9bdf..941dcdf 100644 --- a/python/examples/test.py +++ b/python/examples/test.py @@ -24,8 +24,8 @@ r = laser.doProcessSimple(scan); if r: print("Scan received[",scan.stamp,"]:",scan.points.size(),"ranges is [",1.0/scan.config.scan_time,"]Hz"); - # for point in scan.points: - # print("angle:", point.angle, " range: ", point.range) + for point in scan.points: + print("angle:", point.angle, " range: ", point.range) else : print("Failed to get Lidar Data") time.sleep(0.05); diff --git a/python/examples/ydlidar_test.py b/python/examples/ydlidar_test.py index 9943b63..cbdbdb7 100644 --- a/python/examples/ydlidar_test.py +++ b/python/examples/ydlidar_test.py @@ -14,12 +14,13 @@ laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE); laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL); laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0); - laser.setlidaropt(ydlidar.LidarPropSampleRate, 9); + laser.setlidaropt(ydlidar.LidarPropSampleRate, 5); laser.setlidaropt(ydlidar.LidarPropSingleChannel, False); laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0); laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0); laser.setlidaropt(ydlidar.LidarPropMaxRange, 16.0); laser.setlidaropt(ydlidar.LidarPropMinRange, 0.08); + laser.setlidaropt(ydlidar.LidarPropIntenstiy, False); ret = laser.initialize(); if ret: diff --git a/samples/tof_test.cpp b/samples/tof_test.cpp index ff72321..1a41ae0 100644 --- a/samples/tof_test.cpp +++ b/samples/tof_test.cpp @@ -257,7 +257,7 @@ int main(int argc, char *argv[]) { if (laser.doProcessSimple(scan)) { fprintf(stdout, "Scan received[%llu]: %u ranges is [%f]Hz\n", - scan.stamp, + scan.stamp/1000000, (unsigned int)scan.points.size(), 1.0 / scan.config.scan_time); fflush(stdout); } else { diff --git a/samples/ydlidar_test.cpp b/samples/ydlidar_test.cpp index 71b1829..b4e5a11 100644 --- a/samples/ydlidar_test.cpp +++ b/samples/ydlidar_test.cpp @@ -209,6 +209,9 @@ int main(int argc, char *argv[]) { /// abnormal count optval = 4; laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); + /// Intenstiy bit count + optval = 10; + laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); //////////////////////bool property///////////////// /// fixed angle resolution @@ -223,7 +226,7 @@ int main(int argc, char *argv[]) { /// one-way communication laser.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool)); /// intensity - b_optvalue = false; + b_optvalue = true; laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); /// Motor DTR b_optvalue = true; @@ -257,17 +260,18 @@ int main(int argc, char *argv[]) { LaserScan scan; - while (ret && ydlidar::os_isOk()) { - if (laser.doProcessSimple(scan)) { - fprintf(stdout, "Scan received[%llu]: %u ranges is [%f]Hz\n", - scan.stamp, - (unsigned int)scan.points.size(), 1.0 / scan.config.scan_time); - fflush(stdout); - } else { - fprintf(stderr, "Failed to get Lidar Data\n"); - fflush(stderr); - } - + while (ret && ydlidar::os_isOk()) + { + if (laser.doProcessSimple(scan)) { + fprintf(stdout, "Scan received[%llu]: %u ranges is [%f]Hz\n", + scan.stamp/1000000, + (unsigned int)scan.points.size(), 1.0 / scan.config.scan_time); + fflush(stdout); + } + else { + fprintf(stderr, "Failed to get Lidar Data\n"); + fflush(stderr); + } } laser.turnOff(); diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp index 1b6912e..5ab220d 100644 --- a/src/CYdLidar.cpp +++ b/src/CYdLidar.cpp @@ -71,6 +71,7 @@ CYdLidar::CYdLidar(): lidarPtr(nullptr) { m_AngleOffset = 0.0f; lidar_model = DriverInterface::YDLIDAR_G2B; m_Intensity = false; + m_IntensityBit = 10; last_node_time = getTime(); global_nodes = new node_info[DriverInterface::MAX_SCAN_NODES]; last_frequency = 0; @@ -182,6 +183,9 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { case LidarPropIntenstiy: m_Intensity = *(bool *)(optval); break; + case LidarPropIntenstiyBit: + m_IntensityBit = *(int *)(optval); + break; case LidarPropSupportMotorDtrCtrl: m_SupportMotorDtrCtrl = *(bool *)(optval); @@ -315,6 +319,9 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) { case LidarPropIntenstiy: memcpy(optval, &m_Intensity, optlen); break; + case LidarPropIntenstiyBit: + memcpy(optval, &m_IntensityBit, optlen); + break; case LidarPropSupportMotorDtrCtrl: memcpy(optval, &m_SupportMotorDtrCtrl, optlen); @@ -592,6 +599,9 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) { } intensity = static_cast(global_nodes[i].sync_quality); + +// printf("intensity: %f\n", intensity); + angle = math::from_degrees(angle); if (global_nodes[i].scan_frequence != 0) { @@ -1156,6 +1166,8 @@ bool CYdLidar::getDeviceInfo() { 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 (printfVersionInfo(devinfo, m_SerialPort, m_SerialBaudrate)) { diff --git a/src/CYdLidar.h b/src/CYdLidar.h index 4e6f742..fe46498 100644 --- a/src/CYdLidar.h +++ b/src/CYdLidar.h @@ -1073,6 +1073,7 @@ class YDLIDAR_API CYdLidar { bool m_AutoReconnect; ///< LiDAR hot plug bool m_SingleChannel; ///< LiDAR single channel bool m_Intensity; ///< LiDAR Intensity + int m_IntensityBit; ///< LiDAR Intensity bit bool m_SupportMotorDtrCtrl; ///< LiDAR Motor DTR bool m_SupportHearBeat; ///< LiDAR HeartBeat diff --git a/src/ydlidar_driver.cpp b/src/ydlidar_driver.cpp index 47c6c6c..f12e085 100644 --- a/src/ydlidar_driver.cpp +++ b/src/ydlidar_driver.cpp @@ -1197,87 +1197,104 @@ void YDlidarDriver::parseNodeDebugFromBuffer(node_info *node) { } void YDlidarDriver::parseNodeFromeBuffer(node_info *node) { - int32_t AngleCorrectForDistance = 0; - (*node).sync_quality = Node_Default_Quality; - (*node).delay_time = 0; - (*node).stamp = 0; - - if (CheckSumResult) { - if (m_intensities) { - if (isTriangleLidar(m_LidarType)) { - (*node).sync_quality = ((uint16_t)(( - package.packageSample[package_Sample_Index].PakageSampleDistance & 0x03) << - LIDAR_RESP_MEASUREMENT_ANGLE_SAMPLE_SHIFT) | - (package.packageSample[package_Sample_Index].PakageSampleQuality)); - (*node).distance_q2 = - package.packageSample[package_Sample_Index].PakageSampleDistance & 0xfffc; - } else { - (*node).sync_quality = - tof_package.packageSample[package_Sample_Index].PakageSampleQuality; - (*node).distance_q2 = - tof_package.packageSample[package_Sample_Index].PakageSampleDistance; - } - } else { - (*node).distance_q2 = packages.packageSampleDistance[package_Sample_Index]; + int32_t AngleCorrectForDistance = 0; + (*node).sync_quality = Node_Default_Quality; + (*node).delay_time = 0; + (*node).stamp = 0; - if (isTriangleLidar(m_LidarType)) { - (*node).sync_quality = ((uint16_t)(0xfc | - packages.packageSampleDistance[package_Sample_Index] & - 0x0003)) << LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; - } + if (CheckSumResult) { + if (m_intensities) + { + if (isTriangleLidar(m_LidarType)) + { + if (8 == m_intensityBit) + { + (*node).sync_quality = uint16_t(package.packageSample[package_Sample_Index].PakageSampleQuality); + } + 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)); + } +// 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; + } + else + { + (*node).sync_quality = + tof_package.packageSample[package_Sample_Index].PakageSampleQuality; + (*node).distance_q2 = + tof_package.packageSample[package_Sample_Index].PakageSampleDistance; + } + } + else + { + (*node).distance_q2 = packages.packageSampleDistance[package_Sample_Index]; + + if (isTriangleLidar(m_LidarType)) { + (*node).sync_quality = ((uint16_t)(0xfc | + packages.packageSampleDistance[package_Sample_Index] & + 0x0003)) << LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; + } + } - } + if ((*node).distance_q2 != 0) { + if (isOctaveLidar(model)) { + AngleCorrectForDistance = (int32_t)(((atan(((21.8 * (155.3 - (( + *node).distance_q2 / 2.0))) / 155.3) / (( + *node).distance_q2 / 2.0))) * 180.0 / 3.1415) * 64.0); + } else { + if (isTriangleLidar(m_LidarType)) { + AngleCorrectForDistance = (int32_t)(((atan(((21.8 * (155.3 - (( + *node).distance_q2 / 4.0))) / 155.3) / (( + *node).distance_q2 / 4.0))) * 180.0 / 3.1415) * 64.0); + } + } - if ((*node).distance_q2 != 0) { - if (isOctaveLidar(model)) { - AngleCorrectForDistance = (int32_t)(((atan(((21.8 * (155.3 - (( - *node).distance_q2 / 2.0))) / 155.3) / (( - *node).distance_q2 / 2.0))) * 180.0 / 3.1415) * 64.0); - } else { - if (isTriangleLidar(m_LidarType)) { - AngleCorrectForDistance = (int32_t)(((atan(((21.8 * (155.3 - (( - *node).distance_q2 / 4.0))) / 155.3) / (( - *node).distance_q2 / 4.0))) * 180.0 / 3.1415) * 64.0); + m_InvalidNodeCount++; + } else { + AngleCorrectForDistance = 0; } - } - m_InvalidNodeCount++; - } else { - AngleCorrectForDistance = 0; - } + float sampleAngle = IntervalSampleAngle * package_Sample_Index; - float sampleAngle = IntervalSampleAngle * package_Sample_Index; - - if ((FirstSampleAngle + sampleAngle + - AngleCorrectForDistance) < 0) { - (*node).angle_q6_checkbit = (((uint16_t)(FirstSampleAngle + sampleAngle + - AngleCorrectForDistance + 23040)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + - LIDAR_RESP_MEASUREMENT_CHECKBIT; + if ((FirstSampleAngle + sampleAngle + + AngleCorrectForDistance) < 0) { + (*node).angle_q6_checkbit = (((uint16_t)(FirstSampleAngle + sampleAngle + + AngleCorrectForDistance + 23040)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + + LIDAR_RESP_MEASUREMENT_CHECKBIT; + } else { + if ((FirstSampleAngle + sampleAngle + AngleCorrectForDistance) > 23040) { + (*node).angle_q6_checkbit = (((uint16_t)(FirstSampleAngle + sampleAngle + + AngleCorrectForDistance - 23040)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + + LIDAR_RESP_MEASUREMENT_CHECKBIT; + } else { + (*node).angle_q6_checkbit = (((uint16_t)(FirstSampleAngle + sampleAngle + + AngleCorrectForDistance)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + + LIDAR_RESP_MEASUREMENT_CHECKBIT; + } + } } else { - if ((FirstSampleAngle + sampleAngle + AngleCorrectForDistance) > 23040) { - (*node).angle_q6_checkbit = (((uint16_t)(FirstSampleAngle + sampleAngle + - AngleCorrectForDistance - 23040)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + - LIDAR_RESP_MEASUREMENT_CHECKBIT; - } else { - (*node).angle_q6_checkbit = (((uint16_t)(FirstSampleAngle + sampleAngle + - AngleCorrectForDistance)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + - LIDAR_RESP_MEASUREMENT_CHECKBIT; - } + (*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; } - } else { - (*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; - } - package_Sample_Index++; + package_Sample_Index++; - if (package_Sample_Index >= nowPackageNum) { - package_Sample_Index = 0; - CheckSumResult = false; - } + if (package_Sample_Index >= nowPackageNum) { + package_Sample_Index = 0; + CheckSumResult = false; + } } result_t YDlidarDriver::waitScanData(node_info *nodebuffer, size_t &count, diff --git a/startup/initenv.sh b/startup/initenv.sh old mode 100755 new mode 100644