Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

AP_Proximity: add RPLidar S2, remove memory copy for each measurement #26835

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
151 changes: 37 additions & 114 deletions libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View file
Open in desktop
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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Has this PR been tested on S1?

Copy link
Member Author

@mirkix mirkix Apr 29, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@peterbarker Yes, tested with S1. Also with A1 and S2, S1 and S2 send the same data after reset. What is tested is also in the header of this PR.

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) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this limit here?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is a wait for 1000 ms after a Lidar reset. We can make sure the Lidar has rebooted and skip the startup message.

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));
Copy link
Contributor

@rmackay9 rmackay9 Apr 17, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think maybe we should change this to read (or peek) just a single byte until we find the RPLIDAR_PREAMBLE byte. As it is, it always reads 7 bytes at a time so if a stray byte gets in there it will only recover if it gets lucky and the preamble byte of some later packet happens to be at a 7-byte interval in the buffer.

I think one easy way for a stray byte to be in the buffer is immediately after a RESET (see above). Just from inspection I would have thought that after, "_ujart->discard_input()" is called another message could arrive or we could catch half a message as it is on its way into the buffer.

Copy link
Member Author

@mirkix mirkix Apr 18, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@rmackay9 What I saw, after a reset the response will send by RPLidar, we will skip that after a second, until then no new message is send by RPLidar so we can use the read of 7 bytes, otherwise there is a protocol error. But I check to add the byte looking think for RPILIDAR_PREAMBLE.

_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
Open in desktop
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
Loading