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

feat: Battery voltage telemetry and LVC (Issue #613)

Browse Source 
- Add include/lvc.h + src/lvc.c: 3-stage low voltage cutoff state machine
  WARNING  21.0V: MELODY_LOW_BATTERY buzzer, full motor power
  CRITICAL 19.8V: double-beep every 10s, 50% motor power scaling
  CUTOFF   18.6V: MELODY_ERROR one-shot, motors disabled + latched
  200mV hysteresis on recovery; CUTOFF latched until reboot
- Add JLINK_TLM_LVC (0x8B, 4 bytes): voltage_mv, percent, protection_state
  jlink_send_lvc_tlm() frame encoder in jlink.c
- Wire into main.c:
  lvc_init() at startup; lvc_tick() each 1kHz loop tick
  lvc_is_cutoff() triggers safety_arm_cancel + balance_disarm + motor_driver_estop
  lvc_get_power_scale() applied to ESC speed command (100/50/0%)
  1Hz JLINK_TLM_LVC telemetry with fuel-gauge percent field
- Add LVC thresholds to config.h (LVC_WARNING/CRITICAL/CUTOFF/HYSTERESIS_MV)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-firmware 2026-03-15 10:14:57 -04:00
parent a4da93de7e
commit 3e60d7552a
6 changed files with 272 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

16
include/jlink.h
View File

@ -50,6 +50,7 @@
* 0x87 FAULT_LOG - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
* 0x88 SLOPE - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600)
* 0x89 CAN_STATS - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597)
* 0x8B LVC - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613)
*
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@ -93,6 +94,7 @@
#define JLINK_TLM_FAULT_LOG 0x87u /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
#define JLINK_TLM_SLOPE 0x88u /* jlink_tlm_slope_t (4 bytes, Issue #600) */
#define JLINK_TLM_CAN_STATS 0x89u /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */
#define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
@ -204,6 +206,14 @@ typedef struct __attribute__((packed)) {
int16_t vel1_rpm; /* node 1 current velocity (RPM) */
} jlink_tlm_can_stats_t; /* 16 bytes */
/* ---- Telemetry LVC payload (4 bytes, packed) Issue #613 ---- */
/* Sent at LVC_TLM_HZ (1 Hz); reports battery voltage and LVC protection state. */
typedef struct __attribute__((packed)) {
uint16_t voltage_mv; /* battery voltage (mV) */
uint8_t percent; /* 0-100: fuel gauge within CUTOFF..WARNING; 255=unknown */
uint8_t protection_state; /* LvcState: 0=NORMAL,1=WARNING,2=CRITICAL,3=CUTOFF */
} jlink_tlm_lvc_t; /* 4 bytes */
/* ---- Volatile state (read from main loop) ---- */
typedef struct {
/* Drive command - updated on JLINK_CMD_DRIVE */
@ -322,4 +332,10 @@ void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
*/
void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm);
/*
* jlink_send_lvc_tlm(tlm) - transmit JLINK_TLM_LVC (0x8B) frame
* (10 bytes total) at LVC_TLM_HZ (1 Hz). Issue #613.
*/
void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm);
#endif /* JLINK_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 */

24
src/jlink.c
View File

@ -618,3 +618,27 @@ void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm)
jlink_tx_locked(frame, sizeof(frame));
}
/* ---- jlink_send_lvc_tlm() -- Issue #613 ---- */
void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm)
{
/*
* Frame: [STX][LEN][0x8B][4 bytes LVC][CRC_hi][CRC_lo][ETX]
* LEN = 1 + 4 = 5; total = 10 bytes
*/
static uint8_t frame[10];
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_lvc_t); /* 4 */
const uint8_t len = 1u + plen; /* 5 */
frame[0] = JLINK_STX;
frame[1] = len;
frame[2] = JLINK_TLM_LVC;
memcpy(&frame[3], tlm, plen);
uint16_t crc = crc16_xmodem(&frame[2], len);
frame[3 + plen] = (uint8_t)(crc >> 8);
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
frame[3 + plen + 2] = JLINK_ETX;
jlink_tx_locked(frame, sizeof(frame));
}

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;