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 changed data processing and publication design, moved to a publishing per sector design.

Browse Source 
other minor improvements
This commit is contained in:
Claudio Chies 2024-11-08 09:11:57 +01:00 committed by Silvan Fuhrer
parent a751e900f5
commit 1718b37fe4
5 changed files with 121 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,9 +47,8 @@
#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"))
{
@ -69,15 +68,18 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
}
_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.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
_obstacle_map_msg.increment = 5;
_obstacle_map_msg.angle_offset = 2.5;
_obstacle_map_msg.min_distance = UINT16_MAX;
_obstacle_map_msg.min_distance = 20;
_obstacle_map_msg.max_distance = 5000;
_obstacle_map_msg.angle_offset = 0;
for (uint32_t i = 0 ; i < BIN_COUNT; i++) {
_obstacle_map_msg.distances[i] = UINT16_MAX;
}
}
@ -91,16 +93,11 @@ 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;
@ -161,7 +158,6 @@ int SF45LaserSerial::collect()
float distance_m = -1.0f;
/* read from the sensor (uart buffer) */
const hrt_abstime timestamp_sample = hrt_absolute_time();
@ -214,7 +210,6 @@ int SF45LaserSerial::collect()
// Stream data from sensor
} else {
ret = ::read(_fd, &readbuf[0], 10);
if (ret < 0) {
@ -262,7 +257,7 @@ int SF45LaserSerial::collect()
}
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);
@ -687,8 +682,6 @@ 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;
@ -700,7 +693,7 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
}
// 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;
}
@ -708,10 +701,10 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
switch (_yaw_cfg) {
case 0:
case ROTATION_FORWARD_FACING:
break;
case 1:
case ROTATION_BACKWARD_FACING:
if (scaled_yaw > 180) {
scaled_yaw = scaled_yaw - 180;
@ -721,11 +714,11 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
break;
case 2:
case ROTATION_RIGHT_FACING:
scaled_yaw = scaled_yaw + 90; // rotation facing right
break;
case 3:
case ROTATION_LEFT_FACING:
scaled_yaw = scaled_yaw - 90; // rotation facing left
break;
@ -733,27 +726,66 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
break;
}
// 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);
if (_current_bin_dist > _obstacle_map_msg.max_distance) {
_current_bin_dist = _obstacle_map_msg.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;
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;}
}
// 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();
_obstacle_distance_pub.publish(_obstacle_map_msg);
// 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;
}
_previous_bin = current_bin;
_obstacle_distance_pub.publish(_obstacle_map_msg);
break;
}

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

@ -63,10 +63,20 @@ 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
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();
@ -77,11 +87,11 @@ public:
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 */
private:
obstacle_distance_s _obstacle_map_msg{};
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]);
void start();
void stop();
@ -89,43 +99,46 @@ private:
int measure();
int collect();
bool _crc_valid{false};
PX4Rangefinder _px4_rangefinder;
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};
int _linebuf[256] {};
unsigned _linebuf_index{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};
int32_t _collision_constraint{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};
uint8_t _previous_bin{0};
uint16_t _current_bin_dist{UINT16_MAX};
// end of SF45/B data members
unsigned _consecutive_fail_count;
unsigned _consecutive_fail_count;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
};

16
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,7 @@ 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
Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
### Examples
@ -116,7 +116,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 +124,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 +147,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();

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

@ -1,6 +1,6 @@
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
@ -41,11 +41,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 +55,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

@ -323,7 +323,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);