From 4a08ebafbc7dcb27fe0157ed5216b106d06ab134 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 7 Oct 2024 18:33:27 +0200 Subject: [PATCH] failsafe: prevent immediate low battery failsafe for existing low battery state This happens when BAT_CRIT_THR allows arming with a critical battery level. With this change it still fails the checks but only warns instead of doing the failsafe action because that would immediately land the vehicle before it has taken off. --- src/modules/commander/failsafe/failsafe.cpp | 12 +++++++++--- src/modules/commander/failsafe/failsafe.h | 1 + 2 files changed, 10 insertions(+), 3 deletions(-) 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,