From 6be8cbe43966d57630e6743e163307c216444be7 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 21 Dec 2023 16:58:22 +0100 Subject: [PATCH] Navigator: mission_block: reduce enforce eexit course margin to 105% (#22511) With this margin it is made sure that if the loiter is not perfectly tracked, (vehicle outside of path setpoint) a wp just at the border of the loiter is still reachable. It should though be as small as necessary, as otherwise, with good loiter tracking, waypoints that are close but not right on the loiter radius are not enforcing the exit course neither. Signed-off-by: Silvan Fuhrer --- 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 357c3c72ad23..27330190a019 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -465,8 +465,8 @@ MissionBlock::is_mission_item_reached_or_completed() && curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER && (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT); - // can only enforce exit course if next waypoint is not within loiter radius of current waypoint - const bool exit_course_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius; + // can only enforce exit course if next waypoint is not within loiter radius of current waypoint (with small margin) + const bool exit_course_is_reachable = dist_current_next > 1.05f * curr_sp_new->loiter_radius; if (enforce_exit_course && exit_course_is_reachable) {