From 811dcebe4febeaaf794e4a04d5745abc50e594fc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 5 Feb 2021 12:12:24 +1100 Subject: [PATCH] AP_Proximity: RPLidarA2: interpret DEVICE_INFO response to determine model --- .../AP_Proximity/AP_Proximity_RPLidarA2.cpp | 56 +++++++++++++++++-- .../AP_Proximity/AP_Proximity_RPLidarA2.h | 16 ++++++ 2 files changed, 68 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index 98e3709b4f..19e4724327 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/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_health(); + send_request_for_device_info(); _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) { diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h index 50468aecfa..051e0b75b3 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h +++ b/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); };