Skip to content

Commit

Permalink
AP_Proximity: add RPLidar S2, remove memory copy for each measurement
Browse files Browse the repository at this point in the history
  • Loading branch information
@mirkix
mirkix committed Apr 29, 2024
1 parent 7f61ace commit f08db1f
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 133 deletions.
151 changes: 37 additions & 114 deletions libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
Expand All @@ -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
Expand All @@ -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 }
Expand All @@ -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;
}
}
Expand All @@ -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);
}
Expand All @@ -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;
}
}
Expand Down Expand Up @@ -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");
}
Expand Down
22 changes: 3 additions & 19 deletions libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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
Expand Down Expand Up @@ -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

0 comments on commit f08db1f

Please sign in to comment.