sl-perception
410ace3540
Add coulomb counter for accurate SoC estimation independent of load: - New coulomb_counter module: integrate current over time to track Ah consumed * coulomb_counter_init(capacity_mah) initializes with battery capacity * coulomb_counter_accumulate(current_ma) integrates current at 100 Hz * coulomb_counter_get_soc_pct() returns SoC 0-100% (255 = invalid) * coulomb_counter_reset() for charge-complete reset - Battery module integration: * battery_accumulate_coulombs() reads motor INA219 currents and accumulates * battery_get_soc_coulomb() returns coulomb-based SoC with fallback to voltage * Initialize coulomb counter at startup with DEFAULT_BATTERY_CAPACITY_MAH - Telemetry updates: * JLink STATUS: use coulomb SoC if available, fallback to voltage-based * CRSF battery frame: now includes remaining capacity in mAh (from coulomb counter) * CRSF capacity field was always 0; now reflects actual remaining mAh - Mainloop integration: * Call battery_accumulate_coulombs() every tick for continuous integration * INA219 motor currents + 200 mA subsystem baseline = total battery draw Motor current sources (INA219 addresses 0x40/0x41) provide most power draw; Jetson ROS2 battery_node already prioritizes coulomb-based soc_pct from STATUS frame. Default capacity: 2200 mAh (typical lab 3S LiPo); configurable via firmware parameter. Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
119 lines
3.7 KiB
C
119 lines
3.7 KiB
C
/*
|
||
* 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;
|
||
}
|