diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 926bdfe44d..534d62807f 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -501,22 +501,27 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } // Battery low failsafe + // If battery was low and arming was allowed through COM_ARM_BAT_MIN, don't failsafe immediately for the current low battery warning state + const bool warning_worse_than_at_arming = (status_flags.battery_warning > _battery_warning_at_arming); + const int32_t low_battery_action = warning_worse_than_at_arming ? + _param_com_low_bat_act.get() : (int32_t)LowBatteryAction::Warning; + switch (status_flags.battery_warning) { case battery_status_s::BATTERY_WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, - true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_LOW)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_LOW)); break; case battery_status_s::BATTERY_WARNING_CRITICAL: _last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical, _last_state_battery_warning_critical, - true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_CRITICAL)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_CRITICAL)); break; case battery_status_s::BATTERY_WARNING_EMERGENCY: _last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency, _last_state_battery_warning_emergency, - true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_EMERGENCY)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_EMERGENCY)); break; default: @@ -563,6 +568,7 @@ void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const f if (!_was_armed && armed) { _armed_time = time_us; _manual_control_lost_at_arming = status_flags.manual_control_signal_lost; + _battery_warning_at_arming = status_flags.battery_warning; } else if (!armed) { _manual_control_lost_at_arming = status_flags.manual_control_signal_lost; // ensure action isn't added while disarmed diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 742b84ce36..2ebb962cb1 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -173,6 +173,7 @@ private: 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,