diff --git a/include/config.h b/include/config.h index 6692378..63c6541 100644 --- a/include/config.h +++ b/include/config.h @@ -120,6 +120,16 @@ #define PINIO2_PORT GPIOC #define PINIO2_PIN GPIO_PIN_0 // pinio_config = 129 (USER2) +// --- JLink: Jetson Serial Binary Protocol (USART1, Issue #120) --- +#define JLINK_BAUD 921600 /* USART1 baud rate */ +#define JLINK_HB_TIMEOUT_MS 1000 /* Jetson heartbeat timeout (ms) */ +#define JLINK_TLM_HZ 50 /* STATUS telemetry TX rate (Hz) */ + +// --- Firmware Version --- +#define FW_MAJOR 1 +#define FW_MINOR 0 +#define FW_PATCH 0 + // --- SaltyLab Assignments --- // Hoverboard ESC: USART2 (PA2=TX, PA3=RX) or USART3 // ELRS Receiver: UART4 (PA0=TX, PA1=RX) — CRSF 420000 baud diff --git a/include/jlink.h b/include/jlink.h new file mode 100644 index 0000000..d2699aa --- /dev/null +++ b/include/jlink.h @@ -0,0 +1,127 @@ +#ifndef JLINK_H +#define JLINK_H + +#include +#include + +/* + * JLink — Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX). + * + * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated + * hardware UART at 921600 baud using DMA circular RX and IDLE interrupt. + * + * Frame format (both directions): + * [STX=0x02][LEN][CMD][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03] + * + * STX : frame start sentinel (0x02) + * LEN : count of CMD + PAYLOAD bytes (1 + payload_len) + * CMD : command/telemetry type byte + * PAYLOAD: 0..N bytes depending on CMD + * CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian + * ETX : frame end sentinel (0x03) + * + * Jetson → STM32 commands: + * 0x01 HEARTBEAT — no payload; refreshes heartbeat timer + * 0x02 DRIVE — int16 speed (-1000..+1000), int16 steer (-1000..+1000) + * 0x03 ARM — no payload; request arm (same interlock as CDC 'A') + * 0x04 DISARM — no payload; disarm immediately + * 0x05 PID_SET — float kp, float ki, float kd (12 bytes, IEEE-754 LE) + * 0x07 ESTOP — no payload; engage emergency stop + * + * STM32 → Jetson telemetry: + * 0x80 STATUS — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ + * + * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied + * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and + * RC_ASSISTED modes the Jetson speed offset and steer are injected via + * mode_manager_set_auto_cmd() and blended per the existing blend ramp. + * + * Heartbeat: if no valid frame arrives within JLINK_HB_TIMEOUT_MS (1000ms), + * jlink_is_active() returns false and the main loop clears the auto command. + */ + +/* ---- Frame constants ---- */ +#define JLINK_STX 0x02u +#define JLINK_ETX 0x03u + +/* ---- Command IDs (Jetson → STM32) ---- */ +#define JLINK_CMD_HEARTBEAT 0x01u +#define JLINK_CMD_DRIVE 0x02u +#define JLINK_CMD_ARM 0x03u +#define JLINK_CMD_DISARM 0x04u +#define JLINK_CMD_PID_SET 0x05u +#define JLINK_CMD_ESTOP 0x07u + +/* ---- Telemetry IDs (STM32 → Jetson) ---- */ +#define JLINK_TLM_STATUS 0x80u + +/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ +typedef struct __attribute__((packed)) { + int16_t pitch_x10; /* pitch degrees ×10 */ + int16_t roll_x10; /* roll degrees ×10 */ + int16_t yaw_x10; /* yaw degrees ×10 (gyro-integrated) */ + int16_t motor_cmd; /* ESC output -1000..+1000 */ + uint16_t vbat_mv; /* battery millivolts */ + int8_t rssi_dbm; /* CRSF RSSI (dBm, negative) */ + uint8_t link_quality; /* CRSF LQ 0-100 */ + uint8_t balance_state; /* 0=DISARMED, 1=ARMED, 2=TILT_FAULT */ + uint8_t rc_armed; /* crsf_state.armed (1=armed) */ + uint8_t mode; /* robot_mode_t: 0=RC_MANUAL,1=ASSISTED,2=AUTONOMOUS */ + uint8_t estop; /* EstopSource value */ + uint8_t soc_pct; /* state-of-charge 0-100, 255=unknown */ + uint8_t fw_major; + uint8_t fw_minor; + uint8_t fw_patch; +} jlink_tlm_status_t; /* 20 bytes */ + +/* ---- Volatile state (read from main loop) ---- */ +typedef struct { + /* Drive command — updated on JLINK_CMD_DRIVE */ + volatile int16_t speed; /* -1000..+1000 */ + volatile int16_t steer; /* -1000..+1000 */ + + /* Heartbeat timer — updated on any valid frame */ + volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame; 0=none */ + + /* One-shot request flags — set by parser, cleared by main loop */ + volatile uint8_t arm_req; + volatile uint8_t disarm_req; + volatile uint8_t estop_req; + + /* PID update — set by parser, cleared by main loop */ + volatile uint8_t pid_updated; + volatile float pid_kp; + volatile float pid_ki; + volatile float pid_kd; +} JLinkState; + +extern volatile JLinkState jlink_state; + +/* ---- API ---- */ + +/* + * jlink_init() — configure USART1 (PB6=TX, PB7=RX) at 921600 baud with + * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt. + * Call once before safety_init(). + */ +void jlink_init(void); + +/* + * jlink_is_active(now_ms) — returns true if a valid frame arrived within + * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received. + */ +bool jlink_is_active(uint32_t now_ms); + +/* + * jlink_send_telemetry(status) — build and transmit a JLINK_TLM_STATUS frame + * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ. + */ +void jlink_send_telemetry(const jlink_tlm_status_t *status); + +/* + * jlink_process() — drain DMA circular buffer and parse frames. + * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending). + */ +void jlink_process(void); + +#endif /* JLINK_H */ diff --git a/src/jlink.c b/src/jlink.c new file mode 100644 index 0000000..bdf83ef --- /dev/null +++ b/src/jlink.c @@ -0,0 +1,279 @@ +#include "jlink.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* ---- DMA circular RX buffer ---- */ +#define JLINK_RX_BUF_LEN 128u /* must be power-of-2 */ +static uint8_t s_rx_buf[JLINK_RX_BUF_LEN]; +static uint32_t s_rx_tail = 0; /* consumer index (byte already processed) */ + +/* ---- HAL handles ---- */ +static UART_HandleTypeDef s_uart; +static DMA_HandleTypeDef s_dma_rx; + +/* ---- Volatile state ---- */ +volatile JLinkState jlink_state; + +/* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */ +static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len) +{ + uint16_t crc = 0x0000u; + for (uint16_t i = 0; i < len; i++) { + crc ^= (uint16_t)data[i] << 8; + for (uint8_t b = 0; b < 8; b++) { + if (crc & 0x8000u) + crc = (crc << 1) ^ 0x1021u; + else + crc <<= 1; + } + } + return crc; +} + +/* ---- jlink_init() ---- */ +void jlink_init(void) +{ + /* GPIO: PB6=TX AF7 (USART1_TX), PB7=RX AF7 (USART1_RX) */ + __HAL_RCC_GPIOB_CLK_ENABLE(); + GPIO_InitTypeDef gpio = {0}; + gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_PULLUP; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = GPIO_AF7_USART1; + HAL_GPIO_Init(GPIOB, &gpio); + + /* DMA2 Stream2 Channel4 — USART1_RX circular */ + __HAL_RCC_DMA2_CLK_ENABLE(); + s_dma_rx.Instance = DMA2_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_MEDIUM; + s_dma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + HAL_DMA_Init(&s_dma_rx); + __HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx); + + /* USART1 at JLINK_BAUD (921600) */ + __HAL_RCC_USART1_CLK_ENABLE(); + s_uart.Instance = USART1; + s_uart.Init.BaudRate = JLINK_BAUD; + 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_16; + HAL_UART_Init(&s_uart); + + /* Enable USART1 IDLE interrupt for circular buffer draining */ + __HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE); + HAL_NVIC_SetPriority(USART1_IRQn, 6, 0); + HAL_NVIC_EnableIRQ(USART1_IRQn); + + /* DMA2_Stream2 IRQ (for error handling) */ + HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn); + + /* Start circular DMA RX — never stops */ + HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN); + + memset((void *)&jlink_state, 0, sizeof(jlink_state)); + s_rx_tail = 0; +} + +/* ---- IRQ handlers ---- */ +void USART1_IRQHandler(void) +{ + /* Clear IDLE flag by reading SR then DR */ + if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) { + __HAL_UART_CLEAR_IDLEFLAG(&s_uart); + /* jlink_process() drains the buffer from main loop — no work here */ + } + HAL_UART_IRQHandler(&s_uart); +} + +void DMA2_Stream2_IRQHandler(void) +{ + HAL_DMA_IRQHandler(&s_dma_rx); +} + +/* ---- jlink_is_active() ---- */ +bool jlink_is_active(uint32_t now_ms) +{ + if (jlink_state.last_rx_ms == 0u) return false; + return (now_ms - jlink_state.last_rx_ms) < JLINK_HB_TIMEOUT_MS; +} + +/* ---- Frame dispatch ---- */ +static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen) +{ + /* Update heartbeat timestamp on every valid frame */ + jlink_state.last_rx_ms = HAL_GetTick(); + + switch (cmd) { + + case JLINK_CMD_HEARTBEAT: + /* Heartbeat only — no payload action needed */ + break; + + case JLINK_CMD_DRIVE: + if (plen == 4u) { + int16_t spd, str; + memcpy(&spd, payload, 2); + memcpy(&str, payload + 2, 2); + /* Clamp to ±1000 */ + if (spd > 1000) spd = 1000; + if (spd < -1000) spd = -1000; + if (str > 1000) str = 1000; + if (str < -1000) str = -1000; + jlink_state.speed = spd; + jlink_state.steer = str; + } + break; + + case JLINK_CMD_ARM: + jlink_state.arm_req = 1u; + break; + + case JLINK_CMD_DISARM: + jlink_state.disarm_req = 1u; + break; + + case JLINK_CMD_PID_SET: + if (plen == 12u) { + float kp, ki, kd; + memcpy(&kp, payload, 4); + memcpy(&ki, payload + 4, 4); + memcpy(&kd, payload + 8, 4); + /* Sanity bounds — same as USB CDC PID handler in main.c */ + if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp; + if (ki >= 0.0f && ki <= 50.0f) jlink_state.pid_ki = ki; + if (kd >= 0.0f && kd <= 50.0f) jlink_state.pid_kd = kd; + jlink_state.pid_updated = 1u; + } + break; + + case JLINK_CMD_ESTOP: + jlink_state.estop_req = 1u; + break; + + default: + break; + } +} + +/* ---- jlink_process() — call from main loop every tick ---- */ +/* + * Parser state machine. + * Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX] + * LEN = count of CMD + PAYLOAD bytes (1..253). + * CRC16-XModem over CMD+PAYLOAD. + * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255, + * but we cap at 64 for safety). + */ +#define JLINK_MAX_PAYLOAD 64u + +typedef enum { + PS_WAIT_STX = 0, + PS_WAIT_LEN, + PS_WAIT_DATA, /* receiving CMD + PAYLOAD (len bytes total) */ + PS_WAIT_CRC_HI, + PS_WAIT_CRC_LO, + PS_WAIT_ETX, +} ParseState; + +void jlink_process(void) +{ + static ParseState s_state = PS_WAIT_STX; + static uint8_t s_len = 0; /* expected CMD+PAYLOAD length */ + static uint8_t s_count = 0; /* bytes received so far in PS_WAIT_DATA */ + static uint8_t s_frame[JLINK_MAX_PAYLOAD + 1u]; /* [0]=CMD, [1..]=PAYLOAD */ + static uint8_t s_crc_hi = 0; + + /* Compute how many bytes the DMA has written since last drain */ + uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx); + uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u); + + for (uint32_t i = 0; i < bytes; i++) { + uint8_t b = s_rx_buf[s_rx_tail]; + s_rx_tail = (s_rx_tail + 1u) & (JLINK_RX_BUF_LEN - 1u); + + switch (s_state) { + case PS_WAIT_STX: + if (b == JLINK_STX) s_state = PS_WAIT_LEN; + break; + + case PS_WAIT_LEN: + if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) { + /* Invalid length — resync */ + s_state = PS_WAIT_STX; + } else { + s_len = b; + s_count = 0; + s_state = PS_WAIT_DATA; + } + break; + + case PS_WAIT_DATA: + s_frame[s_count++] = b; + if (s_count == s_len) s_state = PS_WAIT_CRC_HI; + break; + + case PS_WAIT_CRC_HI: + s_crc_hi = b; + s_state = PS_WAIT_CRC_LO; + break; + + case PS_WAIT_CRC_LO: { + uint16_t rx_crc = ((uint16_t)s_crc_hi << 8) | b; + uint16_t calc_crc = crc16_xmodem(s_frame, s_len); + if (rx_crc == calc_crc) + s_state = PS_WAIT_ETX; + else + s_state = PS_WAIT_STX; /* CRC mismatch — drop */ + break; + } + + case PS_WAIT_ETX: + if (b == JLINK_ETX) { + /* Valid frame: s_frame[0]=CMD, s_frame[1..s_len-1]=PAYLOAD */ + dispatch(s_frame + 1, s_frame[0], s_len - 1u); + } + /* Either way, go back to idle (resync on bad ETX) */ + s_state = PS_WAIT_STX; + break; + } + } +} + +/* ---- jlink_send_telemetry() ---- */ +void jlink_send_telemetry(const jlink_tlm_status_t *status) +{ + /* + * Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX] + * LEN = 1 (CMD) + 20 (payload) = 21 + * Total frame length = 1+1+1+20+2+1 = 26 bytes + * At 921600 baud (10 bits/byte): 26×10/921600 ≈ 0.28ms — safe to block. + */ + static uint8_t frame[26]; + const uint8_t plen = (uint8_t)sizeof(jlink_tlm_status_t); /* 20 */ + const uint8_t len = 1u + plen; /* 21 */ + + frame[0] = JLINK_STX; + frame[1] = len; + frame[2] = JLINK_TLM_STATUS; + memcpy(&frame[3], status, plen); + + uint16_t crc = crc16_xmodem(&frame[2], len); /* over CMD + PAYLOAD */ + frame[3 + plen] = (uint8_t)(crc >> 8); + frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); + frame[3 + plen + 2] = JLINK_ETX; + + HAL_UART_Transmit(&s_uart, frame, sizeof(frame), 5u); +} diff --git a/src/main.c b/src/main.c index 60dbad6..be97481 100644 --- a/src/main.c +++ b/src/main.c @@ -16,6 +16,7 @@ #include "bmp280.h" #include "mag.h" #include "jetson_cmd.h" +#include "jlink.h" #include "battery.h" #include #include @@ -139,6 +140,9 @@ int main(void) { /* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */ crsf_init(); + /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */ + jlink_init(); + /* Init mode manager (RC/autonomous blend; CH6 mode switch) */ mode_manager_t mode; mode_manager_init(&mode); @@ -172,6 +176,7 @@ int main(void) { uint32_t balance_tick = 0; uint32_t esc_tick = 0; uint32_t crsf_telem_tick = 0; /* CRSF uplink telemetry TX timer */ + uint32_t jlink_tlm_tick = 0; /* Jetson binary telemetry TX timer */ const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ while (1) { @@ -199,6 +204,37 @@ int main(void) { safety_remote_estop_clear(); } + /* Drain JLink DMA circular buffer and parse frames */ + jlink_process(); + + /* Handle JLink one-shot flags from Jetson binary protocol */ + if (jlink_state.estop_req) { + jlink_state.estop_req = 0u; + safety_remote_estop(ESTOP_REMOTE); + safety_arm_cancel(); + balance_disarm(&bal); + motor_driver_estop(&motors); + } + if (jlink_state.arm_req) { + jlink_state.arm_req = 0u; + if (!safety_remote_estop_active() && + mpu6000_is_calibrated() && + bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { + safety_arm_start(now); + } + } + if (jlink_state.disarm_req) { + jlink_state.disarm_req = 0u; + safety_arm_cancel(); + balance_disarm(&bal); + } + if (jlink_state.pid_updated) { + jlink_state.pid_updated = 0u; + bal.kp = jlink_state.pid_kp; + bal.ki = jlink_state.pid_ki; + bal.kd = jlink_state.pid_kd; + } + /* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off. * Applies regardless of active mode (CH5 always has kill authority). */ if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) { @@ -279,11 +315,15 @@ int main(void) { jetson_cmd_process(); } - /* Balance PID at 1kHz — apply Jetson speed offset when active */ + /* Balance PID at 1kHz — apply Jetson speed offset when active. + * jlink takes priority; fall back to legacy CDC jetson_cmd if alive. */ if (imu_ret == 0 && now - balance_tick >= 1) { balance_tick = now; float base_sp = bal.setpoint; - if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset(); + if (jlink_is_active(now)) + bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG; + else if (jetson_cmd_is_active(now)) + bal.setpoint += jetson_cmd_sp_offset(); balance_update(&bal, &imu, dt); bal.setpoint = base_sp; } @@ -297,8 +337,11 @@ int main(void) { } /* Feed autonomous steer from Jetson into mode manager. + * jlink takes priority over legacy CDC jetson_cmd. * mode_manager_get_steer() blends it with RC steer per active mode. */ - if (jetson_cmd_is_active(now)) + if (jlink_is_active(now)) + mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0); + else if (jetson_cmd_is_active(now)) mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0); /* Send to hoverboard ESC at 50Hz (every 20ms) */ @@ -329,6 +372,30 @@ int main(void) { crsf_send_flight_mode(bal.state == BALANCE_ARMED); } + /* JLink STATUS telemetry at JLINK_TLM_HZ (50Hz) back to Jetson */ + if (now - jlink_tlm_tick >= (1000u / JLINK_TLM_HZ)) { + jlink_tlm_tick = now; + jlink_tlm_status_t tlm; + tlm.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); + tlm.roll_x10 = (int16_t)(imu.roll * 10.0f); + tlm.yaw_x10 = (int16_t)(imu.yaw * 10.0f); + tlm.motor_cmd = bal.motor_cmd; + uint32_t vbat = battery_read_mv(); + tlm.vbat_mv = (vbat > 65535u) ? 65535u : (uint16_t)vbat; + tlm.rssi_dbm = crsf_state.rssi_dbm; + tlm.link_quality = crsf_state.link_quality; + tlm.balance_state = (uint8_t)bal.state; + tlm.rc_armed = crsf_state.armed ? 1u : 0u; + tlm.mode = (uint8_t)mode_manager_active(&mode); + EstopSource _es = safety_get_estop(); + tlm.estop = (uint8_t)_es; + tlm.soc_pct = battery_estimate_pct(vbat); + tlm.fw_major = FW_MAJOR; + tlm.fw_minor = FW_MINOR; + tlm.fw_patch = FW_PATCH; + jlink_send_telemetry(&tlm); + } + /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */ if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) { send_tick = now; @@ -391,8 +458,9 @@ int main(void) { if (_rs >= ESTOP_REMOTE) es = (int)_rs; else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT; else es = ESTOP_CLEAR; - n = snprintf(p, rem, ",\"ja\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n", + n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n", jetson_cmd_is_active(now) ? 1 : 0, + jlink_is_active(now) ? 1 : 0, (unsigned)tx_count, (unsigned)cdc_rx_count, es); len = (int)(p + n - buf); diff --git a/test/test_jlink_frames.py b/test/test_jlink_frames.py new file mode 100644 index 0000000..b790b90 --- /dev/null +++ b/test/test_jlink_frames.py @@ -0,0 +1,416 @@ +""" +test_jlink_frames.py — Issue #120: JLink binary protocol unit tests. + +Tests CRC16-XModem, frame building, frame parsing (state machine), +command payload encoding, and telemetry frame layout. + +No HAL or STM32 hardware required — pure Python logic. +""" + +import struct +import pytest + + +# --------------------------------------------------------------------------- +# CRC16-XModem (poly 0x1021, init 0x0000) — mirrors crc16_xmodem() in jlink.c +# --------------------------------------------------------------------------- + +def crc16_xmodem(data: bytes) -> int: + crc = 0x0000 + for byte in data: + crc ^= byte << 8 + for _ in range(8): + if crc & 0x8000: + crc = (crc << 1) ^ 0x1021 + else: + crc <<= 1 + crc &= 0xFFFF + return crc + + +# --------------------------------------------------------------------------- +# Frame builder — mirrors jlink_send_telemetry() structure +# --------------------------------------------------------------------------- + +JLINK_STX = 0x02 +JLINK_ETX = 0x03 + +JLINK_CMD_HEARTBEAT = 0x01 +JLINK_CMD_DRIVE = 0x02 +JLINK_CMD_ARM = 0x03 +JLINK_CMD_DISARM = 0x04 +JLINK_CMD_PID_SET = 0x05 +JLINK_CMD_ESTOP = 0x07 +JLINK_TLM_STATUS = 0x80 + + +def build_frame(cmd: int, payload: bytes = b"") -> bytes: + """Build a JLink frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX]""" + body = bytes([cmd]) + payload + length = len(body) + crc = crc16_xmodem(body) + return bytes([JLINK_STX, length, cmd]) + payload + bytes([crc >> 8, crc & 0xFF, JLINK_ETX]) + + +def parse_frames(data: bytes) -> list: + """ + Parse all valid JLink frames from a byte stream. + Returns list of (cmd, payload) tuples for each valid frame. + """ + WAIT_STX, WAIT_LEN, WAIT_DATA, WAIT_CRC_HI, WAIT_CRC_LO, WAIT_ETX = range(6) + state = WAIT_STX + results = [] + length = 0 + buf = [] + crc_hi = 0 + + for b in data: + if state == WAIT_STX: + if b == JLINK_STX: + state = WAIT_LEN + elif state == WAIT_LEN: + if b == 0 or b > 65: + state = WAIT_STX + else: + length = b + buf = [] + state = WAIT_DATA + elif state == WAIT_DATA: + buf.append(b) + if len(buf) == length: + state = WAIT_CRC_HI + elif state == WAIT_CRC_HI: + crc_hi = b + state = WAIT_CRC_LO + elif state == WAIT_CRC_LO: + rx_crc = (crc_hi << 8) | b + calc_crc = crc16_xmodem(bytes(buf)) + if rx_crc == calc_crc: + state = WAIT_ETX + else: + state = WAIT_STX + elif state == WAIT_ETX: + if b == JLINK_ETX: + cmd = buf[0] + payload = bytes(buf[1:]) + results.append((cmd, payload)) + state = WAIT_STX + + return results + + +# --------------------------------------------------------------------------- +# CRC16 Tests +# --------------------------------------------------------------------------- + +class TestCRC16XModem: + + def test_empty(self): + assert crc16_xmodem(b"") == 0x0000 + + def test_known_vector_1(self): + # CRC16/XMODEM of b"123456789" = 0x31C3 + assert crc16_xmodem(b"123456789") == 0x31C3 + + def test_single_zero(self): + assert crc16_xmodem(b"\x00") == 0x0000 + + def test_single_ff(self): + assert crc16_xmodem(b"\xFF") == 0x1EF0 + + def test_heartbeat_cmd_byte(self): + # CRC of just CMD=0x01 + crc = crc16_xmodem(bytes([JLINK_CMD_HEARTBEAT])) + assert isinstance(crc, int) + assert 0 <= crc <= 0xFFFF + + def test_drive_payload(self): + payload = struct.pack(" bytes: + return struct.pack(STATUS_FMT, + pitch, roll, yaw, motor_cmd, vbat_mv, + rssi_dbm, link_quality, balance_state, rc_armed, + mode, estop, soc_pct, fw_major, fw_minor, fw_patch) + + +class TestStatusTelemetry: + + def test_status_payload_size(self): + assert STATUS_SIZE == 20 + + def test_status_frame_total_length(self): + payload = make_status() + frame = build_frame(JLINK_TLM_STATUS, payload) + # [STX][LEN=21][0x80][20 bytes][CRC_hi][CRC_lo][ETX] = 26 bytes + assert len(frame) == 26 + + def test_status_len_field(self): + payload = make_status() + frame = build_frame(JLINK_TLM_STATUS, payload) + assert frame[1] == 21 # 1 (CMD) + 20 (STATUS payload) + + def test_status_cmd_byte(self): + payload = make_status() + frame = build_frame(JLINK_TLM_STATUS, payload) + assert frame[2] == JLINK_TLM_STATUS + + def test_status_parseable(self): + payload = make_status(pitch=123, roll=-45, yaw=678, motor_cmd=500, + vbat_mv=11800, rssi_dbm=-90, link_quality=80, + balance_state=1, rc_armed=1, mode=2, estop=0, + soc_pct=60, fw_major=1, fw_minor=0, fw_patch=0) + frame = build_frame(JLINK_TLM_STATUS, payload) + results = parse_frames(frame) + assert len(results) == 1 + cmd, data = results[0] + assert cmd == JLINK_TLM_STATUS + vals = struct.unpack(STATUS_FMT, data) + pitch, roll, yaw, motor_cmd, vbat_mv, rssi_dbm, lq, bs, ra, mode, es, soc, fwma, fwmi, fwpa = vals + assert pitch == 123 + assert roll == -45 + assert yaw == 678 + assert motor_cmd == 500 + assert vbat_mv == 11800 + assert rssi_dbm == -90 + assert lq == 80 + assert bs == 1 + assert ra == 1 + assert mode == 2 + assert es == 0 + assert soc == 60 + assert fwma == 1 + assert fwmi == 0 + assert fwpa == 0 + + def test_pitch_encoding(self): + """pitch_deg * 10 fits in int16 for ±3276 degrees.""" + for deg in [-20.0, 0.0, 5.5, 15.3, -10.1]: + encoded = int(deg * 10) + payload = make_status(pitch=encoded) + frame = build_frame(JLINK_TLM_STATUS, payload) + results = parse_frames(frame) + assert len(results) == 1 + vals = struct.unpack(STATUS_FMT, results[0][1]) + assert vals[0] == encoded + + def test_vbat_range_3s(self): + """3S battery 9900–12600 mV fits in uint16.""" + for mv in [9900, 11100, 12600]: + payload = make_status(vbat_mv=mv) + frame = build_frame(JLINK_TLM_STATUS, payload) + results = parse_frames(frame) + vals = struct.unpack(STATUS_FMT, results[0][1]) + assert vals[4] == mv + + def test_vbat_range_4s(self): + """4S battery 13200–16800 mV fits in uint16.""" + for mv in [13200, 14800, 16800]: + payload = make_status(vbat_mv=mv) + frame = build_frame(JLINK_TLM_STATUS, payload) + results = parse_frames(frame) + vals = struct.unpack(STATUS_FMT, results[0][1]) + assert vals[4] == mv + + def test_fw_version_fields(self): + payload = make_status(fw_major=2, fw_minor=3, fw_patch=7) + frame = build_frame(JLINK_TLM_STATUS, payload) + results = parse_frames(frame) + vals = struct.unpack(STATUS_FMT, results[0][1]) + assert vals[-3] == 2 + assert vals[-2] == 3 + assert vals[-1] == 7 + + def test_soc_pct_unknown(self): + """soc_pct=255 encodes as 0xFF (unknown).""" + payload = make_status(soc_pct=255) + frame = build_frame(JLINK_TLM_STATUS, payload) + results = parse_frames(frame) + vals = struct.unpack(STATUS_FMT, results[0][1]) + assert vals[11] == 255