From a46125b1354d41c6e9f3aea89b4ac25c878c6b54 Mon Sep 17 00:00:00 2001 From: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com> Date: Wed, 13 Aug 2025 07:46:52 -0500 Subject: [PATCH] SF45 revert missing bin logic (#25173) * revert missing bin logic Signed-off-by: dirksavage88 * removed else and initialized at variable declaration Signed-off-by: dirksavage88 --------- Signed-off-by: dirksavage88 Co-authored-by: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> --- .../lightware_sf45_serial.cpp | 41 +++++++++++++------ 1 file changed, 28 insertions(+), 13 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 587348ca7d..e15c43a44e 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 @@ -686,22 +686,37 @@ 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 = current_bin; + uint8_t end = previous_bin - 1; - // 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); + 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); - 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 if (previous_bin < current_bin) { // Scanning clockwise + start = previous_bin + 1; + end = current_bin; - // 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; + } + + 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; + } } }