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

obstacle-math: add standard obstacle map functions.

Browse Source 
These functions help simplify repeated calculations accross driver and collision prevention files that are computing bins, angles and sensor offsets in obstacle maps.
This commit is contained in:
mahimayoga
2025-02-03 17:17:35 +01:00
committed by Silvan Fuhrer
parent f7dadd9b89
commit cb332e047d
3 changed files with 251 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/ObstacleMath.hpp
+44
View File
@@ -36,6 +36,28 @@
namespace ObstacleMath
{
enum SensorOrientation {
ROTATION_YAW_0 = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_YAW_45 = 1, // MAV_SENSOR_ROTATION_YAW_45
ROTATION_YAW_90 = 2, // MAV_SENSOR_ROTATION_YAW_90
ROTATION_YAW_135 = 3, // MAV_SENSOR_ROTATION_YAW_135
ROTATION_YAW_180 = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_YAW_225 = 5, // MAV_SENSOR_ROTATION_YAW_225
ROTATION_YAW_270 = 6, // MAV_SENSOR_ROTATION_YAW_270
ROTATION_YAW_315 = 7, // MAV_SENSOR_ROTATION_YAW_315
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
};
/**
* Converts a sensor orientation to a yaw offset
* @param orientation sensor orientation
*/
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
/**
* Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
* @param distance measurement which is scaled down
@@ -44,4 +66,26 @@ namespace ObstacleMath
*/
void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle);
/**
* Returns bin index at a given angle from the 0th bin
* @param bin_width width of a bin in degrees
* @param angle clockwise angle from start bin in degrees
*/
int get_bin_at_angle(float bin_width, float angle);
/**
* Returns bin index for the current bin after an angle offset
* @param bin current bin index
* @param bin_width width of a bin in degrees
* @param angle_offset clockwise angle offset in degrees
*/
int get_offset_bin_index(int bin, float bin_width, float angle_offset);
/**
* Wraps a bin index to the range [0, bin_count)
* @param bin bin index
* @param bin_count number of bins
*/
int wrap_bin(int bin, int bin_count);
} // ObstacleMath