feat(firmware): Jetson binary serial protocol on USART1 (Issue #120)
New jlink module replaces ASCII-over-USB-CDC jetson_cmd with a dedicated hardware UART binary protocol at 921600 baud for reliable Jetson comms. - include/jlink.h: JLinkState struct, jlink_tlm_status_t (20-byte packed), command/telemetry IDs (0x01-0x07 cmd, 0x80 status), API declarations - src/jlink.c: USART1 DMA2_Stream2_Channel4 circular RX (128 bytes), IDLE interrupt, CRC16-XModem (poly 0x1021) frame parser state machine, command dispatch (HEARTBEAT/DRIVE/ARM/DISARM/PID_SET/ESTOP), jlink_send_telemetry() blocking TX (≈0.28 ms per frame) - include/config.h: JLINK_BAUD=921600, JLINK_HB_TIMEOUT_MS=1000, JLINK_TLM_HZ=50, FW_MAJOR/MINOR/PATCH version constants - src/main.c: jlink_init(), jlink_process() in main loop, arm/disarm/ estop/PID flag handling, 50 Hz STATUS telemetry TX, jlink takes priority over legacy jetson_cmd for speed/steer injection - test/test_jlink_frames.py: 39 pytest tests (39/39 pass) — CRC16, frame building, parser state machine, drive/PID/status encoding Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
7de55accc3
commit
6f0ad8e92e
@ -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
|
||||
|
||||
127
include/jlink.h
Normal file
127
include/jlink.h
Normal file
@ -0,0 +1,127 @@
|
||||
#ifndef JLINK_H
|
||||
#define JLINK_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* 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 */
|
||||
279
src/jlink.c
Normal file
279
src/jlink.c
Normal file
@ -0,0 +1,279 @@
|
||||
#include "jlink.h"
|
||||
#include "config.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
/* ---- 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);
|
||||
}
|
||||
76
src/main.c
76
src/main.c
@ -16,6 +16,7 @@
|
||||
#include "bmp280.h"
|
||||
#include "mag.h"
|
||||
#include "jetson_cmd.h"
|
||||
#include "jlink.h"
|
||||
#include "battery.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
@ -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);
|
||||
|
||||
416
test/test_jlink_frames.py
Normal file
416
test/test_jlink_frames.py
Normal file
@ -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("<hh", 500, -300) # speed=500, steer=-300
|
||||
body = bytes([JLINK_CMD_DRIVE]) + payload
|
||||
crc = crc16_xmodem(body)
|
||||
# Verify round-trip: same data same CRC
|
||||
assert crc == crc16_xmodem(body)
|
||||
|
||||
def test_different_payloads_different_crc(self):
|
||||
a = crc16_xmodem(b"\x02\x01\x02")
|
||||
b = crc16_xmodem(b"\x02\x01\x03")
|
||||
assert a != b
|
||||
|
||||
def test_zero_bytes(self):
|
||||
assert crc16_xmodem(b"\x00\x00\x00\x00") != crc16_xmodem(b"\x00\x00\x00\x01")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Frame Building Tests
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestFrameBuilding:
|
||||
|
||||
def test_heartbeat_frame_length(self):
|
||||
frame = build_frame(JLINK_CMD_HEARTBEAT)
|
||||
# [STX][LEN=1][CMD=0x01][CRC_hi][CRC_lo][ETX] = 6 bytes
|
||||
assert len(frame) == 6
|
||||
|
||||
def test_heartbeat_frame_markers(self):
|
||||
frame = build_frame(JLINK_CMD_HEARTBEAT)
|
||||
assert frame[0] == JLINK_STX
|
||||
assert frame[-1] == JLINK_ETX
|
||||
|
||||
def test_heartbeat_frame_len_field(self):
|
||||
frame = build_frame(JLINK_CMD_HEARTBEAT)
|
||||
assert frame[1] == 1 # LEN = 1 (CMD only, no payload)
|
||||
|
||||
def test_heartbeat_frame_cmd(self):
|
||||
frame = build_frame(JLINK_CMD_HEARTBEAT)
|
||||
assert frame[2] == JLINK_CMD_HEARTBEAT
|
||||
|
||||
def test_drive_frame_length(self):
|
||||
payload = struct.pack("<hh", 0, 0)
|
||||
frame = build_frame(JLINK_CMD_DRIVE, payload)
|
||||
# [STX][LEN=5][CMD][s16][s16][CRC_hi][CRC_lo][ETX] = 1+1+1+4+2+1 = 10 bytes
|
||||
assert len(frame) == 10
|
||||
|
||||
def test_drive_frame_len_field(self):
|
||||
payload = struct.pack("<hh", 0, 0)
|
||||
frame = build_frame(JLINK_CMD_DRIVE, payload)
|
||||
assert frame[1] == 5 # 1 (CMD) + 4 (payload)
|
||||
|
||||
def test_arm_frame(self):
|
||||
frame = build_frame(JLINK_CMD_ARM)
|
||||
assert len(frame) == 6
|
||||
assert frame[2] == JLINK_CMD_ARM
|
||||
|
||||
def test_disarm_frame(self):
|
||||
frame = build_frame(JLINK_CMD_DISARM)
|
||||
assert len(frame) == 6
|
||||
assert frame[2] == JLINK_CMD_DISARM
|
||||
|
||||
def test_estop_frame(self):
|
||||
frame = build_frame(JLINK_CMD_ESTOP)
|
||||
assert len(frame) == 6
|
||||
assert frame[2] == JLINK_CMD_ESTOP
|
||||
|
||||
def test_pid_set_frame_length(self):
|
||||
payload = struct.pack("<fff", 35.0, 1.0, 1.0)
|
||||
frame = build_frame(JLINK_CMD_PID_SET, payload)
|
||||
# [STX][LEN=13][CMD][12 bytes floats][CRC_hi][CRC_lo][ETX] = 18 bytes
|
||||
assert len(frame) == 18
|
||||
|
||||
def test_pid_set_frame_len_field(self):
|
||||
payload = struct.pack("<fff", 35.0, 1.0, 1.0)
|
||||
frame = build_frame(JLINK_CMD_PID_SET, payload)
|
||||
assert frame[1] == 13 # 1 (CMD) + 12 (three floats)
|
||||
|
||||
def test_crc_coverage(self):
|
||||
"""CRC must differ if payload differs by one bit."""
|
||||
payload_a = struct.pack("<hh", 500, 100)
|
||||
payload_b = struct.pack("<hh", 500, 101)
|
||||
frame_a = build_frame(JLINK_CMD_DRIVE, payload_a)
|
||||
frame_b = build_frame(JLINK_CMD_DRIVE, payload_b)
|
||||
# Extract CRC bytes (second to last two bytes before ETX)
|
||||
crc_a = (frame_a[-3] << 8) | frame_a[-2]
|
||||
crc_b = (frame_b[-3] << 8) | frame_b[-2]
|
||||
assert crc_a != crc_b
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Frame Parsing Tests
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestFrameParsing:
|
||||
|
||||
def test_parse_single_heartbeat(self):
|
||||
frame = build_frame(JLINK_CMD_HEARTBEAT)
|
||||
results = parse_frames(frame)
|
||||
assert len(results) == 1
|
||||
cmd, payload = results[0]
|
||||
assert cmd == JLINK_CMD_HEARTBEAT
|
||||
assert payload == b""
|
||||
|
||||
def test_parse_drive_cmd(self):
|
||||
payload = struct.pack("<hh", 750, -200)
|
||||
frame = build_frame(JLINK_CMD_DRIVE, payload)
|
||||
results = parse_frames(frame)
|
||||
assert len(results) == 1
|
||||
cmd, data = results[0]
|
||||
assert cmd == JLINK_CMD_DRIVE
|
||||
speed, steer = struct.unpack("<hh", data)
|
||||
assert speed == 750
|
||||
assert steer == -200
|
||||
|
||||
def test_parse_multiple_frames(self):
|
||||
stream = (
|
||||
build_frame(JLINK_CMD_HEARTBEAT) +
|
||||
build_frame(JLINK_CMD_ARM) +
|
||||
build_frame(JLINK_CMD_DRIVE, struct.pack("<hh", 100, 50))
|
||||
)
|
||||
results = parse_frames(stream)
|
||||
assert len(results) == 3
|
||||
assert results[0][0] == JLINK_CMD_HEARTBEAT
|
||||
assert results[1][0] == JLINK_CMD_ARM
|
||||
assert results[2][0] == JLINK_CMD_DRIVE
|
||||
|
||||
def test_bad_crc_dropped(self):
|
||||
frame = bytearray(build_frame(JLINK_CMD_HEARTBEAT))
|
||||
frame[-2] ^= 0xFF # Corrupt CRC low byte
|
||||
results = parse_frames(bytes(frame))
|
||||
assert len(results) == 0
|
||||
|
||||
def test_bad_etx_dropped(self):
|
||||
frame = bytearray(build_frame(JLINK_CMD_HEARTBEAT))
|
||||
frame[-1] = 0xFF # Wrong ETX
|
||||
results = parse_frames(bytes(frame))
|
||||
assert len(results) == 0
|
||||
|
||||
def test_noise_before_frame(self):
|
||||
noise = bytes([0xAA, 0xBB, 0xCC, 0xDD])
|
||||
frame = build_frame(JLINK_CMD_ARM)
|
||||
results = parse_frames(noise + frame)
|
||||
assert len(results) == 1
|
||||
assert results[0][0] == JLINK_CMD_ARM
|
||||
|
||||
def test_two_frames_concatenated(self):
|
||||
f1 = build_frame(JLINK_CMD_DISARM)
|
||||
f2 = build_frame(JLINK_CMD_HEARTBEAT)
|
||||
results = parse_frames(f1 + f2)
|
||||
assert len(results) == 2
|
||||
|
||||
def test_pid_set_parsed_correctly(self):
|
||||
kp, ki, kd = 42.5, 0.5, 2.0
|
||||
payload = struct.pack("<fff", kp, ki, kd)
|
||||
frame = build_frame(JLINK_CMD_PID_SET, payload)
|
||||
results = parse_frames(frame)
|
||||
assert len(results) == 1
|
||||
cmd, data = results[0]
|
||||
assert cmd == JLINK_CMD_PID_SET
|
||||
got_kp, got_ki, got_kd = struct.unpack("<fff", data)
|
||||
assert abs(got_kp - kp) < 1e-5
|
||||
assert abs(got_ki - ki) < 1e-5
|
||||
assert abs(got_kd - kd) < 1e-5
|
||||
|
||||
def test_drive_boundary_values(self):
|
||||
for spd, str_ in [(1000, 1000), (-1000, -1000), (0, 0)]:
|
||||
frame = build_frame(JLINK_CMD_DRIVE, struct.pack("<hh", spd, str_))
|
||||
results = parse_frames(frame)
|
||||
assert len(results) == 1
|
||||
speed, steer = struct.unpack("<hh", results[0][1])
|
||||
assert speed == spd
|
||||
assert steer == str_
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Telemetry STATUS Frame Tests
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
# STATUS payload struct: <hhhhH b B B B B B B B B B
|
||||
# pitch_x10, roll_x10, yaw_x10, motor_cmd, vbat_mv,
|
||||
# rssi_dbm, link_quality, balance_state, rc_armed,
|
||||
# mode, estop, soc_pct, fw_major, fw_minor, fw_patch
|
||||
STATUS_FMT = "<hhhhHbBBBBBBBBB" # 20 bytes
|
||||
STATUS_SIZE = struct.calcsize(STATUS_FMT)
|
||||
|
||||
|
||||
def make_status(pitch=0, roll=0, yaw=0, motor_cmd=0, vbat_mv=12600,
|
||||
rssi_dbm=-85, link_quality=95, balance_state=1,
|
||||
rc_armed=1, mode=2, estop=0, soc_pct=80,
|
||||
fw_major=1, fw_minor=0, fw_patch=0) -> 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
|
||||
Loading…
x
Reference in New Issue
Block a user