From 5d0faf485720a91673de2b2e5e040cf48bffb64a Mon Sep 17 00:00:00 2001 From: Claudio Chies Date: Fri, 19 Dec 2025 15:37:50 +0100 Subject: [PATCH] reuse failsafe functions --- src/modules/commander/failsafe/failsafe.h | 14 ++++---- .../commander/failsafe/failsafe_test.cpp | 34 ++++++------------- 2 files changed, 17 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index d5b3f0a79e..55d5f1aa35 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 @@ protected: 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 @@ private: 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 56744adbbe..69132c467f 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" @@ -568,36 +569,25 @@ 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 FailsafeBase + class BatteryFailsafeTester : public Failsafe { public: - BatteryFailsafeTester(ModuleParams *parent) : FailsafeBase(parent) + param_t spoolup_param; + float spoolup_time; + + BatteryFailsafeTester(ModuleParams *parent) : Failsafe(parent) { // Set spoolup time parameter for testing - param_t spoolup_param = param_handle(px4::params::COM_SPOOLUP_TIME); - float spoolup_time = 2.0f; // 2 seconds 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); } - void updateArmingState(const hrt_abstime &time_us, bool armed) - { - if (!_was_armed && armed) { - _armed_time = time_us; - - } else if (!armed) { - _armed_time = 0; - } - - _was_armed = armed; - } - 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_t spoolup_param = param_handle(px4::params::COM_SPOOLUP_TIME); - float spoolup_time = 2.0f; param_get(spoolup_param, &spoolup_time); if ((_armed_time != 0) @@ -619,10 +609,6 @@ TEST_F(FailsafeTest, battery_unhealthy_during_spoolup) { return Action::None; } - - private: - hrt_abstime _armed_time{0}; - bool _was_armed{false}; }; BatteryFailsafeTester failsafe(nullptr); @@ -635,10 +621,10 @@ TEST_F(FailsafeTest, battery_unhealthy_during_spoolup) hrt_abstime time = 5_s; // Update arming state - failsafe.updateArmingState(time, true); + failsafe.updateArmingState(time, true, failsafe_flags); // Test 1: Battery unhealthy during spoolup phase (within 2 seconds of arming) - time += spoolup_time - 100_ms; // still in spoolup + 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);