Neato bugfix

kaiaai · Feb 2, 2024 · e843e83 · e843e83
1 parent 9ad754b
commit e843e83
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 13 deletions.
diff --git a/src/LDS_NEATO_XV11.cpp b/src/LDS_NEATO_XV11.cpp
@@ -223,7 +223,7 @@ LDS::result_t LDS_NEATO_XV11::processByte(int inByte) {
 
           int angle = startingAngle + ix;
           bool scan_completed = angle == 0;
-          if (!cw))
+          if (!cw)
             angle = (360 - angle) % 360;
           postScanPoint(angle, err ? 0 : aryDist[ix], aryQuality[ix], scan_completed);
         }

diff --git a/src/LDS_YDLIDAR_X4.cpp b/src/LDS_YDLIDAR_X4.cpp
@@ -325,15 +325,16 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
 
   // Process the buffered packet
   while(true) {
-    uint8_t package_CT;
+//    uint8_t package_CT;
     node_info node;
 
-    package_CT = package.package_CT;    
-    if (package_CT == CT_NORMAL) {
-      node.sync_quality = NODE_DEFAULT_QUALITY + NODE_NOT_SYNC;
-    } else {
-      node.sync_quality = NODE_DEFAULT_QUALITY + NODE_SYNC;
-    }
+    //package_CT = package.package_CT;    
+    //if ((package_CT & 0x01) == CT_NORMAL) {
+    //  node.sync_quality = NODE_DEFAULT_QUALITY + NODE_NOT_SYNC;
+    //} else {
+    //  node.sync_quality = NODE_DEFAULT_QUALITY + NODE_SYNC;
+    //}
+    node.sync_quality = NODE_DEFAULT_QUALITY;
 
     if (CheckSumResult == true) {
       int32_t AngleCorrectForDistance;
@@ -365,7 +366,7 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
       }
     } else {
       // Invalid checksum
-      node.sync_quality = NODE_DEFAULT_QUALITY + NODE_NOT_SYNC;
+      //node.sync_quality = NODE_DEFAULT_QUALITY + NODE_NOT_SYNC;
       node.angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT;
       node.distance_q2 = 0;
       package_Sample_Index = 0;
@@ -375,8 +376,9 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
 
     // Dump out processed data
     float point_distance_mm = node.distance_q2*0.25f;
-    float point_angle = (node.angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
-    uint8_t point_quality = (node.sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+    float point_angle = (node.angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)*0.015625f; // /64.0f
+    //uint8_t point_quality = (node.sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
+    uint8_t point_quality = node.sync_quality;
     //bool point_startBit = (node.sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT);
 
     //postScanPoint(point_angle, point_distance_mm, point_quality, point_startBit);

diff --git a/src/LDS_YDLIDAR_X4.h b/src/LDS_YDLIDAR_X4.h
@@ -131,7 +131,7 @@ class LDS_YDLIDAR_X4 : public LDS {
     static const uint8_t LIDAR_ANS_TYPE_MEASUREMENT = 0x81;
 
     static const uint8_t LIDAR_RESP_MEASUREMENT_SYNCBIT = (0x1<<0);
-    static const uint8_t LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT = 2;
+    //static const uint8_t LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT = 2;
     static const uint8_t LIDAR_RESP_MEASUREMENT_CHECKBIT = (0x1<<0);
     static const uint8_t LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT = 1;
     static const uint8_t LIDAR_RESP_MEASUREMENT_ANGLE_SAMPLE_SHIFT = 8;
@@ -151,7 +151,7 @@ class LDS_YDLIDAR_X4 : public LDS {
     static const uint8_t LIDAR_STATUS_ERROR = 0x2;
 
     static const uint8_t PACKAGE_SAMPLE_BYTES = 2;
-    static const uint16_t NODE_DEFAULT_QUALITY = (10<<2);
+    static const uint16_t NODE_DEFAULT_QUALITY = 10; // (10<<2)
     static const uint8_t NODE_SYNC = 0x01;
     static const uint8_t NODE_NOT_SYNC = 2;
     static const uint8_t PACKAGE_PAID_BYTES = 10;