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/collision-prevention: replace repeated code with ObstacleMath library functions.

Browse Source
This commit is contained in:
mahimayoga
2025-02-07 13:28:17 +01:00
committed by Silvan Fuhrer
parent e7e76e2e21
commit 093b379b6b
7 changed files with 78 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+2 -16
View File
@@ -620,7 +620,8 @@ void SF45LaserSerial::sf45_process_replies()
float distance_m = raw_distance * SF45_SCALE_FACTOR;
_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
uint8_t current_bin = sf45_convert_angle(scaled_yaw);
// Find bin index for the current sensor yaw angle (in sensor frame)
const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame);
if (current_bin != _previous_bin) {
PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
@@ -702,21 +703,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
}
}
uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{
uint8_t mapped_sector = 0;
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
return mapped_sector;
}
float SF45LaserSerial::sf45_wrap_360(float f)
{
return matrix::wrap(f, 0.f, 360.f);
}
uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)
{
uint32_t i;
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
-6
View File
@@ -74,10 +74,6 @@ enum SF45_PARSED_STATE {
};
enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90
ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_LEFT_FACING = 6, // MAV_SENSOR_ROTATION_YAW_270
ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90
ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270
};
@@ -95,8 +91,6 @@ public:
void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
void sf45_process_replies();
uint8_t sf45_convert_angle(const int16_t yaw);
float sf45_wrap_360(float f);
private:
obstacle_distance_s _obstacle_distance{};