diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index d5b3f0a79ecd..55d5f1aa35c8 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -41,6 +41,8 @@ class Failsafe : public FailsafeBase public: Failsafe(ModuleParams *parent) : FailsafeBase(parent) {} + void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags); + protected: void checkStateAndMode(const hrt_abstime &time_us, const State &state, @@ -50,9 +52,12 @@ class Failsafe : public FailsafeBase uint8_t modifyUserIntendedMode(Action previous_action, Action current_action, uint8_t user_intended_mode) const override; -private: - void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags); + hrt_abstime _armed_time{0}; + bool _was_armed{false}; + bool _manual_control_lost_at_arming{false}; ///< true if manual control was lost at arming time + uint8_t _battery_warning_at_arming{0}; ///< low battery state at arming time +private: enum class LinkLossExceptionBits : int32_t { Mission = (1 << 0), AutoModes = (1 << 1), @@ -187,11 +192,6 @@ class Failsafe : public FailsafeBase const int _caller_id_battery_unhealthy_spoolup{genCallerId()}; bool _last_state_battery_unhealthy_spoolup{false}; - hrt_abstime _armed_time{0}; - bool _was_armed{false}; - bool _manual_control_lost_at_arming{false}; ///< true if manual control was lost at arming time - uint8_t _battery_warning_at_arming{0}; ///< low battery state at arming time - DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase, (ParamInt) _param_nav_dll_act, (ParamInt) _param_nav_rcl_act, diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index 81eba458ae81..69132c467f7b 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -34,6 +34,7 @@ #include #include "framework.h" +#include "failsafe.h" #include #include "../ModeUtil/mode_requirements.hpp" @@ -562,3 +563,85 @@ 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, battery_unhealthy_during_spoolup) +{ + // Test that battery unhealthy during spoolup phase causes immediate disarm (existing behavior) + + // Create a custom failsafe tester that includes battery unhealthy checks + class BatteryFailsafeTester : public Failsafe + { + public: + param_t spoolup_param; + float spoolup_time; + + BatteryFailsafeTester(ModuleParams *parent) : Failsafe(parent) + { + // Set spoolup time parameter for testing + spoolup_param = param_handle(px4::params::COM_SPOOLUP_TIME); + spoolup_time = 2.0f; // 2 seconds for testing + param_set(spoolup_param, &spoolup_time); + } + + protected: + void checkStateAndMode(const hrt_abstime &time_us, const State &state, + const failsafe_flags_s &status_flags) override + { + // Simulate the battery unhealthy check logic from failsafe.cpp + param_get(spoolup_param, &spoolup_time); + + if ((_armed_time != 0) + && (time_us < _armed_time + static_cast(spoolup_time * 1000000)) + ) { + // During spoolup phase - should disarm immediately + CHECK_FAILSAFE(status_flags, battery_unhealthy, ActionOptions(Action::Disarm).cannotBeDeferred()); + + } else { + // After spoolup phase - should trigger LAND mode with user takeover + CHECK_FAILSAFE(status_flags, battery_unhealthy, + ActionOptions(Action::Land) + .allowUserTakeover(UserTakeoverAllowed::Always) + .clearOn(ClearCondition::OnModeChangeOrDisarm)); + } + } + + Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override + { + return Action::None; + } + }; + + BatteryFailsafeTester failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + FailsafeBase::State state{}; + state.armed = true; + 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; + + // Update arming state + failsafe.updateArmingState(time, true, failsafe_flags); + + // Test 1: Battery unhealthy during spoolup phase (within 2 seconds of arming) + time += failsafe.spoolup_time - 100_ms; // still in spoolup + failsafe_flags.battery_unhealthy = true; + uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Disarm); + + // Clear battery unhealthy and move past spoolup time + failsafe_flags.battery_unhealthy = false; + time += 2_s; // Now past spoolup time + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + + // Test 2: Battery unhealthy after spoolup phase - should trigger LAND mode + time += 100_ms; + failsafe_flags.battery_unhealthy = true; + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Land); +}