From 45647718cb4b0d7443b46e1a23989eca566738be Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 27 Jul 2024 14:46:43 +0100
Subject: [PATCH] AP_BattMonitor: ESC: integrate consumed mah if not provided
 by ESC

---
 .../AP_BattMonitor/AP_BattMonitor_ESC.cpp     | 23 ++++++++++++++-----
 libraries/AP_BattMonitor/AP_BattMonitor_ESC.h |  3 +++
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp
index a1c309a6e6..7b76295f3b 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp
@@ -59,8 +59,8 @@ void AP_BattMonitor_ESC::read(void)
     float voltage_sum = 0;
     float current_sum = 0;
     float temperature_sum = 0;
+    float consumed_mah_sum = 0.0;
     uint32_t highest_ms = 0;
-    _state.consumed_mah = delta_mah;
 
     const bool all_enabled = _mask == 0;
     for (uint8_t i=0; i<ESC_TELEM_MAX_ESCS; i++) {
@@ -77,7 +77,8 @@ void AP_BattMonitor_ESC::read(void)
         if (telem.get_consumption_mah(i, consumption_mah)) {
             // accumulate consumed_sum regardless of age, to cope with ESC
             // dropping out
-            _state.consumed_mah += consumption_mah;
+            consumed_mah_sum += consumption_mah;
+            have_consumed_mah = true;
         }
 
         if (telem.get_voltage(i, voltage)) {
@@ -87,6 +88,7 @@ void AP_BattMonitor_ESC::read(void)
 
         if (telem.get_current(i, current)) {
             current_sum += current;
+            have_current = true;
         }
 
         if (telem.get_temperature(i, temperature_cdeg)) {
@@ -117,10 +119,19 @@ void AP_BattMonitor_ESC::read(void)
     _state.last_time_micros = highest_ms * 1000;
     _state.temperature_time = highest_ms;
 
-    if (current_sum > 0) {
-        // if we have ever got a current value then we know we have a
-        // current sensor
-        have_current = true;
+    const uint32_t now_us = AP_HAL::micros();
+    const uint32_t dt_us = now_us - last_read_us;
+    last_read_us = now_us;
+
+    if (have_consumed_mah) {
+        // Report the cumulative consumed mah as reported by the ESCs
+        // delta_mah allows reset_remaining to function without being able to reset the values sent by the ESCs
+        _state.consumed_mah = delta_mah + consumed_mah_sum;
+
+    } else if (have_current) {
+        // ESCs provide current but not consumed mah, integrate manually
+        update_consumed(_state, dt_us);
+
     }
 }
 
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h
index 4d3eb33bac..400520dfbe 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h
@@ -51,8 +51,11 @@ private:
     AP_Int32  _mask;
 
     bool have_current;
+    bool have_consumed_mah;
     bool have_temperature;
     float delta_mah;
+
+    uint32_t last_read_us;
 };
 
 #endif  // AP_BATTERY_ESC_ENABLED