sl-controls
cc0ffd1999
STM32F7 ADC driver for battery voltage/current monitoring using DMA-based continuous sampling, IIR low-pass filter, voltage divider calibration, and USART telemetry to Jetson. Integrates with power management for low-battery sleep (Issue #467). Implementation: - include/battery_adc.h: New driver header with calibration struct and public API (init, tick, get_voltage_mv, get_current_ma, calibrate, publish, check_pm, is_low, is_critical) - src/battery_adc.c: ADC3 continuous-scan DMA (DMA2_Stream0/Ch2), 4x hardware oversampling of both Vbat (PC1/IN11) and Ibat (PC3/IN13), IIR LPF (alpha=1/8, cutoff ~4 Hz at 100 Hz tick rate), calibration with ±500 mV offset clamp, 3S/4S auto-detection, 1 Hz USART publish - include/jlink.h + src/jlink.c: Add JLINK_TLM_BATTERY (0x82) telemetry type and jlink_tlm_battery_t (10-byte packed struct), implement jlink_send_battery_telemetry() using CRC16-XModem framing - include/power_mgmt.h + src/power_mgmt.c: Add power_mgmt_notify_battery() — triggers STOP-mode sleep when Vbat sustains critical level (Issue #467) - test/test_battery_adc.c: 27 unit tests (27/27 passing): voltage conversion, calibration offset/scale, IIR LPF convergence, SoC estimation (3S/4S), low/critical flags, PM notification timing, calibration reset, publish rate-limiting Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
395 lines
14 KiB
C
395 lines
14 KiB
C
/*
|
||
* battery_adc.c — DMA-based battery voltage/current ADC driver (Issue #533)
|
||
*
|
||
* Voltage divider calibration, DMA-based continuous sampling, IIR low-pass
|
||
* filter, and USART telemetry publish to Jetson via jlink (JLINK_TLM_BATTERY).
|
||
* Integrates with power_mgmt for low-battery sleep (Issue #467).
|
||
*
|
||
* DMA mapping (STM32F7 RM Table 27):
|
||
* DMA2_Stream0 Channel2 → ADC3 (no conflict: Stream2/Ch4 = USART1_RX)
|
||
*
|
||
* ADC3 scan sequence:
|
||
* Rank 1: IN11 (PC1) — Vbat
|
||
* Rank 2: IN13 (PC3) — Ibat
|
||
* Continuous mode, 480-cycle sampling → ~14 kSPS per channel @ 13.5 MHz ADC
|
||
*
|
||
* DMA circular buffer layout (8 × uint16_t, pairs repeat continuously):
|
||
* [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3]
|
||
*
|
||
* On each battery_adc_tick() call the 4 Vbat and 4 Ibat samples are averaged
|
||
* (4× hardware oversampling) then fed into an IIR low-pass filter.
|
||
*/
|
||
|
||
#include "battery_adc.h"
|
||
#include "jlink.h"
|
||
#include "power_mgmt.h"
|
||
#include "config.h"
|
||
#include "stm32f7xx_hal.h"
|
||
#include <string.h>
|
||
|
||
/* ---- DMA buffer ---- */
|
||
#define ADC_DMA_BUF_LEN 8u /* 4 Vbat/Ibat pairs */
|
||
#define ADC_OVERSAMPLE 4u /* pairs per tick average */
|
||
|
||
static volatile uint16_t s_dma_buf[ADC_DMA_BUF_LEN];
|
||
|
||
/* ---- HAL handles ---- */
|
||
static ADC_HandleTypeDef s_hadc;
|
||
static DMA_HandleTypeDef s_hdma;
|
||
|
||
/* ---- Filtered state (Q0 integer, units: raw ADC counts × 8 for sub-LSB) ---- */
|
||
/*
|
||
* IIR accumulator: s_lpf_vbat = filtered Vbat in (raw_counts × 8).
|
||
* Multiply by 8 gives 3 bits of sub-LSB precision with integer arithmetic.
|
||
* On each tick: s_lpf_vbat += (raw_avg - (s_lpf_vbat >> 3)) >> LPF_SHIFT
|
||
* Simplified: s_lpf_vbat8 = s_lpf_vbat8 + (raw_avg_x8 - s_lpf_vbat8) >> SHIFT
|
||
*/
|
||
static uint32_t s_lpf_vbat8 = 0; /* Vbat accumulator × 8 */
|
||
static int32_t s_lpf_ibat8 = 0; /* Ibat accumulator × 8 (signed) */
|
||
|
||
/* ---- Calibrated outputs (updated each tick) ---- */
|
||
static uint32_t s_vbat_mv = 0; /* calibrated LPF Vbat (mV) */
|
||
static int32_t s_ibat_ma = 0; /* calibrated LPF Ibat (mA) */
|
||
static uint32_t s_vbat_raw_mv = 0; /* unfiltered last-tick average (mV) */
|
||
|
||
/* ---- Calibration ---- */
|
||
static battery_adc_cal_t s_cal = {
|
||
.vbat_offset_mv = 0,
|
||
.ibat_offset_ma = 0,
|
||
.vbat_scale_num = 0, /* 0 = use VBAT_SCALE_NUM from config.h */
|
||
.vbat_scale_den = 0, /* 0 = use 1 */
|
||
};
|
||
|
||
/* ---- Low-voltage tracking ---- */
|
||
static uint32_t s_low_since_ms = 0; /* tick when Vbat first went low */
|
||
static bool s_low_active = false; /* currently below LOW_MV threshold */
|
||
static bool s_critical_notified = false; /* pm notified for this event */
|
||
static uint32_t s_low_mv = BATTERY_ADC_LOW_MV;
|
||
static uint32_t s_critical_mv = BATTERY_ADC_CRITICAL_MV;
|
||
|
||
/* ---- Ready flag ---- */
|
||
static bool s_ready = false;
|
||
|
||
/* ---- Telemetry rate limiting ---- */
|
||
static uint32_t s_last_publish_ms = 0;
|
||
static bool s_published_once = false; /* force send on first call */
|
||
|
||
/* ---- Helper: convert raw ADC count to mV ---- */
|
||
/*
|
||
* Apply voltage divider and calibration:
|
||
* vbat_mv = (raw × Vref_mV × scale_num) / (4096 × scale_den)
|
||
* + cal.vbat_offset_mv
|
||
* Default: Vref=3300 mV, scale_num=VBAT_SCALE_NUM=11, scale_den=1.
|
||
*/
|
||
static uint32_t raw_to_vbat_mv(uint32_t raw_counts)
|
||
{
|
||
uint16_t snum = (s_cal.vbat_scale_num != 0u) ? s_cal.vbat_scale_num
|
||
: (uint16_t)VBAT_SCALE_NUM;
|
||
uint16_t sden = (s_cal.vbat_scale_den != 0u) ? s_cal.vbat_scale_den : 1u;
|
||
|
||
/* raw_counts × (3300 × 11) / 4096 — keep 32-bit intermediate */
|
||
uint32_t mv = (raw_counts * (uint32_t)VBAT_AREF_MV * snum) /
|
||
((1u << VBAT_ADC_BITS) * sden);
|
||
|
||
int32_t cal_mv = (int32_t)mv + (int32_t)s_cal.vbat_offset_mv;
|
||
if (cal_mv < 0) cal_mv = 0;
|
||
return (uint32_t)cal_mv;
|
||
}
|
||
|
||
/* ---- Helper: convert raw ADC count to mA ---- */
|
||
/*
|
||
* ADC_IBAT_SCALE = 115 (betaflight ibata_scale units: mA per count × 10).
|
||
* Formula: ibat_ma = raw_counts × ADC_IBAT_SCALE / 10 + cal.ibat_offset_ma
|
||
* Betaflight ibata_scale is defined as: current = raw * scale / (4096 * 10)
|
||
* in units of 100mA per count → ibat_ma = raw × ibata_scale * 100 / 4096
|
||
*
|
||
* With ibata_scale=115: ibat_ma = raw × 115 × 100 / 4096
|
||
* ≈ raw × 2.808 mA/count
|
||
* For raw=4095 (full-scale): ~11,500 mA (11.5 A) — appropriate for 3S robot.
|
||
*/
|
||
static int32_t raw_to_ibat_ma(uint32_t raw_counts)
|
||
{
|
||
int32_t ma = (int32_t)((raw_counts * (uint32_t)ADC_IBAT_SCALE * 100u) /
|
||
(1u << VBAT_ADC_BITS));
|
||
ma += (int32_t)s_cal.ibat_offset_ma;
|
||
return ma;
|
||
}
|
||
|
||
/* ---- battery_adc_init() ---- */
|
||
void battery_adc_init(void)
|
||
{
|
||
/* ---- GPIO: PC1 (Vbat) and PC3 (Ibat) as analog input ---- */
|
||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||
GPIO_InitTypeDef gpio = {0};
|
||
gpio.Pin = GPIO_PIN_1 | GPIO_PIN_3;
|
||
gpio.Mode = GPIO_MODE_ANALOG;
|
||
gpio.Pull = GPIO_NOPULL;
|
||
HAL_GPIO_Init(GPIOC, &gpio);
|
||
|
||
/* ---- DMA2_Stream0 Channel2 → ADC3 (circular, 16-bit, 8 words) ---- */
|
||
__HAL_RCC_DMA2_CLK_ENABLE();
|
||
s_hdma.Instance = DMA2_Stream0;
|
||
s_hdma.Init.Channel = DMA_CHANNEL_2;
|
||
s_hdma.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
||
s_hdma.Init.PeriphInc = DMA_PINC_DISABLE;
|
||
s_hdma.Init.MemInc = DMA_MINC_ENABLE;
|
||
s_hdma.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
|
||
s_hdma.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
|
||
s_hdma.Init.Mode = DMA_CIRCULAR;
|
||
s_hdma.Init.Priority = DMA_PRIORITY_LOW;
|
||
s_hdma.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||
if (HAL_DMA_Init(&s_hdma) != HAL_OK) return;
|
||
|
||
/* ---- ADC3: continuous scan, DMA, 12-bit, 2 channels ---- */
|
||
__HAL_RCC_ADC3_CLK_ENABLE();
|
||
s_hadc.Instance = ADC3;
|
||
s_hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; /* 108/8 ≈ 13.5 MHz */
|
||
s_hadc.Init.Resolution = ADC_RESOLUTION_12B;
|
||
s_hadc.Init.ScanConvMode = ENABLE; /* scan both channels */
|
||
s_hadc.Init.ContinuousConvMode = ENABLE; /* free-running */
|
||
s_hadc.Init.DiscontinuousConvMode = DISABLE;
|
||
s_hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
|
||
s_hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START;
|
||
s_hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
|
||
s_hadc.Init.NbrOfConversion = 2;
|
||
s_hadc.Init.DMAContinuousRequests = ENABLE; /* DMA circular */
|
||
s_hadc.Init.EOCSelection = ADC_EOC_SEQ_CONV;
|
||
__HAL_LINKDMA(&s_hadc, DMA_Handle, s_hdma);
|
||
if (HAL_ADC_Init(&s_hadc) != HAL_OK) return;
|
||
|
||
/* ---- Rank 1: IN11 (PC1) — Vbat ---- */
|
||
ADC_ChannelConfTypeDef ch = {0};
|
||
ch.Channel = ADC_CHANNEL_11;
|
||
ch.Rank = 1;
|
||
ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
|
||
if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
|
||
|
||
/* ---- Rank 2: IN13 (PC3) — Ibat ---- */
|
||
ch.Channel = ADC_CHANNEL_13;
|
||
ch.Rank = 2;
|
||
ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
|
||
if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
|
||
|
||
/* ---- Detect battery cell count at init for correct thresholds ---- */
|
||
/*
|
||
* Note: at init Vbat may not be settled; thresholds are updated after
|
||
* first tick once LPF has stabilised. Set 4S thresholds if voltage
|
||
* detected ≥ 13 V in battery_adc_tick() on first valid reading.
|
||
*/
|
||
|
||
/* ---- Start continuous DMA conversion ---- */
|
||
if (HAL_ADC_Start_DMA(&s_hadc, (uint32_t *)s_dma_buf, ADC_DMA_BUF_LEN)
|
||
!= HAL_OK) return;
|
||
|
||
s_ready = true;
|
||
}
|
||
|
||
/* ---- battery_adc_tick() ---- */
|
||
void battery_adc_tick(uint32_t now_ms)
|
||
{
|
||
if (!s_ready) return;
|
||
(void)now_ms; /* captured for caller context; may be used for future rate ctrl */
|
||
|
||
/* ---- Snapshot DMA buffer with IRQ guard to prevent torn reads ---- */
|
||
uint16_t snap[ADC_DMA_BUF_LEN];
|
||
uint32_t primask = __get_PRIMASK();
|
||
__disable_irq();
|
||
memcpy(snap, (void *)s_dma_buf, sizeof(snap));
|
||
__set_PRIMASK(primask);
|
||
|
||
/* ---- Average 4 Vbat and 4 Ibat samples (interleaved pairs) ---- */
|
||
uint32_t vbat_sum = 0u;
|
||
uint32_t ibat_sum = 0u;
|
||
for (uint32_t i = 0u; i < ADC_DMA_BUF_LEN; i += 2u) {
|
||
vbat_sum += snap[i]; /* even indices: Vbat (rank 1) */
|
||
ibat_sum += snap[i + 1u]; /* odd indices: Ibat (rank 2) */
|
||
}
|
||
uint32_t vbat_avg = vbat_sum / ADC_OVERSAMPLE; /* 0–4095 */
|
||
uint32_t ibat_avg = ibat_sum / ADC_OVERSAMPLE;
|
||
|
||
/* ---- Raw (unfiltered) calibrated voltage — for calibration query ---- */
|
||
s_vbat_raw_mv = raw_to_vbat_mv(vbat_avg);
|
||
|
||
/* ---- IIR low-pass filter ---- */
|
||
/*
|
||
* Accumulator stores value × 8 for sub-LSB precision.
|
||
* Update: acc8 += (raw × 8 - acc8) >> LPF_SHIFT
|
||
* Result: filtered_raw = acc8 >> 3
|
||
*
|
||
* With LPF_SHIFT=3: α = 1/(1+8) ≈ 0.111 → cutoff ≈ 4 Hz at 100 Hz tick.
|
||
*/
|
||
s_lpf_vbat8 += ((vbat_avg << 3u) - s_lpf_vbat8) >> BATTERY_ADC_LPF_SHIFT;
|
||
s_lpf_ibat8 += (((int32_t)ibat_avg << 3u) - s_lpf_ibat8)
|
||
>> BATTERY_ADC_LPF_SHIFT;
|
||
|
||
uint32_t vbat_filtered = s_lpf_vbat8 >> 3u; /* back to ADC counts */
|
||
uint32_t ibat_filtered = (uint32_t)((s_lpf_ibat8 < 0 ? 0 : s_lpf_ibat8) >> 3u);
|
||
|
||
/* ---- Apply calibration and convert to engineering units ---- */
|
||
s_vbat_mv = raw_to_vbat_mv(vbat_filtered);
|
||
s_ibat_ma = raw_to_ibat_ma(ibat_filtered);
|
||
|
||
/* ---- Auto-set thresholds based on detected cell count ---- */
|
||
/*
|
||
* Detect 4S (Vbat ≥ 13 V) vs 3S (<13 V). Only update thresholds once
|
||
* after initial settling (first non-zero filtered reading).
|
||
*/
|
||
static bool s_thresholds_set = false;
|
||
if (!s_thresholds_set && s_vbat_mv > 500u) {
|
||
if (s_vbat_mv >= 13000u) {
|
||
s_low_mv = BATTERY_ADC_LOW_MV_4S;
|
||
s_critical_mv = BATTERY_ADC_CRITICAL_MV_4S;
|
||
}
|
||
/* else keep 3S defaults set at file scope */
|
||
s_thresholds_set = true;
|
||
}
|
||
}
|
||
|
||
/* ---- battery_adc_get_voltage_mv() ---- */
|
||
uint32_t battery_adc_get_voltage_mv(void)
|
||
{
|
||
return s_vbat_mv;
|
||
}
|
||
|
||
/* ---- battery_adc_get_current_ma() ---- */
|
||
int32_t battery_adc_get_current_ma(void)
|
||
{
|
||
return s_ibat_ma;
|
||
}
|
||
|
||
/* ---- battery_adc_get_raw_voltage_mv() ---- */
|
||
uint32_t battery_adc_get_raw_voltage_mv(void)
|
||
{
|
||
return s_vbat_raw_mv;
|
||
}
|
||
|
||
/* ---- battery_adc_calibrate() ---- */
|
||
void battery_adc_calibrate(const battery_adc_cal_t *cal)
|
||
{
|
||
if (cal == NULL) {
|
||
/* Reset to defaults */
|
||
s_cal.vbat_offset_mv = 0;
|
||
s_cal.ibat_offset_ma = 0;
|
||
s_cal.vbat_scale_num = 0;
|
||
s_cal.vbat_scale_den = 0;
|
||
return;
|
||
}
|
||
/* Clamp offsets to prevent runaway calibration values */
|
||
int16_t voff = cal->vbat_offset_mv;
|
||
if (voff > 500) voff = 500;
|
||
if (voff < -500) voff = -500;
|
||
int16_t ioff = cal->ibat_offset_ma;
|
||
if (ioff > 200) ioff = 200;
|
||
if (ioff < -200) ioff = -200;
|
||
s_cal.vbat_offset_mv = voff;
|
||
s_cal.ibat_offset_ma = ioff;
|
||
s_cal.vbat_scale_num = cal->vbat_scale_num;
|
||
s_cal.vbat_scale_den = cal->vbat_scale_den;
|
||
}
|
||
|
||
/* ---- battery_adc_get_calibration() ---- */
|
||
void battery_adc_get_calibration(battery_adc_cal_t *out_cal)
|
||
{
|
||
if (out_cal != NULL) {
|
||
*out_cal = s_cal;
|
||
}
|
||
}
|
||
|
||
/* ---- battery_adc_publish() ---- */
|
||
/*
|
||
* Sends JLINK_TLM_BATTERY (0x82) frame to Jetson at BATTERY_ADC_PUBLISH_HZ.
|
||
*
|
||
* Payload (jlink_tlm_battery_t, 10 bytes packed):
|
||
* vbat_mv uint16 — calibrated LPF voltage (mV)
|
||
* ibat_ma int16 — calibrated LPF current (mA)
|
||
* vbat_raw_mv uint16 — unfiltered last-tick voltage (mV)
|
||
* flags uint8 — bit0=low, bit1=critical, bit2=4S detected, bit3=ready
|
||
* cal_offset int8 — vbat_offset_mv / 4 (compressed, ±127 → ±508 mV)
|
||
* lpf_shift uint8 — BATTERY_ADC_LPF_SHIFT value (for Jetson info)
|
||
* soc_pct uint8 — voltage-based SoC estimate (0-100, 255=unknown)
|
||
*/
|
||
void battery_adc_publish(uint32_t now_ms)
|
||
{
|
||
if (!s_ready) return;
|
||
|
||
uint32_t period_ms = 1000u / BATTERY_ADC_PUBLISH_HZ;
|
||
if (s_published_once && (now_ms - s_last_publish_ms) < period_ms) return;
|
||
s_last_publish_ms = now_ms;
|
||
s_published_once = true;
|
||
|
||
/* Build SoC estimate (voltage-based) */
|
||
uint8_t soc = 255u; /* unknown */
|
||
if (s_vbat_mv > 500u) {
|
||
uint32_t v_min, v_max;
|
||
if (s_vbat_mv >= 13000u) {
|
||
v_min = 13200u; v_max = 16800u; /* 4S */
|
||
} else {
|
||
v_min = 9900u; v_max = 12600u; /* 3S */
|
||
}
|
||
if (s_vbat_mv <= v_min) {
|
||
soc = 0u;
|
||
} else if (s_vbat_mv >= v_max) {
|
||
soc = 100u;
|
||
} else {
|
||
soc = (uint8_t)(((s_vbat_mv - v_min) * 100u) / (v_max - v_min));
|
||
}
|
||
}
|
||
|
||
/* Flags byte */
|
||
uint8_t flags = 0u;
|
||
if (s_vbat_mv > 0u && s_vbat_mv < s_low_mv) flags |= 0x01u;
|
||
if (s_vbat_mv > 0u && s_vbat_mv < s_critical_mv) flags |= 0x02u;
|
||
if (s_low_mv == BATTERY_ADC_LOW_MV_4S) flags |= 0x04u;
|
||
if (s_ready) flags |= 0x08u;
|
||
|
||
/* Build JLINK_TLM_BATTERY frame via jlink_send_battery_telemetry() */
|
||
jlink_tlm_battery_t tlm = {
|
||
.vbat_mv = (uint16_t)(s_vbat_mv & 0xFFFFu),
|
||
.ibat_ma = (int16_t) (s_ibat_ma & 0xFFFF),
|
||
.vbat_raw_mv = (uint16_t)(s_vbat_raw_mv & 0xFFFFu),
|
||
.flags = flags,
|
||
.cal_offset = (int8_t) (s_cal.vbat_offset_mv / 4),
|
||
.lpf_shift = (uint8_t) BATTERY_ADC_LPF_SHIFT,
|
||
.soc_pct = soc,
|
||
};
|
||
jlink_send_battery_telemetry(&tlm);
|
||
}
|
||
|
||
/* ---- battery_adc_check_pm() ---- */
|
||
void battery_adc_check_pm(uint32_t now_ms)
|
||
{
|
||
if (!s_ready || s_vbat_mv == 0u) return;
|
||
|
||
bool below_low = (s_vbat_mv < s_low_mv);
|
||
bool below_critical = (s_vbat_mv < s_critical_mv);
|
||
|
||
/* Track first entry below low threshold */
|
||
if (below_low && !s_low_active) {
|
||
s_low_active = true;
|
||
s_low_since_ms = now_ms;
|
||
s_critical_notified = false;
|
||
} else if (!below_low) {
|
||
s_low_active = false;
|
||
s_critical_notified = false;
|
||
}
|
||
|
||
/* Notify power management on sustained critical voltage */
|
||
if (below_critical && !s_critical_notified &&
|
||
(now_ms - s_low_since_ms) >= BATTERY_ADC_LOW_HOLD_MS) {
|
||
s_critical_notified = true;
|
||
power_mgmt_notify_battery(s_vbat_mv);
|
||
}
|
||
}
|
||
|
||
/* ---- battery_adc_is_low() ---- */
|
||
bool battery_adc_is_low(void)
|
||
{
|
||
return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_low_mv);
|
||
}
|
||
|
||
/* ---- battery_adc_is_critical() ---- */
|
||
bool battery_adc_is_critical(void)
|
||
{
|
||
return (s_ready && s_vbat_mv > 0u && s_vbat_mv < s_critical_mv);
|
||
}
|