diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 037572cb9763..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 @@ -754,6 +753,10 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB; } + if (status_flags.manual_control_signal_lost && manualControlFallbackAction(action)) { + action = manualControlLossFallbackAction(); + } + // Last, check can_run for intended mode if (!modeCanRun(status_flags, user_intended_mode)) { action = Action::RTL; 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 81eba458ae81..19854641309f 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,98 @@ 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) +{ + 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{}; + 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_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{}; + 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); + // 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); +}