From 38922d10a004adac2d03b49e0e06ef28e2fd07c2 Mon Sep 17 00:00:00 2001 From: Peter Breuer Date: Thu, 24 Apr 2025 13:09:46 -0700 Subject: [PATCH] fix: fallback to mode set by COM_POSCTL_NAVL upon navigation loss in pos slow mode --- src/modules/commander/failsafe/failsafe.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 07fb568812..6d2bc83d83 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -661,8 +661,9 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ switch (position_control_navigation_loss_response(_param_com_posctl_navl.get())) { case position_control_navigation_loss_response::Altitude_Manual: // AltCtrl/Manual - // PosCtrl -> AltCtrl - if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL + // PosCtrl/PositionSlow -> AltCtrl + if ((user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL || + user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) && !modeCanRun(status_flags, user_intended_mode)) { action = Action::FallbackAltCtrl; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL; @@ -679,8 +680,9 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ case position_control_navigation_loss_response::Land_Descend: // Land/Terminate - // PosCtrl -> Land - if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL + // PosCtrl/PositionSlow -> Land + if ((user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL || + user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) && !modeCanRun(status_flags, user_intended_mode)) { action = Action::Land; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;