saltylab-firmware/src/crsf.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 "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>

/* ------------------------------------------------------------------ */
/* 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};

/* ------------------------------------------------------------------ */
/* 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;
}

/* ------------------------------------------------------------------ */
/* 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);
}