From d678e792cc805c7d7a3bcb92bfc92ef9f5152e4f Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 13 Jul 2021 16:57:50 +0300 Subject: [PATCH] mission_block: don't require an exiting heading when loitering if the next waypoint is within the loiter radius of the current waypoint Signed-off-by: RomanBapst --- src/modules/navigator/mission_block.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4a74c487e3..7bbe693e20 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -375,6 +375,9 @@ MissionBlock::is_mission_item_reached() struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current; const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; + const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat, + next_sp.lon); + /* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set, or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */ const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING @@ -383,11 +386,10 @@ MissionBlock::is_mission_item_reached() curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER && (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT); - if (enforce_exit_heading) { + // can only enforce exit heading if next waypoint is not within loiter radius of current waypoint + const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius; - - const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat, - next_sp.lon); + if (enforce_exit_heading && exit_heading_is_reachable) { float yaw_err = 0.0f;