sl-firmware
7785a16bff
- 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>
145 lines
4.1 KiB
C
145 lines
4.1 KiB
C
/*
|
|
* 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;
|
|
}
|