From 08542d397f07244072fc0a0f0f2907f835ff617a Mon Sep 17 00:00:00 2001 From: Balduin Date: Mon, 20 Apr 2026 12:31:12 +0200 Subject: [PATCH] fix(fw_mode_manager): only use line-following backtransition if position and velocity good enough --- src/modules/fw_mode_manager/FixedWingModeManager.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 5202e5a913..bb8c232b8e 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -405,7 +405,12 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) // Enter this mode only if the current waypoint has valid 3D position setpoints. if (doing_backtransition) { - _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW; + + const bool position_good_enough = _local_pos.eph < 10.0f && _local_pos.evh < 10.0f; + + _control_mode_current = position_good_enough + ? FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW + : FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {