AP_BattMonitor: Split the failsafe conditions from their timers

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -98,25 +98,14 @@ float AP_BattMonitor_Backend::voltage_resting_estimate() const
     return MAX(_state.voltage, _state.voltage_resting_estimate);
 }
 
-
 AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
 {
     const uint32_t now = AP_HAL::millis();
 
-    // use voltage or sag compensated voltage
-    float voltage_used;
-    switch (_params.failsafe_voltage_source()) {
-        case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_Raw:
-        default:
-            voltage_used = _state.voltage;
-            break;
-        case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_SagCompensated:
-            voltage_used = voltage_resting_estimate();
-            break;
-    }
+    bool low_voltage, low_capacity, critical_voltage, critical_capacity;
+    check_failsafe_types(low_voltage, low_capacity, critical_voltage, critical_capacity);
 
-    // check critical battery levels
-    if ((voltage_used > 0) && (_params._critical_voltage > 0) && (voltage_used < _params._critical_voltage)) {
+    if (critical_voltage) {
         // this is the first time our voltage has dropped below minimum so start timer
         if (_state.critical_voltage_start_ms == 0) {
             _state.critical_voltage_start_ms = now;
@@ -129,13 +118,11 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
         _state.critical_voltage_start_ms = 0;
     }
 
-    // check capacity if current monitoring is enabled
-    if (has_current() && (_params._critical_capacity > 0) &&
-        ((_params._pack_capacity - _state.consumed_mah) < _params._critical_capacity)) {
+    if (critical_capacity) {
         return AP_BattMonitor::BatteryFailsafe_Critical;
     }
 
-    if ((voltage_used > 0) && (_params._low_voltage > 0) && (voltage_used < _params._low_voltage)) {
+    if (low_voltage) {
         // this is the first time our voltage has dropped below minimum so start timer
         if (_state.low_voltage_start_ms == 0) {
             _state.low_voltage_start_ms = now;
@@ -148,12 +135,55 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
         _state.low_voltage_start_ms = 0;
     }
 
-    // check capacity if current monitoring is enabled
-    if (has_current() && (_params._low_capacity > 0) &&
-        ((_params._pack_capacity - _state.consumed_mah) < _params._low_capacity)) {
+    if (low_capacity) {
         return AP_BattMonitor::BatteryFailsafe_Low;
     }
 
     // if we've gotten this far then battery is ok
     return AP_BattMonitor::BatteryFailsafe_None;
 }
+
+void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const
+{
+    // use voltage or sag compensated voltage
+    float voltage_used;
+    switch (_params.failsafe_voltage_source()) {
+        case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_Raw:
+        default:
+            voltage_used = _state.voltage;
+            break;
+        case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_SagCompensated:
+            voltage_used = voltage_resting_estimate();
+            break;
+    }
+
+    // check critical battery levels
+    if ((voltage_used > 0) && (_params._critical_voltage > 0) && (voltage_used < _params._critical_voltage)) {
+        critical_voltage = true;
+        // this is the first time our voltage has dropped below minimum so start timer
+    } else {
+        critical_voltage = false;
+    }
+
+    // check capacity if current monitoring is enabled
+    if (has_current() && (_params._critical_capacity > 0) &&
+        ((_params._pack_capacity - _state.consumed_mah) < _params._critical_capacity)) {
+        critical_capacity = true;
+    } else {
+        critical_capacity = false;
+    }
+
+    if ((voltage_used > 0) && (_params._low_voltage > 0) && (voltage_used < _params._low_voltage)) {
+        low_voltage = true;
+    } else {
+        low_voltage = false;
+    }
+
+    // check capacity if current monitoring is enabled
+    if (has_current() && (_params._low_capacity > 0) &&
+        ((_params._pack_capacity - _state.consumed_mah) < _params._low_capacity)) {
+        low_capacity = true;
+    } else {
+        low_capacity = false;
+    }
+}

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -61,6 +61,9 @@ protected:
     AP_BattMonitor::BattMonitor_State   &_state;    // reference to this instances state (held in the front-end)
     AP_BattMonitor_Params               &_params;   // reference to this instances parameters (held in the front-end)
 
+    // checks what failsafes could be triggered
+    void check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const;
+
 private:
     // resistance estimate
     uint32_t    _resistance_timer_ms;    // system time of last resistance estimate update