The problem of intensity bit resolution of S2 lidar is solved

YDLIDAR · Feb 11, 2022 · 9177c43 · 9177c43
1 parent 8b287ed
commit 9177c43
Show file tree

Hide file tree

Showing 33 changed files with 127 additions and 87 deletions.
diff --git a/cmake/script_show_final_summary.cmake b/cmake/script_show_final_summary.cmake
diff --git 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
@@ -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
diff --git a/doc/Diagram.md b/doc/Diagram.md
diff --git a/doc/FAQs/General_FAQs.md b/doc/FAQs/General_FAQs.md
diff --git a/doc/FAQs/General_FAQs_cn.md b/doc/FAQs/General_FAQs_cn.md
diff --git a/doc/FAQs/Hardware_FAQs.md b/doc/FAQs/Hardware_FAQs.md
diff --git a/doc/FAQs/Hardware_FAQs_cn.md b/doc/FAQs/Hardware_FAQs_cn.md
diff --git a/doc/FAQs/README.md b/doc/FAQs/README.md
diff --git a/doc/FAQs/Software_FAQs.md b/doc/FAQs/Software_FAQs.md
diff --git a/doc/FAQs/Software_FAQs_cn.md b/doc/FAQs/Software_FAQs_cn.md
diff --git a/doc/README.md b/doc/README.md
diff --git a/doc/Tutorials.md b/doc/Tutorials.md
diff --git a/doc/YDLIDAR_SDK_API_for_Developers.md b/doc/YDLIDAR_SDK_API_for_Developers.md
diff --git a/doc/howto/README.md b/doc/howto/README.md
diff --git a/doc/howto/how_to_build_and_debug_using_vscode.md b/doc/howto/how_to_build_and_debug_using_vscode.md
diff --git a/doc/howto/how_to_build_and_install.md b/doc/howto/how_to_build_and_install.md
diff --git a/doc/howto/how_to_create_a_csharp_project.md b/doc/howto/how_to_create_a_csharp_project.md
diff --git a/doc/howto/how_to_create_a_pull.md b/doc/howto/how_to_create_a_pull.md
diff --git a/doc/howto/how_to_create_a_udev_rules.md b/doc/howto/how_to_create_a_udev_rules.md
diff --git a/doc/howto/how_to_gerenrate_vs_project_by_cmake.md b/doc/howto/how_to_gerenrate_vs_project_by_cmake.md
diff --git a/doc/howto/how_to_solve_slow_pull_from_cn.md b/doc/howto/how_to_solve_slow_pull_from_cn.md
diff --git a/doc/quickstart/README.md b/doc/quickstart/README.md
diff --git a/doc/quickstart/ydlidar_sdk_software_installation_guide.md b/doc/quickstart/ydlidar_sdk_software_installation_guide.md
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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<float>(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
@@ -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
@@ -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