From 65a8cc0e0a8731111ad47968a15fda3567aba2cf Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 20 Jan 2025 10:33:30 +0100 Subject: [PATCH] sf45: scale measured distance with pitch and roll. Calls function from ObstacleMath library that accounts for the vehicle's attitude w.r.t the obstacle. Obstacles are assumed to be flat, vertical walls. --- .../lightware_sf45_serial/CMakeLists.txt | 1 + .../lightware_sf45_serial.cpp | 37 ++++++++++++------- .../lightware_sf45_serial.hpp | 6 ++- 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt index 561b7755c76d..6d561f72db08 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt +++ b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( DEPENDS drivers_rangefinder px4_work_queue + CollisionPrevention MODULE_CONFIG module.yaml ) 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 a8ed20383d11..a7a94c67915e 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 @@ -33,19 +33,18 @@ #include "lightware_sf45_serial.hpp" -#include #include +#include +#include #include + #include #include - -#include +#include +#include using namespace time_literals; -/* Configuration Constants */ - - SF45LaserSerial::SF45LaserSerial(const char *port) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), @@ -136,8 +135,6 @@ int SF45LaserSerial::measure() int SF45LaserSerial::collect() { - float distance_m = -1.0f; - if (_sensor_state == STATE_UNINIT) { perf_begin(_sample_perf); @@ -196,8 +193,7 @@ int SF45LaserSerial::collect() sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM); if (_crc_valid) { - sf45_process_replies(&distance_m); - PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); + sf45_process_replies(); perf_end(_sample_perf); return PX4_OK; } @@ -592,7 +588,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8 } } -void SF45LaserSerial::sf45_process_replies(float *distance_m) +void SF45LaserSerial::sf45_process_replies() { switch (rx_field.msg_id) { case SF_DISTANCE_DATA_CM: { @@ -640,20 +636,34 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } // Convert to meters for the debug message - *distance_m = raw_distance * SF45_SCALE_FACTOR; + float distance_m = raw_distance * SF45_SCALE_FACTOR; _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 (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); + (double)distance_m); + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = matrix::Quatf(vehicle_attitude.q); + } + } + + float current_bin_dist = static_cast(_current_bin_dist); + float scaled_yaw_rad = math::radians(static_cast(scaled_yaw)); + ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude); + _current_bin_dist = static_cast(current_bin_dist); if (_current_bin_dist > _obstacle_distance.max_distance) { _current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition } hrt_abstime now = hrt_absolute_time(); + _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); _publish_obstacle_msg(now); @@ -727,6 +737,7 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ } } } + uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { uint8_t mapped_sector = 0; 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 c5cae5e049df..bfe44f0da2a5 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "sf45_commands.h" @@ -92,7 +94,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id); void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); - void sf45_process_replies(float *data); + void sf45_process_replies(); uint8_t sf45_convert_angle(const int16_t yaw); float sf45_wrap_360(float f); @@ -113,6 +115,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now); void _publish_obstacle_msg(hrt_abstime now); + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uint64_t _data_timestamps[BIN_COUNT]; @@ -141,6 +144,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem int32_t _orient_cfg{0}; uint8_t _previous_bin{0}; uint16_t _current_bin_dist{UINT16_MAX}; + matrix::Quatf _vehicle_attitude{}; // end of SF45/B data members