diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 255a8e2ab8..66db8f94f0 100644 --- a/src/modules/commander/Commander.cpp +++ b/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()); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 0ec1bd576f..625826513a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -211,6 +211,7 @@ private: (ParamInt) _param_com_pos_fs_gain, (ParamInt) _param_com_low_bat_act, + (ParamFloat) _param_com_bat_act_t, (ParamInt) _param_com_imb_prop_act, (ParamFloat) _param_com_disarm_land, (ParamFloat) _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] {}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 480bdcf147..dd22a64daa 100644 --- a/src/modules/commander/commander_params.c +++ b/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 *