From 1cfa312719ec5ac62961d7084cba4a4cc60ca363 Mon Sep 17 00:00:00 2001 From: Wang Jiayue Date: Wed, 20 May 2026 14:10:58 +0800 Subject: [PATCH 1/2] failsafe: prevent manual fallback without RC --- src/modules/commander/failsafe/failsafe.cpp | 14 +++++ .../commander/failsafe/failsafe_test.cpp | 57 +++++++++++++++++++ 2 files changed, 71 insertions(+) diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 037572cb9763..ba6d2f99a7cc 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -745,6 +745,13 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ && !modeCanRun(status_flags, user_intended_mode)) { action = Action::FallbackAltCtrl; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + + // Check if RC is available, if not use the mode specified in NAV_RCL_ACT + if (status_flags.manual_control_signal_lost) { + ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get()); + action = rc_loss_action.action; + return action; + } } // AltCtrl -> Stabilized @@ -752,6 +759,13 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ && !modeCanRun(status_flags, user_intended_mode)) { action = Action::FallbackStab; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB; + + // Check if RC is available, if not use the mode specified in NAV_RCL_ACT + if (status_flags.manual_control_signal_lost) { + ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get()); + action = rc_loss_action.action; + return action; + } } // Last, check can_run for intended mode diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index 81eba458ae81..c8b83fa723ea 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -33,6 +33,7 @@ #include +#include "failsafe.h" #include "framework.h" #include #include "../ModeUtil/mode_requirements.hpp" @@ -562,3 +563,59 @@ TEST_F(FailsafeTest, user_termination) EXPECT_EQ(updated_user_intented_mode, state.user_intended_mode); EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate); } + +TEST_F(FailsafeTest, fallback_altitude_requires_manual_control) +{ + Failsafe failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags); + failsafe_flags.manual_control_signal_lost = true; + + FailsafeBase::State state{}; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 5_s; + + // If arming was allowed without manual control, RC loss is ignored until manual control returns. + failsafe.update(time, state, false, false, failsafe_flags); + + state.armed = true; + time += 10_ms; + failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + + // Losing position in PosCtl would normally fall back to AltCtl, but AltCtl needs manual control. + failsafe_flags.local_position_invalid_relaxed = true; + time += 10_ms; + failsafe.update(time, state, false, false, failsafe_flags); + EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL); +} + +TEST_F(FailsafeTest, fallback_stabilized_requires_manual_control) +{ + Failsafe failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags); + failsafe_flags.manual_control_signal_lost = true; + + FailsafeBase::State state{}; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 5_s; + + // If arming was allowed without manual control, RC loss is ignored until manual control returns. + failsafe.update(time, state, false, false, failsafe_flags); + + state.armed = true; + time += 10_ms; + failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + + // Losing altitude in AltCtl would normally fall back to Stabilized, but Stabilized needs manual control. + failsafe_flags.local_altitude_invalid = true; + time += 10_ms; + failsafe.update(time, state, false, false, failsafe_flags); + EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend); +} From 4593153ad08a974a0aff11b33ac0f2e8b7e24174 Mon Sep 17 00:00:00 2001 From: Wang Jiayue Date: Thu, 21 May 2026 12:43:56 +0800 Subject: [PATCH 2/2] fix(commander): consolidate manual fallback RC loss handling Use NAV_RCL_ACT from a single post-cascade check for manual fallback modes, and add coverage for non-default NAV_RCL_ACT behavior. --- src/modules/commander/failsafe/failsafe.cpp | 31 +++++---------- src/modules/commander/failsafe/failsafe.h | 2 + .../commander/failsafe/failsafe_test.cpp | 39 +++++++++++++++++++ 3 files changed, 51 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index ba6d2f99a7cc..b2927e5703bf 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -40,6 +40,13 @@ using namespace time_literals; +static bool manualControlFallbackAction(FailsafeBase::Action action) +{ + return action == FailsafeBase::Action::FallbackPosCtrl + || action == FailsafeBase::Action::FallbackAltCtrl + || action == FailsafeBase::Action::FallbackStab; +} + FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) { ActionOptions options{}; @@ -729,14 +736,6 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ return action; } - if (action == Action::FallbackPosCtrl || action == Action::FallbackAltCtrl || action == Action::FallbackStab) { - // Check if RC is available, if not use the mode specified in NAV_RCL_ACT - if (status_flags.manual_control_signal_lost) { - ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get()); - action = rc_loss_action.action; - } - - } } // PosCtrl/PositionSlow -> AltCtrl @@ -745,13 +744,6 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ && !modeCanRun(status_flags, user_intended_mode)) { action = Action::FallbackAltCtrl; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - - // Check if RC is available, if not use the mode specified in NAV_RCL_ACT - if (status_flags.manual_control_signal_lost) { - ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get()); - action = rc_loss_action.action; - return action; - } } // AltCtrl -> Stabilized @@ -759,13 +751,10 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ && !modeCanRun(status_flags, user_intended_mode)) { action = Action::FallbackStab; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB; + } - // Check if RC is available, if not use the mode specified in NAV_RCL_ACT - if (status_flags.manual_control_signal_lost) { - ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get()); - action = rc_loss_action.action; - return action; - } + if (status_flags.manual_control_signal_lost && manualControlFallbackAction(action)) { + action = manualControlLossFallbackAction(); } // Last, check can_run for intended mode diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index f17cd2c1510c..8a0e506e50fe 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -189,6 +189,8 @@ class Failsafe : public FailsafeBase static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter); + Action manualControlLossFallbackAction() const { return fromNavDllOrRclActParam(_param_nav_rcl_act.get()).action; } + const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; const int _caller_id_mission_control_lost{genCallerId()}; diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index c8b83fa723ea..19854641309f 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -566,6 +566,9 @@ TEST_F(FailsafeTest, user_termination) TEST_F(FailsafeTest, fallback_altitude_requires_manual_control) { + int nav_rcl_act = 2; + param_set(param_handle(px4::params::NAV_RCL_ACT), &nav_rcl_act); + Failsafe failsafe(nullptr); failsafe_flags_s failsafe_flags{}; @@ -592,8 +595,42 @@ TEST_F(FailsafeTest, fallback_altitude_requires_manual_control) EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL); } +TEST_F(FailsafeTest, fallback_altitude_uses_nav_rcl_act_param) +{ + int nav_rcl_act = 5; + param_set(param_handle(px4::params::NAV_RCL_ACT), &nav_rcl_act); + + Failsafe failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags); + failsafe_flags.manual_control_signal_lost = true; + + FailsafeBase::State state{}; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 5_s; + + // If arming was allowed without manual control, RC loss is ignored until manual control returns. + failsafe.update(time, state, false, false, failsafe_flags); + + state.armed = true; + time += 10_ms; + failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + + // Losing position in PosCtl triggers the manual-control fallback, which should use NAV_RCL_ACT. + failsafe_flags.local_position_invalid_relaxed = true; + time += 10_ms; + failsafe.update(time, state, false, false, failsafe_flags); + EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate); +} + TEST_F(FailsafeTest, fallback_stabilized_requires_manual_control) { + int nav_rcl_act = 2; + param_set(param_handle(px4::params::NAV_RCL_ACT), &nav_rcl_act); + Failsafe failsafe(nullptr); failsafe_flags_s failsafe_flags{}; @@ -617,5 +654,7 @@ TEST_F(FailsafeTest, fallback_stabilized_requires_manual_control) failsafe_flags.local_altitude_invalid = true; time += 10_ms; failsafe.update(time, state, false, false, failsafe_flags); + // checkModeFallback returns RTL from NAV_RCL_ACT. The framework then cascades RTL -> Land -> Descend + // because local altitude loss blocks both AUTO_RTL and AUTO_LAND. EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend); }