From 3452811ce5a9ed5f7611c5956653ff8e52fcc06b Mon Sep 17 00:00:00 2001
From: "Ilia O." <iliao@kaia.ai>
Date: Sun, 4 Feb 2024 20:49:34 -0800
Subject: [PATCH] RPLIDAR A1 debug

---
 src/LDS_RPLIDAR_A1.cpp | 28 ++++++++--------------------
 1 file changed, 8 insertions(+), 20 deletions(-)

diff --git a/src/LDS_RPLIDAR_A1.cpp b/src/LDS_RPLIDAR_A1.cpp
index 45880d0..5a3ab30 100644
--- a/src/LDS_RPLIDAR_A1.cpp
+++ b/src/LDS_RPLIDAR_A1.cpp
@@ -118,7 +118,6 @@ void LDS_RPLIDAR_A1::markScanTime() {
 LDS::result_t LDS_RPLIDAR_A1::waitScanDot() {
 
   while (true) {
-
     if (recvPos >= sizeof(node_info_t))
       recvPos = 0;
 
@@ -126,29 +125,18 @@ LDS::result_t LDS_RPLIDAR_A1::waitScanDot() {
     if (current_byte < 0)
       return ERROR_NOT_READY;
 
-    uint8_t test;
     switch (recvPos) {
     case 0:
-      test = ((current_byte >> 1) ^ current_byte) & 0x01;
-      if (!test) {
-        Serial.print(current_byte < 16 ? ".0" : ".");
-        Serial.print(current_byte, HEX);
-        continue;
-      }
-      break;
+      if (((current_byte >> 1) ^ current_byte) & 0x01)
+        break;
+      continue;
     case 1:
-      test = current_byte & RESP_MEAS_CHECKBIT;
-      if (!test) {
-        recvPos = 0;
-        test = ((current_byte >> 1) ^ current_byte) & 0x01;
-        if (!test) {
-          Serial.print(current_byte < 16 ? ".0" : ".");
-          Serial.print(current_byte, HEX);
-          continue;
-        }
+      if (current_byte & RESP_MEAS_CHECKBIT)
+        break;
+      recvPos = 0;
+      if (((current_byte >> 1) ^ current_byte) & 0x01)
         break;
-      }
-      break;
+      continue;
     }
 
     uint8_t *nodebuf = (uint8_t*)&node;