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

AP_Proximity: rewrite RPLidarA2 to use more efficient read() method

This commit is contained in:
Peter Barker 2020-05-22 09:37:40 +10:00 committed by Randy Mackay
parent 742416c2aa
commit 4ffdc7239c
2 changed files with 249 additions and 263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

447
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -32,7 +32,21 @@
#include "AP_Proximity_RPLidarA2.h"
/*
DEVICE=/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=uart:$DEVICE:115200 --speedup=1
param set SERIAL5_PROTOCOL 11
param set PRX_TYPE 5
reboot
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity_RPLidarA2.h"
#include <AP_InternalError/AP_InternalError.h>
#include <ctype.h>
#include <stdio.h>
@ -46,8 +60,6 @@
#endif
#define COMM_ACTIVITY_TIMEOUT_MS 200
#define RESET_RPA2_WAIT_MS 8
#define RESYNC_TIMEOUT 5000
// Commands
//-----------------------------------------
@ -68,27 +80,21 @@
extern const AP_HAL::HAL& hal;
// update the _rp_state of the sensor
void AP_Proximity_RPLidarA2::update(void)
{
if (_uart == nullptr) {
return;
}
// initialise sensor if necessary
if (!_initialised) {
_initialised = initialise(); //returns true if everything initialized properly
}
// if LIDAR in known state
if (_initialised) {
get_readings();
}
get_readings();
// check for timeout and set health status
if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > COMM_ACTIVITY_TIMEOUT_MS)) {
if (AP_HAL::millis() - _last_distance_received_ms > COMM_ACTIVITY_TIMEOUT_MS) {
set_status(AP_Proximity::Status::NoData);
Debug(1, "LIDAR NO DATA");
if (AP_HAL::millis() - _last_reset_ms > 10000) {
reset_rplidar();
}
} else {
set_status(AP_Proximity::Status::Good);
}
@ -106,262 +112,239 @@ float AP_Proximity_RPLidarA2::distance_min() const
return 0.20f; //20cm min range RPLIDAR2
}
bool AP_Proximity_RPLidarA2::initialise()
{
if (!_initialised) {
reset_rplidar(); // set to a known state
Debug(1, "LIDAR initialised");
}
return true;
}
void AP_Proximity_RPLidarA2::reset_rplidar()
{
if (_uart == nullptr) {
return;
}
uint8_t tx_buffer[2] = {RPLIDAR_PREAMBLE, RPLIDAR_CMD_RESET};
static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_RESET};
_uart->write(tx_buffer, 2);
_resetted = true; ///< be aware of extra 63 bytes coming after reset containing FW information
Debug(1, "LIDAR reset");
// To-Do: ensure delay of 8m after sending reset request
_last_reset_ms = AP_HAL::millis();
_rp_state = rp_resetted;
_state = State::RESET;
reset();
}
// set Lidar into SCAN mode
void AP_Proximity_RPLidarA2::set_scan_mode()
void AP_Proximity_RPLidarA2::send_scan_mode_request()
{
if (_uart == nullptr) {
return;
}
uint8_t tx_buffer[2] = {RPLIDAR_PREAMBLE, RPLIDAR_CMD_SCAN};
static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_SCAN};
_uart->write(tx_buffer, 2);
_last_request_ms = AP_HAL::millis();
Debug(1, "LIDAR SCAN MODE ACTIVATED");
_rp_state = rp_responding;
Debug(1, "Sent scan mode request");
}
// send request for sensor health
void AP_Proximity_RPLidarA2::send_request_for_health() //not called yet
{
if (_uart == nullptr) {
return;
}
uint8_t tx_buffer[2] = {RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_HEALTH};
static const uint8_t tx_buffer[2] {RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_HEALTH};
_uart->write(tx_buffer, 2);
_last_request_ms = AP_HAL::millis();
_rp_state = rp_health;
Debug(1, "Sent health request");
}
void AP_Proximity_RPLidarA2::consume_bytes(uint16_t count)
{
_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 (uint8_t i=1; i<sizeof(_payload); 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()
{
if (_uart == nullptr) {
Debug(2, " CURRENT STATE: %u ", (unsigned)_state);
const uint32_t nbytes = _uart->available();
if (nbytes == 0) {
return;
}
Debug(2, " CURRENT STATE: %d ", _rp_state);
uint32_t nbytes = _uart->available();
while (nbytes-- > 0) {
int16_t c = _uart->read();
Debug(2, "UART READ %x <%c>", c, c); //show HEX values
STATE:
switch(_rp_state){
case rp_resetted:
Debug(3, " BYTE_COUNT %d", _byte_count);
if ((c == 0x52 || _information_data) && _byte_count < 62) {
if (c == 0x52) {
_information_data = true;
}
_rp_systeminfo[_byte_count] = c;
Debug(3, "_rp_systeminfo[%d]=%x",_byte_count,_rp_systeminfo[_byte_count]);
_byte_count++;
break;
} else {
if (_information_data) {
Debug(1, "GOT RPLIDAR INFORMATION");
_information_data = false;
_byte_count = 0;
set_scan_mode();
break;
}
if (_cnt>5) {
_rp_state = rp_unknown;
_cnt=0;
break;
}
_cnt++;
break;
}
break;
case rp_responding:
Debug(2, "RESPONDING");
if (c == RPLIDAR_PREAMBLE || _descriptor_data) {
_descriptor_data = true;
_descriptor[_byte_count] = c;
_byte_count++;
// descriptor packet has 7 byte in total
if (_byte_count == sizeof(_descriptor)) {
Debug(2,"LIDAR DESCRIPTOR CATCHED");
_response_type = ResponseType_Descriptor;
// identify the payload data after the descriptor
parse_response_descriptor();
_byte_count = 0;
}
} else {
_rp_state = rp_unknown;
}
break;
case rp_measurements:
if (_sync_error) {
// out of 5-byte sync mask -> catch new revolution
Debug(1, " OUT OF SYNC");
// on first revolution bit 1 = 1, bit 2 = 0 of the first byte
if ((c & 0x03) == 0x01) {
_sync_error = 0;
Debug(1, " RESYNC");
} else {
if (AP_HAL::millis() - _last_distance_received_ms > RESYNC_TIMEOUT) {
reset_rplidar();
}
break;
}
}
Debug(3, "READ PAYLOAD");
payload[_byte_count] = c;
_byte_count++;
if (_byte_count == _payload_length) {
Debug(2, "LIDAR MEASUREMENT CATCHED");
parse_response_data();
_byte_count = 0;
}
break;
case rp_health:
Debug(1, "state: HEALTH");
break;
case rp_unknown:
Debug(1, "state: UNKNOWN");
if (c == RPLIDAR_PREAMBLE) {
_rp_state = rp_responding;
goto STATE;
break;
}
_cnt++;
if (_cnt>10) {
reset_rplidar();
_rp_state = rp_resetted;
_cnt=0;
}
break;
default:
Debug(1, "UNKNOWN LIDAR STATE");
break;
}
}
}
void AP_Proximity_RPLidarA2::parse_response_descriptor()
{
// check if descriptor packet is valid
if (_descriptor[0] == RPLIDAR_PREAMBLE && _descriptor[1] == 0x5A) {
if (_descriptor[2] == 0x05 && _descriptor[3] == 0x00 && _descriptor[4] == 0x00 && _descriptor[5] == 0x40 && _descriptor[6] == 0x81) {
// payload is SCAN measurement data
_payload_length = sizeof(payload.sensor_scan);
static_assert(sizeof(payload.sensor_scan) == 5, "Unexpected payload.sensor_scan data structure size");
_response_type = ResponseType_SCAN;
Debug(2, "Measurement response detected");
_last_distance_received_ms = AP_HAL::millis();
_rp_state = rp_measurements;
}
if (_descriptor[2] == 0x03 && _descriptor[3] == 0x00 && _descriptor[4] == 0x00 && _descriptor[5] == 0x00 && _descriptor[6] == 0x06) {
// payload is health data
_payload_length = sizeof(payload.sensor_health);
static_assert(sizeof(payload.sensor_health) == 3, "Unexpected payload.sensor_health data structure size");
_response_type = ResponseType_Health;
_last_distance_received_ms = AP_HAL::millis();
_rp_state= rp_health;
}
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;
}
Debug(1, "Invalid response descriptor");
_rp_state = rp_unknown;
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;
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
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);
send_scan_mode_request();
// send_request_for_health();
_state = State::AWAITING_RESPONSE;
continue;
}
case State::AWAITING_RESPONSE:
if (_payload[0] != RPLIDAR_PREAMBLE) {
// this is a protocol error. Reset.
reset();
return;
}
// descriptor packet has 7 byte in total
if (_byte_count < sizeof(_descriptor)) {
return;
}
// identify the payload data after the descriptor
static const _descriptor SCAN_DATA_DESCRIPTOR[] {
RPLIDAR_PREAMBLE, 0x5A, 0x05, 0x00, 0x00, 0x40, 0x81
};
static const _descriptor HEALTH_DESCRIPTOR[] {
RPLIDAR_PREAMBLE, 0x5A, 0x03, 0x00, 0x00, 0x00, 0x06
};
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], HEALTH_DESCRIPTOR, sizeof(_descriptor)) == 0) {
_state = State::AWAITING_HEALTH;
} else {
// unknown descriptor. Ignore it.
}
consume_bytes(sizeof(_descriptor));
break;
case State::AWAITING_SCAN_DATA:
if (_byte_count < sizeof(_payload.sensor_scan)) {
return;
}
parse_response_data();
consume_bytes(sizeof(_payload.sensor_scan));
break;
case State::AWAITING_HEALTH:
if (_byte_count < sizeof(_payload.sensor_health)) {
return;
}
parse_response_health();
consume_bytes(sizeof(_payload.sensor_health));
break;
}
}
}
void AP_Proximity_RPLidarA2::parse_response_data()
{
switch (_response_type){
case ResponseType_SCAN:
Debug(2, "UART %02x %02x%02x %02x%02x", payload[0], payload[2], payload[1], payload[4], payload[3]); //show HEX values
// check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1
if ((payload.sensor_scan.startbit == !payload.sensor_scan.not_startbit) && payload.sensor_scan.checkbit) {
const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;
const float angle_deg = wrap_360(payload.sensor_scan.angle_q6/64.0f * angle_sign + params.yaw_correction);
const float distance_m = (payload.sensor_scan.distance_q2/4000.0f);
if (_sync_error) {
// out of 5-byte sync mask -> catch new revolution
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");
} else {
return;
}
}
Debug(2, "UART %02x %02x%02x %02x%02x", _payload[0], _payload[2], _payload[1], _payload[4], _payload[3]); //show HEX values
// check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1
if (!((_payload.sensor_scan.startbit == !_payload.sensor_scan.not_startbit) && _payload.sensor_scan.checkbit)) {
Debug(1, "Invalid Payload");
_sync_error++;
return;
}
const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;
const float angle_deg = wrap_360(_payload.sensor_scan.angle_q6/64.0f * angle_sign + params.yaw_correction);
const float distance_m = (_payload.sensor_scan.distance_q2/4000.0f);
#if RP_DEBUG_LEVEL >= 2
const uint8_t quality = payload.sensor_scan.quality;
Debug(2, " D%02.2f A%03.1f Q%02d", distance_m, angle_deg, quality);
const float quality = _payload.sensor_scan.quality;
Debug(2, " D%02.2f A%03.1f Q%02d", distance_m, angle_deg, quality);
#endif
_last_distance_received_ms = AP_HAL::millis();
if (!ignore_reading(angle_deg, distance_m)) {
const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
_last_distance_received_ms = AP_HAL::millis();
if (!ignore_reading(angle_deg, distance_m)) {
const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
if (face != _last_face) {
// distance is for a new face, the previous one can be updated now
if (_last_distance_valid) {
frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
} else {
// reset distance from last face
frontend.boundary.reset_face(face, state.instance);
}
// initialize the new face
_last_face = face;
_last_distance_valid = false;
}
if (distance_m > distance_min()) {
// update shortest distance
if (!_last_distance_valid || (distance_m < _last_distance_m)) {
_last_distance_m = distance_m;
_last_distance_valid = true;
_last_angle_deg = angle_deg;
}
// update OA database
database_push(_last_angle_deg, _last_distance_m);
}
}
if (face != _last_face) {
// distance is for a new face, the previous one can be updated now
if (_last_distance_valid) {
frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
} else {
// not valid payload packet
Debug(1, "Invalid Payload");
_sync_error++;
// reset distance from last face
frontend.boundary.reset_face(face, state.instance);
}
break;
case ResponseType_Health:
// health issue if status is "3" ->HW error
if (payload.sensor_health.status == 3) {
Debug(1, "LIDAR Error");
// initialize the new face
_last_face = face;
_last_distance_valid = false;
}
if (distance_m > distance_min()) {
// update shortest distance
if (!_last_distance_valid || (distance_m < _last_distance_m)) {
_last_distance_m = distance_m;
_last_distance_valid = true;
_last_angle_deg = angle_deg;
}
break;
default:
// no valid payload packets recognized: return payload data=0
Debug(1, "Unknown LIDAR packet");
break;
// update OA database
database_push(_last_angle_deg, _last_distance_m);
}
}
}
void AP_Proximity_RPLidarA2::parse_response_health()
{
// health issue if status is "3" ->HW error
if (_payload.sensor_health.status == 3) {
Debug(1, "LIDAR Error");
}
Debug(1, "LIDAR Healthy");
}
#endif // AP_PROXIMITY_RPLIDARA2_ENABLED

65
libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
View File

@ -50,49 +50,32 @@ public:
float distance_min() const override;
private:
enum rp_state {
rp_unknown = 0,
rp_resetted,
rp_responding,
rp_measurements,
rp_health
};
enum ResponseType {
ResponseType_Descriptor = 0,
ResponseType_SCAN,
ResponseType_EXPRESS,
ResponseType_Health
};
// initialise sensor (returns true if sensor is successfully initialised)
bool initialise();
void set_scan_mode();
enum class State {
RESET = 56,
AWAITING_RESPONSE,
AWAITING_SCAN_DATA,
AWAITING_HEALTH,
} _state = State::RESET;
// send request for something from sensor
void send_request_for_health();
void send_scan_mode_request();
void parse_response_data();
void parse_response_descriptor();
void parse_response_health();
void get_readings();
void reset_rplidar();
void reset();
// reply related variables
uint8_t _descriptor[7];
char _rp_systeminfo[63];
bool _descriptor_data;
bool _information_data;
bool _resetted;
bool _initialised;
// remove bytes from read buffer:
void consume_bytes(uint16_t count);
uint8_t _payload_length;
uint8_t _cnt;
uint8_t _sync_error ;
uint16_t _byte_count;
// request related variables
enum ResponseType _response_type; ///< response from the lidar
enum rp_state _rp_state;
uint32_t _last_request_ms; ///< system time of last request
uint32_t _last_distance_received_ms; ///< system time of last distance measurement received from sensor
uint32_t _last_reset_ms;
@ -116,11 +99,31 @@ private:
uint16_t error_code; ///< the related error code
};
struct PACKED _descriptor {
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;
} payload;
_descriptor descriptor;
_rpi_information information;
uint8_t forced_buffer_size[128]; // just so we read(...) efficiently
} _payload;
static_assert(sizeof(_payload) >= 63, "Needed for parsing out reboot data");
bool make_first_byte_in_payload(uint8_t desired_byte);
};
#endif // AP_PROXIMITY_RPLIDARA2_ENABLED