reuse failsafe functions

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<px4::params::NAV_DLL_ACT>) 	_param_nav_dll_act,
 					(ParamInt<px4::params::NAV_RCL_ACT>) 	_param_nav_rcl_act,

src/modules/commander/failsafe/failsafe_test.cpp

@@ -34,6 +34,7 @@
 #include <gtest/gtest.h>
 
 #include "framework.h"
+#include "failsafe.h"
 #include <uORB/topics/vehicle_status.h>
 #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);