Skip to content

Commit

Permalink
sf45: fix small error from rebase.
Browse files Browse the repository at this point in the history
  • Loading branch information
mahimayoga committed Feb 4, 2025
1 parent cc5cccf commit 7a045c4
Show file tree
Hide file tree
Showing 2 changed files with 2 additions and 20 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -681,7 +681,7 @@ void SF45LaserSerial::_publish_obstacle_msg(hrt_abstime now)
_obstacle_distance_pub.publish(_obstacle_distance);
}

void SF45LaserSerial::_handle_missed_bins(int current_bin, int previous_bin, uint16_t measurement,
void SF45LaserSerial::_handle_missed_bins(uint16_t current_bin, uint16_t previous_bin, uint16_t measurement,
hrt_abstime now)
{
// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
Expand All @@ -702,24 +702,6 @@ void SF45LaserSerial::_handle_missed_bins(int current_bin, int previous_bin, uin
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 start;
uint8_t end;

// Shift bin indices such that we can never have the wrap-around case.
float fov_offset_angle = 360.0f - SF45_FIELDOF_VIEW / 2;
int current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, _obstacle_distance.increment,
fov_offset_angle);
int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment,
fov_offset_angle);

int start = math::min(current_bin_offset, previous_bin_offset) + 1;
int end = math::max(current_bin_offset, previous_bin_offset);

// populate the missed bins with the measurement
for (uint16_t i = start; i < end; i++) {
int 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;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
int collect();
bool _crc_valid{false};

void _handle_missed_bins(int current_bin, int previous_bin, uint16_t measurement, hrt_abstime now);
void _handle_missed_bins(uint16_t current_bin, uint16_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];
Expand Down

0 comments on commit 7a045c4

Please sign in to comment.