AP_BattMonitor: fixed battery remaining of sum battery

and move to common function for update_consumed()

libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp

@@ -82,8 +82,8 @@ AP_BattMonitor_Analog::read()
     // read current
     if (has_current()) {
         // calculate time since last current read
-        uint32_t tnow = AP_HAL::micros();
+        const uint32_t tnow = AP_HAL::micros();
-        float dt = tnow - _state.last_time_micros;
+        const uint32_t dt_us = tnow - _state.last_time_micros;
 
         // this copes with changing the pin at runtime
         _state.healthy &= _curr_pin_analog_source->set_pin(_curr_pin);
@@ -91,12 +91,7 @@ AP_BattMonitor_Analog::read()
         // read current
         _state.current_amps = (_curr_pin_analog_source->voltage_average() - _curr_amp_offset) * _curr_amp_per_volt;
 
-        // update total current drawn since startup
+        update_consumed(_state, dt_us);
-        if (_state.last_time_micros != 0 && dt < 2000000.0f) {
-            float mah = calculate_mah(_state.current_amps, dt);
-            _state.consumed_mah += mah;
-            _state.consumed_wh  += 0.001f * mah * _state.voltage;
-        }
 
         // record time
         _state.last_time_micros = tnow;

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -253,3 +253,16 @@ bool AP_BattMonitor_Backend::reset_remaining(float percentage)
 
     return true;
 }
+
+/*
+  update consumed mAh and Wh
+ */
+void AP_BattMonitor_Backend::update_consumed(AP_BattMonitor::BattMonitor_State &state, uint32_t dt_us)
+{
+    // update total current drawn since startup
+    if (state.last_time_micros != 0 && dt_us < 2000000) {
+        const float mah = calculate_mah(state.current_amps, dt_us);
+        state.consumed_mah += mah;
+        state.consumed_wh  += 0.001 * mah * state.voltage;
+    }
+}

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -90,6 +90,7 @@ protected:
 
     // checks what failsafes could be triggered
     void check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const;
+    void update_consumed(AP_BattMonitor::BattMonitor_State &state, uint32_t dt_us);
 
 private:
     // resistance estimate

libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp

@@ -109,15 +109,11 @@ void AP_BattMonitor_INA2XX::read(void)
     accumulate.count = 0;
 
     const uint32_t tnow = AP_HAL::micros();
-    const float dt = tnow - _state.last_time_micros;
+    const uint32_t dt_us = tnow - _state.last_time_micros;
     
     // update total current drawn since startup
-    if (_state.last_time_micros != 0 && dt < 2000000.0) {
+    update_consumed(_state, dt_us);
-        // .0002778 is 1/3600 (conversion to hours)
+
-        const float mah = _state.current_amps * dt * 0.0000002778;
-        _state.consumed_mah += mah;
-        _state.consumed_wh  += 0.001 * mah * _state.voltage;
-    }
     _state.last_time_micros = tnow;
 }
 

libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp

@@ -67,15 +67,11 @@ void AP_BattMonitor_LTC2946::read(void)
     accumulate.count = 0;
 
     const uint32_t tnow = AP_HAL::micros();
-    const float dt = tnow - _state.last_time_micros;
+    const uint32_t dt_us = tnow - _state.last_time_micros;
     
     // update total current drawn since startup
-    if (_state.last_time_micros != 0 && dt < 2000000.0) {
+    update_consumed(_state, dt_us);
-        // .0002778 is 1/3600 (conversion to hours)
+
-        const float mah = _state.current_amps * dt * 0.0000002778;
-        _state.consumed_mah += mah;
-        _state.consumed_wh  += 0.001 * mah * _state.voltage;
-    }
     _state.last_time_micros = tnow;
 }
 

libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp

@@ -73,13 +73,22 @@ AP_BattMonitor_Sum::read()
             current_count++;
         }
     }
+    const uint32_t tnow_us = AP_HAL::micros();
+    const uint32_t dt_us = tnow_us - _state.last_time_micros;
+
     if (voltage_count > 0) {
         _state.voltage = voltage_sum / voltage_count;
-        _state.last_time_micros = AP_HAL::micros();
     }
     if (current_count > 0) {
         _state.current_amps = current_sum;
     }
+
+    update_consumed(_state, dt_us);
+
     _has_current = (current_count > 0);
     _state.healthy = (voltage_count > 0);
+
+    if (_state.healthy) {
+        _state.last_time_micros = tnow_us;
+    }
 }

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp

@@ -154,14 +154,10 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa
     const uint32_t tnow = AP_HAL::micros();
 
     if (!_has_battery_info_aux || _mppt.is_detected) {
-        uint32_t dt = tnow - _interim_state.last_time_micros;
+        const uint32_t dt_us = tnow - _interim_state.last_time_micros;
 
         // update total current drawn since startup
-        if (_interim_state.last_time_micros != 0 && dt < 2000000) {
+        update_consumed(_interim_state, dt_us);
-            float mah = calculate_mah(_interim_state.current_amps, dt);
-            _interim_state.consumed_mah += mah;
-            _interim_state.consumed_wh  += 0.001f * mah * _interim_state.voltage;
-        }
     }
 
     // record time