From 31bff3e5bbf8aefb68d28215ed03f0cf5dedaac1 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:37:00 +0100 Subject: [PATCH] sf45: change handle_missed_bins() function logic. To simplify logic for wrap-around cases and cases in which bins outside the FOV may be filled. Bin indices are offset such that the 0th bin is the first bin within the sensor FOV. This way we can always fill bins from smallest to largest index. --- .../lightware_sf45_serial.cpp | 46 ++++++------------- .../lightware_sf45_serial.hpp | 1 + 2 files changed, 16 insertions(+), 31 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 a7a94c6791..e6fbe3e4b9 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 @@ -664,6 +664,7 @@ void SF45LaserSerial::sf45_process_replies() hrt_abstime now = hrt_absolute_time(); + _obstacle_distance.distances[current_bin] = _current_bin_dist; _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); _publish_obstacle_msg(now); @@ -701,43 +702,26 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ { // 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. - 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 - // THis is simplyfied as we are not considering the scaning direction - start = math::max(previous_bin, current_bin); - end = math::min(previous_bin, current_bin); + // Shift bin indices such that we can never have the wrap-around case. + const float fov_offset_angle = 360.0f - SF45_FIELDOF_VIEW / 2.f; + const uint16_t current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, _obstacle_distance.increment, + fov_offset_angle); + const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment, + fov_offset_angle); - } else if (previous_bin < current_bin) { // Scanning clockwise - start = previous_bin + 1; - end = current_bin; + const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1; + const uint16_t end = math::max(current_bin_offset, previous_bin_offset); - } else { // scanning counter-clockwise - start = current_bin; - end = previous_bin - 1; - } - - if (start <= end) { - for (uint8_t i = start; i <= end; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } - - } else { // wrap-around case - for (uint8_t i = start; i < BIN_COUNT; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } - - for (uint8_t i = 0; i <= end; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } + // populate the missed bins with the measurement + for (uint16_t i = start; i < end; i++) { + uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle); + _obstacle_distance.distances[bin_index] = measurement; + _data_timestamps[bin_index] = now; } } + 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 bfe44f0da2..d308a474d4 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 @@ -105,6 +105,7 @@ private: obstacle_distance_s::distances[0]); static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms}; static constexpr float SF45_SCALE_FACTOR = 0.01f; + static constexpr float SF45_FIELDOF_VIEW = 320.f; // degrees void start(); void stop();