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

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

Browse Source
This commit is contained in:
Claudio Chies 2024-12-04 15:08:25 +01:00 committed by Silvan Fuhrer
parent f34b22907c
commit 051ced0fee
3 changed files with 111 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,7 +44,7 @@
using namespace time_literals;
/* Configuration Constants */
#define SF45_SCALE_FACTOR 0.01f
SF45LaserSerial::SF45LaserSerial(const char *port) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
@ -67,15 +67,15 @@ SF45LaserSerial::SF45LaserSerial(const char *port) :
}
// populate obstacle map members
_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.increment = 5;
_obstacle_map_msg.min_distance = 20;
_obstacle_map_msg.max_distance = 5000;
_obstacle_map_msg.angle_offset = 0;
_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_map_msg.distances[i] = UINT16_MAX;
_obstacle_distance.distances[i] = UINT16_MAX;
}
}
@ -346,63 +346,63 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
while (restart_flag != true) {
switch (_parsed_state) {
case 0: {
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 = 1;
_parsed_state = SF45_PARSED_STATE::FLG_LOW;
break;
} else {
_sop_valid = false;
_crc_valid = false;
_parsed_state = 0;
_parsed_state = SF45_PARSED_STATE::START;
restart_flag = true;
_calc_crc = 0;
PX4_DEBUG("Start of packet not valid: %d", _sensor_state);
break;
} // end else
} // end case 0
}
}
case 1: {
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 = 2;
_parsed_state = SF45_PARSED_STATE::FLG_HIGH;
break;
}
case 2: {
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 = 0;
_parsed_state = SF45_PARSED_STATE::START;
restart_flag = true;
_calc_crc = 0;
PX4_DEBUG("Payload length error: %d", _sensor_state);
break;
} else {
_parsed_state = 3;
_parsed_state = SF45_PARSED_STATE::ID;
break;
}
}
case 3: {
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 = 4;
_parsed_state = SF45_PARSED_STATE::DATA;
break;
}
// Ignore message ID's that aren't searched
else {
_parsed_state = 0;
_parsed_state = SF45_PARSED_STATE::START;
_calc_crc = 0;
restart_flag = true;
PX4_DEBUG("Non needed message ID: %d", _sensor_state);
@ -410,8 +410,7 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
}
}
// Data
case 4: {
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) {
@ -420,30 +419,28 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
_calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]);
_data_bytes_recv = _data_bytes_recv + 1;
} // end for
} //end if
}
}
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);
}
_parsed_state = 5;
_parsed_state = SF45_PARSED_STATE::CRC_LOW;
_data_bytes_recv = 0;
break;
}
// CRC low byte
case 5: {
case SF45_PARSED_STATE::CRC_LOW: {
rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len];
_parsed_state = 6;
_parsed_state = SF45_PARSED_STATE::CRC_HIGH;
break;
}
// CRC high byte
case 6: {
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];
@ -452,7 +449,7 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
// 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 = 0;
_parsed_state = SF45_PARSED_STATE::START;
_calc_crc = 0;
restart_flag = true;
break;
@ -460,7 +457,7 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
} else {
_crc_valid = false;
_parsed_state = 0;
_parsed_state = SF45_PARSED_STATE::START;
_calc_crc = 0;
restart_flag = true;
perf_count(_comms_errors);
@ -468,8 +465,8 @@ void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, cons
break;
}
}
} // end switch
} //end while
}
}
index++;
}
@ -652,52 +649,16 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
(double)*distance_m);
if (_current_bin_dist > _obstacle_map_msg.max_distance) {
_current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition
if (_current_bin_dist > _obstacle_distance.max_distance) {
_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
}
// 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. win
uint8_t start;
uint8_t end;
hrt_abstime now = hrt_absolute_time();
_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
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
// TODO: differentiate direction of wrap-around, currently it overwrites a previous measurement.
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_map_msg.distances[i] = _current_bin_dist;}
} else { // wrap-around case
for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;}
for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = _current_bin_dist;}
}
_obstacle_map_msg.timestamp = hrt_absolute_time();
_obstacle_distance_pub.publish(_obstacle_map_msg);
_publish_obstacle_msg(now);
// reset the values for the next measurement
if (start <= end) {
for (uint8_t i = start; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;}
} else { // wrap-around case
for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;}
for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;}
}
_current_bin_dist = UINT16_MAX;
_previous_bin = current_bin;
}
@ -710,12 +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;
}

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

@ -50,7 +50,6 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/distance_sensor.h>
#include "sf45_commands.h"
@ -62,6 +61,15 @@ enum SF_SERIAL_STATE {
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
@ -71,6 +79,7 @@ enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTA
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
{
@ -88,9 +97,12 @@ public:
float sf45_wrap_360(float f);
private:
obstacle_distance_s _obstacle_map_msg{};
obstacle_distance_s _obstacle_distance{};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */
static constexpr int BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]);
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();
@ -99,6 +111,7 @@ private:
int collect();
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];

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

@ -102,7 +102,6 @@ static int usage()
Serial bus driver for the Lightware SF45/b Laser rangefinder.
Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
### Examples