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 2 commits into
base: main
Choose a base branch
from
+172 −127
Open

SENS: SF45: Improved Datahandling and fixes #23918

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fd0e550
SENS: RNG: SF45 changed data processing and publication design, moved…
Claudio-Chies Nov 8, 2024
457e67c
SENS: RNG: SF45: Fixed sf45 parser, added general checks to avoid pot…
alexcekay Nov 19, 2024
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
179 changes: 108 additions & 71 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 @@ -32,7 +32,6 @@
****************************************************************************/

#include "lightware_sf45_serial.hpp"
#include "sf45_commands.h"

#include <inttypes.h>
#include <fcntl.h>
Expand All @@ -44,12 +43,10 @@
#include <matrix/matrix/math.hpp>

/* 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"))
{
Expand All @@ -69,15 +66,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;
Claudio-Chies marked this conversation as resolved.
Show resolved Hide resolved

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

}

Expand All @@ -91,24 +91,18 @@ 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
_stream_data = 5; // enable constant streaming
Expand Down Expand Up @@ -153,21 +147,13 @@ int SF45LaserSerial::collect()
{
perf_begin(_sample_perf);

/* clear buffer if last read was too long ago */
int ret;
/* the buffer for read chars is buflen minus null termination */
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_SEND_PRODUCT_NAME) {

ret = ::read(_fd, &readbuf[0], 22);
const int payload_length = 22;
ret = ::read(_fd, &_linebuf[0], payload_length);

if (ret < 0) {
PX4_ERR("ERROR (ack from sending product name cmd): %d", ret);
Expand All @@ -176,12 +162,13 @@ int SF45LaserSerial::collect()
return ret;
}

sf45_request_handle(ret, readbuf);
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);

} else if (_sensor_state == STATE_SEND_UPDATE_RATE) {

ret = ::read(_fd, &readbuf[0], 7);
const int payload_length = 7;
ret = ::read(_fd, &_linebuf[0], payload_length);

if (ret < 0) {
PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret);
Expand All @@ -190,14 +177,15 @@ int SF45LaserSerial::collect()
return ret;
}

if (readbuf[3] == SF_UPDATE_RATE) {
sf45_request_handle(ret, readbuf);
if (ret == payload_length && _linebuf[3] == SF_UPDATE_RATE) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}

} else if (_sensor_state == STATE_SEND_DISTANCE_DATA) {

ret = ::read(_fd, &readbuf[0], 8);
const int payload_length = 8;
ret = ::read(_fd, &_linebuf[0], payload_length);

if (ret < 0) {
PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret);
Expand All @@ -206,47 +194,58 @@ int SF45LaserSerial::collect()
return ret;
}

if (readbuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(ret, readbuf);
if (ret == payload_length && _linebuf[3] == SF_DISTANCE_OUTPUT) {
sf45_request_handle(_linebuf);
ScheduleDelayed(_interval * 3);
}

// Stream data from sensor

} else {
// Stream data from sensor
const int payload_length = 10;

ret = ::read(_fd, &readbuf[0], 10);
size_t max_read = sizeof(_linebuf) - _linebuf_size;
ret = ::read(_fd, &_linebuf[_linebuf_size], max_read);
_linebuf_size += ret;

if (ret < 0) {
PX4_ERR("ERROR (ack from streaming distance data): %d", ret);
_linebuf_size = 0;
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}

uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2);
// Not enough data to parse a complete packet. Gather more data in the next cycle.
if (_linebuf_size < payload_length) {
return -EAGAIN;
}

// Process the incoming distance data
if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
int index = _linebuf_size - payload_length;

for (uint8_t i = 0; i < ret; ++i) {
sf45_request_handle(ret, readbuf);
}
while (index >= 0) {
if (_linebuf[index] == 0xAA) {
uint8_t flags_payload = (_linebuf[index + 1] >> 6) | (_linebuf[index + 2] << 2);

if (_init_complete) {
sf45_process_replies(&distance_m);
} // end if
// Process the incoming distance data
if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) {
sf45_request_handle(&_linebuf[index]);

} else {
if (_init_complete && _crc_valid) {
sf45_process_replies(&distance_m);
_linebuf_size = 0;
break;
}
}
}

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

if (ret < 0) {
PX4_ERR("ERROR (unknown sensor data): %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
// 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);
}
}

Expand All @@ -261,8 +260,7 @@ int SF45LaserSerial::collect()
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);
PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));

perf_end(_sample_perf);

Expand All @@ -274,6 +272,9 @@ void SF45LaserSerial::start()
/* reset the report ring and state machine */
_collect_phase = false;

/* reset the UART receive buffer size */
_linebuf_size = 0;

/* schedule a cycle to start things */
ScheduleNow();
}
Expand Down Expand Up @@ -399,9 +400,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_request_handle(uint8_t *input_buf)
{

// SF45 protocol
// Start byte is 0xAA and is the start of packet
// Payload length sanity check (0-1023) bytes
Expand Down Expand Up @@ -687,8 +687,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 +698,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,38 +719,77 @@ 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) {
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);
Claudio-Chies marked this conversation as resolved.
Show resolved Hide resolved
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;}
Copy link
Contributor

@dirksavage88 dirksavage88 Nov 22, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

Looks like you covered all the edge cases here. Eventually setting the cycle delay can be done in PX4, but getting it working with default settings is probably best. Lightware studio should really be used once to set the scan angle and enable scanning at first, and then not touched again unless debugging.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes, but usually you wouldn't change the cycle delay after the initial setup, no?
For the UAS use case, the lowest cycle delay is probably the most suited? Or I don't know a need for a slower one from the top of my head.


} 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;}

_previous_bin = current_bin;
for (uint8_t i = 0; i <= end; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;}
}

_obstacle_distance_pub.publish(_obstacle_map_msg);
_current_bin_dist = UINT16_MAX;
_previous_bin = current_bin;
}

break;
}
Expand Down
Loading
Loading