feat(rc): CRSF/ELRS RC integration — telemetry uplink + channel fix (Issue #103)
## 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:
parent
0f2ea7931b
commit
00c97bd902
35
include/battery.h
Normal file
35
include/battery.h
Normal 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 */
|
||||||
@ -127,10 +127,20 @@
|
|||||||
// Debug: UART5 (PC12=TX, PD2=RX)
|
// Debug: UART5 (PC12=TX, PD2=RX)
|
||||||
|
|
||||||
// --- CRSF / ExpressLRS ---
|
// --- 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_ARM_THRESHOLD 1750 /* CH5 raw value; > threshold = armed */
|
||||||
#define CRSF_STEER_MAX 400 /* CH1 range: -400..+400 motor counts */
|
#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 ---
|
// --- PID Tuning ---
|
||||||
#define PID_KP 35.0f
|
#define PID_KP 35.0f
|
||||||
@ -155,8 +165,8 @@
|
|||||||
|
|
||||||
// --- RC / Mode Manager ---
|
// --- RC / Mode Manager ---
|
||||||
/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */
|
/* 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 0 /* CH1 — right stick horizontal (steer) */
|
||||||
#define CRSF_CH_STEER 3 /* CH4 — left stick horizontal (yaw) */
|
#define CRSF_CH_SPEED 1 /* CH2 — right stick vertical (throttle) */
|
||||||
#define CRSF_CH_ARM 4 /* CH5 — arm switch (2-pos) */
|
#define CRSF_CH_ARM 4 /* CH5 — arm switch (2-pos) */
|
||||||
#define CRSF_CH_MODE 5 /* CH6 — mode switch (3-pos) */
|
#define CRSF_CH_MODE 5 /* CH6 — mode switch (3-pos) */
|
||||||
/* Deadband around CRSF center (992) in raw counts (~2% of range) */
|
/* Deadband around CRSF center (992) in raw counts (~2% of range) */
|
||||||
|
|||||||
@ -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);
|
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;
|
extern volatile CRSFState crsf_state;
|
||||||
|
|
||||||
#endif /* CRSF_H */
|
#endif /* CRSF_H */
|
||||||
|
|||||||
89
src/battery.c
Normal file
89
src/battery.c
Normal 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
61
src/crsf.c
@ -291,3 +291,64 @@ int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) {
|
|||||||
if (r > max) r = max;
|
if (r > max) r = max;
|
||||||
return (int16_t)r;
|
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
16
src/main.c
@ -16,6 +16,7 @@
|
|||||||
#include "bmp280.h"
|
#include "bmp280.h"
|
||||||
#include "mag.h"
|
#include "mag.h"
|
||||||
#include "jetson_cmd.h"
|
#include "jetson_cmd.h"
|
||||||
|
#include "battery.h"
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
@ -142,6 +143,9 @@ int main(void) {
|
|||||||
mode_manager_t mode;
|
mode_manager_t mode;
|
||||||
mode_manager_init(&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 */
|
/* Probe I2C1 for optional sensors — skip gracefully if not found */
|
||||||
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
|
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
|
||||||
mag_type_t mag_type = MAG_NONE;
|
mag_type_t mag_type = MAG_NONE;
|
||||||
@ -167,6 +171,7 @@ int main(void) {
|
|||||||
uint32_t send_tick = 0;
|
uint32_t send_tick = 0;
|
||||||
uint32_t balance_tick = 0;
|
uint32_t balance_tick = 0;
|
||||||
uint32_t esc_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 */
|
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
|
||||||
|
|
||||||
while (1) {
|
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) */
|
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
|
||||||
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
||||||
send_tick = now;
|
send_tick = now;
|
||||||
|
|||||||
237
test/test_crsf_frames.py
Normal file
237
test/test_crsf_frames.py
Normal 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
|
||||||
Loading…
x
Reference in New Issue
Block a user