dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator: bugfix to prevent NaN setpoints 

The asinf function is NaN outside of -1 to 1. Therefore, it probably
makes sense to constrain the input to prevent NaN setpoints further down
the line.
This commit is contained in:
Julian Oes 2019-10-09 12:12:19 +02:00 committed by Daniel Agar
parent ca711fecc7
commit ea35923d3a
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/navigator/mission_block.cpp
View File

@ -387,7 +387,9 @@ MissionBlock::is_mission_item_reached()
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range);
// We should not use asinf outside of [-1..1].
const float ratio = math::constrain(_mission_item.loiter_radius / range, -1.0f, 1.0f);
float inner_angle = M_PI_2_F - asinf(ratio);
// Compute "ideal" tangent origin
if (curr_sp.loiter_direction > 0) {