Commander: add delay before executing low battery failsafe action

src/modules/commander/Commander.cpp

@@ -3876,7 +3876,6 @@ void Commander::battery_status_check()
 		_battery_warning = worst_warning;
 	}
 
-
 	_status_flags.condition_battery_healthy =
 		// All connected batteries are regularly being published
 		(hrt_elapsed_time(&oldest_update) < 5_s)
@@ -3890,6 +3889,18 @@ void Commander::battery_status_check()
 	// execute battery failsafe if the state has gotten worse while we are armed
 	if (battery_warning_level_increased_while_armed) {
 		warn_user_about_battery(&_mavlink_log_pub, _battery_warning);
+		_battery_failsafe_timestamp = hrt_absolute_time();
+
+		if (get_battery_failsafe_action(_internal_state, _battery_warning,
+						(low_battery_action_t)_param_com_low_bat_act.get()) != commander_state_s::MAIN_STATE_MAX) {
+			main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
+		}
+	}
+
+	if (_battery_failsafe_timestamp != 0
+	    && hrt_elapsed_time(&_battery_failsafe_timestamp) > _param_com_bat_act_t.get() * 1_s
+	    && _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER) {
+		_battery_failsafe_timestamp = 0;
 		uint8_t failsafe_action = get_battery_failsafe_action(_internal_state, _battery_warning,
 					  (low_battery_action_t)_param_com_low_bat_act.get());
 

src/modules/commander/Commander.hpp

@@ -211,6 +211,7 @@ private:
 		(ParamInt<px4::params::COM_POS_FS_GAIN>) _param_com_pos_fs_gain,
 
 		(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
+		(ParamFloat<px4::params::COM_BAT_ACT_T>) _param_com_bat_act_t,
 		(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
 		(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
 		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
@@ -343,6 +344,7 @@ private:
 	hrt_abstime	_last_esc_status_updated{0};
 
 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
+	hrt_abstime	_battery_failsafe_timestamp{0};
 	float		_battery_current{0.0f};
 	uint8_t		_last_connected_batteries{0};
 	uint32_t	_last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};

src/modules/commander/commander_params.c

@@ -352,6 +352,22 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
  */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
+/**
+ * Delay between battery state change and failsafe reaction
+ *
+ * Battery state requires action -> wait COM_BAT_ACT_T seconds for the user to realize and take custom action
+ * React with failsafe action COM_LOW_BAT_ACT
+ *
+ * A zero value disables the delay.
+ *
+ * @group Commander
+ * @unit s
+ * @min 0.0
+ * @max 25.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 10.0f);
+
 /**
  * Imbalanced propeller failsafe mode
  *