seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: Battery voltage telemetry + LVC (Issue #613) #626

Merged
sl-jetson merged 1 commits from sl-firmware/issue-613-battery-voltage into main 2026-03-15 13:29:32 -04:00
Conversation 0 Commits 1 Files Changed 4 +232
4 changed files with 232 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/config.h
View File

@ -262,4 +262,13 @@
#define CAN_RPM_SCALE 10 // motor_cmd to RPM: 1 cmd count = 10 RPM
#define CAN_TLM_HZ 1u // JLINK_TLM_CAN_STATS transmit rate (Hz)
// --- LVC: Low Voltage Cutoff (Issue #613) ---
// 3-stage undervoltage protection; voltages in mV
#define LVC_WARNING_MV 21000u // 21.0 V -- buzzer alert, full power
#define LVC_CRITICAL_MV 19800u // 19.8 V -- 50% motor power reduction
#define LVC_CUTOFF_MV 18600u // 18.6 V -- motors disabled, latch until reboot
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz)
#endif // CONFIG_H

39
include/lvc.h Normal file
View File

@ -0,0 +1,39 @@
#ifndef LVC_H
#define LVC_H
#include <stdint.h>
#include <stdbool.h>
/*
* lvc.h -- Low Voltage Cutoff (LVC) protection (Issue #613)
*
* 3-stage battery voltage protection using battery_read_mv():
*
* LVC_WARNING (21.0 V) -- periodic buzzer alert; full power maintained
* LVC_CRITICAL (19.8 V) -- faster buzzer; motor commands scaled to 50%
* LVC_CUTOFF (18.6 V) -- error buzzer; motors disabled; latched until reboot
*
* Recovery uses LVC_HYSTERESIS_MV to prevent threshold chatter.
* CUTOFF is one-way: once latched, only a power-cycle clears it.
*
* Integration:
* lvc_init() -- call once during system init
* lvc_tick(now_ms, vbat_mv) -- call each main loop tick (1 kHz)
* lvc_get_power_scale() -- returns 0/50/100; apply to motor speed
* lvc_is_cutoff() -- true when motors must be disabled
*/
typedef enum {
LVC_NORMAL = 0, /* Vbat >= WARNING threshold */
LVC_WARNING = 1, /* Vbat < 21.0 V -- alert only */
LVC_CRITICAL = 2, /* Vbat < 19.8 V -- 50% power */
LVC_CUTOFF = 3, /* Vbat < 18.6 V -- motors off */
} LvcState;
void lvc_init(void);
void lvc_tick(uint32_t now_ms, uint32_t vbat_mv);
LvcState lvc_get_state(void);
uint8_t lvc_get_power_scale(void); /* 100 = full, 50 = critical, 0 = cutoff */
bool lvc_is_cutoff(void);
#endif /* LVC_H */

144
src/lvc.c Normal file
View File

@ -0,0 +1,144 @@
/*
* lvc.c -- Low Voltage Cutoff (LVC) protection (Issue #613)
*
* State machine:
* NORMAL -> WARNING when vbat < LVC_WARNING_MV
* WARNING -> CRITICAL when vbat < LVC_CRITICAL_MV
* CRITICAL -> CUTOFF when vbat < LVC_CUTOFF_MV
*
* Recovery: step down severity only when voltage exceeds the threshold
* by LVC_HYSTERESIS_MV. CUTOFF is latched for the remainder of the session.
*
* Buzzer alerts:
* WARNING -- MELODY_LOW_BATTERY once, then every 30 s
* CRITICAL -- MELODY_LOW_BATTERY x2, then every 10 s
* CUTOFF -- MELODY_ERROR (one-shot; motor disable handled by main.c)
*/
#include "lvc.h"
#include "buzzer.h"
#include "config.h"
/* Periodic buzzer reminder intervals */
#define LVC_WARN_INTERVAL_MS 30000u /* 30 s */
#define LVC_CRIT_INTERVAL_MS 10000u /* 10 s */
static LvcState s_state = LVC_NORMAL;
static bool s_cutoff_latched = false;
static uint32_t s_buzzer_tick = 0u;
/* ---- lvc_init() ---- */
void lvc_init(void)
{
s_state = LVC_NORMAL;
s_cutoff_latched = false;
s_buzzer_tick = 0u;
}
/* ---- lvc_tick() ---- */
void lvc_tick(uint32_t now_ms, uint32_t vbat_mv)
{
if (vbat_mv == 0u) {
return; /* ADC not ready; hold current state */
}
/* Determine new state from raw voltage */
LvcState new_state;
if (vbat_mv < LVC_CUTOFF_MV) {
new_state = LVC_CUTOFF;
} else if (vbat_mv < LVC_CRITICAL_MV) {
new_state = LVC_CRITICAL;
} else if (vbat_mv < LVC_WARNING_MV) {
new_state = LVC_WARNING;
} else {
new_state = LVC_NORMAL;
}
/* Hysteresis on recovery: only decrease severity when voltage exceeds
* the threshold by LVC_HYSTERESIS_MV to prevent rapid toggling. */
if (new_state < s_state) {
LvcState recovered;
if (vbat_mv >= LVC_CUTOFF_MV + LVC_HYSTERESIS_MV) {
recovered = LVC_CRITICAL;
} else if (vbat_mv >= LVC_CRITICAL_MV + LVC_HYSTERESIS_MV) {
recovered = LVC_WARNING;
} else if (vbat_mv >= LVC_WARNING_MV + LVC_HYSTERESIS_MV) {
recovered = LVC_NORMAL;
} else {
recovered = s_state; /* insufficient margin; stay at current level */
}
new_state = recovered;
}
/* CUTOFF latch: once triggered, only a reboot clears it */
if (s_cutoff_latched) {
new_state = LVC_CUTOFF;
}
if (new_state == LVC_CUTOFF) {
s_cutoff_latched = true;
}
/* Buzzer alerts */
bool state_changed = (new_state != s_state);
s_state = new_state;
switch (s_state) {
case LVC_WARNING:
if (state_changed ||
(now_ms - s_buzzer_tick) >= LVC_WARN_INTERVAL_MS) {
s_buzzer_tick = now_ms;
buzzer_play_melody(MELODY_LOW_BATTERY);
}
break;
case LVC_CRITICAL:
if (state_changed ||
(now_ms - s_buzzer_tick) >= LVC_CRIT_INTERVAL_MS) {
s_buzzer_tick = now_ms;
/* Double alert to distinguish critical from warning */
buzzer_play_melody(MELODY_LOW_BATTERY);
buzzer_play_melody(MELODY_LOW_BATTERY);
}
break;
case LVC_CUTOFF:
if (state_changed) {
/* One-shot alarm; motors disabled by main.c */
buzzer_play_melody(MELODY_ERROR);
}
break;
default:
break;
}
}
/* ---- lvc_get_state() ---- */
LvcState lvc_get_state(void)
{
return s_state;
}
/* ---- lvc_get_power_scale() ---- */
/*
* Returns the motor power scale factor (0-100):
* NORMAL / WARNING : 100% -- no reduction
* CRITICAL : 50% -- halve motor commands
* CUTOFF : 0% -- all commands zeroed
*/
uint8_t lvc_get_power_scale(void)
{
switch (s_state) {
case LVC_NORMAL: /* fall-through */
case LVC_WARNING: return 100u;
case LVC_CRITICAL: return 50u;
case LVC_CUTOFF: return 0u;
default: return 100u;
}
}
/* ---- lvc_is_cutoff() ---- */
bool lvc_is_cutoff(void)
{
return s_cutoff_latched;
}

40
src/main.c
View File

@ -35,6 +35,7 @@
#include "can_driver.h"
#include "servo_bus.h"
#include "gimbal.h"
#include "lvc.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
@ -257,6 +258,9 @@ int main(void) {
/* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */
battery_init();
/* Init LVC: low voltage cutoff state machine (Issue #613) */
lvc_init();
/* Probe I2C1 for optional sensors — skip gracefully if not found */
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
mag_type_t mag_type = MAG_NONE;
@ -288,6 +292,7 @@ int main(void) {
uint32_t pm_tlm_tick = 0; /* JLINK_TLM_POWER transmit timer */
uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */
uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
uint32_t lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
@ -320,6 +325,9 @@ int main(void) {
/* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
battery_accumulate_coulombs();
/* LVC: update low-voltage protection state machine (Issue #613) */
lvc_tick(now, battery_read_mv());
/* 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++;
@ -600,6 +608,13 @@ int main(void) {
motor_driver_estop_clear(&motors);
}
/* LVC cutoff: disarm and estop on undervoltage latch (Issue #613) */
if (lvc_is_cutoff() && bal.state == BALANCE_ARMED) {
safety_arm_cancel();
balance_disarm(&bal);
motor_driver_estop(&motors);
}
/* Feed autonomous steer from Jetson into mode manager.
* jlink takes priority over legacy CDC jetson_cmd.
* mode_manager_get_steer() blends it with RC steer per active mode. */
@ -616,6 +631,11 @@ int main(void) {
int16_t steer = mode_manager_get_steer(&mode);
int16_t spd_bias = mode_manager_get_speed_bias(&mode);
int32_t speed = (int32_t)bal.motor_cmd + spd_bias;
/* LVC power scaling: 100% normal, 50% critical, 0% cutoff (Issue #613) */
uint8_t lvc_scale = lvc_get_power_scale();
if (lvc_scale < 100u) {
speed = (speed * (int32_t)lvc_scale) / 100;
}
if (speed > 1000) speed = 1000;
if (speed < -1000) speed = -1000;
motor_driver_update(&motors, (int16_t)speed, steer, now);
@ -728,6 +748,26 @@ int main(void) {
jlink_send_power_telemetry(&pow);
}
/* JLINK_TLM_LVC telemetry at LVC_TLM_HZ (1 Hz) -- battery voltage + protection state (Issue #613) */
if (now - lvc_tlm_tick >= (1000u / LVC_TLM_HZ)) {
lvc_tlm_tick = now;
uint32_t lvc_vbat = battery_read_mv();
jlink_tlm_lvc_t ltlm;
ltlm.voltage_mv = (lvc_vbat > 65535u) ? 65535u : (uint16_t)lvc_vbat;
ltlm.protection_state = (uint8_t)lvc_get_state();
if (lvc_vbat == 0u) {
ltlm.percent = 255u;
} else if (lvc_vbat <= LVC_CUTOFF_MV) {
ltlm.percent = 0u;
} else if (lvc_vbat >= LVC_WARNING_MV) {
ltlm.percent = 100u;
} else {
ltlm.percent = (uint8_t)(((lvc_vbat - LVC_CUTOFF_MV) * 100u) /
(LVC_WARNING_MV - LVC_CUTOFF_MV));
}
jlink_send_lvc_tlm(&ltlm);
}
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
send_tick = now;