From 487d8ae21528adbd3dbe138264a271af0d531155 Mon Sep 17 00:00:00 2001 From: Silvan Date: Mon, 3 Nov 2025 12:07:06 +0100 Subject: [PATCH] rtl_mission_fast: fix FW landing by setting previous wp in landing Signed-off-by: Silvan --- src/modules/navigator/rtl_direct_mission_land.cpp | 1 - src/modules/navigator/rtl_mission_fast.cpp | 10 +++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 10ab907c0e..7fdbe1add5 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -206,7 +206,6 @@ void RtlDirectMissionLand::setActiveMissionItems() _mission_item.time_inside = 0.0f; } - if (num_found_items > 0) { mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); } 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;