Merge pull request 'feat: battery coulomb counter (Issue #325)' (#378) from sl-perception/issue-325-battery-coulomb into main

.pio/build/project.checksum

@@ -1 +1 @@
-ee8efb31f6b185f16e4d385971f1a0e3291fe5fd
+8700a44a6597bcade0f371945c539630ba0e78b1

include/battery.h

@@ -32,4 +32,18 @@ uint32_t battery_read_mv(void);
  */
 uint8_t battery_estimate_pct(uint32_t voltage_mv);
 
+/*
+ * battery_accumulate_coulombs() — periodically integrate battery current.
+ * Call every 10-20 ms (50-100 Hz) from main loop to accumulate coulombs.
+ * Reads motor currents from INA219 sensors.
+ */
+void battery_accumulate_coulombs(void);
+
+/*
+ * battery_get_soc_coulomb() — get coulomb-based SoC estimate.
+ * Returns 0–100 (percent), or 255 if coulomb counter not yet valid.
+ * Preferred over voltage-based when valid.
+ */
+uint8_t battery_get_soc_coulomb(void);
+
 #endif /* BATTERY_H */

include/coulomb_counter.h

@@ -0,0 +1,45 @@
+#ifndef COULOMB_COUNTER_H
+#define COULOMB_COUNTER_H
+
+/*
+ * coulomb_counter.h — Battery coulomb counter for SoC estimation (Issue #325)
+ *
+ * Integrates battery current over time to track Ah consumed and remaining.
+ * Provides accurate SoC independent of load, with fallback to voltage.
+ *
+ * Usage:
+ *   1. Call coulomb_counter_init(capacity_mah) at startup
+ *   2. Call coulomb_counter_accumulate(current_ma) at 50–100 Hz
+ *   3. Call coulomb_counter_get_soc_pct() to get current SoC
+ *   4. Call coulomb_counter_reset() on charge complete
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Initialize coulomb counter with battery capacity (mAh). */
+void coulomb_counter_init(uint16_t capacity_mah);
+
+/*
+ * Accumulate coulomb from current reading + elapsed time.
+ * Call this at regular intervals (e.g., 50–100 Hz from telemetry loop).
+ * current_ma: battery current in milliamps (positive = discharge)
+ */
+void coulomb_counter_accumulate(int16_t current_ma);
+
+/* Get current SoC as percentage (0–100, 255 = error). */
+uint8_t coulomb_counter_get_soc_pct(void);
+
+/* Get consumed mAh (total charge removed from battery). */
+uint16_t coulomb_counter_get_consumed_mah(void);
+
+/* Get remaining capacity in mAh. */
+uint16_t coulomb_counter_get_remaining_mah(void);
+
+/* Reset accumulated coulombs (e.g., on charge complete). */
+void coulomb_counter_reset(void);
+
+/* Check if coulomb counter is active (initialized and has measurements). */
+bool coulomb_counter_is_valid(void);
+
+#endif /* COULOMB_COUNTER_H */

include/crsf.h

@@ -45,14 +45,14 @@ int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max);
  * back to the ELRS TX module over UART4 TX.  Call at CRSF_TELEMETRY_HZ (1 Hz).
  *
  *   voltage_mv   : battery voltage in millivolts (e.g. 12600 for 3S full)
- *   current_ma   : current draw in milliamps     (0 if no sensor)
+ *   capacity_mah : remaining battery capacity in mAh (Issue #325, coulomb counter)
  *   remaining_pct: state-of-charge 0–100 %       (255 = unknown)
  *
  * Frame: [0xC8][12][0x08][v16_hi][v16_lo][c16_hi][c16_lo][cap24×3][rem][CRC]
  * voltage unit: 100 mV  (12600 mV → 126)
- * current unit: 100 mA
+ * capacity unit: mAh (3-byte big-endian, max 16.7M mAh)
  */
-void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
+void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
                        uint8_t remaining_pct);
 
 /*

src/battery.c

@@ -9,11 +9,18 @@
  */
 
 #include "battery.h"
+#include "coulomb_counter.h"
 #include "config.h"
 #include "stm32f7xx_hal.h"
+#include "ina219.h"
+#include <stdbool.h>
 
 static ADC_HandleTypeDef s_hadc;
 static bool              s_ready = false;
+static bool              s_coulomb_valid = false;
+
+/* Default battery capacity: 2200 mAh (typical lab 3S LiPo) */
+#define DEFAULT_BATTERY_CAPACITY_MAH 2200u
 
 void battery_init(void) {
     __HAL_RCC_ADC3_CLK_ENABLE();
@@ -48,6 +55,10 @@ void battery_init(void) {
     ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
     if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
 
+    /* Initialize coulomb counter with default battery capacity */
+    coulomb_counter_init(DEFAULT_BATTERY_CAPACITY_MAH);
+    s_coulomb_valid = true;
+
     s_ready = true;
 }
 
@@ -65,7 +76,7 @@ uint32_t battery_read_mv(void) {
 }
 
 /*
- * Coarse SoC estimate.
+ * Coarse SoC estimate (voltage-based fallback).
  * 3S LiPo: 9.9 V (0%) – 12.6 V (100%) — detect by Vbat < 13 V
  * 4S LiPo: 13.2 V (0%) – 16.8 V (100%) — detect by Vbat ≥ 13 V
  */
@@ -87,3 +98,34 @@ uint8_t battery_estimate_pct(uint32_t voltage_mv) {
 
     return (uint8_t)(((voltage_mv - v_min_mv) * 100u) / (v_max_mv - v_min_mv));
 }
+
+/*
+ * battery_accumulate_coulombs() — call periodically (50-100 Hz) to track
+ * battery current and integrate coulombs. Reads motor currents via INA219.
+ */
+void battery_accumulate_coulombs(void) {
+    if (!s_coulomb_valid) return;
+
+    /* Sum left + right motor currents as proxy for battery draw
+     * (simple approach; doesn't include subsystem drain like OSD, audio) */
+    int16_t left_ma = 0, right_ma = 0;
+    ina219_read_current_ma(INA219_LEFT_MOTOR, &left_ma);
+    ina219_read_current_ma(INA219_RIGHT_MOTOR, &right_ma);
+
+    /* Total battery current ≈ motors + subsystem baseline (~200 mA) */
+    int16_t total_ma = left_ma + right_ma + 200;
+
+    /* Accumulate to coulomb counter */
+    coulomb_counter_accumulate(total_ma);
+}
+
+/*
+ * battery_get_soc_coulomb() — get coulomb-based SoC (0-100, 255=invalid).
+ * Preferred over voltage-based when available.
+ */
+uint8_t battery_get_soc_coulomb(void) {
+    if (!s_coulomb_valid || !coulomb_counter_is_valid()) {
+        return 255; /* Invalid */
+    }
+    return coulomb_counter_get_soc_pct();
+}

src/coulomb_counter.c

@@ -0,0 +1,118 @@
+/*
+ * coulomb_counter.c — Battery coulomb counter (Issue #325)
+ *
+ * Tracks Ah consumed from current readings, provides SoC independent of load.
+ * Time integration: consumed_mah += current_ma * dt_ms / 3600000
+ */
+
+#include "coulomb_counter.h"
+#include "stm32f7xx_hal.h"
+
+/* State structure */
+static struct {
+    bool     initialized;
+    bool     valid;                /* At least one measurement taken */
+    uint16_t capacity_mah;         /* Battery capacity in mAh */
+    uint32_t accumulated_mah_x100; /* Accumulated coulombs in mAh×100 (fixed-point) */
+    uint32_t last_tick_ms;         /* Last update timestamp (ms) */
+} s_state = {0};
+
+void coulomb_counter_init(uint16_t capacity_mah) {
+    if (capacity_mah == 0 || capacity_mah > 20000) {
+        /* Sanity check: reasonable battery is 100–20000 mAh */
+        return;
+    }
+
+    s_state.capacity_mah          = capacity_mah;
+    s_state.accumulated_mah_x100  = 0;
+    s_state.last_tick_ms          = HAL_GetTick();
+    s_state.initialized           = true;
+    s_state.valid                 = false;
+}
+
+void coulomb_counter_accumulate(int16_t current_ma) {
+    if (!s_state.initialized) return;
+
+    uint32_t now_ms = HAL_GetTick();
+    uint32_t dt_ms  = now_ms - s_state.last_tick_ms;
+
+    /* Handle tick wraparound (~49.7 days at 32-bit ms) */
+    if (dt_ms > 86400000UL) {
+        /* If jump > 1 day, likely wraparound; skip this sample */
+        s_state.last_tick_ms = now_ms;
+        return;
+    }
+
+    /* Prevent negative dt or dt=0 */
+    if (dt_ms == 0) return;
+    if (dt_ms > 1000) {
+        /* Cap to 1 second max per call to prevent overflow */
+        dt_ms = 1000;
+    }
+
+    /* Accumulate: mAh += mA × dt_ms / 3600000
+     * Using fixed-point (×100): accumulated_mah_x100 += mA × dt_ms / 36000 */
+    int32_t coulomb_x100 = (int32_t)current_ma * (int32_t)dt_ms / 36000;
+
+    /* Only accumulate if discharging (positive current) or realistic charging */
+    if (coulomb_x100 > 0) {
+        s_state.accumulated_mah_x100 += (uint32_t)coulomb_x100;
+    } else if (coulomb_x100 < 0 && s_state.accumulated_mah_x100 > 0) {
+        /* Allow charging (negative current) to reduce accumulated coulombs */
+        int32_t new_val = (int32_t)s_state.accumulated_mah_x100 + coulomb_x100;
+        if (new_val < 0) {
+            s_state.accumulated_mah_x100 = 0;
+        } else {
+            s_state.accumulated_mah_x100 = (uint32_t)new_val;
+        }
+    }
+
+    /* Clamp to capacity */
+    if (s_state.accumulated_mah_x100 > (uint32_t)s_state.capacity_mah * 100) {
+        s_state.accumulated_mah_x100 = (uint32_t)s_state.capacity_mah * 100;
+    }
+
+    s_state.last_tick_ms = now_ms;
+    s_state.valid        = true;
+}
+
+uint8_t coulomb_counter_get_soc_pct(void) {
+    if (!s_state.valid) return 255; /* 255 = invalid/not measured */
+
+    /* SoC = 100 - (consumed_mah / capacity_mah) * 100 */
+    uint32_t consumed_mah = s_state.accumulated_mah_x100 / 100;
+
+    if (consumed_mah >= s_state.capacity_mah) {
+        return 0; /* Fully discharged */
+    }
+
+    uint32_t remaining_mah = s_state.capacity_mah - consumed_mah;
+    uint8_t soc = (uint8_t)((remaining_mah * 100u) / s_state.capacity_mah);
+
+    return soc;
+}
+
+uint16_t coulomb_counter_get_consumed_mah(void) {
+    return (uint16_t)(s_state.accumulated_mah_x100 / 100);
+}
+
+uint16_t coulomb_counter_get_remaining_mah(void) {
+    if (!s_state.valid) return s_state.capacity_mah;
+
+    uint32_t consumed = s_state.accumulated_mah_x100 / 100;
+    if (consumed >= s_state.capacity_mah) {
+        return 0;
+    }
+    return (uint16_t)(s_state.capacity_mah - consumed);
+}
+
+void coulomb_counter_reset(void) {
+    if (!s_state.initialized) return;
+
+    s_state.accumulated_mah_x100 = 0;
+    s_state.last_tick_ms         = HAL_GetTick();
+}
+
+bool coulomb_counter_is_valid(void) {
+    return s_state.valid;
+}

src/crsf.c

@@ -320,18 +320,21 @@ static uint8_t crsf_build_frame(uint8_t *buf, uint8_t frame_type,
 /*
  * crsf_send_battery() — type 0x08 battery sensor.
  * voltage_mv   → units of 100 mV (big-endian uint16)
- * current_ma   → units of 100 mA (big-endian uint16)
+ * capacity_mah → remaining capacity in mAh (Issue #325, coulomb counter)
- * remaining_pct→ 0–100 % (uint8); capacity mAh always 0 (no coulomb counter)
+ * remaining_pct→ 0–100 % (uint8)
  */
-void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
+void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
                         uint8_t remaining_pct) {
     uint16_t v100 = (uint16_t)(voltage_mv / 100u);   /* 100 mV units */
-    uint16_t c100 = (uint16_t)(current_ma / 100u);   /* 100 mA units */
+    /* Convert capacity (mAh) to 3-byte big-endian: cap_hi, cap_mid, cap_lo */
-    /* Payload: [v_hi][v_lo][c_hi][c_lo][cap_hi][cap_mid][cap_lo][remaining] */
+    uint32_t cap = capacity_mah & 0xFFFFFFu;  /* 24-bit cap max */
+    /* Payload: [v_hi][v_lo][current_hi][current_lo][cap_hi][cap_mid][cap_lo][remaining] */
     uint8_t payload[8] = {
         (uint8_t)(v100 >> 8), (uint8_t)(v100 & 0xFF),
-        (uint8_t)(c100 >> 8), (uint8_t)(c100 & 0xFF),
+        0, 0,                 /* current: not available on STM32, always 0 for now */
-        0, 0, 0,              /* capacity mAh — not tracked */
+        (uint8_t)((cap >> 16) & 0xFF),  /* cap_hi */
+        (uint8_t)((cap >> 8) & 0xFF),   /* cap_mid */
+        (uint8_t)(cap & 0xFF),          /* cap_lo */
         remaining_pct,
     };
     uint8_t frame[CRSF_MAX_FRAME_LEN];

src/main.c

@@ -26,6 +26,7 @@
 #include "ultrasonic.h"
 #include "power_mgmt.h"
 #include "battery.h"
+#include "coulomb_counter.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -231,6 +232,9 @@ int main(void) {
         /* Servo pan-tilt animation tick — updates smooth sweeps */
         servo_tick(now);
 
+        /* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
+        battery_accumulate_coulombs();
+
         /* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness.
          * pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */
         pm_pwm_phase++;
@@ -457,8 +461,12 @@ int main(void) {
         if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) {
             crsf_telem_tick = now;
             uint32_t vbat_mv  = battery_read_mv();
-            uint8_t  soc_pct  = battery_estimate_pct(vbat_mv);
+            /* Use coulomb-based SoC if available, fallback to voltage-based */
-            crsf_send_battery(vbat_mv, 0u, soc_pct);
+            uint8_t soc_pct = battery_get_soc_coulomb();
+            if (soc_pct == 255) {
+                soc_pct = battery_estimate_pct(vbat_mv);
+            }
+            crsf_send_battery(vbat_mv, coulomb_counter_get_remaining_mah(), soc_pct);
             crsf_send_flight_mode(bal.state == BALANCE_ARMED);
         }
 
@@ -479,7 +487,9 @@ int main(void) {
             tlm.mode         = (uint8_t)mode_manager_active(&mode);
             EstopSource _es  = safety_get_estop();
             tlm.estop        = (uint8_t)_es;
-            tlm.soc_pct      = battery_estimate_pct(vbat);
+            /* Use coulomb-based SoC if available, fallback to voltage-based */
+            uint8_t soc = battery_get_soc_coulomb();
+            tlm.soc_pct = (soc == 255) ? battery_estimate_pct(vbat) : soc;
             tlm.fw_major     = FW_MAJOR;
             tlm.fw_minor     = FW_MINOR;
             tlm.fw_patch     = FW_PATCH;