diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index e790d352c9..9f572e1fbf 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -161,6 +161,12 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const (_state.voltage < _params._arming_minimum_voltage); bool below_arming_capacity = (_params._arming_minimum_capacity > 0) && ((_params._pack_capacity - _state.consumed_mah) < _params._arming_minimum_capacity); + bool fs_capacity_inversion = is_positive(_params._critical_capacity) && + is_positive(_params._low_capacity) && + (_params._low_capacity < _params._critical_capacity); + bool fs_voltage_inversion = is_positive(_params._critical_voltage) && + is_positive(_params._low_voltage) && + (_params._low_voltage < _params._critical_voltage); bool result = update_check(buflen, buffer, low_voltage, "low voltage failsafe"); result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe"); @@ -168,6 +174,8 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const result = result && update_check(buflen, buffer, critical_capacity, "critical capacity failsafe"); result = result && update_check(buflen, buffer, below_arming_voltage, "below minimum arming voltage"); result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity"); + result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical > low"); + result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical > low"); return result; }