From d5a6174e7f3b50986ba560563e9583a454c895d3 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 19 Apr 2022 13:30:11 +0200 Subject: [PATCH] mission block: fix incorrectly calculated ccw loiter exit (#19487) * mission block: fix incorrectly calculated ccw loiter exit * mission block: update comment on orbit exit location --- src/modules/navigator/mission_block.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c384809676..1312c7a53e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -466,9 +466,10 @@ 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); - // 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); + + // calculate (positive) angle between current bearing vector (orbit center to next waypoint) and vector pointing to tangent exit location + const float ratio = math::min(fabsf(_mission_item.loiter_radius / range), 1.0f); + float inner_angle = acosf(ratio); // Compute "ideal" tangent origin if (curr_sp.loiter_direction > 0) {