fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 11:38:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

SF45 revert missing bin logic (#25173)

Browse Source 
* 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>
This commit is contained in:
Andrew Brahim
2025-08-13 08:46:52 -04:00
committed by GitHub
co-authored by Claudio Chies
parent c320a9240b
commit a46125b135
1 changed files with 28 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+28 -13
View File
@@ -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;
}
}
}