RPLIDAR A1 start fix

Author: Ilia O.

examples/all_lidars_lds02rr_esp32/all_lidars_lds02rr_esp32.ino

@@ -1,4 +1,4 @@
-// Copyright 2023-2024 KAIA.AI
+// Copyright 2023-2025 KAIA.AI
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -44,17 +44,26 @@ const uint8_t LIDAR_GPIO_PWM = 15;// ESP32 GPIO connected to Lidar PWM pin
 //#define YDLIDAR_X4_PRO
 //#define CAMSENSE_X1
 
+// 4. UNCOMMENT debug option(s)
+// and increase SERIAL_MONITOR_BAUD to MAX possible
+//#define DEBUG_GPIO
+//#define DEBUG_PACKETS
+//#define DEBUG_SERIAL_IN
+//#define DEBUG_SERIAL_OUT
 
 const uint32_t SERIAL_MONITOR_BAUD = 115200;
 const uint32_t LIDAR_PWM_FREQ = 10000;
 const uint8_t LIDAR_PWM_BITS = 11;
 const uint8_t LIDAR_PWM_CHANNEL = 2;
 const uint16_t LIDAR_SERIAL_RX_BUF_LEN = 1024;
+const uint16_t PRINT_EVERY_NTH_POINT = 20;
+const uint16_t HEX_DUMP_WIDTH = 16;
 
 #include "lds_all_models.h"
 
 HardwareSerial LidarSerial(1);
 LDS *lidar;
+uint16_t hex_dump_pos = 0;
 
 void setupLidar() {
 
@@ -115,11 +124,14 @@ void setup() {
 
   setupLidar();
 
-  //Serial.print("LiDAR model ");
-  //Serial.println(lidar->getModelName());
-
-  //Serial.print("Lidar baud rate ");
-  //Serial.println(lidar->getSerialBaudRate());
+  Serial.print("LiDAR model ");
+  Serial.print(lidar->getModelName());
+  Serial.print(", baud rate ");
+  Serial.print(lidar->getSerialBaudRate());
+  Serial.print(", TX GPIO ");
+  Serial.print(LIDAR_GPIO_TX);
+  Serial.print(", RX GPIO ");
+  Serial.println(LIDAR_GPIO_RX);
 
   LDS::result_t result = lidar->start();
   Serial.print("startLidar() result: ");
@@ -129,25 +141,52 @@ void setup() {
   // lidar->setScanTargetFreqHz(5.0f);
 }
 
+void printByteAsHex(uint8_t b) {
+  if (b < 16)
+    Serial.print('0');
+  Serial.print(b, HEX);
+  Serial.print(' ');
+}
+
+void printBytesAsHex(const uint8_t * buffer, uint16_t length) {
+  if (length == 0)
+    return;
+
+  for (uint16_t i = 0; i < length; i++) {
+    printByteAsHex(buffer[i]);
+  }
+}
+
 int lidar_serial_read_callback() {
-  return LidarSerial.read();
-/*
+  #ifdef DEBUG_SERIAL_IN
   int ch = LidarSerial.read();
-  if (ch != -1)
-    Serial.println(ch);
+  if (ch != -1) {
+    if (hex_dump_pos++ % HEX_DUMP_WIDTH == 0)
+      Serial.println();
+    printByteAsHex(ch);
+  }
   return ch;
-*/
+  #else
+  return LidarSerial.read();
+  #endif
 }
 
 size_t lidar_serial_write_callback(const uint8_t * buffer, size_t length) {
+  #ifdef DEBUG_SERIAL_OUT
+  Serial.println();
+  Serial.print('>');
+  printBytesAsHex(buffer, length);
+  Serial.println();
+  #endif
+  
   return LidarSerial.write(buffer, length);
 }
 
 void lidar_scan_point_callback(float angle_deg, float distance_mm, float quality,
   bool scan_completed) {
   static int i=0;
 
-  if (i % 20 == 0) {
+  if (i % PRINT_EVERY_NTH_POINT == 0) {
     Serial.print(i);
     Serial.print(' ');
     Serial.print(distance_mm);
@@ -167,16 +206,16 @@ void lidar_motor_pin_callback(float value, LDS::lds_pin_t lidar_pin) {
 
   int pin = (lidar_pin == LDS::LDS_MOTOR_EN_PIN) ? LIDAR_GPIO_EN : LIDAR_GPIO_PWM;
 
-  if (value <= (float)LDS::DIR_INPUT) {
+  #ifdef DEBUG_GPIO
+  Serial.print("GPIO ");
+  Serial.print(pin);
+  Serial.print(' ');
+  Serial.print(lidar->pinIDToString(lidar_pin));
+  Serial.print(" mode set to ");
+  Serial.println(lidar->pinStateToString((LDS::lds_pin_state_t) int(value)));
+  #endif
 
-    /*
-    Serial.print("GPIO ");
-    Serial.print(pin);
-    Serial.print(' ');
-    Serial.print(lidar->pinIDToString(lidar_pin));
-    Serial.print(" mode set to ");
-    Serial.println(lidar->pinStateToString(int(pin_mode)));
-    */
+  if (value <= (float)LDS::DIR_INPUT) {
 
     // Configure pin direction
     if (value == (float)LDS::DIR_OUTPUT_PWM) {
@@ -199,18 +238,11 @@ void lidar_motor_pin_callback(float value, LDS::lds_pin_t lidar_pin) {
     // TODO invert PWM as needed
     digitalWrite(pin, (value == (float)LDS::VALUE_HIGH) ? HIGH : LOW);
 
-    /*
-    Serial.print("GPIO ");
-    Serial.print(pin);
-    Serial.print(' ');
-    Serial.print(lidar->pinIDToString(lidar_pin));
-    Serial.print(" value set to ");
-    Serial.println(lidar->pinStateToString(int(value)));
-    */
-
   } else {
-    //Serial.print("PWM value set to ");
-    //Serial.print(value);
+    #ifdef DEBUG_GPIO
+    Serial.print("PWM value set to ");
+    Serial.print(value);
+    #endif
 
     // set PWM duty cycle
     #ifdef INVERT_PWM_PIN
@@ -225,8 +257,10 @@ void lidar_motor_pin_callback(float value, LDS::lds_pin_t lidar_pin) {
     ledcWriteChannel(LIDAR_PWM_CHANNEL, pwm_value);
     #endif
 
-    //Serial.print(' ');
-    //Serial.println(pwm_value);
+    #ifdef DEBUG_GPIO
+    Serial.print(' ');
+    Serial.println(pwm_value);
+    #endif
   }
 }
 
@@ -245,6 +279,14 @@ void lidar_error_callback(LDS::result_t code, String aux_info) {
 }
 
 void lidar_packet_callback(uint8_t * packet, uint16_t length, bool scan_completed) {
+  #ifdef DEBUG_PACKETS
+  Serial.println();
+  Serial.print("Packet callback, length=");
+  Serial.print(length);
+  Serial.print(", scan_completed=");
+  Serial.println(scan_completed);
+  #endif
+  
   return;
 }