Skip to content

Commit

Permalink
SENS: RNG: SF45 changed data processing and publication design, moved…
Browse files Browse the repository at this point in the history 
… to a publishing per sector design.

other minor improvements
  • Loading branch information
@Claudio-Chies
Claudio-Chies committed Nov 29, 2024
1 parent ce3fcd5 commit fd0e550
Show file tree
Hide file tree
Showing 5 changed files with 120 additions and 79 deletions.
94 changes: 63 additions & 31 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View file
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,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;
}

}

Expand All @@ -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;
Expand Down Expand Up @@ -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();



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

} else {

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

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

Expand Down Expand Up @@ -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;
Expand All @@ -700,18 +693,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 +714,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);
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;}

_previous_bin = current_bin;
} else { // wrap-around case
for (uint8_t i = start; i < BIN_COUNT; i++) {_obstacle_map_msg.distances[i] = UINT16_MAX;}

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

_current_bin_dist = UINT16_MAX;
_previous_bin = current_bin;
}

break;
}
Expand Down
75 changes: 44 additions & 31 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -77,55 +87,58 @@ 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 */
static constexpr int BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]);

void start();
void stop();
void Run() override;
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 changes: 5 additions & 11 deletions src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp
View file
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
Loading

0 comments on commit fd0e550

Please sign in to comment.