From f0f19ce8fbb18d1b5291c470ca961d33a1710d62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 11:01:25 +1100 Subject: [PATCH] AP_BattMonitor: fixed battery remaining of sum battery and move to common function for update_consumed() --- libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp | 11 +++-------- libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp | 13 +++++++++++++ libraries/AP_BattMonitor/AP_BattMonitor_Backend.h | 1 + libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp | 10 +++------- libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp | 10 +++------- libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp | 11 ++++++++++- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 8 ++------ 7 files changed, 35 insertions(+), 29 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index a37257587b..e43e400887 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/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(); - float dt = tnow - _state.last_time_micros; + const uint32_t tnow = AP_HAL::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 - 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; - } + update_consumed(_state, dt_us); // record time _state.last_time_micros = tnow; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index edce254eb1..ee0388ba91 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/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; + } +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 5d8d81f5aa..d2587131c4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp index 84f780ee2a..6ecc8fde94 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp +++ b/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) { - // .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; - } + update_consumed(_state, dt_us); + _state.last_time_micros = tnow; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp index 418698436f..3274a82da6 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp +++ b/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) { - // .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; - } + update_consumed(_state, dt_us); + _state.last_time_micros = tnow; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index cf8d3f973c..7ce61484f8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/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; + } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 81e4187514..3544c87c3f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/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) { - float mah = calculate_mah(_interim_state.current_amps, dt); - _interim_state.consumed_mah += mah; - _interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage; - } + update_consumed(_interim_state, dt_us); } // record time