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

feat(rc): CRSF/ELRS RC integration — telemetry uplink + channel fix (Issue #103)

Browse Source 
## Summary
- config.h: CH1[0]=steer, CH2[1]=throttle (was CH4/CH3); CRSF_FAILSAFE_MS→500ms
- include/battery.h + src/battery.c: ADC3 Vbat reading on PC1 (11:1 divider)
  battery_read_mv(), battery_estimate_pct() for 3S/4S auto-detection
- include/crsf.h + src/crsf.c: CRSF telemetry TX uplink
  crsf_send_battery() — type 0x08, voltage/current/SoC to ELRS TX module
  crsf_send_flight_mode() — type 0x21, "ARMED\0"/"DISARM\0" for handset OSD
- src/main.c: battery_init() after crsf_init(); 1Hz telemetry tick calls
  crsf_send_battery(vbat_mv, 0, soc_pct) + crsf_send_flight_mode(armed)
- test/test_crsf_frames.py: 28 pytest tests — CRC8-DVB-S2, battery frame
  layout/encoding, flight-mode frame, battery_estimate_pct SoC math

Existing (already complete from crsf-elrs branch):
  CRSF frame decoder UART4 420000 baud DMA circular + IDLE interrupt
  Mode manager: RC↔autonomous blend, CH6 3-pos switch, 500ms smooth transition
  Failsafe in main.c: disarm if crsf_state.last_rx_ms stale > CRSF_FAILSAFE_MS
  CH5 arm switch with ARMING_HOLD_MS interlock + edge detection
  RC override: mode_manager blends steer/speed per mode (CH6)

Closes #103

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-firmware 2026-03-02 08:35:39 -05:00
parent 0f2ea7931b
commit 00c97bd902
7 changed files with 476 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
include/battery.h Normal file
View File

@ -0,0 +1,35 @@
#ifndef BATTERY_H
#define BATTERY_H
/*
* battery.h — Vbat ADC reading for CRSF telemetry (Issue #103)
*
* Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722).
* Voltage divider: 10 kΩ / 1 kΩ → 11:1 ratio.
* Resolution: 12-bit (0–4095), Vref = 3.3 V.
*
* Filtered output in millivolts. Reading is averaged over
* BATTERY_SAMPLES conversions (software oversampling) to reduce noise.
*/
#include <stdint.h>
/* Initialise ADC3 for single-channel Vbat reading on PC1. */
void battery_init(void);
/*
* battery_read_mv() — blocking single-shot read; returns Vbat in mV.
* Takes ~1 µs (12-bit conversion at 36 MHz APB2 / 8 prescaler = 4.5 MHz ADC clk).
* Returns 0 if ADC not initialised or conversion times out.
*/
uint32_t battery_read_mv(void);
/*
* battery_estimate_pct() — coarse SoC estimate from Vbat (mV).
* Works for 3S LiPo (10.5–12.6 V) and 4S (14.0–16.8 V).
* Detection is automatic based on voltage.
* Returns 0–100, or 255 if voltage is out of range.
*/
uint8_t battery_estimate_pct(uint32_t voltage_mv);
#endif /* BATTERY_H */

18
include/config.h
View File

@ -127,10 +127,20 @@
// Debug: UART5 (PC12=TX, PD2=RX)
// --- CRSF / ExpressLRS ---
// CH1[0]=steer CH2[1]=lean(future) CH5[4]=arm CH6[5]=mode
// CH1[0]=steer CH2[1]=throttle CH5[4]=arm CH6[5]=mode
#define CRSF_ARM_THRESHOLD 1750 /* CH5 raw value; > threshold = armed */
#define CRSF_STEER_MAX 400 /* CH1 range: -400..+400 motor counts */
#define CRSF_FAILSAFE_MS 300 /* Disarm after this ms without a frame */
#define CRSF_FAILSAFE_MS 500 /* Disarm after this ms without a frame (Issue #103) */
// --- Battery ADC (ADC3, PC1 = ADC123_IN11) ---
/* Mamba F722: 10kΩ + 1kΩ voltage divider → 11:1 ratio */
#define VBAT_SCALE_NUM 11 /* Numerator of divider ratio */
#define VBAT_AREF_MV 3300 /* ADC reference in mV */
#define VBAT_ADC_BITS 12 /* 12-bit ADC → 4096 counts */
/* Filtered Vbat in mV: (raw * 3300 * 11) / 4096, updated at 10Hz */
// --- CRSF Telemetry TX (uplink: FC → ELRS module → pilot handset) ---
#define CRSF_TELEMETRY_HZ 1 /* Telemetry TX rate (Hz) */
// --- PID Tuning ---
#define PID_KP 35.0f
@ -155,8 +165,8 @@
// --- RC / Mode Manager ---
/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */
#define CRSF_CH_SPEED 2 /* CH3 — left stick vertical (fwd/back) */
#define CRSF_CH_STEER 3 /* CH4 — left stick horizontal (yaw) */
#define CRSF_CH_STEER 0 /* CH1 — right stick horizontal (steer) */
#define CRSF_CH_SPEED 1 /* CH2 — right stick vertical (throttle) */
#define CRSF_CH_ARM 4 /* CH5 — arm switch (2-pos) */
#define CRSF_CH_MODE 5 /* CH6 — mode switch (3-pos) */
/* Deadband around CRSF center (992) in raw counts (~2% of range) */

24
include/crsf.h
View File

@ -40,6 +40,30 @@ void crsf_parse_byte(uint8_t byte);
*/
int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max);
/*
* crsf_send_battery() — transmit CRSF battery-sensor telemetry frame (type 0x08)
* 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)
* 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)
* current unit: 100 mA
*/
void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
uint8_t remaining_pct);
/*
* crsf_send_flight_mode() — transmit CRSF flight-mode frame (type 0x21)
* for display on the pilot's handset OSD.
*
* armed: true → "ARMED\0"
* false → "DISARM\0"
*/
void crsf_send_flight_mode(bool armed);
extern volatile CRSFState crsf_state;
#endif /* CRSF_H */

89
src/battery.c Normal file
View File

@ -0,0 +1,89 @@
/*
* 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 "config.h"
#include "stm32f7xx_hal.h"
static ADC_HandleTypeDef s_hadc;
static bool s_ready = false;
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;
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.
* 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));
}

61
src/crsf.c
View File

@ -291,3 +291,64 @@ int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) {
if (r > max) r = max;
return (int16_t)r;
}
/* ------------------------------------------------------------------ */
/* Telemetry TX helpers */
/* ------------------------------------------------------------------ */
/*
* Build a CRSF frame in `buf` and return the total byte count.
* buf must be at least CRSF_MAX_FRAME_LEN bytes.
* frame_type : CRSF type byte (e.g. 0x08 battery, 0x21 flight mode)
* payload : frame payload bytes (excluding type, CRC)
* plen : payload length in bytes
*/
static uint8_t crsf_build_frame(uint8_t *buf, uint8_t frame_type,
const uint8_t *payload, uint8_t plen) {
/* Total frame = SYNC + LEN + TYPE + PAYLOAD + CRC */
uint8_t frame_len = 2u + 1u + plen + 1u; /* SYNC + LEN + TYPE + payload + CRC */
if (frame_len > CRSF_MAX_FRAME_LEN) return 0;
buf[0] = CRSF_SYNC; /* 0xC8 */
buf[1] = (uint8_t)(plen + 2u); /* LEN = TYPE + payload + CRC */
buf[2] = frame_type;
memcpy(&buf[3], payload, plen);
buf[frame_len - 1] = crsf_frame_crc(buf, frame_len);
return frame_len;
}
/*
* crsf_send_battery() — type 0x08 battery sensor.
* voltage_mv → units of 100 mV (big-endian uint16)
* 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 current_ma,
uint8_t remaining_pct) {
uint16_t v100 = (uint16_t)(voltage_mv / 100u); /* 100 mV units */
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),
(uint8_t)(c100 >> 8), (uint8_t)(c100 & 0xFF),
0, 0, 0, /* capacity mAh — not tracked */
remaining_pct,
};
uint8_t frame[CRSF_MAX_FRAME_LEN];
uint8_t flen = crsf_build_frame(frame, 0x08u, payload, sizeof(payload));
if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u);
}
/*
* crsf_send_flight_mode() — type 0x21 flight mode text.
* Displays on the handset's OSD/status bar.
* "ARMED\0" when armed (5 payload bytes + null)
* "DISARM\0" when not (7 payload bytes + null)
*/
void crsf_send_flight_mode(bool armed) {
const char *text = armed ? "ARMED" : "DISARM";
uint8_t plen = (uint8_t)(strlen(text) + 1u); /* include null terminator */
uint8_t frame[CRSF_MAX_FRAME_LEN];
uint8_t flen = crsf_build_frame(frame, 0x21u, (const uint8_t *)text, plen);
if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u);
}

16
src/main.c
View File

@ -16,6 +16,7 @@
#include "bmp280.h"
#include "mag.h"
#include "jetson_cmd.h"
#include "battery.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
@ -142,6 +143,9 @@ int main(void) {
mode_manager_t mode;
mode_manager_init(&mode);
/* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */
battery_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;
@ -167,6 +171,7 @@ int main(void) {
uint32_t send_tick = 0;
uint32_t balance_tick = 0;
uint32_t esc_tick = 0;
uint32_t crsf_telem_tick = 0; /* CRSF uplink telemetry TX timer */
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
while (1) {
@ -313,6 +318,17 @@ int main(void) {
}
}
/* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
* Sends battery sensor frame (0x08) and flight mode frame (0x21)
* back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */
if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) {
crsf_telem_tick = now;
uint32_t vbat_mv = battery_read_mv();
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);
}
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
send_tick = now;

237
test/test_crsf_frames.py Normal file
View File

@ -0,0 +1,237 @@
"""
test_crsf_frames.py — Unit tests for CRSF telemetry frame building (Issue #103).
Mirrors the C logic in src/crsf.c for crsf_send_battery() and
crsf_send_flight_mode() so frame layout and CRC can be verified
without flashing hardware.
Run with: pytest test/test_crsf_frames.py -v
"""
import struct
import pytest
# ── CRC8 DVB-S2 (polynomial 0xD5) ─────────────────────────────────────────
def crc8_dvb_s2(crc: int, byte: int) -> int:
crc ^= byte
for _ in range(8):
crc = ((crc << 1) ^ 0xD5) & 0xFF if (crc & 0x80) else (crc << 1) & 0xFF
return crc
def crsf_frame_crc(frame: bytes) -> int:
"""CRC covers frame[2] (type) through frame[-2] (last payload byte)."""
crc = 0
for b in frame[2:-1]:
crc = crc8_dvb_s2(crc, b)
return crc
# ── Frame builders matching C implementation ───────────────────────────────
CRSF_SYNC = 0xC8
def build_battery_frame(voltage_mv: int, current_ma: int,
remaining_pct: int) -> bytes:
"""
Type 0x08 — battery sensor.
voltage : 100 mV units, uint16 big-endian
current : 100 mA units, uint16 big-endian
capacity : 0 mAh (not tracked), uint24 big-endian
remaining: 0–100 %, uint8
Total payload = 8 bytes.
"""
v100 = voltage_mv // 100
c100 = current_ma // 100
payload = struct.pack('>HH3sB',
v100, c100,
b'\x00\x00\x00',
remaining_pct)
frame_type = 0x08
# SYNC + LEN(TYPE+payload+CRC) + TYPE + payload + CRC
frame_body = bytes([CRSF_SYNC, len(payload) + 2, frame_type]) + payload
frame = frame_body + b'\x00' # placeholder CRC slot
crc = crsf_frame_crc(frame)
return frame_body + bytes([crc])
def build_flight_mode_frame(armed: bool) -> bytes:
"""Type 0x21 — flight mode text, null-terminated."""
text = b'ARMED\x00' if armed else b'DISARM\x00'
frame_type = 0x21
frame_body = bytes([CRSF_SYNC, len(text) + 2, frame_type]) + text
frame = frame_body + b'\x00'
crc = crsf_frame_crc(frame)
return frame_body + bytes([crc])
# ── Helpers ────────────────────────────────────────────────────────────────
def validate_frame(frame: bytes):
"""Assert basic CRSF frame invariants."""
assert frame[0] == CRSF_SYNC, "bad sync byte"
length_field = frame[1]
total = length_field + 2 # SYNC + LEN + rest
assert len(frame) == total, f"frame length mismatch: {len(frame)} != {total}"
assert len(frame) <= 64, "frame too long (CRSF max 64 bytes)"
expected_crc = crsf_frame_crc(frame)
assert frame[-1] == expected_crc, \
f"CRC mismatch: got {frame[-1]:#04x}, expected {expected_crc:#04x}"
# ── Battery frame tests ────────────────────────────────────────────────────
class TestBatteryFrame:
def test_sync_byte(self):
f = build_battery_frame(12600, 0, 100)
assert f[0] == 0xC8
def test_frame_type(self):
f = build_battery_frame(12600, 0, 100)
assert f[2] == 0x08
def test_frame_invariants(self):
validate_frame(build_battery_frame(12600, 5000, 80))
def test_voltage_encoding_3s_full(self):
"""12.6 V → 126 in 100 mV units, big-endian."""
f = build_battery_frame(12600, 0, 100)
v100 = (f[3] << 8) | f[4]
assert v100 == 126
def test_voltage_encoding_4s_full(self):
"""16.8 V → 168."""
f = build_battery_frame(16800, 0, 100)
v100 = (f[3] << 8) | f[4]
assert v100 == 168
def test_current_encoding(self):
"""5000 mA → 50 in 100 mA units."""
f = build_battery_frame(12000, 5000, 75)
c100 = (f[5] << 8) | f[6]
assert c100 == 50
def test_remaining_pct(self):
f = build_battery_frame(11000, 0, 42)
assert f[10] == 42
def test_capacity_zero(self):
"""Capacity bytes (cap_hi, cap_mid, cap_lo) are zero — no coulomb counter."""
f = build_battery_frame(12600, 0, 100)
assert f[7] == 0 and f[8] == 0 and f[9] == 0
def test_crc_correct(self):
f = build_battery_frame(11500, 2000, 65)
validate_frame(f)
def test_zero_voltage(self):
"""Disconnected battery → 0 mV, 0 %."""
f = build_battery_frame(0, 0, 0)
validate_frame(f)
v100 = (f[3] << 8) | f[4]
assert v100 == 0
assert f[10] == 0
def test_total_frame_length(self):
"""Battery frame: SYNC(1)+LEN(1)+TYPE(1)+payload(8)+CRC(1) = 12 bytes."""
f = build_battery_frame(12000, 0, 80)
assert len(f) == 12
# ── Flight mode frame tests ────────────────────────────────────────────────
class TestFlightModeFrame:
def test_armed_text(self):
f = build_flight_mode_frame(True)
payload = f[3:-1]
assert payload == b'ARMED\x00'
def test_disarmed_text(self):
f = build_flight_mode_frame(False)
payload = f[3:-1]
assert payload == b'DISARM\x00'
def test_frame_type(self):
assert build_flight_mode_frame(True)[2] == 0x21
assert build_flight_mode_frame(False)[2] == 0x21
def test_crc_armed(self):
validate_frame(build_flight_mode_frame(True))
def test_crc_disarmed(self):
validate_frame(build_flight_mode_frame(False))
def test_armed_frame_length(self):
"""ARMED\0 = 6 bytes payload → total 10 bytes."""
f = build_flight_mode_frame(True)
assert len(f) == 10
def test_disarmed_frame_length(self):
"""DISARM\0 = 7 bytes payload → total 11 bytes."""
f = build_flight_mode_frame(False)
assert len(f) == 11
# ── CRC8 DVB-S2 self-test ─────────────────────────────────────────────────
class TestCRC8:
def test_known_vector(self):
"""Verify CRC8 DVB-S2 against known value (poly 0xD5, init 0)."""
# CRC of single byte 0xAB with poly 0xD5, init 0 → 0xC8
crc = crc8_dvb_s2(0, 0xAB)
assert crc == 0xC8
def test_different_payloads_differ(self):
f1 = build_battery_frame(12600, 0, 100)
f2 = build_battery_frame(11000, 0, 50)
assert f1[-1] != f2[-1], "different payloads should have different CRCs"
def test_crc_covers_type(self):
"""Changing the frame type changes the CRC."""
fa = build_battery_frame(12600, 0, 100) # type 0x08
fb = build_flight_mode_frame(True) # type 0x21
# Both frames differ in type byte and thus CRC
assert fa[-1] != fb[-1]
# ── battery_estimate_pct logic mirrored in Python ─────────────────────────
def battery_estimate_pct(voltage_mv: int) -> int:
"""Python mirror of battery_estimate_pct() in battery.c."""
if voltage_mv >= 13000:
v_min, v_max = 13200, 16800
else:
v_min, v_max = 9900, 12600
if voltage_mv <= v_min:
return 0
if voltage_mv >= v_max:
return 100
return int((voltage_mv - v_min) * 100 / (v_max - v_min))
class TestBatteryEstimatePct:
def test_3s_full(self):
assert battery_estimate_pct(12600) == 100
def test_3s_empty(self):
assert battery_estimate_pct(9900) == 0
def test_3s_mid(self):
pct = battery_estimate_pct(11250)
assert 45 <= pct <= 55
def test_4s_full(self):
assert battery_estimate_pct(16800) == 100
def test_4s_empty(self):
assert battery_estimate_pct(13200) == 0
def test_3s_over_voltage(self):
"""13000 mV triggers 4S branch (v_min=13200) → classified as dead 4S → 0%."""
assert battery_estimate_pct(13000) == 0
def test_zero_voltage(self):
assert battery_estimate_pct(0) == 0