From b0a6ae0ced944e045558d2e17e451605ebe6e1c8 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Thu, 14 Apr 2022 17:59:30 +0200 Subject: [PATCH 1/2] mission block: fix incorrectly calculated ccw loiter exit --- src/modules/navigator/mission_block.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c38480967610..c3f6b9084023 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -467,8 +467,8 @@ MissionBlock::is_mission_item_reached() 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); + 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) { From f7f2d3ffba6cdb82f3027b0c9411dfa5c7684ee7 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 19 Apr 2022 10:51:25 +0200 Subject: [PATCH 2/2] mission block: update comment on orbit exit location --- src/modules/navigator/mission_block.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c3f6b9084023..1312c7a53e30 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -466,7 +466,8 @@ 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]. + + // 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);