fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 10:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

failsafe: prevent immediate low battery failsafe for existing low battery state

Browse Source 
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.
This commit is contained in:
Matthias Grob
2024-10-07 18:33:27 +02:00
parent 7941310afd
commit 4a08ebafbc
2 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failsafe/failsafe.cpp
+9 -3
View File
@@ -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
src/modules/commander/failsafe/failsafe.h
+1
View File
@@ -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<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,