/* * 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 /* ---- 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); }