AP_Proximity: RPLidarA2: interpret DEVICE_INFO response to determine model

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -103,13 +103,25 @@ void AP_Proximity_RPLidarA2::update(void)
 // get maximum distance (in meters) of sensor
 float AP_Proximity_RPLidarA2::distance_max() const
 {
-    return 16.0f;  //16m max range RPLIDAR2, if you want to support the 8m version this is the only line to change
+    switch (model) {
+    case Model::UNKNOWN:
+        return 0.0f;
+    case Model::A2:
+        return 16.0f;
+    }
+    return 0.0f;
 }
 
 // get minimum distance (in meters) of sensor
 float AP_Proximity_RPLidarA2::distance_min() const
 {
-    return 0.20f;  //20cm min range RPLIDAR2
+    switch (model) {
+    case Model::UNKNOWN:
+        return 0.0f;
+    case Model::A2:
+        return 0.2f;
+    }
+    return 0.0f;
 }
 
 void AP_Proximity_RPLidarA2::reset_rplidar()
@@ -139,6 +151,14 @@ void AP_Proximity_RPLidarA2::send_request_for_health()
     Debug(1, "Sent health request");
 }
 
+// send request for device information
+void AP_Proximity_RPLidarA2::send_request_for_device_info()
+{
+    static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_INFO};
+    _uart->write(tx_buffer, 2);
+    Debug(1, "Sent device information request");
+}
+
 void AP_Proximity_RPLidarA2::consume_bytes(uint16_t count)
 {
     _byte_count -= count;
@@ -227,8 +247,7 @@ void AP_Proximity_RPLidarA2::get_readings()
             // reset data ... so now we'll just drop that stuff on
             // the floor.
             consume_bytes(63);
-            send_scan_mode_request();
+            send_request_for_device_info();
-//            send_request_for_health();
             _state = State::AWAITING_RESPONSE;
             continue;
         }
@@ -250,9 +269,14 @@ void AP_Proximity_RPLidarA2::get_readings()
             static const _descriptor HEALTH_DESCRIPTOR[] {
                 RPLIDAR_PREAMBLE, 0x5A, 0x03, 0x00, 0x00, 0x00, 0x06
             };
+            static const _descriptor DEVICE_INFO_DESCRIPTOR[] {
+                RPLIDAR_PREAMBLE, 0x5A, 0x14, 0x00, 0x00, 0x00, 0x04
+            };
             Debug(2,"LIDAR descriptor found");
             if (memcmp((void*)&_payload[0], SCAN_DATA_DESCRIPTOR, sizeof(_descriptor)) == 0) {
                 _state = State::AWAITING_SCAN_DATA;
+            } else if (memcmp((void*)&_payload[0], DEVICE_INFO_DESCRIPTOR, sizeof(_descriptor)) == 0) {
+                _state = State::AWAITING_DEVICE_INFO;
             } else if (memcmp((void*)&_payload[0], HEALTH_DESCRIPTOR, sizeof(_descriptor)) == 0) {
                 _state = State::AWAITING_HEALTH;
             } else {
@@ -261,6 +285,14 @@ void AP_Proximity_RPLidarA2::get_readings()
             consume_bytes(sizeof(_descriptor));
             break;
 
+        case State::AWAITING_DEVICE_INFO:
+            if (_byte_count < sizeof(_payload.device_info)) {
+                return;
+            }
+            parse_response_device_info();
+            consume_bytes(sizeof(_payload.device_info));
+            break;
+
         case State::AWAITING_SCAN_DATA:
             if (_byte_count < sizeof(_payload.sensor_scan)) {
                 return;
@@ -280,6 +312,22 @@ void AP_Proximity_RPLidarA2::get_readings()
     }
 }
 
+void AP_Proximity_RPLidarA2::parse_response_device_info()
+{
+    Debug(1, "Received DEVICE_INFO");
+    switch (_payload.device_info.model) {
+    case 0x28:
+        model = Model::A2;
+        break;
+    default:
+        Debug(1, "Unknown device (%u)", _payload.device_info.model);
+    }
+    Debug(1, "firmware (%u.%u)", _payload.device_info.firmware_minor, _payload.device_info.firmware_major);
+    Debug(1, "Hardware (%u)", _payload.device_info.hardware);
+    send_scan_mode_request();
+    _state = State::AWAITING_RESPONSE;
+}
+
 void AP_Proximity_RPLidarA2::parse_response_data()
 {
     if (_sync_error) {

libraries/AP_Proximity/AP_Proximity_RPLidarA2.h

@@ -56,14 +56,17 @@ private:
         AWAITING_RESPONSE,
         AWAITING_SCAN_DATA,
         AWAITING_HEALTH,
+        AWAITING_DEVICE_INFO,
     } _state = State::RESET;
 
     // send request for something from sensor
     void send_request_for_health();
     void send_scan_mode_request();
+    void send_request_for_device_info();
 
     void parse_response_data();
     void parse_response_health();
+    void parse_response_device_info();
 
     void get_readings();
     void reset_rplidar();
@@ -85,6 +88,14 @@ private:
     float _last_distance_m;                   ///< shortest distance for _last_face
     bool _last_distance_valid;                ///< true if _last_distance_m is valid
 
+    struct PACKED _device_info {
+        uint8_t model;
+        uint8_t firmware_minor;
+        uint8_t firmware_major;
+        uint8_t hardware;
+        uint8_t serial[16];
+   };
+
     struct PACKED _sensor_scan {
         uint8_t startbit      : 1;            ///< on the first revolution 1 else 0
         uint8_t not_startbit  : 1;            ///< complementary to startbit
@@ -118,10 +129,15 @@ private:
         _sensor_health sensor_health;
         _descriptor descriptor;
         _rpi_information information;
+        _device_info device_info;
         uint8_t forced_buffer_size[128]; // just so we read(...) efficiently
     } _payload;
     static_assert(sizeof(_payload) >= 63, "Needed for parsing out reboot data");
 
+    enum class Model {
+        UNKNOWN,
+        A2,
+    } model = Model::UNKNOWN;
 
     bool make_first_byte_in_payload(uint8_t desired_byte);
 };