From 91d110460e734c37d76ce6390b27ad975b744106 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Thu, 21 Nov 2024 13:28:38 +0100 Subject: [PATCH] initial --- .../lightware_sf45_serial.cpp | 154 ++++++++---------- .../lightware_sf45_serial.hpp | 80 ++++----- .../lightware_sf45_serial_main.cpp | 16 +- .../lightware_sf45_serial/module.yaml | 10 +- .../drivers/rangefinder/PX4Rangefinder.cpp | 29 ++++ .../drivers/rangefinder/PX4Rangefinder.hpp | 1 + src/modules/logger/logged_topics.cpp | 2 + 7 files changed, 148 insertions(+), 144 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..d4adda6786 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 @@ -32,7 +32,6 @@ ****************************************************************************/ #include "lightware_sf45_serial.hpp" -#include "sf45_commands.h" #include #include @@ -44,12 +43,12 @@ #include /* Configuration Constants */ -#define SF45_MAX_PAYLOAD 256 #define SF45_SCALE_FACTOR 0.01f +using namespace matrix; -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), + _px4_rangefinder(0, distance_sensor_s::ROTATION_CUSTOM), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { @@ -71,13 +70,7 @@ 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.increment = 5; - _obstacle_map_msg.angle_offset = 2.5; - _obstacle_map_msg.min_distance = UINT16_MAX; - _obstacle_map_msg.max_distance = 5000; + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); } @@ -97,6 +90,7 @@ int SF45LaserSerial::init() param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); /* SF45/B (50M) */ + _px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_CUSTOM); _px4_rangefinder.set_min_distance(0.2f); _px4_rangefinder.set_max_distance(50.0f); _interval = 10000; @@ -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); @@ -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); @@ -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); @@ -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); + + // Process the incoming distance data + if (_linebuf[index + 3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { + sf45_request_handle(&_linebuf[index]); + + if (_init_complete && _crc_valid) { + sf45_process_replies(distance_m); + _linebuf_size = 0; + break; + } + } } - if (_init_complete) { - sf45_process_replies(&distance_m); - } // end if + index--; + } - } else { - - ret = ::read(_fd, &readbuf[0], 10); - - 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); } } @@ -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); @@ -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(); } @@ -399,7 +400,7 @@ 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 @@ -683,24 +684,17 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d } } -void SF45LaserSerial::sf45_process_replies(float *distance_m) +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; - // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float - if (raw_yaw > 32000) { - raw_yaw = raw_yaw - 65535; - - } - // 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 +702,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 +715,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; @@ -734,26 +728,16 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } // Convert to meters for rangefinder update - *distance_m = raw_distance * SF45_SCALE_FACTOR; - obstacle_dist_cm = (uint16_t)raw_distance; + distance_m = raw_distance * SF45_SCALE_FACTOR; - uint8_t current_bin = sf45_convert_angle(scaled_yaw); - - // If we have moved to a new bin - - if (current_bin != _previous_bin) { - - // 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(); + Quatf quaternion(Eulerf{0, 0, sf45_wrap_360(scaled_yaw)*M_DEG_TO_RAD_F}); + float q[4]; + for (int i = 0; i < 4; i++) { + q[i] = quaternion(i); } - _previous_bin = current_bin; - - _obstacle_distance_pub.publish(_obstacle_map_msg); - + _px4_rangefinder.update(hrt_absolute_time(), distance_m, -1, q); break; } @@ -763,16 +747,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } } -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); - - return mapped_sector; -} - float SF45LaserSerial::sf45_wrap_360(float f) { return matrix::wrap(f, 0.f, 360.f); 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..c9436969dc 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,69 +63,73 @@ 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(); + int init(); void print_info(); - void sf45_request_handle(int val, uint8_t *value); - void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); - uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); - 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 */ + void sf45_request_handle(uint8_t *value); + void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); + uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); + void sf45_process_replies(float &data); + float sf45_wrap_360(float f); private: - 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}; + uint8_t _linebuf[SF45_MAX_PAYLOAD] {}; + unsigned _linebuf_size{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}; // 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..110c1649e8 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -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/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 98c479efb1..fd074ad2b2 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -83,3 +83,32 @@ void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float dis _distance_sensor_pub.update(); } + +void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality, + const float q[4]) +{ + + if (_distance_sensor_pub.get().orientation != distance_sensor_s::ROTATION_CUSTOM) { + PX4_ERR("Orientation not set to ROTATION_CUSTOM"); + return; + } + + distance_sensor_s &report = _distance_sensor_pub.get(); + + report.timestamp = timestamp_sample; + report.current_distance = distance; + report.signal_quality = quality; + + // if quality is unavailable (-1) set to 0 if distance is outside bounds + if (quality < 0) { + if ((distance < report.min_distance) || (distance > report.max_distance)) { + report.signal_quality = 0; + } + } + + for (int i = 0; i < 4; i++) { + report.q[i] = q[i]; + } + + _distance_sensor_pub.update(); +} diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index 725f03d321..eadef4ef93 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -63,6 +63,7 @@ public: void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; } void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality = -1); + void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality, const float q[4]); int get_instance() { return _distance_sensor_pub.get_instance(); }; 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);