Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: RPLidarA2: interpret DEVICE_INFO response to determine model

This commit is contained in:
Peter Barker 2021-02-05 12:12:24 +11:00 committed by Randy Mackay
parent 4ffdc7239c
commit 811dcebe4f
2 changed files with 68 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -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) {

16
libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
View File

@ -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);
};