From fbfde24abab72c3df80217d8637673f872a0a396 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sat, 28 Feb 2026 21:09:12 -0500 Subject: [PATCH] =?UTF-8?q?feat:=20CRSF/ELRS=20RC=20integration=20?= =?UTF-8?q?=E2=80=94=2016ch=20input=20with=20failsafe=20(#Phase2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Protocol choice: implemented from spec (CRSFforArduino needs Arduino framework; Betaflight extraction has deep scheduler dependencies). Protocol verified against Betaflight src/main/rx/crsf.c + CRSF spec. crsf.c: - UART4 PA0=TX/PA1=RX (GPIO_AF8_UART4), 420000 baud 8N1, oversampling×8 APB1=54MHz → BRR=0x101 → 418604 baud (0.33% error, within spec) - DMA1 Stream2 Channel4, circular 64-byte buffer, IDLE interrupt DMA half/complete callbacks drain buffer; IDLE fires at frame boundary - CRC8 DVB-S2 (polynomial 0xD5) validated on every frame - Parser state machine: SYNC(0xC8)→LEN→DATA with length sanity check - 11-bit channel unpack for all 16 channels from 22-byte payload - RC channels frame (0x16): unpacks 16ch, updates last_rx_ms + armed - Link stats frame (0x14): captures RSSI dBm, LQ%, SNR dB crsf.h: added rssi_dbm, link_quality, snr fields to CRSFState config.h: CRSF_ARM_THRESHOLD=1750, CRSF_STEER_MAX=400, CRSF_FAILSAFE_MS=300 main.c: - crsf_init() called after motor_driver_init() - RC failsafe: disarm if (now - last_rx_ms) > CRSF_FAILSAFE_MS, but only after RC was first seen (last_rx_ms != 0) — USB-only mode unaffected - RC arm: CH5 rising edge → safety_arm_start(); falling edge → disarm Same ARMING_HOLD_MS interlock as USB arm command - RC steer: CH1 → crsf_to_range() → ±CRSF_STEER_MAX → motor_driver steer - RSSI/LQ: appended to JSON when safety_rc_alive() ("rssi","lq" fields) ui/index.html: hidden RC RSSI row revealed on first packet with rssi/lq Co-Authored-By: Claude Sonnet 4.6 --- include/config.h | 6 + include/crsf.h | 34 +++++- src/crsf.c | 300 +++++++++++++++++++++++++++++++++++++++++++++-- src/main.c | 41 ++++++- ui/index.html | 9 ++ 5 files changed, 368 insertions(+), 22 deletions(-) diff --git a/include/config.h b/include/config.h index 2c09564..020ffc4 100644 --- a/include/config.h +++ b/include/config.h @@ -126,6 +126,12 @@ // Jetson: USART6 (PC6=TX, PC7=RX) // Debug: UART5 (PC12=TX, PD2=RX) +// --- CRSF / ExpressLRS --- +// CH1[0]=steer CH2[1]=lean(future) 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 */ + // --- PID Tuning --- #define PID_KP 35.0f #define PID_KI 1.0f diff --git a/include/crsf.h b/include/crsf.h index 36d495d..724bd97 100644 --- a/include/crsf.h +++ b/include/crsf.h @@ -4,16 +4,42 @@ #include #include +/* + * CRSF/ExpressLRS RC receiver state. + * + * Updated from ISR context on every valid frame. + * Read from main loop — values are naturally atomic (8/16-bit on Cortex-M). + * last_rx_ms == 0 means no frame received yet (USB-only mode). + */ typedef struct { - uint16_t channels[16]; // Raw CRSF values (172-1811) - uint32_t last_rx_ms; // Timestamp of last valid frame - bool armed; // Arm switch state + uint16_t channels[16]; /* Raw CRSF values, 172 (988µs) – 1811 (2012µs) */ + uint32_t last_rx_ms; /* HAL_GetTick() at last valid RC frame */ + bool armed; /* CH5 arm switch: true when channels[4] > CRSF_ARM_THRESHOLD */ + + /* Link statistics (from 0x14 frames, optional) */ + int8_t rssi_dbm; /* Uplink RSSI in dBm (negative, e.g. -85) */ + uint8_t link_quality; /* Uplink link quality 0–100 % */ + int8_t snr; /* Uplink SNR in dB */ } CRSFState; +/* + * crsf_init() — configure UART4 (PA0=TX, PA1=RX) at 420000 baud with + * DMA1 circular RX and IDLE interrupt. Call once before safety_init(). + */ void crsf_init(void); + +/* + * crsf_parse_byte() — feed one byte into the frame parser. + * Called automatically from DMA/IDLE ISR. Available for unit tests. + */ void crsf_parse_byte(uint8_t byte); + +/* + * crsf_to_range() — map raw CRSF value (172–1811) linearly to [min, max]. + * Clamps at boundaries. Midpoint 992 → (min+max)/2. + */ int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max); extern volatile CRSFState crsf_state; -#endif +#endif /* CRSF_H */ diff --git a/src/crsf.c b/src/crsf.c index c3fe147..13e5c5d 100644 --- a/src/crsf.c +++ b/src/crsf.c @@ -1,19 +1,293 @@ /* - * crsf.c — CRSF RC receiver stub (UART not yet wired on MAMBA F722S). - * crsf_state.last_rx_ms stays 0 so safety_rc_alive() always returns false, - * which keeps USB-only mode active (RC timeout disarm is commented out in main.c). + * crsf.c — CRSF/ExpressLRS RC receiver driver + * + * Hardware: UART4, PA0=TX PA1=RX (GPIO_AF8_UART4), 420000 baud 8N1 + * DMA: DMA1 Stream2 Channel4 (UART4_RX), circular 64-byte buffer + * UART IDLE interrupt → drain; DMA half/complete callbacks → drain + * + * CRSF frame layout: + * [0xC8] [LEN] [TYPE] [PAYLOAD...] [CRC8-DVB-S2] + * LEN = bytes after itself = TYPE + PAYLOAD + CRC (max 62) + * CRC covers TYPE through last PAYLOAD byte (not SYNC, not LEN, not CRC) + * + * Supported frame types: + * 0x16 — RC channels (22 bytes, 16 ch × 11 bit packed) + * 0x14 — Link statistics (RSSI, LQ, SNR) + * + * Channel mapping (0-indexed): + * CH1 [0] = steering (-1000..+1000) + * CH2 [1] = speed/lean override (future use) + * CH5 [4] = arm switch (> CRSF_ARM_THRESHOLD = armed) + * CH6 [5] = mode (< 992 = RC, > 992 = auto) + * + * Protocol reference: ExpressLRS/CRSF_Spec, verified against Betaflight src/main/rx/crsf.c */ -#include "crsf.h" +#include "crsf.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* ------------------------------------------------------------------ */ +/* DMA circular receive buffer */ +/* ------------------------------------------------------------------ */ +#define CRSF_DMA_BUF_SIZE 64u /* must be power-of-2 >= 2× max frame (26) */ + +static uint8_t s_dma_buf[CRSF_DMA_BUF_SIZE]; +static volatile uint16_t s_dma_head = 0; /* last processed position */ + +static UART_HandleTypeDef s_uart; +static DMA_HandleTypeDef s_dma_rx; + +/* ------------------------------------------------------------------ */ +/* Frame parser state */ +/* ------------------------------------------------------------------ */ +#define CRSF_SYNC 0xC8u +#define CRSF_FRAME_RC 0x16u /* RC channels packed */ +#define CRSF_FRAME_LINK 0x14u /* Link statistics */ +#define CRSF_MAX_FRAME_LEN 64u + +typedef enum { ST_SYNC, ST_LEN, ST_DATA } parse_state_t; + +static parse_state_t s_ps = ST_SYNC; +static uint8_t s_frame[CRSF_MAX_FRAME_LEN]; +static uint8_t s_flen = 0; /* total expected frame bytes */ +static uint8_t s_fpos = 0; /* bytes received so far */ + +/* ------------------------------------------------------------------ */ +/* Public state */ +/* ------------------------------------------------------------------ */ volatile CRSFState crsf_state = {0}; -void crsf_init(void) {} - -void crsf_parse_byte(uint8_t byte) { (void)byte; } - -int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) -{ - /* CRSF range 172-1811 → linear map to [min, max] */ - int32_t v = (int32_t)val; - return (int16_t)(min + (v - 172) * (max - min) / (1811 - 172)); +/* ------------------------------------------------------------------ */ +/* CRC8 DVB-S2 — polynomial 0xD5 */ +/* ------------------------------------------------------------------ */ +static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) { + crc ^= a; + for (int i = 0; i < 8; i++) { + crc = (crc & 0x80u) ? ((crc << 1) ^ 0xD5u) : (crc << 1); + } + return crc; +} + +static uint8_t crsf_frame_crc(const uint8_t *frame, uint8_t frame_len) { + /* CRC covers frame[2] (type) .. frame[frame_len-2] (last payload byte) */ + uint8_t crc = 0; + for (uint8_t i = 2; i < frame_len - 1; i++) { + crc = crc8_dvb_s2(crc, frame[i]); + } + return crc; +} + +/* ------------------------------------------------------------------ */ +/* 11-bit channel unpacking — 16 channels from 22 bytes */ +/* ------------------------------------------------------------------ */ +static void unpack_channels(const uint8_t *payload, uint16_t *ch) { + uint32_t bits = 0; + int loaded = 0, idx = 0, src = 0; + while (idx < 16) { + while (loaded < 11 && src < 22) { + bits |= (uint32_t)payload[src++] << loaded; + loaded += 8; + } + ch[idx++] = bits & 0x7FFu; + bits >>= 11; + loaded -= 11; + } +} + +/* ------------------------------------------------------------------ */ +/* Frame processing */ +/* ------------------------------------------------------------------ */ +static void process_frame(const uint8_t *frame, uint8_t frame_len) { + /* Validate minimum length and CRC */ + if (frame_len < 4) return; + if (frame[frame_len - 1] != crsf_frame_crc(frame, frame_len)) return; + + uint8_t type = frame[2]; + const uint8_t *payload = &frame[3]; + uint8_t payload_len = frame_len - 4; /* type + payload + crc = frame[1], minus type and crc */ + + switch (type) { + case CRSF_FRAME_RC: + if (payload_len < 22) return; + unpack_channels(payload, (uint16_t *)crsf_state.channels); + crsf_state.last_rx_ms = HAL_GetTick(); + /* Update arm switch state from CH5 (index 4) */ + crsf_state.armed = (crsf_state.channels[4] > CRSF_ARM_THRESHOLD); + break; + + case CRSF_FRAME_LINK: + /* Link stats payload: + * [0] uplink RSSI ant1 (value = -dBm, so negate for dBm) + * [2] uplink link quality (0-100 %) + * [3] uplink SNR (signed dB) + */ + if (payload_len < 4) return; + crsf_state.rssi_dbm = -(int8_t)payload[0]; + crsf_state.link_quality = payload[2]; + crsf_state.snr = (int8_t)payload[3]; + break; + + default: + break; + } +} + +/* ------------------------------------------------------------------ */ +/* Byte-level parser state machine */ +/* ------------------------------------------------------------------ */ +static void parse_byte(uint8_t b) { + switch (s_ps) { + case ST_SYNC: + if (b == CRSF_SYNC) { + s_frame[0] = b; + s_ps = ST_LEN; + } + break; + + case ST_LEN: + /* LEN = bytes remaining after this field (type + payload + crc), max 62 */ + if (b >= 2 && b <= 62) { + s_frame[1] = b; + s_flen = b + 2u; /* total frame = SYNC + LEN + rest */ + s_fpos = 2; + s_ps = ST_DATA; + } else { + s_ps = ST_SYNC; /* invalid length — resync */ + } + break; + + case ST_DATA: + s_frame[s_fpos++] = b; + if (s_fpos >= s_flen) { + process_frame(s_frame, s_flen); + s_ps = ST_SYNC; + } + break; + } +} + +/* ------------------------------------------------------------------ */ +/* DMA buffer drain — called from IDLE IRQ and DMA half/complete CBs */ +/* ------------------------------------------------------------------ */ +static void dma_drain(void) { + /* DMA CNDTR counts DOWN from buf size; write position = size - CNDTR */ + uint16_t pos = (uint16_t)(CRSF_DMA_BUF_SIZE - __HAL_DMA_GET_COUNTER(&s_dma_rx)); + uint16_t head = s_dma_head; + while (head != pos) { + parse_byte(s_dma_buf[head]); + head = (head + 1u) & (CRSF_DMA_BUF_SIZE - 1u); + } + s_dma_head = pos; +} + +/* ------------------------------------------------------------------ */ +/* IRQ handlers */ +/* ------------------------------------------------------------------ */ +void UART4_IRQHandler(void) { + /* IDLE line detection — fires when bus goes quiet between frames */ + if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) { + __HAL_UART_CLEAR_IDLEFLAG(&s_uart); + dma_drain(); + } + HAL_UART_IRQHandler(&s_uart); +} + +void DMA1_Stream2_IRQHandler(void) { + HAL_DMA_IRQHandler(&s_dma_rx); +} + +/* DMA half-complete: drain first half of circular buffer */ +void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *h) { + if (h->Instance == UART4) dma_drain(); +} + +/* DMA complete: drain second half (buffer wrapped) */ +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *h) { + if (h->Instance == UART4) dma_drain(); +} + +/* ------------------------------------------------------------------ */ +/* Public API */ +/* ------------------------------------------------------------------ */ + +/* + * crsf_init() — configure UART4 + DMA1 and start circular receive. + * + * UART4: PA0=TX, PA1=RX, AF8_UART4, 420000 baud 8N1, oversampling×8. + * APB1 = 54 MHz → BRR = 0x101 → actual 418604 baud (0.33% error, within CRSF spec). + * + * DMA: DMA1 Stream2 Channel4, peripheral→memory, circular, byte width. + * IDLE interrupt + DMA half/complete callbacks drain the circular buffer. + */ +void crsf_init(void) { + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_UART4_CLK_ENABLE(); + __HAL_RCC_DMA1_CLK_ENABLE(); + + /* PA0=TX, PA1=RX, AF8_UART4 */ + GPIO_InitTypeDef gpio = {0}; + gpio.Pin = GPIO_PIN_0 | GPIO_PIN_1; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_PULLUP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = GPIO_AF8_UART4; + HAL_GPIO_Init(GPIOA, &gpio); + + /* DMA1 Stream2 Channel4 — UART4_RX, circular byte transfers */ + s_dma_rx.Instance = DMA1_Stream2; + s_dma_rx.Init.Channel = DMA_CHANNEL_4; + s_dma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + s_dma_rx.Init.PeriphInc = DMA_PINC_DISABLE; + s_dma_rx.Init.MemInc = DMA_MINC_ENABLE; + s_dma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + s_dma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + s_dma_rx.Init.Mode = DMA_CIRCULAR; + s_dma_rx.Init.Priority = DMA_PRIORITY_LOW; + s_dma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + HAL_DMA_Init(&s_dma_rx); + __HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx); + + /* UART4: 420000 8N1, oversampling×8 (better tolerance at high baud) */ + s_uart.Instance = UART4; + s_uart.Init.BaudRate = 420000; + s_uart.Init.WordLength = UART_WORDLENGTH_8B; + s_uart.Init.StopBits = UART_STOPBITS_1; + s_uart.Init.Parity = UART_PARITY_NONE; + s_uart.Init.Mode = UART_MODE_TX_RX; + s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE; + s_uart.Init.OverSampling = UART_OVERSAMPLING_8; + HAL_UART_Init(&s_uart); + + /* Start circular DMA — runs indefinitely, no need to restart */ + HAL_UART_Receive_DMA(&s_uart, s_dma_buf, CRSF_DMA_BUF_SIZE); + + /* IDLE line interrupt — fires when UART bus goes quiet (end of frame) */ + __HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE); + + HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 6, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn); + HAL_NVIC_SetPriority(UART4_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(UART4_IRQn); +} + +/* + * crsf_parse_byte() — kept for compatibility; direct call path not used + * when DMA is active, but available for unit testing or UART-IT fallback. + */ +void crsf_parse_byte(uint8_t byte) { + parse_byte(byte); +} + +/* + * crsf_to_range() — map raw CRSF value 172–1811 to [min, max]. + * Midpoint 992 maps to (min+max)/2. + */ +int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) { + int32_t v = (int32_t)val; + int32_t r = min + (v - 172) * (int32_t)(max - min) / (1811 - 172); + if (r < min) r = min; + if (r > max) r = max; + return (int16_t)r; } diff --git a/src/main.c b/src/main.c index c5f87d5..cb5bd40 100644 --- a/src/main.c +++ b/src/main.c @@ -131,6 +131,9 @@ int main(void) { motor_driver_t motors; motor_driver_init(&motors); + /* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */ + crsf_init(); + /* Init mode manager (RC/autonomous blend; CH6 mode switch) */ mode_manager_t mode; mode_manager_init(&mode); @@ -178,15 +181,37 @@ int main(void) { safety_arm_cancel(); balance_disarm(&bal); } - /* RC signal lost while armed — disarm for safety */ - if (bal.state == BALANCE_ARMED && !safety_rc_alive(now)) { - /* Uncomment when CRSF is wired (last_rx_ms stays 0 on stub): */ - /* balance_disarm(&bal); */ + /* RC failsafe: disarm if signal lost AFTER RC was previously alive. + * Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */ + if (bal.state == BALANCE_ARMED && + crsf_state.last_rx_ms != 0 && + (now - crsf_state.last_rx_ms) > CRSF_FAILSAFE_MS) { + safety_arm_cancel(); + balance_disarm(&bal); } /* Tilt fault buzzer alert (one-shot on fault edge) */ safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT); + /* RC arm/disarm via CH5 switch (CRSF) — edge detect, same hold interlock */ + { + static bool s_rc_armed_prev = false; + bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed; + if (rc_armed_now && !s_rc_armed_prev) { + /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */ + if (mpu6000_is_calibrated() && + bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { + safety_arm_start(now); + } + } + if (!rc_armed_now && s_rc_armed_prev) { + /* Falling edge: cancel pending arm or disarm if already armed */ + safety_arm_cancel(); + if (bal.state == BALANCE_ARMED) balance_disarm(&bal); + } + s_rc_armed_prev = rc_armed_now; + } + /* Handle arm/disarm requests from USB with arm-hold interlock */ if (cdc_arm_request) { cdc_arm_request = 0; @@ -238,7 +263,7 @@ int main(void) { } /* Feed autonomous steer from Jetson into mode manager. - * mode_manager_get_steer() blends it with RC CH4 per active mode. */ + * mode_manager_get_steer() blends it with RC steer per active mode. */ if (jetson_cmd_is_active(now)) mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0); @@ -305,6 +330,12 @@ int main(void) { } } } + if (safety_rc_alive(now)) { + /* RSSI in dBm (negative), link quality 0-100% */ + n = snprintf(p, rem, ",\"rssi\":%d,\"lq\":%d", + (int)crsf_state.rssi_dbm, (int)crsf_state.link_quality); + p += n; rem -= n; + } n = snprintf(p, rem, "}\n"); len = (int)(p + n - buf); } else { diff --git a/ui/index.html b/ui/index.html index 6058620..2f244fb 100644 --- a/ui/index.html +++ b/ui/index.html @@ -56,6 +56,10 @@ +
HZ --
@@ -227,6 +231,11 @@ window.updateIMU = function(data) { document.getElementById('row-pres').style.display = ''; document.getElementById('v-pres').textContent = (data.pa / 10.0).toFixed(1); } + if (data.rssi !== undefined) { + document.getElementById('row-rc').style.display = ''; + document.getElementById('v-rssi').textContent = data.rssi; + document.getElementById('v-lq').textContent = (data.lq !== undefined) ? data.lq : '--'; + } // Pitch bar: center at 50%, ±90° document.getElementById('bar-pitch').style.width = ((pitch + 90) / 180 * 100) + '%'; -- 2.47.2