SF45 revert missing bin logic (#25173)

* revert missing bin logic

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* removed else and initialized at variable declaration

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

---------

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
Co-authored-by: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>

Author: dirksavage88

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);
-
-	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);
-
-	// 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 (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);
+
+	} else if (previous_bin < current_bin) {	// Scanning clockwise
+		start = previous_bin + 1;
+		end = current_bin;
+
+	}
+
+	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;
+		}
 	}
 }