Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

SENS: RNG: SF45: Fixed sf45 parser, added general checks to avoid potential out-of-bound access

Browse Source
This commit is contained in:
Alexander Lerach
2024-11-19 17:09:25 +01:00
committed by Silvan Fuhrer
parent 1718b37fe4
commit 88d771e3e5
2 changed files with 60 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+48 -43
View File
@@ -32,7 +32,6 @@
****************************************************************************/
#include "lightware_sf45_serial.hpp"
#include "sf45_commands.h"
#include <inttypes.h>
#include <fcntl.h>
@@ -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) :
@@ -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
@@ -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);
@@ -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);
@@ -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);
@@ -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);
// Process the incoming distance data
if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
sf45_request_handle(&_linebuf[index]);
if (_init_complete && _crc_valid) {
sf45_process_replies(&distance_m);
_linebuf_size = 0;
break;
}
}
}
if (_init_complete) {
sf45_process_replies(&distance_m);
} // end if
index--;
}
} else {
ret = ::read(_fd, &readbuf[0], 10);
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);
}
}
@@ -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);
@@ -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();
}
@@ -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
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
+12 -13
View File
@@ -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
@@ -79,14 +78,14 @@ public:
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{};
@@ -98,19 +97,19 @@ private:
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