From 9702a2a899c9a48a3f22444d8ee49e066fdbfe83 Mon Sep 17 00:00:00 2001 From: Matthew Berk <8104397+msberk@users.noreply.github.com> Date: Mon, 3 Nov 2025 10:52:26 -0600 Subject: [PATCH] Navigator: Fix mission RTL for fixed-wing by setting previous waypoint correctly (#25600) 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 7e8b3d4aa4..7fdbe1add5 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 a8a7809549..bb73fd6952 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -108,6 +108,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) && @@ -175,17 +176,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;