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

CollisionPrevention: Sanitize input of _getObstacleDistance()

Browse Source 
It could cause array out of bound problems before.
This commit is contained in:
Matthias Grob 2024-11-11 18:31:39 +01:00 committed by Claudio Chies
parent eb06ace8e4
commit 50ee5bd1b4
1 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -500,17 +500,23 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, con
}
}
float
CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
{
const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi();
Vector2f dir = direction / direction.norm();
const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad;
const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
_obstacle_map_body_frame.angle_offset);
int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
const float direction_norm = direction.norm();
return _obstacle_map_body_frame.distances[dir_index] * 0.01f;
if (direction_norm > FLT_EPSILON) {
Vector2f dir = direction / direction_norm;
const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi();
const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad;
const float sp_angle_with_offset_deg =
wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
dir_index = math::constrain(dir_index, 0, 71);
return _obstacle_map_body_frame.distances[dir_index] * 0.01f;
} else {
return 0.f;
}
}
Vector2f