fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:28:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

SENS: RNG: SF45:Fix startup problems, increase frequency, robust parser, use nonblocking reads

Browse Source
This commit is contained in:
Alexander Lerach
2024-12-16 10:49:13 +01:00
committed by Silvan Fuhrer
parent 88d771e3e5
commit f34b22907c
3 changed files with 246 additions and 342 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+232 -322
View File
@@ -37,10 +37,11 @@
#include <fcntl.h> #include <fcntl.h>
#include <termios.h> #include <termios.h>
#include <lib/crc/crc.h> #include <lib/crc/crc.h>
#include <lib/mathlib/mathlib.h>
#include <float.h> #include <float.h>
#include <mathlib/mathlib.h>
#include <matrix/matrix/math.hpp> using namespace time_literals;
/* Configuration Constants */ /* Configuration Constants */
#define SF45_SCALE_FACTOR 0.01f #define SF45_SCALE_FACTOR 0.01f
@@ -65,8 +66,6 @@ SF45LaserSerial::SF45LaserSerial(const char *port) :
device_id.devid_s.bus = bus_num; device_id.devid_s.bus = bus_num;
} }
_num_retries = 2;
// populate obstacle map members // populate obstacle map members
_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
_obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; _obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
@@ -78,7 +77,6 @@ SF45LaserSerial::SF45LaserSerial(const char *port) :
for (uint32_t i = 0 ; i < BIN_COUNT; i++) { for (uint32_t i = 0 ; i < BIN_COUNT; i++) {
_obstacle_map_msg.distances[i] = UINT16_MAX; _obstacle_map_msg.distances[i] = UINT16_MAX;
} }
} }
SF45LaserSerial::~SF45LaserSerial() SF45LaserSerial::~SF45LaserSerial()
@@ -95,43 +93,36 @@ int SF45LaserSerial::init()
param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
_interval = 10000;
start(); start();
return PX4_OK; return PX4_OK;
} }
int SF45LaserSerial::measure() int SF45LaserSerial::measure()
{ {
int rate = (int)_update_rate; int32_t rate = (int32_t)_update_rate;
_data_output = 0x101; // raw distance + yaw readings _data_output = 0x101; // raw distance (first return) + yaw readings
_stream_data = 5; // enable constant streaming _stream_data = 5; // enable constant streaming
// send some packets so the sensor starts scanning // send packets to the sensor depending on the state
switch (_sensor_state) { switch (_sensor_state) {
// sensor should now respond
case STATE_UNINIT: case STATE_UNINIT:
while (_num_retries--) { // Used to probe if the sensor is alive
sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
_sensor_state = STATE_UNINIT;
}
_sensor_state = STATE_SEND_PRODUCT_NAME;
break; break;
case STATE_SEND_PRODUCT_NAME: case STATE_ACK_PRODUCT_NAME:
// Update rate default to 50 readings/s // Update rate default to 50 readings/s
sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t)); sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t));
_sensor_state = STATE_SEND_UPDATE_RATE;
break; break;
case STATE_SEND_UPDATE_RATE: case STATE_ACK_UPDATE_RATE:
// Configure the data that the sensor shall output
sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output)); sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
_sensor_state = STATE_SEND_DISTANCE_DATA;
break; break;
case STATE_SEND_DISTANCE_DATA: case STATE_ACK_DISTANCE_OUTPUT:
// Configure the sensor to automatically output data at the configured update rate
sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data)); sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data));
_sensor_state = STATE_SEND_STREAM; _sensor_state = STATE_SEND_STREAM;
break; break;
@@ -145,136 +136,90 @@ int SF45LaserSerial::measure()
int SF45LaserSerial::collect() int SF45LaserSerial::collect()
{ {
perf_begin(_sample_perf);
int ret;
float distance_m = -1.0f; float distance_m = -1.0f;
if (_sensor_state == STATE_SEND_PRODUCT_NAME) { if (_sensor_state == STATE_UNINIT) {
perf_begin(_sample_perf);
const int payload_length = 22; const int payload_length = 22;
ret = ::read(_fd, &_linebuf[0], payload_length);
if (ret < 0) { _crc_valid = false;
PX4_ERR("ERROR (ack from sending product name cmd): %d", ret); sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME);
perf_count(_comms_errors);
if (_crc_valid) {
_sensor_state = STATE_ACK_PRODUCT_NAME;
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return PX4_OK;
} }
sf45_request_handle(_linebuf); return -EAGAIN;
ScheduleDelayed(_interval * 3);
} else if (_sensor_state == STATE_SEND_UPDATE_RATE) { } else if (_sensor_state == STATE_ACK_PRODUCT_NAME) {
perf_begin(_sample_perf);
const int payload_length = 7; const int payload_length = 7;
ret = ::read(_fd, &_linebuf[0], payload_length);
if (ret < 0) { _crc_valid = false;
PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret); sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE);
perf_count(_comms_errors);
if (_crc_valid) {
_sensor_state = STATE_ACK_UPDATE_RATE;
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return PX4_OK;
} }
if (ret == payload_length && _linebuf[3] == SF_UPDATE_RATE) { return -EAGAIN;
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}
} else if (_sensor_state == STATE_SEND_DISTANCE_DATA) { } else if (_sensor_state == STATE_ACK_UPDATE_RATE) {
const int payload_length = 8; perf_begin(_sample_perf);
ret = ::read(_fd, &_linebuf[0], payload_length);
if (ret < 0) {
PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}
} else {
// Stream data from sensor
const int payload_length = 10; const int payload_length = 10;
size_t max_read = sizeof(_linebuf) - _linebuf_size; _crc_valid = false;
ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT);
_linebuf_size += ret;
if (ret < 0) { if (_crc_valid) {
PX4_ERR("ERROR (ack from streaming distance data): %d", ret); _sensor_state = STATE_ACK_DISTANCE_OUTPUT;
_linebuf_size = 0;
perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return PX4_OK;
} }
// Not enough data to parse a complete packet. Gather more data in the next cycle. return -EAGAIN;
if (_linebuf_size < payload_length) {
return -EAGAIN; } else {
// Stream data from sensor
perf_begin(_sample_perf);
const int payload_length = 10;
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
if (_crc_valid) {
sf45_process_replies(&distance_m);
PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
perf_end(_sample_perf);
return PX4_OK;
} }
int index = _linebuf_size - payload_length;
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;
}
}
}
index--;
}
// 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);
}
}
if (_consecutive_fail_count > 35 && !_sensor_ready) {
PX4_ERR("Restarting the state machine");
return PX4_ERROR;
}
_last_read = hrt_absolute_time();
if (!_crc_valid) {
return -EAGAIN; return -EAGAIN;
} }
PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
perf_end(_sample_perf);
return PX4_OK;
} }
void SF45LaserSerial::start() void SF45LaserSerial::start()
{ {
/* reset the report ring and state machine */ /* reset the sensor state */
_sensor_state = STATE_UNINIT;
/* reset the report ring */
_collect_phase = false; _collect_phase = false;
/* reset the UART receive buffer size */ /* reset the UART receive buffer size */
_linebuf_size = 0; _linebuf_size = 0;
/* reset the fail counter */
_last_received_time = hrt_absolute_time();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
ScheduleNow(); ScheduleNow();
} }
@@ -289,8 +234,7 @@ void SF45LaserSerial::Run()
/* fds initialized? */ /* fds initialized? */
if (_fd < 0) { if (_fd < 0) {
/* open fd: non-blocking read mode*/ /* open fd: non-blocking read mode*/
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_fd < 0) { if (_fd < 0) {
PX4_ERR("serial open failed (%i)", errno); PX4_ERR("serial open failed (%i)", errno);
@@ -304,34 +248,11 @@ void SF45LaserSerial::Run()
/* fill the struct for the new configuration */ /* fill the struct for the new configuration */
tcgetattr(_fd, &uart_config); tcgetattr(_fd, &uart_config);
uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8; /* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
uart_config.c_cflag |= (CLOCAL | CREAD); /* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
// no parity, 1 stop bit, flow control disabled
uart_config.c_cflag &= ~(PARENB | PARODD);
uart_config.c_cflag |= 0;
uart_config.c_cflag &= ~CSTOPB;
uart_config.c_cflag &= ~CRTSCTS;
uart_config.c_iflag &= ~IGNBRK;
uart_config.c_iflag &= ~ICRNL;
uart_config.c_iflag &= ~(IXON | IXOFF | IXANY);
// echo and echo NL off, canonical mode off (raw mode)
// extended input processing off, signal chars off
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
uart_config.c_oflag = 0;
uart_config.c_cc[VMIN] = 0;
uart_config.c_cc[VTIME] = 1;
unsigned speed = B921600; unsigned speed = B921600;
@@ -347,51 +268,37 @@ void SF45LaserSerial::Run()
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("baud %d ATTR", termios_state); PX4_ERR("baud %d ATTR", termios_state);
} }
} }
if (_collect_phase) { if (_collect_phase) {
if (hrt_absolute_time() - _last_received_time >= 1_s) {
/* perform collection */
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
ScheduleDelayed(1042 * 8);
return;
}
if (OK != collect_ret) {
// Too many packet errors in init, restart the consecutive fail count
_consecutive_fail_count = 0;
/* restart the measurement state machine */
start(); start();
return; return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
} }
/* next phase is measurement */ /* perform collection */
_collect_phase = false; if (collect() != PX4_OK && errno != EAGAIN) {
PX4_DEBUG("collect error");
}
if (_sensor_state != STATE_SEND_STREAM) {
/* next phase is measurement */
_collect_phase = false;
}
} else {
/* measurement phase */
if (measure() != PX4_OK) {
PX4_DEBUG("measure error");
}
/* next phase is collection */
_collect_phase = true;
} }
/* measurement phase */
if (OK != measure()) {
PX4_DEBUG("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */ /* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(_interval); ScheduleDelayed(_interval);
} }
void SF45LaserSerial::print_info() void SF45LaserSerial::print_info()
@@ -400,7 +307,7 @@ void SF45LaserSerial::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
} }
void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf) void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id)
{ {
// SF45 protocol // SF45 protocol
// Start byte is 0xAA and is the start of packet // Start byte is 0xAA and is the start of packet
@@ -410,172 +317,178 @@ void SF45LaserSerial::sf45_request_handle(uint8_t *input_buf)
// ID byte precedes the data in the payload // ID byte precedes the data in the payload
// CRC comprised of 16-bit checksum (not included in checksum calc.) // CRC comprised of 16-bit checksum (not included in checksum calc.)
uint16_t recv_crc = 0; int ret;
bool restart_flag = false; size_t max_read = sizeof(_linebuf) - _linebuf_size;
ret = ::read(_fd, &_linebuf[_linebuf_size], max_read);
while (restart_flag != true) { if (ret < 0 && errno != EAGAIN) {
PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
_linebuf_size = 0;
perf_count(_comms_errors);
perf_end(_sample_perf);
return;
}
switch (_parsed_state) { if (ret > 0) {
case 0: { _last_received_time = hrt_absolute_time();
if (input_buf[0] == 0xAA) { _linebuf_size += ret;
// start of frame is valid, continue }
_sop_valid = true;
_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
_parsed_state = 1;
break;
} else { // Not enough data to parse a complete packet. Gather more data in the next cycle.
_sop_valid = false; if (_linebuf_size < payload_length) {
_crc_valid = false; return;
_parsed_state = 0; }
restart_flag = true;
_calc_crc = 0;
perf_count(_comms_errors);
perf_end(_sample_perf);
PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
_consecutive_fail_count++;
break;
} // end else
} // end case 0
case 1: { int index = 0;
rx_field.flags_lo = input_buf[1];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
_parsed_state = 2;
break;
}
case 2: { while (index <= _linebuf_size - payload_length && _crc_valid == false) {
rx_field.flags_hi = input_buf[2]; bool restart_flag = false;
rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
// Check payload length against known max value while (restart_flag != true) {
if (rx_field.data_len > 17) { switch (_parsed_state) {
_parsed_state = 0; case 0: {
restart_flag = true; if (_linebuf[index] == 0xAA) {
_calc_crc = 0; // start of frame is valid, continue
perf_count(_comms_errors); _sop_valid = true;
perf_end(_sample_perf); _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
PX4_DEBUG("Payload length error: %d", _sensor_state); _parsed_state = 1;
_consecutive_fail_count++; break;
break;
} else {
_parsed_state = 3;
break;
}
}
case 3: {
rx_field.msg_id = input_buf[3];
if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT
|| rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) {
if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) {
_sensor_ready = true;
} else { } else {
_sensor_ready = false; _sop_valid = false;
_crc_valid = false;
_parsed_state = 0;
restart_flag = true;
_calc_crc = 0;
PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
break;
} // end else
} // end case 0
case 1: {
rx_field.flags_lo = _linebuf[index + 1];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
_parsed_state = 2;
break;
}
case 2: {
rx_field.flags_hi = _linebuf[index + 2];
rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6);
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi);
// Check payload length against known max value
if (rx_field.data_len > 17) {
_parsed_state = 0;
restart_flag = true;
_calc_crc = 0;
PX4_DEBUG("Payload length error: %d", _sensor_state);
break;
} else {
_parsed_state = 3;
break;
}
}
case 3: {
rx_field.msg_id = _linebuf[index + 3];
if (rx_field.msg_id == msg_id) {
_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
_parsed_state = 4;
break;
} }
_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); // Ignore message ID's that aren't searched
else {
_parsed_state = 4; _parsed_state = 0;
break; _calc_crc = 0;
restart_flag = true;
PX4_DEBUG("Non needed message ID: %d", _sensor_state);
break;
}
} }
// Ignore message ID's that aren't defined // Data
else { case 4: {
_parsed_state = 0; // Process commands with & w/out data bytes
_calc_crc = 0; if (rx_field.data_len > 1) {
restart_flag = true; for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
perf_count(_comms_errors);
perf_end(_sample_perf);
_consecutive_fail_count++;
PX4_DEBUG("Unknown message ID: %d", _sensor_state);
break;
} rx_field.data[_data_bytes_recv] = _linebuf[index + i];
} _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
_data_bytes_recv = _data_bytes_recv + 1;
// Data } // end for
case 4: { } //end if
// Process commands with & w/out data bytes
if (rx_field.data_len > 1) {
for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) {
rx_field.data[_data_bytes_recv] = input_buf[i]; else {
_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
_data_bytes_recv = _data_bytes_recv + 1;
} // end for _parsed_state = 5;
} //end if _data_bytes_recv = 0;
_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
else { }
_parsed_state = 5; _parsed_state = 5;
_data_bytes_recv = 0; _data_bytes_recv = 0;
_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); break;
} }
_parsed_state = 5; // CRC low byte
_data_bytes_recv = 0; case 5: {
break; rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
} _parsed_state = 6;
break;
}
// CRC low byte // CRC high byte
case 5: { case 6: {
rx_field.crc[0] = input_buf[3 + rx_field.data_len]; rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len];
_parsed_state = 6; uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
break;
}
// CRC high byte // Check the received crc bytes from the sf45 against our own CRC calcuation
case 6: { // If it matches, we can check if sensor ready
rx_field.crc[1] = input_buf[4 + rx_field.data_len]; // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; if (recv_crc == _calc_crc) {
_crc_valid = true;
// Check the received crc bytes from the sf45 against our own CRC calcuation _parsed_state = 0;
// If it matches, we can check if sensor ready _calc_crc = 0;
// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete restart_flag = true;
if (recv_crc == _calc_crc) { break;
_crc_valid = true;
// Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM
if (_sensor_ready) {
_init_complete = true;
} else { } else {
_init_complete = false;
_crc_valid = false;
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
perf_count(_comms_errors);
PX4_DEBUG("CRC mismatch: %d", _sensor_state);
break;
} }
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
break;
} else {
_crc_valid = false;
_init_complete = false;
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
perf_count(_comms_errors);
perf_end(_sample_perf);
PX4_DEBUG("CRC mismatch: %d", _sensor_state);
break;
} }
} } // end switch
} // end switch } //end while
} //end while
index++;
}
// If we parsed successfully, remove the parsed part from the buffer if it is still large enough
if (_crc_valid && index + payload_length < _linebuf_size) {
unsigned next_after_index = index + payload_length;
memmove(&_linebuf[0], &_linebuf[next_after_index], _linebuf_size - next_after_index);
_linebuf_size -= next_after_index;
}
// 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 ((unsigned)_linebuf_size >= sizeof(_linebuf)) {
_linebuf_size = 0;
perf_count(_comms_errors);
}
} }
void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len) void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len)
{ {
uint16_t crc_val = 0; uint16_t crc_val = 0;
uint8_t packet_buff[SF45_MAX_PAYLOAD]; uint8_t packet_buff[SF45_MAX_PAYLOAD];
@@ -612,7 +525,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
if (msg_id == SF_DISTANCE_OUTPUT) { if (msg_id == SF_DISTANCE_OUTPUT) {
uint8_t data_convert = data[0] & 0x00FF; uint8_t data_convert = data[0] & 0x00FF;
// write data bytes to the output buffer // write data bytes to the output buffer
packet_buff[data_inc] = data_convert; packet_buff[data_inc] = data_convert;
// Add data bytes to crc add function // Add data bytes to crc add function
crc_val = sf45_format_crc(crc_val, data_convert); crc_val = sf45_format_crc(crc_val, data_convert);
data_inc = data_inc + 1; data_inc = data_inc + 1;
@@ -650,12 +563,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
// Add data bytes to crc add function // Add data bytes to crc add function
crc_val = sf45_format_crc(crc_val, data[0]); crc_val = sf45_format_crc(crc_val, data[0]);
data_inc = data_inc + 1; data_inc = data_inc + 1;
} }
else { else {
// Product Name // Product Name
PX4_INFO("INFO: Product name"); PX4_DEBUG("DEBUG: Product name");
} }
uint8_t crc_lo = crc_val & 0xFF; uint8_t crc_lo = crc_val & 0xFF;
@@ -694,7 +606,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
if (raw_yaw > 32000) { if (raw_yaw > 32000) {
raw_yaw = raw_yaw - 65535; raw_yaw = raw_yaw - 65535;
} }
// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
@@ -802,7 +713,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{ {
uint8_t mapped_sector = 0; uint8_t mapped_sector = 0;
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset); float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment); mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
+13 -19
View File
@@ -56,9 +56,9 @@
enum SF_SERIAL_STATE { enum SF_SERIAL_STATE {
STATE_UNINIT = 0, STATE_UNINIT = 0,
STATE_SEND_PRODUCT_NAME = 1, STATE_ACK_PRODUCT_NAME = 1,
STATE_SEND_UPDATE_RATE = 2, STATE_ACK_UPDATE_RATE = 2,
STATE_SEND_DISTANCE_DATA = 3, STATE_ACK_DISTANCE_OUTPUT = 3,
STATE_SEND_STREAM = 4, STATE_SEND_STREAM = 4,
}; };
@@ -80,8 +80,8 @@ public:
int init(); int init();
void print_info(); void print_info();
void sf45_request_handle(uint8_t *value); void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
void sf45_process_replies(float *data); void sf45_process_replies(float *data);
uint8_t sf45_convert_angle(const int16_t yaw); uint8_t sf45_convert_angle(const int16_t yaw);
@@ -104,30 +104,26 @@ private:
char _port[20] {}; char _port[20] {};
int _interval{10000}; int _interval{2000};
bool _collect_phase{false}; bool _collect_phase{false};
int _fd{-1}; int _fd{-1};
uint8_t _linebuf[SF45_MAX_PAYLOAD] {}; uint8_t _linebuf[SF45_MAX_PAYLOAD] {};
unsigned _linebuf_size{0}; int _linebuf_size{0};
hrt_abstime _last_read{0};
// SF45/B uses a binary protocol to include header,flags // SF45/B uses a binary protocol to include header,flags
// message ID, payload, and checksum // message ID, payload, and checksum
bool _is_sf45{false}; bool _is_sf45{false};
bool _init_complete{false}; SF_SERIAL_STATE _sensor_state{STATE_UNINIT};
bool _sensor_ready{false};
uint8_t _sensor_state{0};
int _baud_rate{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}; int32_t _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 _stream_data{0};
int32_t _update_rate{1}; int32_t _update_rate{0};
int _data_output{0}; int32_t _data_output{0};
const uint8_t _start_of_frame{0xAA}; const uint8_t _start_of_frame{0xAA};
uint16_t _data_bytes_recv{0}; uint16_t _data_bytes_recv{0};
uint8_t _parsed_state{0}; uint8_t _parsed_state{0};
bool _sop_valid{false}; bool _sop_valid{false};
uint16_t _calc_crc{0}; uint16_t _calc_crc{0};
uint8_t _num_retries{0};
int32_t _yaw_cfg{0}; int32_t _yaw_cfg{0};
int32_t _orient_cfg{0}; int32_t _orient_cfg{0};
uint8_t _previous_bin{0}; uint8_t _previous_bin{0};
@@ -135,9 +131,7 @@ private:
// end of SF45/B data members // end of SF45/B data members
unsigned _consecutive_fail_count; hrt_abstime _last_received_time{0};
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
}; };
src/drivers/distance_sensor/lightware_sf45_serial/module.yaml
+1 -1
View File
@@ -32,7 +32,7 @@ parameters:
12: 5000hz 12: 5000hz
reboot_required: true reboot_required: true
num_instances: 1 num_instances: 1
default: 1 default: 5
SF45_ORIENT_CFG: SF45_ORIENT_CFG:
description: description: