failsafe: disarm if battery failure is detected during spoolup

PX4 · Nov 7, 2023 · 3acc294 · 3acc294
1 parent 55fd0bd
commit 3acc294
Showing 1 changed file with 9 additions and 1 deletion.
diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
@@ -447,7 +447,15 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	// Battery
 	CHECK_FAILSAFE(status_flags, battery_low_remaining_time,
 		       ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm));
-	CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
+
+	if ((_armed_time != 0)
+	    && (time_us < _armed_time + static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s))
+	   ) {
+		CHECK_FAILSAFE(status_flags, battery_unhealthy, ActionOptions(Action::Disarm).cannotBeDeferred());
+
+	} else {
+		CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
+	}
 
 	switch (status_flags.battery_warning) {
 	case battery_status_s::BATTERY_WARNING_LOW: