Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

SENS: SF45: Improved Datahandling and fixes #23918

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+58 −56
Open

SENS: SF45: Improved Datahandling and fixes #23918

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 25 additions & 26 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"))
{
Expand All @@ -69,15 +68,19 @@ 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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be non-zero. There has to be some offset since the obstacle distance array element 0 starts and moves clockwise, however with an increment of 10 degrees there won't be coverage immediately to the left of the actual obstacle distance data

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's a bit of a question on the definition of the angle offset.
as the original algorithm was the Vector Field Histogram, I assumed that the angle was also defined the same way, meaning the corresponding angle is in the centre of the bin.

Which would also make sense as on 1D distance sensor pointed to the front with a fov of zero, gets only mapped to the bin with index zero.
image
I mean, we can agree on a definition here, and then also properly document it in the message.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did a quick test where I set the fov of a 1D sensor pointing to the front to be 5 deg.
image
and then the adjacent bins are empty and only the bin with index 0 is populated. Which would support the statement above.

Copy link
Contributor

@dirksavage88 dirksavage88 Nov 13, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe a better test of this is a 1D sensor 20 degree field of view sensor forward facing. If 20 degrees is split down the middle would the bins occupied be 0, 1 and 71, 72? (assuming 5 degree increment).

I guess this could be changed and defined to include that wrapping effect: the angle offset is super non-intuitive to most users, specifically to those implementing new drivers where there is sparse info on the sign of the angle and what it means.

It doesn't make a huge difference on the rotating lidars since the degree resolution is small, but would make a difference on 1D lidars with a significant field of view.

const uint32_t internal_bins = sizeof(_obstacle_map_msg.distances) / sizeof(_obstacle_map_msg.distances[0]);

for (uint32_t i = 0 ; i < internal_bins; i++) {
_obstacle_map_msg.distances[i] = UINT16_MAX;
}

}

Expand All @@ -91,16 +94,12 @@ 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) */
Copy link
Contributor

@dirksavage88 dirksavage88 Nov 15, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can remove the "/* SF45/B (50M) */" comment now that the rangefinder min/max values are no longer relevant

_px4_rangefinder.set_min_distance(0.2f);
_px4_rangefinder.set_max_distance(50.0f);
_interval = 10000;

start();

return PX4_OK;
Expand Down Expand Up @@ -161,7 +160,6 @@ int SF45LaserSerial::collect()
float distance_m = -1.0f;

/* read from the sensor (uart buffer) */
const hrt_abstime timestamp_sample = hrt_absolute_time();



Expand Down Expand Up @@ -214,7 +212,6 @@ int SF45LaserSerial::collect()
// Stream data from sensor

} else {

ret = ::read(_fd, &readbuf[0], 10);

if (ret < 0) {
Expand Down Expand Up @@ -262,7 +259,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);

Expand Down Expand Up @@ -687,8 +684,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;
Expand All @@ -700,18 +695,18 @@ 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;
}

// 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:
case ROTATION_BACKWARD_FACING:
if (scaled_yaw > 180) {
scaled_yaw = scaled_yaw - 180;

Expand All @@ -721,37 +716,41 @@ 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;

default:
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) {

if (_current_bin_dist > _obstacle_map_msg.max_distance) {
_current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition
}

// update the current bin to the distance sensor reading
// readings in cm
_obstacle_map_msg.distances[current_bin] = obstacle_dist_cm;
_obstacle_map_msg.distances[current_bin] = _current_bin_dist;
_obstacle_map_msg.timestamp = hrt_absolute_time();

_current_bin_dist = UINT16_MAX;
_previous_bin = current_bin;
}

_previous_bin = current_bin;

_obstacle_distance_pub.publish(_obstacle_map_msg);

break;
Expand All @@ -768,7 +767,7 @@ 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);
mapped_sector = floor(adjusted_yaw / _obstacle_map_msg.increment);

return mapped_sector;
}
Expand Down
33 changes: 20 additions & 13 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,10 +63,19 @@ 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
};
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();
Expand All @@ -77,27 +86,25 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
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 */

void start();
void stop();
void Run() override;
int measure();
int collect();
bool _crc_valid{false};
PX4Rangefinder _px4_rangefinder;

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
Expand All @@ -118,14 +125,14 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
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};
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 changes: 5 additions & 11 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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;
Expand Down Expand Up @@ -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

Expand All @@ -116,7 +116,6 @@ Stop driver
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;
}
Expand All @@ -125,18 +124,13 @@ Stop driver

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;
Expand All @@ -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();
Expand Down
12 changes: 6 additions & 6 deletions src/drivers/distance_sensor/lightware_sf45_serial/module.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I never actually had the chance to test these on the bench. The non-standard Yaw cfg as well.

reboot_required: true
num_instances: 1
default: 0
default: 24

SF45_YAW_CFG:
description:
Expand All @@ -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 changes: 2 additions & 0 deletions src/modules/logger/logged_topics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading