fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 13:29:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

[BACKPORT 1.15] Sf45 initialization fixes and per-sector publishing (#25399)

Browse Source 
* SF45 fixes to restart the state machine if sensor does not init correctly (#23565)

* fixes to restart the state machine if sensor does not init correctly

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* fixes

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* increase fail count

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* remove extra flush, switch from warn to debug, add enum states for sensor bring-up

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* remove dead code, decrease restart fail count metric, break out of loop with consec errors if over the fail count and not init

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

---------

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* remove tel2 default from sf45 (#24602)

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* fix to orientation offsets for scaled yaw, removed unused param

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* Shift vertical orientation above scaling yaw operation, cp angle sign change

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* SENS: RNG: SF45 changed data processing and publication design, moved to a publishing per sector design.
other minor improvements

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

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

* SENS: RNG: SF45: Added timeout to sensor measurements, to compensate the lower loop time of CollisionPrevention

---------

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
Co-authored-by: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Co-authored-by: Alexander Lerach <alexander@auterion.com>
This commit is contained in:
Andrew Brahim 2025-10-14 01:20:55 -04:00 committed by GitHub
parent d66956a682
commit 85df8c2281
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 428 additions and 406 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

689
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************
*
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -32,24 +32,22 @@
****************************************************************************/
#include "lightware_sf45_serial.hpp"
#include "sf45_commands.h"
#include <inttypes.h>
#include <fcntl.h>
#include <termios.h>
#include <lib/crc/crc.h>
#include <lib/mathlib/mathlib.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
using namespace time_literals;
/* Configuration Constants */
#define SF45_MAX_PAYLOAD 256
#define SF45_SCALE_FACTOR 0.01f
SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
SF45LaserSerial::SF45LaserSerial(const char *port) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_px4_rangefinder(0, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
{
@ -68,17 +66,17 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
device_id.devid_s.bus = bus_num;
}
_num_retries = 2;
_px4_rangefinder.set_device_id(device_id.devid);
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
// populate obstacle map members
_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
_obstacle_map_msg.increment = 5;
_obstacle_map_msg.angle_offset = -2.5;
_obstacle_map_msg.min_distance = UINT16_MAX;
_obstacle_map_msg.max_distance = 5000;
_obstacle_distance.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
_obstacle_distance.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
_obstacle_distance.increment = 5;
_obstacle_distance.min_distance = 20;
_obstacle_distance.max_distance = 5000;
_obstacle_distance.angle_offset = 0;
for (uint32_t i = 0 ; i < BIN_COUNT; i++) {
_obstacle_distance.distances[i] = UINT16_MAX;
}
}
SF45LaserSerial::~SF45LaserSerial()
@ -91,55 +89,42 @@ SF45LaserSerial::~SF45LaserSerial()
int SF45LaserSerial::init()
{
param_get(param_find("SF45_UPDATE_CFG"), &_update_rate);
param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
/* SF45/B (50M) */
_px4_rangefinder.set_min_distance(0.2f);
_px4_rangefinder.set_max_distance(50.0f);
_interval = 10000;
start();
return PX4_OK;
}
int SF45LaserSerial::measure()
{
int rate = (int)_update_rate;
_data_output = 0x101; // raw distance + yaw readings
int32_t rate = (int32_t)_update_rate;
_data_output = 0x101; // raw distance (first return) + yaw readings
_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) {
// sensor should now respond
case 0:
while (_num_retries--) {
sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
_sensor_state = 0;
}
_sensor_state = 1;
case STATE_UNINIT:
// Used to probe if the sensor is alive
sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0);
break;
case 1:
case STATE_ACK_PRODUCT_NAME:
// Update rate default to 50 readings/s
sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t));
_sensor_state = 2;
break;
case 2:
case STATE_ACK_UPDATE_RATE:
// Configure the data that the sensor shall output
sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output));
_sensor_state = 3;
break;
case 3:
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));
_sensor_state = 4;
_sensor_state = STATE_SEND_STREAM;
break;
default:
@ -151,104 +136,90 @@ int SF45LaserSerial::measure()
int SF45LaserSerial::collect()
{
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
int64_t read_elapsed = hrt_elapsed_time(&_last_read);
int ret;
/* the buffer for read chars is buflen minus null termination */
param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint);
uint8_t readbuf[SF45_MAX_PAYLOAD];
float distance_m = -1.0f;
/* read from the sensor (uart buffer) */
const hrt_abstime timestamp_sample = hrt_absolute_time();
if (_sensor_state == STATE_UNINIT) {
if (_sensor_state == 1) {
perf_begin(_sample_perf);
const int payload_length = 22;
ret = ::read(_fd, &readbuf[0], 22);
sf45_request_handle(ret, readbuf);
ScheduleDelayed(_interval * 2);
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME);
} else if (_sensor_state == 2) {
ret = ::read(_fd, &readbuf[0], 7);
if (readbuf[3] == SF_UPDATE_RATE) {
sf45_request_handle(ret, readbuf);
ScheduleDelayed(_interval * 5);
if (_crc_valid) {
_sensor_state = STATE_ACK_PRODUCT_NAME;
perf_end(_sample_perf);
return PX4_OK;
}
} else if (_sensor_state == 3) {
return -EAGAIN;
ret = ::read(_fd, &readbuf[0], 8);
} else if (_sensor_state == STATE_ACK_PRODUCT_NAME) {
if (readbuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(ret, readbuf);
ScheduleDelayed(_interval * 5);
perf_begin(_sample_perf);
const int payload_length = 7;
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE);
if (_crc_valid) {
_sensor_state = STATE_ACK_UPDATE_RATE;
perf_end(_sample_perf);
return PX4_OK;
}
return -EAGAIN;
} else if (_sensor_state == STATE_ACK_UPDATE_RATE) {
perf_begin(_sample_perf);
const int payload_length = 10;
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT);
if (_crc_valid) {
_sensor_state = STATE_ACK_DISTANCE_OUTPUT;
perf_end(_sample_perf);
return PX4_OK;
}
return -EAGAIN;
} else {
ret = ::read(_fd, &readbuf[0], 10);
uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
// Stream data from sensor
perf_begin(_sample_perf);
const int payload_length = 10;
if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
_crc_valid = false;
sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
for (uint8_t i = 0; i < ret; ++i) {
sf45_request_handle(ret, readbuf);
}
if (_init_complete) {
sf45_process_replies(&distance_m);
} // end if
} else {
ret = ::read(_fd, &readbuf[0], 10);
}
}
if (ret < 0) {
PX4_DEBUG("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_interval * 2)) {
PX4_DEBUG("Timing out...");
return ret;
} else {
return -EAGAIN;
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;
}
} else if (ret == 0) {
return -EAGAIN;
}
_last_read = hrt_absolute_time();
if (!_crc_valid) {
return -EAGAIN;
}
PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO"));
_px4_rangefinder.update(timestamp_sample, distance_m);
perf_end(_sample_perf);
return PX4_OK;
}
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;
/* reset the UART receive buffer size */
_linebuf_size = 0;
/* reset the fail counter */
_last_received_time = hrt_absolute_time();
/* schedule a cycle to start things */
ScheduleNow();
}
@ -263,11 +234,10 @@ void SF45LaserSerial::Run()
/* fds initialized? */
if (_fd < 0) {
/* open fd: non-blocking read mode*/
_fd = ::open(_port, O_RDWR | O_NOCTTY);
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (_fd < 0) {
PX4_ERR("open failed (%i)", errno);
PX4_ERR("serial open failed (%i)", errno);
return;
}
@ -278,34 +248,11 @@ void SF45LaserSerial::Run()
/* fill the struct for the new configuration */
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, 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;
/* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
unsigned speed = B921600;
@ -321,54 +268,37 @@ void SF45LaserSerial::Run()
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("baud %d ATTR", termios_state);
}
}
if (_collect_phase) {
/* 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) {
/* we know the sensor needs about four seconds to initialize */
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
PX4_ERR("collection error #%u", _consecutive_fail_count);
}
_consecutive_fail_count++;
/* restart the measurement state machine */
if (hrt_absolute_time() - _last_received_time >= 1_s) {
start();
return;
} else {
/* apparently success */
_consecutive_fail_count = 0;
}
/* next phase is measurement */
_collect_phase = false;
/* perform collection */
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 */
ScheduleDelayed(_interval);
}
void SF45LaserSerial::print_info()
@ -377,9 +307,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_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id)
{
// SF45 protocol
// Start byte is 0xAA and is the start of packet
// Payload length sanity check (0-1023) bytes
@ -388,159 +317,175 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf)
// ID byte precedes the data in the payload
// CRC comprised of 16-bit checksum (not included in checksum calc.)
uint16_t recv_crc = 0;
bool restart_flag = false;
int ret;
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) {
case 0: {
if (input_buf[0] == 0xAA) {
// start of frame is valid, continue
_sop_valid = true;
_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
_parsed_state = 1;
break;
if (ret > 0) {
_last_received_time = hrt_absolute_time();
_linebuf_size += ret;
}
} else {
_sop_valid = false;
_crc_valid = false;
_parsed_state = 0;
restart_flag = true;
_calc_crc = 0;
PX4_INFO("INFO: start of packet not valid");
break;
} // end else
} // end case 0
// Not enough data to parse a complete packet. Gather more data in the next cycle.
if (_linebuf_size < payload_length) {
return;
}
case 1: {
rx_field.flags_lo = input_buf[1];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
_parsed_state = 2;
break;
}
int index = 0;
case 2: {
rx_field.flags_hi = input_buf[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);
while (index <= _linebuf_size - payload_length && _crc_valid == false) {
bool restart_flag = false;
// Check payload length against known max value
if (rx_field.data_len > 17) {
PX4_INFO("INFO: payload length invalid, restarting data request");
_parsed_state = 0;
restart_flag = true;
_calc_crc = 0;
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;
while (restart_flag != true) {
switch (_parsed_state) {
case SF45_PARSED_STATE::START: {
if (_linebuf[index] == 0xAA) {
// start of frame is valid, continue
_sop_valid = true;
_calc_crc = sf45_format_crc(_calc_crc, _start_of_frame);
_parsed_state = SF45_PARSED_STATE::FLG_LOW;
break;
} else {
_sensor_ready = false;
_sop_valid = false;
_crc_valid = false;
_parsed_state = SF45_PARSED_STATE::START;
restart_flag = true;
_calc_crc = 0;
PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
break;
}
}
case SF45_PARSED_STATE::FLG_LOW: {
rx_field.flags_lo = _linebuf[index + 1];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo);
_parsed_state = SF45_PARSED_STATE::FLG_HIGH;
break;
}
case SF45_PARSED_STATE::FLG_HIGH: {
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 = SF45_PARSED_STATE::START;
restart_flag = true;
_calc_crc = 0;
PX4_DEBUG("Payload length error: %d", _sensor_state);
break;
} else {
_parsed_state = SF45_PARSED_STATE::ID;
break;
}
}
case SF45_PARSED_STATE::ID: {
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 = SF45_PARSED_STATE::DATA;
break;
}
_calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id);
_parsed_state = 4;
break;
// Ignore message ID's that aren't searched
else {
_parsed_state = SF45_PARSED_STATE::START;
_calc_crc = 0;
restart_flag = true;
PX4_DEBUG("Non needed message ID: %d", _sensor_state);
break;
}
}
// Ignore message ID's that aren't defined
else {
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
break;
case SF45_PARSED_STATE::DATA: {
// 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] = _linebuf[index + i];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
_data_bytes_recv = _data_bytes_recv + 1;
// Data
case 4: {
// 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];
_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
_data_bytes_recv = _data_bytes_recv + 1;
else {
} // end for
} //end if
_parsed_state = SF45_PARSED_STATE::CRC_LOW;
_data_bytes_recv = 0;
_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
}
else {
_parsed_state = 5;
_parsed_state = SF45_PARSED_STATE::CRC_LOW;
_data_bytes_recv = 0;
_calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv);
break;
}
_parsed_state = 5;
_data_bytes_recv = 0;
break;
}
case SF45_PARSED_STATE::CRC_LOW: {
rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
_parsed_state = SF45_PARSED_STATE::CRC_HIGH;
break;
}
// CRC low byte
case 5: {
rx_field.crc[0] = input_buf[3 + rx_field.data_len];
_parsed_state = 6;
break;
}
case SF45_PARSED_STATE::CRC_HIGH: {
rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len];
uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
// CRC high byte
case 6: {
rx_field.crc[1] = input_buf[4 + rx_field.data_len];
recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0];
// Check the received crc bytes from the sf45 against our own CRC calcuation
// If it matches, we can check if sensor ready
// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
if (recv_crc == _calc_crc) {
_crc_valid = true;
// Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM
if (_sensor_ready) {
_init_complete = true;
// Check the received crc bytes from the sf45 against our own CRC calcuation
// If it matches, we can check if sensor ready
// Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete
if (recv_crc == _calc_crc) {
_crc_valid = true;
_parsed_state = SF45_PARSED_STATE::START;
_calc_crc = 0;
restart_flag = true;
break;
} else {
_init_complete = false;
_crc_valid = false;
_parsed_state = SF45_PARSED_STATE::START;
_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 {
PX4_INFO("INFO: CRC mismatch");
_crc_valid = false;
_init_complete = false;
_parsed_state = 0;
_calc_crc = 0;
restart_flag = true;
break;
}
}
} // end switch
} //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;
uint8_t packet_buff[SF45_MAX_PAYLOAD];
@ -577,7 +522,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
if (msg_id == SF_DISTANCE_OUTPUT) {
uint8_t data_convert = data[0] & 0x00FF;
// 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
crc_val = sf45_format_crc(crc_val, data_convert);
data_inc = data_inc + 1;
@ -615,12 +560,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
// Add data bytes to crc add function
crc_val = sf45_format_crc(crc_val, data[0]);
data_inc = data_inc + 1;
}
else {
// Product Name
PX4_INFO("INFO: Product name");
PX4_DEBUG("DEBUG: Product name");
}
uint8_t crc_lo = crc_val & 0xFF;
@ -635,25 +579,23 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d
// DEBUG
for (uint8_t i = 0; i < packet_len; ++i) {
PX4_INFO("INFO: Send byte: %d", packet_buff[i]);
PX4_DEBUG("DEBUG: Send byte: %d", packet_buff[i]);
}
ret = ::write(_fd, packet_buff, packet_len);
if (ret != packet_len) {
perf_count(_comms_errors);
PX4_DEBUG("write fail %d", ret);
//return ret;
PX4_ERR("serial write fail %d", ret);
// Flush data written, not transmitted
tcflush(_fd, TCOFLUSH);
}
}
void SF45LaserSerial::sf45_process_replies(float *distance_m)
{
switch (rx_field.msg_id) {
case SF_DISTANCE_DATA_CM: {
uint16_t obstacle_dist_cm = 0;
const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
int16_t scaled_yaw = 0;
@ -661,63 +603,66 @@ 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
if (raw_yaw > 32000) {
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
if (_orient_cfg == 1) {
if (_orient_cfg == ROTATION_DOWNWARD_FACING) {
raw_yaw = raw_yaw * -1;
}
// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
switch (_yaw_cfg) {
case 0:
case ROTATION_FORWARD_FACING:
break;
case 1:
if (raw_yaw > 180) {
raw_yaw = raw_yaw - 180;
case ROTATION_BACKWARD_FACING:
if (scaled_yaw > 180) {
scaled_yaw = scaled_yaw - 180;
} else {
raw_yaw = raw_yaw + 180; // rotation facing aft
scaled_yaw = scaled_yaw + 180; // rotation facing aft
}
break;
case 2:
raw_yaw = raw_yaw + 90; // rotation facing right
case ROTATION_RIGHT_FACING:
scaled_yaw = scaled_yaw + 90; // rotation facing right
break;
case 3:
raw_yaw = raw_yaw - 90; // rotation facing left
case ROTATION_LEFT_FACING:
scaled_yaw = scaled_yaw - 90; // rotation facing left
break;
default:
break;
}
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
// Convert to meters for rangefinder update
// Convert to meters for the debug message
*distance_m = raw_distance * SF45_SCALE_FACTOR;
obstacle_dist_cm = (uint16_t)raw_distance;
_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
uint8_t current_bin = sf45_convert_angle(scaled_yaw);
// If we have moved to a new bin
if (current_bin != _previous_bin) {
PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
(double)*distance_m);
// update the current bin to the distance sensor reading
// readings in cm
_obstacle_map_msg.distances[current_bin] = obstacle_dist_cm;
_obstacle_map_msg.timestamp = hrt_absolute_time();
if (_current_bin_dist > _obstacle_distance.max_distance) {
_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
}
hrt_abstime now = hrt_absolute_time();
_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
_publish_obstacle_msg(now);
// reset the values for the next measurement
_current_bin_dist = UINT16_MAX;
_previous_bin = current_bin;
}
_previous_bin = current_bin;
_obstacle_distance_pub.publish(_obstacle_map_msg);
break;
}
@ -726,13 +671,67 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
break;
}
}
void SF45LaserSerial::_publish_obstacle_msg(hrt_abstime now)
{
// This whole logic is in place because CollisionPrevention, just reads in the last published message on the topic, and not every message,
// This can result in messages being misssed if the sensor is publishing at a faster rate than the CollisionPrevention module is reading.
for (uint8_t i = 0; i < BIN_COUNT; i++) {
if (now - _data_timestamps[i] > SF45_MEAS_TIMEOUT) {
// If the data is older than SF45_MEAS_TIMEOUT, we discard the value (UINT16_MAX means no valid data)
_obstacle_distance.distances[i] = UINT16_MAX;
}
}
_obstacle_distance.timestamp = now;
_obstacle_distance_pub.publish(_obstacle_distance);
}
void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement,
hrt_abstime now)
{
// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
// in this case we assume the measurement to be valid for all bins between the previous and the current bin.
uint8_t start;
uint8_t end;
if (abs(current_bin - previous_bin) > BIN_COUNT / 4) {
// wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins
// THis is simplyfied as we are not considering the scaning direction
start = math::max(previous_bin, current_bin);
end = math::min(previous_bin, current_bin);
} else if (previous_bin < current_bin) { // Scanning clockwise
start = previous_bin + 1;
end = current_bin;
} else { // scanning counter-clockwise
start = current_bin;
end = previous_bin - 1;
}
if (start <= end) {
for (uint8_t i = start; i <= end; i++) {
_obstacle_distance.distances[i] = measurement;
_data_timestamps[i] = now;
}
} else { // wrap-around case
for (uint8_t i = start; i < BIN_COUNT; i++) {
_obstacle_distance.distances[i] = measurement;
_data_timestamps[i] = now;
}
for (uint8_t i = 0; i <= end; i++) {
_obstacle_distance.distances[i] = measurement;
_data_timestamps[i] = now;
}
}
}
uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{
uint8_t mapped_sector = 0;
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
return mapped_sector;
}

113
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
View File

@ -50,72 +50,101 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/distance_sensor.h>
#include "sf45_commands.h"
enum SF_SERIAL_STATE {
STATE_UNINIT = 0,
STATE_ACK_PRODUCT_NAME = 1,
STATE_ACK_UPDATE_RATE = 2,
STATE_ACK_DISTANCE_OUTPUT = 3,
STATE_SEND_STREAM = 4,
};
enum SF45_PARSED_STATE {
START = 0,
FLG_LOW,
FLG_HIGH,
ID,
DATA,
CRC_LOW,
CRC_HIGH
};
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
ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_LEFT_FACING = 6, // MAV_SENSOR_ROTATION_YAW_270
ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90
ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270
};
using namespace time_literals;
class SF45LaserSerial : public px4::ScheduledWorkItem
{
public:
SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
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);
protected:
obstacle_distance_s _obstacle_map_msg{};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */
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, int32_t *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_distance{};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */
static constexpr uint8_t BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(
obstacle_distance_s::distances[0]);
static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms};
static constexpr float SF45_SCALE_FACTOR = 0.01f;
void start();
void stop();
void Run() override;
int measure();
int collect();
bool _crc_valid{false};
PX4Rangefinder _px4_rangefinder;
bool _crc_valid{false};
void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
void _publish_obstacle_msg(hrt_abstime now);
uint64_t _data_timestamps[BIN_COUNT];
char _port[20] {};
int _interval{10000};
int _interval{2000};
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] {};
int _linebuf_size{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{2};
int32_t _yaw_cfg{0};
int32_t _orient_cfg{0};
int32_t _collision_constraint{0};
uint16_t _previous_bin{0};
bool _is_sf45{false};
SF_SERIAL_STATE _sensor_state{STATE_UNINIT};
int _baud_rate{0};
int32_t _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int32_t _stream_data{0};
int32_t _update_rate{0};
int32_t _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};
int32_t _yaw_cfg{0};
int32_t _orient_cfg{0};
uint8_t _previous_bin{0};
uint16_t _current_bin_dist{UINT16_MAX};
// end of SF45/B data members
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
hrt_abstime _last_received_time{0};
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
};

15
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp
View File

@ -41,7 +41,7 @@ namespace lightware_sf45
SF45LaserSerial *g_dev{nullptr};
static int start(const char *port, uint8_t rotation)
static int start(const char *port)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation)
}
/* create the driver */
g_dev = new SF45LaserSerial(port, rotation);
g_dev = new SF45LaserSerial(port);
if (g_dev == nullptr) {
return -1;
@ -102,7 +102,6 @@ static int usage()
Serial bus driver for the Lightware SF45/b Laser rangefinder.
Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
### Examples
@ -116,7 +115,6 @@ $ lightware_sf45_serial stop
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
return PX4_OK;
}
@ -125,18 +123,13 @@ $ lightware_sf45_serial stop
extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
const char *device_path = nullptr;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
@ -153,7 +146,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
}
if (!strcmp(argv[myoptind], "start")) {
return lightware_sf45::start(device_path, rotation);
return lightware_sf45::start(device_path);
} else if (!strcmp(argv[myoptind], "stop")) {
return lightware_sf45::stop();

15
src/drivers/distance_sensor/lightware_sf45_serial/module.yaml
View File

@ -1,10 +1,9 @@
module_name: Lightware SF45 Rangefinder (serial)
serial_config:
- command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV}
- command: lightware_sf45_serial start -d ${SERIAL_DEV}
port_config_param:
name: SENS_EN_SF45_CFG
group: Sensors
default: TEL2
num_instances: 1
supports_networking: false
@ -32,7 +31,7 @@ parameters:
12: 5000hz
reboot_required: true
num_instances: 1
default: 1
default: 5
SF45_ORIENT_CFG:
description:
@ -41,11 +40,11 @@ parameters:
The SF45 mounted facing upward or downward on the frame
type: enum
values:
0: Rotation upward
1: Rotation downward
24: Rotation upward
25: Rotation downward
reboot_required: true
num_instances: 1
default: 0
default: 24
SF45_YAW_CFG:
description:
@ -55,9 +54,9 @@ parameters:
type: enum
values:
0: Rotation forward
1: Rotation backward
2: Rotation right
3: Rotation left
4: Rotation backward
6: Rotation left
reboot_required: true
num_instances: 1
default: 0

2
src/modules/logger/logged_topics.cpp
View File

@ -376,7 +376,9 @@ void LoggedTopics::add_sensor_comparison_topics()
void LoggedTopics::add_vision_and_avoidance_topics()
{
add_topic("collision_constraints");
add_topic_multi("distance_sensor");
add_topic("obstacle_distance_fused");
add_topic("obstacle_distance");
add_topic("vehicle_mocap_odometry", 30);
add_topic("vehicle_trajectory_waypoint", 200);
add_topic("vehicle_trajectory_waypoint_desired", 200);