From e998f939657365f0c4764ea04c9fe3415c9982e0 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 12 May 2023 13:25:49 +0900
Subject: [PATCH] AP_Proximity: RPLidarA2 gets S1 support

---
 .../AP_Proximity/AP_Proximity_RPLidarA2.cpp      | 16 ++++++++++++++++
 libraries/AP_Proximity/AP_Proximity_RPLidarA2.h  |  1 +
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
index 5a4149ecbe..846414ee57 100644
--- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
@@ -75,6 +75,15 @@ void AP_Proximity_RPLidarA2::update(void)
         return;
     }
 
+    // request device info 3sec after reset
+    // required for S1 support that sends only 9 bytes after a reset (A1,A2 send 63)
+    uint32_t now_ms = AP_HAL::millis();
+    if ((_state == State::RESET) && (now_ms - _last_reset_ms > 3000)) {
+        send_request_for_device_info();
+        _state = State::AWAITING_RESPONSE;
+        _byte_count = 0;
+    }
+
     get_readings();
 
     // check for timeout and set health status
@@ -99,6 +108,8 @@ float AP_Proximity_RPLidarA2::distance_max() const
         return 8.0f;
     case Model::A2:
         return 16.0f;
+    case Model::S1:
+        return 40.0f;
     }
     return 0.0f;
 }
@@ -112,6 +123,7 @@ float AP_Proximity_RPLidarA2::distance_min() const
     case Model::A1:
         return 0.2f;
     case Model::A2:
+    case Model::S1:
         return 0.2f;
     }
     return 0.0f;
@@ -322,6 +334,10 @@ void AP_Proximity_RPLidarA2::parse_response_device_info()
         model = Model::A2;
         device_type = "A2";
         break;
+    case 0x61:
+        model = Model::S1;
+        device_type = "S1";
+        break;
     default:
         Debug(1, "Unknown device (%u)", _payload.device_info.model);
     }
diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
index 3115426aac..2c9d9db524 100644
--- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
+++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
@@ -151,6 +151,7 @@ private:
         UNKNOWN,
         A1,
         A2,
+        S1,
     } model = Model::UNKNOWN;
 
     bool make_first_byte_in_payload(uint8_t desired_byte);