From ea35923d3aea732518f9a0cb50f400ace0159865 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Oct 2019 12:12:19 +0200 Subject: [PATCH] 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. --- src/modules/navigator/mission_block.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index efbf8faec8..bea7347995 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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) {