AP_Proximity: add RPLidar S2, remove memory copy for each measurement

mirkix · mirkix · commit f08db1f39626 · 2024-04-29T15:59:59.000+02:00
diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
@@ -33,7 +33,6 @@
 #include "AP_Proximity_RPLidarA2.h"
 
 #include <AP_HAL/AP_HAL.h>
-#include "AP_Proximity_RPLidarA2.h"
 #include <AP_InternalError/AP_InternalError.h>
 
 #include <ctype.h>
@@ -75,15 +74,6 @@ 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
@@ -112,6 +102,8 @@ float AP_Proximity_RPLidarA2::distance_max() const
         return 12.0f;
     case Model::S1:
         return 40.0f;
+    case Model::S2:
+        return 50.0f;
     }
     return 0.0f;
 }
@@ -127,18 +119,20 @@ float AP_Proximity_RPLidarA2::distance_min() const
     case Model::C1:
     case Model::S1:
         return 0.2f;
+    case Model::S2:
+        return 0.05f;
     }
     return 0.0f;
 }
 
+// reset lidar
 void AP_Proximity_RPLidarA2::reset_rplidar()
 {
     static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_RESET};
     _uart->write(tx_buffer, 2);
     Debug(1, "LIDAR reset");
-    // To-Do: ensure delay of 8m after sending reset request
     _last_reset_ms =  AP_HAL::millis();
-    reset();
+    _state = State::RESET;
 }
 
 // set Lidar into SCAN mode
@@ -165,114 +159,36 @@ void AP_Proximity_RPLidarA2::send_request_for_device_info()
     Debug(1, "Sent device information request");
 }
 
-void AP_Proximity_RPLidarA2::consume_bytes(uint16_t count)
-{
-    if (count > _byte_count) {
-        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-        _byte_count = 0;
-        return;
-    }
-    _byte_count -= count;
-    if (_byte_count) {
-        memmove((void*)&_payload[0], (void*)&_payload[count], _byte_count);
-    }
-}
-
-void AP_Proximity_RPLidarA2::reset()
-{
-    _state = State::RESET;
-    _byte_count = 0;
-}
-
-bool AP_Proximity_RPLidarA2::make_first_byte_in_payload(uint8_t desired_byte)
-{
-    if (_byte_count == 0) {
-        return false;
-    }
-    if (_payload[0] == desired_byte) {
-        return true;
-    }
-    for (auto i=1; i<_byte_count; i++) {
-        if (_payload[i] == desired_byte) {
-            consume_bytes(i);
-            return true;
-        }
-    }
-    // just not in our buffer.  Throw everything away:
-    _byte_count = 0;
-    return false;
-}
-
 void AP_Proximity_RPLidarA2::get_readings()
 {
-    Debug(2, "             CURRENT STATE: %u ", (unsigned)_state);
-    const uint32_t nbytes = _uart->available();
-    if (nbytes == 0) {
-        return;
-    }
-    const uint32_t bytes_to_read = MIN(nbytes, sizeof(_payload)-_byte_count);
-    if (bytes_to_read == 0) {
-        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-        reset();
-        return;
-    }
-    const uint32_t bytes_read = _uart->read(&_payload[_byte_count], bytes_to_read);
-    if (bytes_read == 0) {
-        // this is bad; we were told there were bytes available
-        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-        reset();
-        return;
-    }
-    _byte_count += bytes_read;
-
-    uint32_t previous_loop_byte_count = UINT32_MAX;
-    while (_byte_count) {
-        if (_byte_count >= previous_loop_byte_count) {
-            // this is a serious error, we should always consume some
-            // bytes.  Avoid looping forever.
-            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-            _uart = nullptr;
-            return;
-        }
-        previous_loop_byte_count = _byte_count;
-
+    uint16_t _bytes_read = 0;
+    Debug(2, "CURRENT STATE: %u ", (unsigned)_state);
+    while (_uart->available() && _bytes_read < MAX_BYTES_CONSUME) {
         switch(_state){
         case State::RESET: {
-            // looking for 0x52 at start of buffer; the 62 following
-            // bytes are "information"
-            if (!make_first_byte_in_payload('R')) { // that's 'R' as in RPiLidar
+            if (AP_HAL::millis() - _last_reset_ms < 1000) {
                 return;
             }
-            if (_byte_count < 63) {
-                return;
-            }
-#if RP_DEBUG_LEVEL
-            // optionally spit out via mavlink the 63-bytes of cruft
-            // that is spat out on device reset
-            Debug(1, "Got RPLidar Information");
-            char xbuffer[64]{};
-            memcpy((void*)xbuffer, (void*)&_payload.information, 63);
-            gcs().send_text(MAV_SEVERITY_INFO, "RPLidar: (%s)", xbuffer);
-#endif
-            // 63 is the magic number of bytes in the spewed-out
-            // reset data ... so now we'll just drop that stuff on
-            // the floor.
-            consume_bytes(63);
+            _uart->discard_input();
             send_request_for_device_info();
             _state = State::AWAITING_RESPONSE;
             continue;
         }
         case State::AWAITING_RESPONSE:
-            if (_payload[0] != RPLIDAR_PREAMBLE) {
-                // this is a protocol error.  Reset.
-                reset();
+            // descriptor packet has 7 byte in total
+            if (_uart->available() < sizeof(_descriptor)) {
                 return;
             }
+            _uart->read(&_payload[0], sizeof(_descriptor));
+            _bytes_read += sizeof(_descriptor);
 
-            // descriptor packet has 7 byte in total
-            if (_byte_count < sizeof(_descriptor)) {
+            if (_payload[0] != RPLIDAR_PREAMBLE) {
+                Debug(1, "protocol error");
+                // this is a protocol error do a reset.
+                reset_rplidar();
                 return;
             }
+
             // identify the payload data after the descriptor
             static const _descriptor SCAN_DATA_DESCRIPTOR[] {
                 { RPLIDAR_PREAMBLE, 0x5A, 0x05, 0x00, 0x00, 0x40, 0x81 }
@@ -293,31 +209,33 @@ void AP_Proximity_RPLidarA2::get_readings()
             } else {
                 // unknown descriptor.  Ignore it.
             }
-            consume_bytes(sizeof(_descriptor));
             break;
 
         case State::AWAITING_DEVICE_INFO:
-            if (_byte_count < sizeof(_payload.device_info)) {
+            if (_uart->available() < sizeof(_payload.device_info)) {
                 return;
             }
+            _uart->read(&_payload[0], sizeof(_payload.device_info));
+            _bytes_read += sizeof(_payload.device_info);
             parse_response_device_info();
-            consume_bytes(sizeof(_payload.device_info));
             break;
 
         case State::AWAITING_SCAN_DATA:
-            if (_byte_count < sizeof(_payload.sensor_scan)) {
+            if (_uart->available() < sizeof(_payload.sensor_scan)) {
                 return;
             }
+            _uart->read(&_payload[0], sizeof(_payload.sensor_scan));
+            _bytes_read += sizeof(_payload.sensor_scan);
             parse_response_data();
-            consume_bytes(sizeof(_payload.sensor_scan));
             break;
 
         case State::AWAITING_HEALTH:
-            if (_byte_count < sizeof(_payload.sensor_health)) {
+            if (_uart->available() < sizeof(_payload.sensor_health)) {
                 return;
             }
+            _uart->read(&_payload[0], sizeof(_payload.sensor_health));
+            _bytes_read += sizeof(_payload.sensor_health);
             parse_response_health();
-            consume_bytes(sizeof(_payload.sensor_health));
             break;
         }
     }
@@ -344,6 +262,10 @@ void AP_Proximity_RPLidarA2::parse_response_device_info()
         model = Model::S1;
         device_type = "S1";
         break;
+    case 0x71:
+        model = Model::S2;
+        device_type = "S2";
+        break;
     default:
         Debug(1, "Unknown device (%u)", _payload.device_info.model);
     }
@@ -356,12 +278,13 @@ void AP_Proximity_RPLidarA2::parse_response_data()
 {
     if (_sync_error) {
         // out of 5-byte sync mask -> catch new revolution
-        Debug(1, "       OUT OF SYNC");
+        Debug(1, "OUT OF SYNC");
         // on first revolution bit 1 = 1, bit 2 = 0 of the first byte
         if ((_payload[0] & 0x03) == 0x01) {
             _sync_error = 0;
-            Debug(1, "                  RESYNC");
+            Debug(1, "RESYNC");
         } else {
+            Debug(1, "NO RESYNC");
             return;
         }
     }
@@ -412,7 +335,7 @@ void AP_Proximity_RPLidarA2::parse_response_data()
 
 void AP_Proximity_RPLidarA2::parse_response_health()
 {
-    // health issue if status is "3" ->HW error
+    // health issue if status is "3" -> HW error
     if (_payload.sensor_health.status == 3) {
         Debug(1, "LIDAR Error");
     }
diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
@@ -72,6 +72,8 @@ class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
         AWAITING_DEVICE_INFO,
     } _state = State::RESET;
 
+    static constexpr uint16_t MAX_BYTES_CONSUME = 1024;
+
     // send request for something from sensor
     void send_request_for_health();
     void send_scan_mode_request();
@@ -83,13 +85,8 @@ class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
 
     void get_readings();
     void reset_rplidar();
-    void reset();
-
-    // remove bytes from read buffer:
-    void consume_bytes(uint16_t count);
 
     uint8_t _sync_error;
-    uint16_t _byte_count;
 
     // request related variables
     uint32_t  _last_distance_received_ms;     ///< system time of last distance measurement received from sensor
@@ -127,35 +124,22 @@ class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
         uint8_t bytes[7];
     };
 
-    // we don't actually *need* to store this.  If we don't, _payload
-    // can be just 7 bytes, but that doesn't make for efficient
-    // reading.  It also simplifies the state machine to have the read
-    // buffer at least this big.  Note that we force the buffer to a
-    // larger size below anyway.
-    struct PACKED _rpi_information {
-        uint8_t bytes[63];
-    };
-
     union PACKED {
         DEFINE_BYTE_ARRAY_METHODS
         _sensor_scan sensor_scan;
         _sensor_health sensor_health;
         _descriptor descriptor;
-        _rpi_information information;
         _device_info device_info;
-        uint8_t forced_buffer_size[256]; // just so we read(...) efficiently
     } _payload;
-    static_assert(sizeof(_payload) >= 63, "Needed for parsing out reboot data");
 
     enum class Model {
         UNKNOWN,
         A1,
         A2,
         C1,
         S1,
+        S2,
     } model = Model::UNKNOWN;
-
-    bool make_first_byte_in_payload(uint8_t desired_byte);
 };
 
 #endif // AP_PROXIMITY_RPLIDARA2_ENABLED
