sl-firmware
fa75c442a7
Archive STM32 firmware to legacy/stm32/: - src/, include/, lib/USB_CDC/, platformio.ini, test stubs, flash_firmware.py - test/test_battery_adc.c, test_hw_button.c, test_pid_schedule.c, test_vesc_can.c, test_can_watchdog.c - USB_CDC_BUG.md Rename: stm32_protocol → esp32_protocol, mamba_protocol → balance_protocol, stm32_cmd_node → esp32_cmd_node, stm32_cmd_params → esp32_cmd_params, stm32_cmd.launch.py → esp32_cmd.launch.py, test_stm32_protocol → test_esp32_protocol, test_stm32_cmd_node → test_esp32_cmd_node Content cleanup across all files: - Mamba F722S → ESP32-S3 BALANCE - BlackPill → ESP32-S3 IO - STM32F722/F7xx → ESP32-S3 - stm32Mode/Version/Port → esp32Mode/Version/Port - STM32 State/Mode labels → ESP32 State/Mode - Jetson Nano → Jetson Orin Nano Super - /dev/stm32 → /dev/esp32 - stm32_bridge → esp32_bridge - STM32 HAL → ESP-IDF docs/SALTYLAB.md: - Update "Drone FC Details" to describe ESP32-S3 BALANCE board (Waveshare ESP32-S3 Touch LCD 1.28) - Replace verbose "Self-Balancing Control" STM32 section with brief note pointing to SAUL-TEE-SYSTEM-REFERENCE.md TEAM.md: Update Embedded Firmware Engineer role to ESP32-S3 / ESP-IDF No new functionality — cleanup only. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
132 lines
4.3 KiB
C
132 lines
4.3 KiB
C
/*
|
||
* battery.c — Vbat ADC reading for CRSF telemetry uplink (Issue #103)
|
||
*
|
||
* Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722S FC).
|
||
* Voltage divider: 10 kΩ (upper) / 1 kΩ (lower) → VBAT_SCALE_NUM = 11.
|
||
*
|
||
* Vbat_mV = (raw × VBAT_AREF_MV × VBAT_SCALE_NUM) >> VBAT_ADC_BITS
|
||
* = (raw × 3300 × 11) / 4096
|
||
*/
|
||
|
||
#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();
|
||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||
|
||
/* PC1 → analog input (no pull, no speed) */
|
||
GPIO_InitTypeDef gpio = {0};
|
||
gpio.Pin = GPIO_PIN_1;
|
||
gpio.Mode = GPIO_MODE_ANALOG;
|
||
gpio.Pull = GPIO_NOPULL;
|
||
HAL_GPIO_Init(GPIOC, &gpio);
|
||
|
||
/* ADC3 — single-conversion, software trigger, 12-bit right-aligned */
|
||
s_hadc.Instance = ADC3;
|
||
s_hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; /* APB2/8 */
|
||
s_hadc.Init.Resolution = ADC_RESOLUTION_12B;
|
||
s_hadc.Init.ScanConvMode = DISABLE;
|
||
s_hadc.Init.ContinuousConvMode = DISABLE;
|
||
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 = 1;
|
||
s_hadc.Init.DMAContinuousRequests = DISABLE;
|
||
s_hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
|
||
if (HAL_ADC_Init(&s_hadc) != HAL_OK) return;
|
||
|
||
/* Channel IN11 (PC1) with 480-cycle sampling for stability */
|
||
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;
|
||
|
||
/* Initialize coulomb counter with default battery capacity */
|
||
coulomb_counter_init(DEFAULT_BATTERY_CAPACITY_MAH);
|
||
s_coulomb_valid = true;
|
||
|
||
s_ready = true;
|
||
}
|
||
|
||
uint32_t battery_read_mv(void) {
|
||
if (!s_ready) return 0u;
|
||
|
||
HAL_ADC_Start(&s_hadc);
|
||
if (HAL_ADC_PollForConversion(&s_hadc, 2u) != HAL_OK) return 0u;
|
||
uint32_t raw = HAL_ADC_GetValue(&s_hadc);
|
||
HAL_ADC_Stop(&s_hadc);
|
||
|
||
/* Vbat_mV = raw × (VREF_mV × scale) / ADC_counts */
|
||
return (raw * (uint32_t)VBAT_AREF_MV * VBAT_SCALE_NUM) /
|
||
((1u << VBAT_ADC_BITS));
|
||
}
|
||
|
||
/*
|
||
* 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
|
||
*/
|
||
uint8_t battery_estimate_pct(uint32_t voltage_mv) {
|
||
uint32_t v_min_mv, v_max_mv;
|
||
|
||
if (voltage_mv >= 13000u) {
|
||
/* 4S LiPo */
|
||
v_min_mv = 13200u;
|
||
v_max_mv = 16800u;
|
||
} else {
|
||
/* 3S LiPo */
|
||
v_min_mv = 9900u;
|
||
v_max_mv = 12600u;
|
||
}
|
||
|
||
if (voltage_mv <= v_min_mv) return 0u;
|
||
if (voltage_mv >= v_max_mv) return 100u;
|
||
|
||
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();
|
||
}
|