Compare commits
No commits in common. "34c7af38b28b2dd6e7c3fd0177d696150d259889" and "b04fd916ff9b467669ade6e47b91cd392e145b3f" have entirely different histories.
34c7af38b2
...
b04fd916ff
Binary file not shown.
BIN
.pio/build/f722/.sconsign39.dblite
Normal file
BIN
.pio/build/f722/.sconsign39.dblite
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
.pio/build/f722/lib737/USB_CDC/usbd_cdc_if.o
Normal file
BIN
.pio/build/f722/lib737/USB_CDC/usbd_cdc_if.o
Normal file
Binary file not shown.
BIN
.pio/build/f722/lib737/USB_CDC/usbd_conf.o
Normal file
BIN
.pio/build/f722/lib737/USB_CDC/usbd_conf.o
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
.pio/build/f722/src/main.o
Normal file
BIN
.pio/build/f722/src/main.o
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1 +1 @@
|
||||
8700a44a6597bcade0f371945c539630ba0e78b1
|
||||
ee8efb31f6b185f16e4d385971f1a0e3291fe5fd
|
||||
@ -32,18 +32,4 @@ 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 */
|
||||
|
||||
@ -1,45 +0,0 @@
|
||||
#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 */
|
||||
@ -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)
|
||||
* capacity_mah : remaining battery capacity in mAh (Issue #325, coulomb counter)
|
||||
* current_ma : current draw in milliamps (0 if no sensor)
|
||||
* 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)
|
||||
* capacity unit: mAh (3-byte big-endian, max 16.7M mAh)
|
||||
* current unit: 100 mA
|
||||
*/
|
||||
void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
|
||||
void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
|
||||
uint8_t remaining_pct);
|
||||
|
||||
/*
|
||||
|
||||
@ -9,18 +9,11 @@
|
||||
*/
|
||||
|
||||
#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();
|
||||
@ -55,10 +48,6 @@ 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;
|
||||
}
|
||||
|
||||
@ -76,7 +65,7 @@ uint32_t battery_read_mv(void) {
|
||||
}
|
||||
|
||||
/*
|
||||
* Coarse SoC estimate (voltage-based fallback).
|
||||
* Coarse SoC estimate.
|
||||
* 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
|
||||
*/
|
||||
@ -98,34 +87,3 @@ 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();
|
||||
}
|
||||
|
||||
@ -1,118 +0,0 @@
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
17
src/crsf.c
17
src/crsf.c
@ -320,21 +320,18 @@ 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)
|
||||
* capacity_mah → remaining capacity in mAh (Issue #325, coulomb counter)
|
||||
* remaining_pct→ 0–100 % (uint8)
|
||||
* current_ma → units of 100 mA (big-endian uint16)
|
||||
* remaining_pct→ 0–100 % (uint8); capacity mAh always 0 (no coulomb counter)
|
||||
*/
|
||||
void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
|
||||
void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
|
||||
uint8_t remaining_pct) {
|
||||
uint16_t v100 = (uint16_t)(voltage_mv / 100u); /* 100 mV units */
|
||||
/* Convert capacity (mAh) to 3-byte big-endian: cap_hi, cap_mid, cap_lo */
|
||||
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] */
|
||||
uint16_t c100 = (uint16_t)(current_ma / 100u); /* 100 mA units */
|
||||
/* Payload: [v_hi][v_lo][c_hi][c_lo][cap_hi][cap_mid][cap_lo][remaining] */
|
||||
uint8_t payload[8] = {
|
||||
(uint8_t)(v100 >> 8), (uint8_t)(v100 & 0xFF),
|
||||
0, 0, /* current: not available on STM32, always 0 for now */
|
||||
(uint8_t)((cap >> 16) & 0xFF), /* cap_hi */
|
||||
(uint8_t)((cap >> 8) & 0xFF), /* cap_mid */
|
||||
(uint8_t)(cap & 0xFF), /* cap_lo */
|
||||
(uint8_t)(c100 >> 8), (uint8_t)(c100 & 0xFF),
|
||||
0, 0, 0, /* capacity mAh — not tracked */
|
||||
remaining_pct,
|
||||
};
|
||||
uint8_t frame[CRSF_MAX_FRAME_LEN];
|
||||
|
||||
16
src/main.c
16
src/main.c
@ -26,7 +26,6 @@
|
||||
#include "ultrasonic.h"
|
||||
#include "power_mgmt.h"
|
||||
#include "battery.h"
|
||||
#include "coulomb_counter.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
@ -232,9 +231,6 @@ 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++;
|
||||
@ -461,12 +457,8 @@ int main(void) {
|
||||
if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) {
|
||||
crsf_telem_tick = now;
|
||||
uint32_t vbat_mv = battery_read_mv();
|
||||
/* Use coulomb-based SoC if available, fallback to voltage-based */
|
||||
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);
|
||||
uint8_t soc_pct = battery_estimate_pct(vbat_mv);
|
||||
crsf_send_battery(vbat_mv, 0u, soc_pct);
|
||||
crsf_send_flight_mode(bal.state == BALANCE_ARMED);
|
||||
}
|
||||
|
||||
@ -487,9 +479,7 @@ int main(void) {
|
||||
tlm.mode = (uint8_t)mode_manager_active(&mode);
|
||||
EstopSource _es = safety_get_estop();
|
||||
tlm.estop = (uint8_t)_es;
|
||||
/* 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.soc_pct = battery_estimate_pct(vbat);
|
||||
tlm.fw_major = FW_MAJOR;
|
||||
tlm.fw_minor = FW_MINOR;
|
||||
tlm.fw_patch = FW_PATCH;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user