Skip to content

Commit

Permalink
SENS: RNG: SF45: Fixed sf45 parser, added general checks to avoid pot…
Browse files Browse the repository at this point in the history 
…ential out-of-bound access
  • Loading branch information
@alexcekay @Claudio-Chies
alexcekay authored and Claudio-Chies committed Nov 22, 2024
1 parent fdb9db6 commit 7f7a4c5
Show file tree
Hide file tree
Showing 2 changed files with 77 additions and 73 deletions.
89 changes: 47 additions & 42 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@
****************************************************************************/

#include "lightware_sf45_serial.hpp"
#include "sf45_commands.h"

#include <inttypes.h>
#include <fcntl.h>
Expand All @@ -44,7 +43,6 @@
#include <matrix/matrix/math.hpp>

/* Configuration Constants */
#define SF45_MAX_PAYLOAD 256
#define SF45_SCALE_FACTOR 0.01f

SF45LaserSerial::SF45LaserSerial(const char *port) :
Expand Down Expand Up @@ -105,7 +103,6 @@ int SF45LaserSerial::init()

int SF45LaserSerial::measure()
{

int rate = (int)_update_rate;
_data_output = 0x101; // raw distance + yaw readings
_stream_data = 5; // enable constant streaming
Expand Down Expand Up @@ -150,20 +147,13 @@ int SF45LaserSerial::collect()
{
perf_begin(_sample_perf);

/* clear buffer if last read was too long ago */
int ret;
/* the buffer for read chars is buflen minus null termination */
uint8_t readbuf[SF45_MAX_PAYLOAD];

float distance_m = -1.0f;

/* read from the sensor (uart buffer) */



if (_sensor_state == STATE_SEND_PRODUCT_NAME) {

ret = ::read(_fd, &readbuf[0], 22);
const int payload_length = 22;
ret = ::read(_fd, &_linebuf[0], payload_length);

if (ret < 0) {
PX4_ERR("ERROR (ack from sending product name cmd): %d", ret);
Expand All @@ -172,12 +162,13 @@ int SF45LaserSerial::collect()
return ret;
}

sf45_request_handle(ret, readbuf);
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);

} else if (_sensor_state == STATE_SEND_UPDATE_RATE) {

ret = ::read(_fd, &readbuf[0], 7);
const int payload_length = 7;
ret = ::read(_fd, &_linebuf[0], payload_length);

if (ret < 0) {
PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret);
Expand All @@ -186,14 +177,15 @@ int SF45LaserSerial::collect()
return ret;
}

if (readbuf[3] == SF_UPDATE_RATE) {
sf45_request_handle(ret, readbuf);
if (ret == payload_length && _linebuf[3] == SF_UPDATE_RATE) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}

} else if (_sensor_state == STATE_SEND_DISTANCE_DATA) {

ret = ::read(_fd, &readbuf[0], 8);
const int payload_length = 8;
ret = ::read(_fd, &_linebuf[0], payload_length);

if (ret < 0) {
PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret);
Expand All @@ -202,46 +194,58 @@ int SF45LaserSerial::collect()
return ret;
}

if (readbuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(ret, readbuf);
if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}

// Stream data from sensor

} else {
ret = ::read(_fd, &readbuf[0], 10);
// Stream data from sensor
const int payload_length = 10;

size_t max_read = sizeof(_linebuf) - _linebuf_size;
ret = ::read(_fd, &_linebuf[_linebuf_size], max_read);
_linebuf_size += ret;

if (ret < 0) {
PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
_linebuf_size = 0;
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}

uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
// Not enough data to parse a complete packet. Gather more data in the next cycle.
if (_linebuf_size < payload_length) {
return -EAGAIN;
}

// Process the incoming distance data
if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
int index = _linebuf_size - payload_length;

for (uint8_t i = 0; i < ret; ++i) {
sf45_request_handle(ret, readbuf);
}
while (index >= 0) {
if (_linebuf[index] == 0xAA) {
uint8_t flags_payload = (_linebuf[index + 1] >> 6) | (_linebuf[index + 2] << 2);

if (_init_complete) {
sf45_process_replies(&distance_m);
} // end if
// Process the incoming distance data
if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
sf45_request_handle(&_linebuf[index]);

} else {
if (_init_complete && _crc_valid) {
sf45_process_replies(&distance_m);
_linebuf_size = 0;
break;
}
}
}

ret = ::read(_fd, &readbuf[0], 10);
index--;
}

if (ret < 0) {
PX4_ERR("ERROR (unknown sensor data): %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
// The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again.
if (_linebuf_size >= sizeof(_linebuf)) {
_linebuf_size = 0;
perf_count(_comms_errors);
}
}

Expand All @@ -256,8 +260,7 @@ int SF45LaserSerial::collect()
return -EAGAIN;
}

PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO"));

PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));

perf_end(_sample_perf);

Expand All @@ -269,6 +272,9 @@ void SF45LaserSerial::start()
/* reset the report ring and state machine */
_collect_phase = false;

/* reset the UART receive buffer size */
_linebuf_size = 0;

/* schedule a cycle to start things */
ScheduleNow();
}
Expand Down Expand Up @@ -394,9 +400,8 @@ void SF45LaserSerial::print_info()
perf_print_counter(_comms_errors);
}

void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf)
{

// SF45 protocol
// Start byte is 0xAA and is the start of packet
// Payload length sanity check (0-1023) bytes
Expand Down
61 changes: 30 additions & 31 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@ enum SF_SERIAL_STATE {
};



enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90
Expand All @@ -79,14 +78,14 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
SF45LaserSerial(const char *port);
~SF45LaserSerial() override;

int init();
int init();
void print_info();
void sf45_request_handle(int val, uint8_t *value);
void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len);
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
void sf45_process_replies(float *data);
uint8_t sf45_convert_angle(const int16_t yaw);
float sf45_wrap_360(float f);
void sf45_request_handle(uint8_t *value);
void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len);
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
void sf45_process_replies(float *data);
uint8_t sf45_convert_angle(const int16_t yaw);
float sf45_wrap_360(float f);

private:
obstacle_distance_s _obstacle_map_msg{};
Expand All @@ -98,40 +97,40 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
void Run() override;
int measure();
int collect();
bool _crc_valid{false};
bool _crc_valid{false};

void _publish_obstacle_msg(hrt_abstime now);
uint64_t _data_timestamps[BIN_COUNT];


char _port[20] {};
int _interval{10000};
int _interval{10000};
bool _collect_phase{false};
int _fd{-1};
int _linebuf[256] {};
unsigned _linebuf_index{0};
hrt_abstime _last_read{0};
uint8_t _linebuf[SF45_MAX_PAYLOAD] {};
unsigned _linebuf_size{0};
hrt_abstime _last_read{0};

// SF45/B uses a binary protocol to include header,flags
// message ID, payload, and checksum
bool _is_sf45{false};
bool _init_complete{false};
bool _sensor_ready{false};
uint8_t _sensor_state{0};
int _baud_rate{0};
int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int _stream_data{0};
int32_t _update_rate{1};
int _data_output{0};
const uint8_t _start_of_frame{0xAA};
uint16_t _data_bytes_recv{0};
uint8_t _parsed_state{0};
bool _sop_valid{false};
uint16_t _calc_crc{0};
uint8_t _num_retries{0};
int32_t _yaw_cfg{0};
int32_t _orient_cfg{0};
uint16_t _previous_bin{0};
bool _is_sf45{false};
bool _init_complete{false};
bool _sensor_ready{false};
uint8_t _sensor_state{0};
int _baud_rate{0};
int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int _stream_data{0};
int32_t _update_rate{1};
int _data_output{0};
const uint8_t _start_of_frame{0xAA};
uint16_t _data_bytes_recv{0};
uint8_t _parsed_state{0};
bool _sop_valid{false};
uint16_t _calc_crc{0};
uint8_t _num_retries{0};
int32_t _yaw_cfg{0};
int32_t _orient_cfg{0};
uint16_t _previous_bin{0};
uint16_t _current_bin_dist{UINT16_MAX};

// end of SF45/B data members
Expand Down

0 comments on commit 7f7a4c5

Please sign in to comment.