From 1718b37fe4ba381d08fd8c1f552f8d685e2e356d Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 8 Nov 2024 09:11:57 +0100 Subject: [PATCH] SENS: RNG: SF45 changed data processing and publication design, moved to a publishing per sector design. other minor improvements --- .../lightware_sf45_serial.cpp | 96 ++++++++++++------- .../lightware_sf45_serial.hpp | 75 +++++++++------ .../lightware_sf45_serial_main.cpp | 16 +--- .../lightware_sf45_serial/module.yaml | 12 +-- src/modules/logger/logged_topics.cpp | 2 + 5 files changed, 121 insertions(+), 80 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index 22ae7c8207..c5e0c750b2 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -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; } diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index 89e7367192..c2d0961f01 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -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_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ private: + obstacle_distance_s _obstacle_map_msg{}; + uORB::Publication _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; }; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp index f05417e053..3446ad1a35 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp @@ -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(); diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml index 6356411fbe..4dbdf25c90 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e7..d1e67a5cd7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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);