From 6b5cfcce70184640e99ea9edd6f1c283782a96fb Mon Sep 17 00:00:00 2001 From: Matthew Berk <8104397+msberk@users.noreply.github.com> Date: Mon, 3 Nov 2025 14:34:27 -0600 Subject: [PATCH] [Backport 1.16] Navigator: Fix mission RTL for fixed-wing by setting previous waypoint correctly(#25861) This aligns setActiveMissionItems() in rtl_direct_mission_land.cpp and in rtl_mission_fast.cpp with what was already in mission.cpp. It probably was on oversight when the RTL restructure happened. The FW landing requires the previous waypoint to be correctly set, that's why it was only noticeable there. * Fix position setpoint update logic in Mission RTL Currently, when proceeding to the landing point the previous setpoint is not updated, which results in an unexpected and off course landing pattern in fixed wing. (see #25436) * Change to work more like `mission.cpp` * Fix rtl_direct_misssion_land formatting for style guide * rtl_mission_fast: fix FW landing by setting previous wp in landing Signed-off-by: Silvan --------- Signed-off-by: Silvan Co-authored-by: Silvan --- src/modules/navigator/rtl_direct_mission_land.cpp | 9 +++++++-- src/modules/navigator/rtl_mission_fast.cpp | 10 +++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 6b7f2f649a..228235f336 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -122,6 +122,7 @@ void RtlDirectMissionLand::setActiveMissionItems() { WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; // Climb to altitude if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { @@ -203,8 +204,6 @@ void RtlDirectMissionLand::setActiveMissionItems() _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; - - pos_sp_triplet->previous = pos_sp_triplet->current; } if (num_found_items > 0) { @@ -213,6 +212,12 @@ void RtlDirectMissionLand::setActiveMissionItems() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + // Only set the previous position item if the current one really changed + if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) && + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + pos_sp_triplet->previous = current_setpoint_copy; + } + // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index ccfc91cfde..810b848781 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -91,6 +91,7 @@ void RtlMissionFast::setActiveMissionItems() { WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; /* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/ if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) && @@ -158,17 +159,20 @@ void RtlMissionFast::setActiveMissionItems() _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; - pos_sp_triplet->previous = pos_sp_triplet->current; } - - if (num_found_items > 0) { mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + // Only set the previous position item if the current one really changed + if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) && + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + pos_sp_triplet->previous = current_setpoint_copy; + } + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;