From 8bc2f9eeacc634ae46852d444a12806e5e4453b1 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Tue, 17 Mar 2026 19:10:11 -0400 Subject: [PATCH] feat: bxCAN integration for VESC motor control and Orin comms (Issue #674) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - can_driver: add filter bank 15 (all ext IDs → FIFO1) and widen bank 14 to accept all standard IDs; add can_driver_send_ext/std and ext/std frame callbacks (can_driver_set_ext_cb / can_driver_set_std_cb) - vesc_can: VESC 29-bit extended CAN protocol driver — send RPM to IDs 56 and 68 (FSESC 6.7 Pro Mini Dual), parse STATUS/STATUS_4/STATUS_5 big-endian payloads, alive timeout, JLINK_TLM_VESC_STATE at 1 Hz - orin_can: Orin↔FC standard CAN protocol — HEARTBEAT/DRIVE/MODE/ESTOP commands in, FC_STATUS + FC_VESC broadcast at 10 Hz - jlink: add JLINK_TLM_VESC_STATE (0x8E), jlink_tlm_vesc_state_t (22 bytes), jlink_send_vesc_state_tlm() - main: wire vesc_can_init/orin_can_init; replace can_driver_send_cmd with vesc_can_send_rpm; inject Orin CAN speed/steer into balance PID; add Orin CAN estop/clear handling; add orin_can_broadcast at 10 Hz - test: 56-test host-side suite for vesc_can; test/stubs/stm32f7xx_hal.h minimal HAL stub for all future host-side tests Safety: balance PID runs independently on Mamba — if Orin CAN link drops (orin_can_is_alive() == false) the robot continues balancing in-place. Co-Authored-By: Claude Sonnet 4.6 --- include/can_driver.h | 20 ++ include/jlink.h | 24 ++ include/orin_can.h | 103 ++++++++ include/vesc_can.h | 117 +++++++++ src/can_driver.c | 136 ++++++++-- src/jlink.c | 25 ++ src/main.c | 96 ++++--- src/orin_can.c | 96 +++++++ src/vesc_can.c | 141 ++++++++++ test/stubs/stm32f7xx_hal.h | 126 +++++++++ test/test_vesc_can.c | 518 +++++++++++++++++++++++++++++++++++++ 11 files changed, 1355 insertions(+), 47 deletions(-) create mode 100644 include/orin_can.h create mode 100644 include/vesc_can.h create mode 100644 src/orin_can.c create mode 100644 src/vesc_can.c create mode 100644 test/stubs/stm32f7xx_hal.h create mode 100644 test/test_vesc_can.c diff --git a/include/can_driver.h b/include/can_driver.h index 15f067d..939b469 100644 --- a/include/can_driver.h +++ b/include/can_driver.h @@ -78,4 +78,24 @@ void can_driver_get_stats(can_stats_t *out); /* Drain RX FIFO0; call every main-loop tick */ void can_driver_process(void); +/* ---- Extended / standard frame support (Issue #674) ---- */ + +/* Callback for extended-ID (29-bit) frames arriving in FIFO1 (VESC STATUS) */ +typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len); + +/* Callback for standard-ID (11-bit) frames arriving in FIFO0 (Orin commands) */ +typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len); + +/* Register callback for 29-bit extended frames (register before can_driver_init) */ +void can_driver_set_ext_cb(can_ext_frame_cb_t cb); + +/* Register callback for 11-bit standard frames (register before can_driver_init) */ +void can_driver_set_std_cb(can_std_frame_cb_t cb); + +/* Transmit a 29-bit extended-ID data frame (VESC RPM/current commands) */ +void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len); + +/* Transmit an 11-bit standard-ID data frame (Orin telemetry broadcast) */ +void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len); + #endif /* CAN_DRIVER_H */ diff --git a/include/jlink.h b/include/jlink.h index 5e55ba6..9ff28b6 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -99,6 +99,7 @@ #define JLINK_TLM_STEERING 0x8Au /* jlink_tlm_steering_t (8 bytes, Issue #616) */ #define JLINK_TLM_LVC 0x8Bu /* jlink_tlm_lvc_t (4 bytes, Issue #613) */ #define JLINK_TLM_ODOM 0x8Cu /* jlink_tlm_odom_t (16 bytes, Issue #632) */ +#define JLINK_TLM_VESC_STATE 0x8Eu /* jlink_tlm_vesc_state_t (22 bytes, Issue #674) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -239,6 +240,22 @@ typedef struct __attribute__((packed)) { int16_t speed_mmps; /* linear speed of centre point (mm/s) */ } jlink_tlm_odom_t; /* 16 bytes */ +/* ---- Telemetry VESC_STATE payload (22 bytes, packed) Issue #674 ---- */ +/* Sent at VESC_TLM_HZ (1 Hz) by vesc_can_send_tlm(). */ +typedef struct __attribute__((packed)) { + int32_t left_rpm; /* left VESC actual RPM */ + int32_t right_rpm; /* right VESC actual RPM */ + int16_t left_current_x10; /* left phase current (A × 10) */ + int16_t right_current_x10; /* right phase current (A × 10) */ + int16_t left_temp_x10; /* left FET temperature (°C × 10) */ + int16_t right_temp_x10; /* right FET temperature (°C × 10) */ + int16_t voltage_x10; /* input voltage (V × 10; from STATUS_5) */ + uint8_t left_fault; /* left VESC fault code (0 = none) */ + uint8_t right_fault; /* right VESC fault code (0 = none) */ + uint8_t left_alive; /* 1 = left VESC alive (STATUS within 1 s) */ + uint8_t right_alive; /* 1 = right VESC alive (STATUS within 1 s) */ +} jlink_tlm_vesc_state_t; /* 22 bytes */ + /* ---- Volatile state (read from main loop) ---- */ typedef struct { /* Drive command - updated on JLINK_CMD_DRIVE */ @@ -377,4 +394,11 @@ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm); */ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm); +/* + * jlink_send_vesc_state_tlm(tlm) - transmit JLINK_TLM_VESC_STATE (0x8E) frame + * (28 bytes total) at VESC_TLM_HZ (1 Hz). Issue #674. + * Rate-limiting handled by vesc_can_send_tlm(); call from there only. + */ +void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm); + #endif /* JLINK_H */ diff --git a/include/orin_can.h b/include/orin_can.h new file mode 100644 index 0000000..36c0c79 --- /dev/null +++ b/include/orin_can.h @@ -0,0 +1,103 @@ +#ifndef ORIN_CAN_H +#define ORIN_CAN_H + +#include +#include + +/* + * orin_can — Orin↔FC CAN protocol driver (Issue #674). + * + * Standard 11-bit CAN IDs on CAN2, FIFO0. + * + * Orin → FC commands: + * 0x300 HEARTBEAT : uint32 sequence counter (4 bytes) + * 0x301 DRIVE : int16 speed (−1000..+1000), int16 steer (−1000..+1000) + * 0x302 MODE : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS) + * 0x303 ESTOP : uint8 action (1=ESTOP, 0=CLEAR) + * + * FC → Orin telemetry (broadcast at ORIN_TLM_HZ): + * 0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, + * uint8 balance_state, uint8 flags [bit0=estop, bit1=armed] + * 0x401 FC_VESC : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10, + * int16 left_current_x10, int16 right_current_x10 + * + * Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC + * continues balancing in-place — Orin commands are simply not injected. + * The balance PID loop runs entirely on Mamba and never depends on Orin. + */ + +/* ---- Orin → FC command IDs ---- */ +#define ORIN_CAN_ID_HEARTBEAT 0x300u +#define ORIN_CAN_ID_DRIVE 0x301u +#define ORIN_CAN_ID_MODE 0x302u +#define ORIN_CAN_ID_ESTOP 0x303u + +/* ---- FC → Orin telemetry IDs ---- */ +#define ORIN_CAN_ID_FC_STATUS 0x400u +#define ORIN_CAN_ID_FC_VESC 0x401u + +/* ---- Timing ---- */ +#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */ +#define ORIN_TLM_HZ 10u /* FC → Orin broadcast rate (Hz) */ + +/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */ +typedef struct { + volatile int16_t speed; /* DRIVE: −1000..+1000 */ + volatile int16_t steer; /* DRIVE: −1000..+1000 */ + volatile uint8_t mode; /* MODE: robot_mode_t value */ + volatile uint8_t drive_updated; /* set on DRIVE, cleared by main */ + volatile uint8_t mode_updated; /* set on MODE, cleared by main */ + volatile uint8_t estop_req; /* set on ESTOP(1), cleared by main */ + volatile uint8_t estop_clear_req; /* set on ESTOP(0), cleared by main */ + volatile uint32_t last_rx_ms; /* HAL_GetTick() of last received frame */ +} OrinCanState; + +extern volatile OrinCanState orin_can_state; + +/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */ + +typedef struct __attribute__((packed)) { + int16_t pitch_x10; /* pitch degrees × 10 */ + int16_t motor_cmd; /* balance PID output −1000..+1000 */ + uint16_t vbat_mv; /* battery voltage (mV) */ + uint8_t balance_state; /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */ + uint8_t flags; /* bit0=estop_active, bit1=armed */ +} orin_can_fc_status_t; /* 8 bytes */ + +typedef struct __attribute__((packed)) { + int16_t left_rpm_x10; /* left wheel RPM / 10 (±32767 × 10 = ±327k RPM) */ + int16_t right_rpm_x10; /* right wheel RPM / 10 */ + int16_t left_current_x10; /* left phase current × 10 (A) */ + int16_t right_current_x10; /* right phase current × 10 (A) */ +} orin_can_fc_vesc_t; /* 8 bytes */ + +/* ---- API ---- */ + +/* + * orin_can_init() — zero state, register orin_can_on_frame as std_cb with + * can_driver. Call after can_driver_init(). + */ +void orin_can_init(void); + +/* + * orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each + * standard-ID frame in FIFO0. Updates orin_can_state. + */ +void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len); + +/* + * orin_can_is_alive(now_ms) — true if a frame from Orin arrived within + * ORIN_HB_TIMEOUT_MS of now_ms. + */ +bool orin_can_is_alive(uint32_t now_ms); + +/* + * orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of + * FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz). + * Safe to call every ms; internally rate-limited. + */ +void orin_can_broadcast(uint32_t now_ms, + const orin_can_fc_status_t *status, + const orin_can_fc_vesc_t *vesc); + +#endif /* ORIN_CAN_H */ diff --git a/include/vesc_can.h b/include/vesc_can.h new file mode 100644 index 0000000..21f67d1 --- /dev/null +++ b/include/vesc_can.h @@ -0,0 +1,117 @@ +#ifndef VESC_CAN_H +#define VESC_CAN_H + +#include +#include + +/* + * vesc_can — VESC CAN protocol driver for FSESC 6.7 Pro Mini Dual (Issue #674). + * + * VESC uses 29-bit extended CAN IDs: + * arbitration_id = (packet_type << 8) | vesc_node_id + * + * Wire format is big-endian throughout (matches VESC FW 6.x). + * + * Physical layer: CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps. + * + * NOTE ON PA11/PA12 vs PB12/PB13: + * PA11/PA12 carry CAN1_RX/TX (AF9) BUT are also USB_OTG_FS DM/DP (AF10). + * USB CDC is active on this board, so PA11/PA12 are occupied. + * PB8/PB9 (CAN1 alternate) are occupied by I2C1 (barometer). + * CAN2 on PB12/PB13 is the only conflict-free choice. + * If the SN65HVD230 is wired to the pads labelled RX6/TX6 on the Mamba + * silkscreen, those pads connect to PB12/PB13 (SPI2/OSD, repurposed). + * + * VESC frames arrive in FIFO1 (extended-ID filter, bank 15). + * Orin standard frames arrive in FIFO0 (standard-ID filter, bank 14). + */ + +/* ---- VESC packet type IDs (upper byte of 29-bit arb ID) ---- */ +#define VESC_PKT_SET_DUTY 0u /* int32 duty × 100000 */ +#define VESC_PKT_SET_CURRENT 1u /* int32 current (mA) */ +#define VESC_PKT_SET_CURRENT_BRAKE 2u /* int32 brake current (mA) */ +#define VESC_PKT_SET_RPM 3u /* int32 target RPM */ +#define VESC_PKT_STATUS 9u /* int32 RPM, int16 I×10, int16 duty×1000 */ +#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */ +#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */ + +/* ---- Default VESC node IDs (configurable via vesc_can_init) ---- */ +#define VESC_CAN_ID_LEFT 56u +#define VESC_CAN_ID_RIGHT 68u + +/* ---- Alive timeout ---- */ +#define VESC_ALIVE_TIMEOUT_MS 1000u /* node offline if no STATUS for 1 s */ + +/* ---- JLink telemetry rate ---- */ +#define VESC_TLM_HZ 1u + +/* ---- Fault codes (VESC FW 6.6) ---- */ +#define VESC_FAULT_NONE 0u +#define VESC_FAULT_OVER_VOLTAGE 1u +#define VESC_FAULT_UNDER_VOLTAGE 2u +#define VESC_FAULT_DRV 3u +#define VESC_FAULT_ABS_OVER_CURRENT 4u +#define VESC_FAULT_OVER_TEMP_FET 5u +#define VESC_FAULT_OVER_TEMP_MOTOR 6u +#define VESC_FAULT_GATE_DRIVER_OVER_VOLTAGE 7u +#define VESC_FAULT_GATE_DRIVER_UNDER_VOLTAGE 8u +#define VESC_FAULT_MCU_UNDER_VOLTAGE 9u +#define VESC_FAULT_WATCHDOG_RESET 10u + +/* ---- Telemetry state per VESC node ---- */ +typedef struct { + int32_t rpm; /* actual RPM (STATUS pkt, int32 BE) */ + int16_t current_x10; /* phase current (A × 10; STATUS pkt) */ + int16_t duty_x1000; /* duty cycle (× 1000; –1000..+1000) */ + int16_t temp_fet_x10; /* FET temperature (°C × 10; STATUS_4) */ + int16_t temp_motor_x10; /* motor temperature (°C × 10; STATUS_4) */ + int16_t current_in_x10; /* input (battery) current (A × 10; STATUS_4) */ + int16_t voltage_x10; /* input voltage (V × 10; STATUS_5) */ + uint8_t fault_code; /* VESC fault code (0 = none) */ + uint8_t _pad; + uint32_t last_rx_ms; /* HAL_GetTick() of last received STATUS frame */ +} vesc_state_t; + +/* ---- API ---- */ + +/* + * vesc_can_init(id_left, id_right) — store VESC node IDs and register the + * extended-frame callback with can_driver. + * Call after can_driver_init(). + */ +void vesc_can_init(uint8_t id_left, uint8_t id_right); + +/* + * vesc_can_send_rpm(vesc_id, rpm) — transmit VESC_PKT_SET_RPM (3) to the + * target VESC. arb_id = (3 << 8) | vesc_id. Payload: int32 big-endian. + */ +void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm); + +/* + * vesc_can_on_frame(ext_id, data, len) — called by can_driver when an + * extended-ID frame arrives (registered via can_driver_set_ext_cb). + * Parses STATUS / STATUS_4 / STATUS_5 into the matching vesc_state_t. + */ +void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len); + +/* + * vesc_can_get_state(vesc_id, out) — copy latest telemetry snapshot. + * vesc_id must match id_left or id_right passed to vesc_can_init. + * Returns false if vesc_id unknown or no frame has arrived yet. + */ +bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out); + +/* + * vesc_can_is_alive(vesc_id, now_ms) — true if a STATUS frame arrived + * within VESC_ALIVE_TIMEOUT_MS of now_ms. + */ +bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms); + +/* + * vesc_can_send_tlm(now_ms) — rate-limited JLINK_TLM_VESC_STATE (0x8E) + * telemetry to Orin over JLink. Safe to call every ms; internally + * rate-limited to VESC_TLM_HZ (1 Hz). + */ +void vesc_can_send_tlm(uint32_t now_ms); + +#endif /* VESC_CAN_H */ diff --git a/src/can_driver.c b/src/can_driver.c index fbdc73d..cd7e61c 100644 --- a/src/can_driver.c +++ b/src/can_driver.c @@ -10,6 +10,8 @@ static CAN_HandleTypeDef s_can; static volatile can_feedback_t s_feedback[CAN_NUM_MOTORS]; static volatile can_stats_t s_stats; +static can_ext_frame_cb_t s_ext_cb = NULL; +static can_std_frame_cb_t s_std_cb = NULL; void can_driver_init(void) { @@ -48,16 +50,17 @@ void can_driver_init(void) return; } - /* Filter bank 14: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F - * FilterIdHigh = 0x200 << 5 = 0x4000 (base ID shifted to bit[15:5]) - * FilterMaskHigh = 0x7E0 << 5 = 0xFC00 (mask: top 6 bits must match) */ + /* Filter bank 14: 32-bit mask, FIFO0, accept ALL standard 11-bit frames. + * FilterIdHigh=0, FilterMaskHigh=0 → mask=0 passes every standard ID. + * FilterIdLow[2]=0 (IDE=0) → only standard frames; mask bit[2]=0 → don't + * check IDE bit, but standard frames have IDE=0 by design so all pass. */ CAN_FilterTypeDef flt = {0}; flt.FilterBank = 14u; flt.FilterMode = CAN_FILTERMODE_IDMASK; flt.FilterScale = CAN_FILTERSCALE_32BIT; - flt.FilterIdHigh = (uint16_t)(CAN_FILTER_STDID << 5u); + flt.FilterIdHigh = 0u; flt.FilterIdLow = 0u; - flt.FilterMaskIdHigh = (uint16_t)(CAN_FILTER_MASK << 5u); + flt.FilterMaskIdHigh = 0u; flt.FilterMaskIdLow = 0u; flt.FilterFIFOAssignment = CAN_RX_FIFO0; flt.FilterActivation = CAN_FILTER_ENABLE; @@ -68,6 +71,28 @@ void can_driver_init(void) return; } + /* Filter bank 15: 32-bit mask, FIFO1, accept ALL extended 29-bit frames. + * For extended frames, the IDE bit sits at FilterIdLow[2]. + * FilterIdLow = 0x0004 (IDE=1) → match extended frames. + * FilterMaskIdLow = 0x0004 → only the IDE bit is checked; all ext IDs pass. + * This routes every VESC STATUS / STATUS_4 / STATUS_5 frame to FIFO1. */ + CAN_FilterTypeDef flt2 = {0}; + flt2.FilterBank = 15u; + flt2.FilterMode = CAN_FILTERMODE_IDMASK; + flt2.FilterScale = CAN_FILTERSCALE_32BIT; + flt2.FilterIdHigh = 0u; + flt2.FilterIdLow = 0x0004u; /* IDE = 1 */ + flt2.FilterMaskIdHigh = 0u; + flt2.FilterMaskIdLow = 0x0004u; /* only check IDE bit */ + flt2.FilterFIFOAssignment = CAN_RX_FIFO1; + flt2.FilterActivation = CAN_FILTER_ENABLE; + flt2.SlaveStartFilterBank = 14u; + + if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) { + s_stats.bus_off = 1u; + return; + } + HAL_CAN_Start(&s_can); memset((void *)s_feedback, 0, sizeof(s_feedback)); @@ -136,7 +161,7 @@ void can_driver_process(void) } s_stats.bus_off = 0u; - /* Drain FIFO0 */ + /* Drain FIFO0 (standard-ID frames: Orin commands + legacy feedback) */ while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) { CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8]; @@ -146,31 +171,106 @@ void can_driver_process(void) break; } - /* Only process data frames with standard IDs */ if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) { continue; } - /* Decode feedback frames: base 0x200, one per node */ + /* Dispatch to registered standard-frame callback (Orin CAN protocol) */ + if (s_std_cb != NULL) { + s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC); + } + + /* Legacy: decode feedback frames at 0x200-0x21F (Issue #597) */ uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE; - if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) { + if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) { + uint8_t nid = (uint8_t)nid_u; + s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u)); + s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u)); + s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u)); + s_feedback[nid].temperature_c = (int8_t)rxdata[6]; + s_feedback[nid].fault = rxdata[7]; + s_feedback[nid].last_rx_ms = HAL_GetTick(); + } + + s_stats.rx_count++; + } + + /* Drain FIFO1 (extended-ID frames: VESC STATUS/STATUS_4/STATUS_5) */ + while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) { + CAN_RxHeaderTypeDef rxhdr; + uint8_t rxdata[8]; + + if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) { + s_stats.err_count++; + break; + } + + if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) { continue; } - uint8_t nid = (uint8_t)nid_u; - - /* Layout: [0-1] vel_rpm [2-3] current_ma [4-5] pos_x100 [6] temp_c [7] fault */ - s_feedback[nid].velocity_rpm = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u)); - s_feedback[nid].current_ma = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u)); - s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u)); - s_feedback[nid].temperature_c = (int8_t)rxdata[6]; - s_feedback[nid].fault = rxdata[7]; - s_feedback[nid].last_rx_ms = HAL_GetTick(); + if (s_ext_cb != NULL) { + s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC); + } s_stats.rx_count++; } } +void can_driver_set_ext_cb(can_ext_frame_cb_t cb) +{ + s_ext_cb = cb; +} + +void can_driver_set_std_cb(can_std_frame_cb_t cb) +{ + s_std_cb = cb; +} + +void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len) +{ + if (s_stats.bus_off || len > 8u) { + return; + } + + CAN_TxHeaderTypeDef hdr = {0}; + hdr.ExtId = ext_id; + hdr.IDE = CAN_ID_EXT; + hdr.RTR = CAN_RTR_DATA; + hdr.DLC = len; + + uint32_t mailbox; + if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) { + if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) { + s_stats.tx_count++; + } else { + s_stats.err_count++; + } + } +} + +void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len) +{ + if (s_stats.bus_off || len > 8u) { + return; + } + + CAN_TxHeaderTypeDef hdr = {0}; + hdr.StdId = std_id; + hdr.IDE = CAN_ID_STD; + hdr.RTR = CAN_RTR_DATA; + hdr.DLC = len; + + uint32_t mailbox; + if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) { + if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) { + s_stats.tx_count++; + } else { + s_stats.err_count++; + } + } +} + bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out) { if (node_id >= CAN_NUM_MOTORS || out == NULL) { diff --git a/src/jlink.c b/src/jlink.c index 3ccee4b..614a399 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -662,3 +662,28 @@ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm) jlink_tx_locked(frame, sizeof(frame)); } + +/* ---- jlink_send_vesc_state_tlm() -- Issue #674 ---- */ +void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm) +{ + /* + * Frame: [STX][LEN][0x8E][22 bytes VESC_STATE][CRC_hi][CRC_lo][ETX] + * LEN = 1 (CMD) + 22 (payload) = 23; total frame = 28 bytes. + * At 921600 baud: 28×10/921600 ≈ 0.30 ms — safe to block. + */ + static uint8_t frame[28]; + const uint8_t plen = (uint8_t)sizeof(jlink_tlm_vesc_state_t); /* 22 */ + const uint8_t len = 1u + plen; /* 23 */ + + frame[0] = JLINK_STX; + frame[1] = len; + frame[2] = JLINK_TLM_VESC_STATE; + memcpy(&frame[3], tlm, plen); + + uint16_t crc = crc16_xmodem(&frame[2], len); + frame[3 + plen] = (uint8_t)(crc >> 8); + frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu); + frame[3 + plen + 2] = JLINK_ETX; + + jlink_tx_locked(frame, sizeof(frame)); +} diff --git a/src/main.c b/src/main.c index 28e8ff6..fb8467d 100644 --- a/src/main.c +++ b/src/main.c @@ -33,6 +33,8 @@ #include "pid_flash.h" #include "fault_handler.h" #include "can_driver.h" +#include "vesc_can.h" +#include "orin_can.h" #include "servo_bus.h" #include "gimbal.h" #include "lvc.h" @@ -195,8 +197,14 @@ int main(void) { /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */ jlink_init(); - /* Init CAN2 BLDC motor controller bus — PB12/PB13, 500 kbps (Issue #597) */ + /* Init CAN2 bus — PB12/PB13, 500 kbps. + * Register VESC and Orin CAN callbacks BEFORE can_driver_init() so filter + * banks are configured with both callbacks ready. (Issue #674) */ + can_driver_set_ext_cb(vesc_can_on_frame); /* VESC STATUS → FIFO1 */ + can_driver_set_std_cb(orin_can_on_frame); /* Orin cmds → FIFO0 */ can_driver_init(); + vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT); + orin_can_init(); /* Send fault log summary on boot if a prior fault was recorded (Issue #565) */ if (fault_get_last_type() != FAULT_NONE) { @@ -469,6 +477,23 @@ int main(void) { } } + /* Orin CAN: handle commands from Orin over CAN bus (Issue #674). + * Estop and clear are latching; drive/mode are consumed each tick. + * Balance independence: if orin_can_is_alive() == false the loop + * simply does not inject any commands and holds upright in-place. */ + if (orin_can_state.estop_req) { + orin_can_state.estop_req = 0u; + safety_remote_estop(ESTOP_REMOTE); + safety_arm_cancel(); + balance_disarm(&bal); + motor_driver_estop(&motors); + } + if (orin_can_state.estop_clear_req) { + orin_can_state.estop_clear_req = 0u; + if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED) + safety_remote_estop_clear(); + } + /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */ if (jlink_state.fault_log_req) { jlink_state.fault_log_req = 0u; @@ -626,6 +651,8 @@ int main(void) { float base_sp = bal.setpoint; if (jlink_is_active(now)) bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG; + else if (orin_can_is_alive(now)) + bal.setpoint += ((float)orin_can_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); @@ -659,6 +686,8 @@ int main(void) { * mode_manager_get_steer() blends it with RC steer per active mode. */ if (jlink_is_active(now)) mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0); + else if (orin_can_is_alive(now)) + mode_manager_set_auto_cmd(&mode, orin_can_state.steer, 0); else if (jetson_cmd_is_active(now)) mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0); @@ -687,38 +716,47 @@ int main(void) { } } - /* CAN BLDC: enable/disable follows arm state (Issue #597) */ - { - static bool s_can_enabled = false; - bool armed_now = (bal.state == BALANCE_ARMED); - if (armed_now && !s_can_enabled) { - can_driver_send_enable(CAN_NODE_LEFT, true); - can_driver_send_enable(CAN_NODE_RIGHT, true); - s_can_enabled = true; - } else if (!armed_now && s_can_enabled) { - can_driver_send_enable(CAN_NODE_LEFT, false); - can_driver_send_enable(CAN_NODE_RIGHT, false); - s_can_enabled = false; - } - } - - /* CAN BLDC: send velocity/torque commands at CAN_TX_RATE_HZ (100 Hz) (Issue #597) - * Converts balance PID output + blended steer to per-wheel RPM. + /* VESC: send RPM commands at CAN_TX_RATE_HZ (100 Hz) via 29-bit extended CAN. + * VESC does not need enable/disable frames — RPM=0 while disarmed holds brakes. * Left wheel: speed_rpm + steer_rpm (differential drive) - * Right wheel: speed_rpm - steer_rpm */ + * Right wheel: speed_rpm − steer_rpm (Issue #674) */ if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) { can_cmd_tick = now; - int16_t can_steer = mode_manager_get_steer(&mode); - int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE; - int32_t steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2; - can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 }; - can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 }; - if (bal.state != BALANCE_ARMED) { - cmd_l.velocity_rpm = 0; - cmd_r.velocity_rpm = 0; + int32_t speed_rpm = 0; + int32_t steer_rpm = 0; + if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) { + int16_t can_steer = mode_manager_get_steer(&mode); + speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE; + steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2; } - can_driver_send_cmd(CAN_NODE_LEFT, &cmd_l); - can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r); + vesc_can_send_rpm(VESC_CAN_ID_LEFT, speed_rpm + steer_rpm); + vesc_can_send_rpm(VESC_CAN_ID_RIGHT, speed_rpm - steer_rpm); + } + + /* VESC telemetry: send JLINK_TLM_VESC_STATE at 1 Hz (Issue #674) */ + vesc_can_send_tlm(now); + + /* Orin CAN broadcast: FC_STATUS + FC_VESC at ORIN_TLM_HZ (10 Hz) (Issue #674) */ + { + orin_can_fc_status_t fc_st; + fc_st.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); + fc_st.motor_cmd = bal.motor_cmd; + uint32_t _vbat = battery_read_mv(); + fc_st.vbat_mv = (_vbat > 65535u) ? 65535u : (uint16_t)_vbat; + fc_st.balance_state = (uint8_t)bal.state; + fc_st.flags = (safety_remote_estop_active() ? 0x01u : 0u) | + (bal.state == BALANCE_ARMED ? 0x02u : 0u); + + orin_can_fc_vesc_t fc_vesc; + vesc_state_t vl, vr; + bool vl_ok = vesc_can_get_state(VESC_CAN_ID_LEFT, &vl); + bool vr_ok = vesc_can_get_state(VESC_CAN_ID_RIGHT, &vr); + fc_vesc.left_rpm_x10 = vl_ok ? (int16_t)(vl.rpm / 10) : 0; + fc_vesc.right_rpm_x10 = vr_ok ? (int16_t)(vr.rpm / 10) : 0; + fc_vesc.left_current_x10 = vl_ok ? vl.current_x10 : 0; + fc_vesc.right_current_x10 = vr_ok ? vr.current_x10 : 0; + + orin_can_broadcast(now, &fc_st, &fc_vesc); } /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz. diff --git a/src/orin_can.c b/src/orin_can.c new file mode 100644 index 0000000..ced4f98 --- /dev/null +++ b/src/orin_can.c @@ -0,0 +1,96 @@ +/* orin_can.c — Orin↔FC CAN protocol driver (Issue #674) + * + * Receives high-level drive/mode/estop commands from Orin over CAN. + * Broadcasts FC status and VESC telemetry back to Orin at ORIN_TLM_HZ. + * + * Registered as the standard-ID callback with can_driver via + * can_driver_set_std_cb(orin_can_on_frame). + * + * Balance independence: if Orin link drops (orin_can_is_alive() == false), + * main loop stops injecting Orin commands and the balance PID holds + * upright in-place. No action required here — the safety is in main.c. + */ + +#include "orin_can.h" +#include "can_driver.h" +#include "stm32f7xx_hal.h" +#include + +volatile OrinCanState orin_can_state; +static uint32_t s_tlm_tick; + +void orin_can_init(void) +{ + memset((void *)&orin_can_state, 0, sizeof(orin_can_state)); + /* Pre-wind so first broadcast fires on the first eligible tick */ + s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ)); + can_driver_set_std_cb(orin_can_on_frame); +} + +void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len) +{ + /* Any frame from Orin refreshes the heartbeat */ + orin_can_state.last_rx_ms = HAL_GetTick(); + + switch (std_id) { + case ORIN_CAN_ID_HEARTBEAT: + /* Heartbeat payload (sequence counter) ignored — timestamp is enough */ + break; + + case ORIN_CAN_ID_DRIVE: + /* int16 speed (BE), int16 steer (BE) */ + if (len < 4u) { break; } + orin_can_state.speed = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]); + orin_can_state.steer = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]); + orin_can_state.drive_updated = 1u; + break; + + case ORIN_CAN_ID_MODE: + /* uint8 mode */ + if (len < 1u) { break; } + orin_can_state.mode = data[0]; + orin_can_state.mode_updated = 1u; + break; + + case ORIN_CAN_ID_ESTOP: + /* uint8: 1 = assert estop, 0 = clear estop */ + if (len < 1u) { break; } + if (data[0] != 0u) { + orin_can_state.estop_req = 1u; + } else { + orin_can_state.estop_clear_req = 1u; + } + break; + + default: + break; + } +} + +bool orin_can_is_alive(uint32_t now_ms) +{ + if (orin_can_state.last_rx_ms == 0u) { + return false; + } + return (now_ms - orin_can_state.last_rx_ms) < ORIN_HB_TIMEOUT_MS; +} + +void orin_can_broadcast(uint32_t now_ms, + const orin_can_fc_status_t *status, + const orin_can_fc_vesc_t *vesc) +{ + if ((now_ms - s_tlm_tick) < (1000u / ORIN_TLM_HZ)) { + return; + } + s_tlm_tick = now_ms; + + uint8_t buf[8]; + + /* FC_STATUS (0x400): 8 bytes */ + memcpy(buf, status, sizeof(orin_can_fc_status_t)); + can_driver_send_std(ORIN_CAN_ID_FC_STATUS, buf, (uint8_t)sizeof(orin_can_fc_status_t)); + + /* FC_VESC (0x401): 8 bytes */ + memcpy(buf, vesc, sizeof(orin_can_fc_vesc_t)); + can_driver_send_std(ORIN_CAN_ID_FC_VESC, buf, (uint8_t)sizeof(orin_can_fc_vesc_t)); +} diff --git a/src/vesc_can.c b/src/vesc_can.c new file mode 100644 index 0000000..df1b62b --- /dev/null +++ b/src/vesc_can.c @@ -0,0 +1,141 @@ +/* vesc_can.c — VESC CAN protocol driver (Issue #674) + * + * Registers vesc_can_on_frame as the extended-frame callback with can_driver. + * VESC uses 29-bit arb IDs: (pkt_type << 8) | vesc_node_id. + * All wire values are big-endian (VESC FW 6.x). + */ + +#include "vesc_can.h" +#include "can_driver.h" +#include "jlink.h" +#include "stm32f7xx_hal.h" +#include +#include + +static uint8_t s_id_left; +static uint8_t s_id_right; +static vesc_state_t s_state[2]; /* [0] = left, [1] = right */ +static uint32_t s_tlm_tick; + +static vesc_state_t *state_for_id(uint8_t vesc_id) +{ + if (vesc_id == s_id_left) return &s_state[0]; + if (vesc_id == s_id_right) return &s_state[1]; + return NULL; +} + +void vesc_can_init(uint8_t id_left, uint8_t id_right) +{ + s_id_left = id_left; + s_id_right = id_right; + memset(s_state, 0, sizeof(s_state)); + /* Pre-wind so first send_tlm call fires immediately */ + s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / VESC_TLM_HZ)); + can_driver_set_ext_cb(vesc_can_on_frame); +} + +void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm) +{ + /* arb_id = (VESC_PKT_SET_RPM << 8) | vesc_id */ + uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | (uint32_t)vesc_id; + + /* Payload: int32 RPM, big-endian */ + uint32_t urpm = (uint32_t)rpm; + uint8_t data[4]; + data[0] = (uint8_t)(urpm >> 24u); + data[1] = (uint8_t)(urpm >> 16u); + data[2] = (uint8_t)(urpm >> 8u); + data[3] = (uint8_t)(urpm); + + can_driver_send_ext(ext_id, data, 4u); +} + +void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len) +{ + uint8_t pkt_type = (uint8_t)(ext_id >> 8u); + uint8_t vesc_id = (uint8_t)(ext_id & 0xFFu); + vesc_state_t *s = state_for_id(vesc_id); + + if (s == NULL) { + return; + } + + switch (pkt_type) { + case VESC_PKT_STATUS: /* 9: int32 RPM, int16 I×10, int16 duty×1000 (8 bytes) */ + if (len < 8u) { break; } + s->rpm = (int32_t)(((uint32_t)data[0] << 24u) | + ((uint32_t)data[1] << 16u) | + ((uint32_t)data[2] << 8u) | + (uint32_t)data[3]); + s->current_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); + s->duty_x1000 = (int16_t)(((uint16_t)data[6] << 8u) | (uint16_t)data[7]); + s->last_rx_ms = HAL_GetTick(); + break; + + case VESC_PKT_STATUS_4: /* 16: int16 T_fet×10, T_mot×10, I_in×10 (6 bytes) */ + if (len < 6u) { break; } + s->temp_fet_x10 = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]); + s->temp_motor_x10 = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]); + s->current_in_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); + break; + + case VESC_PKT_STATUS_5: /* 27: int32 tacho (ignored), int16 V_in×10 (6 bytes) */ + if (len < 6u) { break; } + /* bytes [0..3] = odometer tachometer — not stored */ + s->voltage_x10 = (int16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]); + break; + + default: + break; + } +} + +bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out) +{ + if (out == NULL) { + return false; + } + vesc_state_t *s = state_for_id(vesc_id); + if (s == NULL || s->last_rx_ms == 0u) { + return false; + } + memcpy(out, s, sizeof(vesc_state_t)); + return true; +} + +bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms) +{ + vesc_state_t *s = state_for_id(vesc_id); + if (s == NULL || s->last_rx_ms == 0u) { + return false; + } + return (now_ms - s->last_rx_ms) < VESC_ALIVE_TIMEOUT_MS; +} + +void vesc_can_send_tlm(uint32_t now_ms) +{ + if ((now_ms - s_tlm_tick) < (1000u / VESC_TLM_HZ)) { + return; + } + s_tlm_tick = now_ms; + + jlink_tlm_vesc_state_t tlm; + memset(&tlm, 0, sizeof(tlm)); + + tlm.left_rpm = s_state[0].rpm; + tlm.right_rpm = s_state[1].rpm; + tlm.left_current_x10 = s_state[0].current_x10; + tlm.right_current_x10 = s_state[1].current_x10; + tlm.left_temp_x10 = s_state[0].temp_fet_x10; + tlm.right_temp_x10 = s_state[1].temp_fet_x10; + /* Use left voltage; fall back to right if left not yet received */ + tlm.voltage_x10 = s_state[0].voltage_x10 + ? s_state[0].voltage_x10 + : s_state[1].voltage_x10; + tlm.left_fault = s_state[0].fault_code; + tlm.right_fault = s_state[1].fault_code; + tlm.left_alive = vesc_can_is_alive(s_id_left, now_ms) ? 1u : 0u; + tlm.right_alive = vesc_can_is_alive(s_id_right, now_ms) ? 1u : 0u; + + jlink_send_vesc_state_tlm(&tlm); +} diff --git a/test/stubs/stm32f7xx_hal.h b/test/stubs/stm32f7xx_hal.h new file mode 100644 index 0000000..301898b --- /dev/null +++ b/test/stubs/stm32f7xx_hal.h @@ -0,0 +1,126 @@ +/* test/stubs/stm32f7xx_hal.h — minimal HAL stub for host-side unit tests. + * + * Provides just enough types and functions so that the firmware source files + * compile on a host (Linux/macOS) with -DTEST_HOST. + * Each test file must define the actual behavior of HAL_GetTick() etc. + */ + +#ifndef STM32F7XX_HAL_H +#define STM32F7XX_HAL_H + +#include +#include +#include + +/* ---- Return codes ---- */ +#define HAL_OK 0 +#define HAL_ERROR 1 +#define HAL_BUSY 2 +#define HAL_TIMEOUT 3 +typedef uint32_t HAL_StatusTypeDef; + +/* ---- Enable / Disable ---- */ +#define ENABLE 1 +#define DISABLE 0 + +/* ---- HAL_GetTick: each test declares its own implementation ---- */ +uint32_t HAL_GetTick(void); + +/* ---- Minimal CAN types used by can_driver.c / vesc_can.c ---- */ +typedef struct { uint32_t dummy; } CAN_TypeDef; + +typedef struct { + uint32_t Prescaler; + uint32_t Mode; + uint32_t SyncJumpWidth; + uint32_t TimeSeg1; + uint32_t TimeSeg2; + uint32_t TimeTriggeredMode; + uint32_t AutoBusOff; + uint32_t AutoWakeUp; + uint32_t AutoRetransmission; + uint32_t ReceiveFifoLocked; + uint32_t TransmitFifoPriority; +} CAN_InitTypeDef; + +typedef struct { + CAN_TypeDef *Instance; + CAN_InitTypeDef Init; + /* opaque HAL internals omitted */ +} CAN_HandleTypeDef; + +typedef struct { + uint32_t StdId; + uint32_t ExtId; + uint32_t IDE; + uint32_t RTR; + uint32_t DLC; + uint32_t Timestamp; + uint32_t FilterMatchIndex; +} CAN_RxHeaderTypeDef; + +typedef struct { + uint32_t StdId; + uint32_t ExtId; + uint32_t IDE; + uint32_t RTR; + uint32_t DLC; + uint32_t TransmitGlobalTime; +} CAN_TxHeaderTypeDef; + +typedef struct { + uint32_t FilterIdHigh; + uint32_t FilterIdLow; + uint32_t FilterMaskIdHigh; + uint32_t FilterMaskIdLow; + uint32_t FilterFIFOAssignment; + uint32_t FilterBank; + uint32_t FilterMode; + uint32_t FilterScale; + uint32_t FilterActivation; + uint32_t SlaveStartFilterBank; +} CAN_FilterTypeDef; + +#define CAN_ID_STD 0x00000000u +#define CAN_ID_EXT 0x00000004u +#define CAN_RTR_DATA 0x00000000u +#define CAN_RTR_REMOTE 0x00000002u +#define CAN_FILTERMODE_IDMASK 0u +#define CAN_FILTERSCALE_32BIT 1u +#define CAN_RX_FIFO0 0u +#define CAN_RX_FIFO1 1u +#define CAN_FILTER_ENABLE 1u +#define CAN_MODE_NORMAL 0u +#define CAN_SJW_1TQ 0u +#define CAN_BS1_13TQ 0u +#define CAN_BS2_4TQ 0u +#define CAN_ESR_BOFF 0x00000004u + +static inline HAL_StatusTypeDef HAL_CAN_Init(CAN_HandleTypeDef *h){(void)h;return HAL_OK;} +static inline HAL_StatusTypeDef HAL_CAN_ConfigFilter(CAN_HandleTypeDef *h, CAN_FilterTypeDef *f){(void)h;(void)f;return HAL_OK;} +static inline HAL_StatusTypeDef HAL_CAN_Start(CAN_HandleTypeDef *h){(void)h;return HAL_OK;} +static inline uint32_t HAL_CAN_GetTxMailboxesFreeLevel(CAN_HandleTypeDef *h){(void)h;return 3u;} +static inline HAL_StatusTypeDef HAL_CAN_AddTxMessage(CAN_HandleTypeDef *h, CAN_TxHeaderTypeDef *hdr, uint8_t *d, uint32_t *mb){(void)h;(void)hdr;(void)d;(void)mb;return HAL_OK;} +static inline uint32_t HAL_CAN_GetRxFifoFillLevel(CAN_HandleTypeDef *h, uint32_t f){(void)h;(void)f;return 0u;} +static inline HAL_StatusTypeDef HAL_CAN_GetRxMessage(CAN_HandleTypeDef *h, uint32_t f, CAN_RxHeaderTypeDef *hdr, uint8_t *d){(void)h;(void)f;(void)hdr;(void)d;return HAL_OK;} + +/* ---- GPIO (minimal, for can_driver GPIO init) ---- */ +typedef struct { uint32_t dummy; } GPIO_TypeDef; +typedef struct { + uint32_t Pin; uint32_t Mode; uint32_t Pull; uint32_t Speed; uint32_t Alternate; +} GPIO_InitTypeDef; +static inline void HAL_GPIO_Init(GPIO_TypeDef *p, GPIO_InitTypeDef *g){(void)p;(void)g;} +#define GPIOB ((GPIO_TypeDef *)0) +#define GPIO_PIN_12 (1u<<12) +#define GPIO_PIN_13 (1u<<13) +#define GPIO_MODE_AF_PP 0u +#define GPIO_NOPULL 0u +#define GPIO_SPEED_FREQ_HIGH 0u +#define GPIO_AF9_CAN2 9u + +/* ---- RCC stubs ---- */ +#define __HAL_RCC_CAN1_CLK_ENABLE() ((void)0) +#define __HAL_RCC_CAN2_CLK_ENABLE() ((void)0) +#define __HAL_RCC_GPIOB_CLK_ENABLE() ((void)0) + +#endif /* STM32F7XX_HAL_H */ diff --git a/test/test_vesc_can.c b/test/test_vesc_can.c new file mode 100644 index 0000000..8557a17 --- /dev/null +++ b/test/test_vesc_can.c @@ -0,0 +1,518 @@ +/* + * test_vesc_can.c — Unit tests for VESC CAN protocol driver (Issue #674). + * + * Build (host, no hardware): + * gcc -I include -I test/stubs -DTEST_HOST -lm \ + * -o /tmp/test_vesc_can test/test_vesc_can.c + * + * All tests are self-contained; no HAL, no CAN peripheral required. + * vesc_can.c calls can_driver_send_ext / can_driver_set_ext_cb and + * jlink_send_vesc_state_tlm — all stubbed below. + */ + +/* ---- Block HAL and board-specific headers ---- */ +/* Must appear before any board include is transitively pulled */ +#define STM32F7XX_HAL_H /* skip stm32f7xx_hal.h */ +#define STM32F722xx /* satisfy any chip guard */ +#define JLINK_H /* skip jlink.h (pid_flash / HAL deps) */ +#define CAN_DRIVER_H /* skip can_driver.h body (we stub functions below) */ + +#include +#include +#include + +/* Minimal HAL types needed by vesc_can.c (none for this module, but keep HAL_OK) */ +#define HAL_OK 0 + +/* ---- Minimal type replicas (must match the real packed structs) ---- */ + +typedef struct __attribute__((packed)) { + int32_t left_rpm; + int32_t right_rpm; + int16_t left_current_x10; + int16_t right_current_x10; + int16_t left_temp_x10; + int16_t right_temp_x10; + int16_t voltage_x10; + uint8_t left_fault; + uint8_t right_fault; + uint8_t left_alive; + uint8_t right_alive; +} jlink_tlm_vesc_state_t; /* 22 bytes */ + +/* ---- Stubs ---- */ + +/* Simulated tick counter */ +static uint32_t g_tick_ms = 0; +uint32_t HAL_GetTick(void) { return g_tick_ms; } + +/* Capture last extended CAN TX */ +static uint32_t g_last_ext_id = 0; +static uint8_t g_last_ext_data[8]; +static uint8_t g_last_ext_len = 0; +static int g_ext_tx_count = 0; + +void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len) +{ + g_last_ext_id = ext_id; + if (len > 8u) len = 8u; + for (uint8_t i = 0; i < len; i++) g_last_ext_data[i] = data[i]; + g_last_ext_len = len; + g_ext_tx_count++; +} + +/* Replicate types from can_driver.h (header is blocked by #define CAN_DRIVER_H) */ +typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len); +typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len); + +/* Capture registered ext callback */ +static can_ext_frame_cb_t g_registered_cb = NULL; +void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { g_registered_cb = cb; } + +/* Capture last TLM sent to JLink */ +static jlink_tlm_vesc_state_t g_last_tlm; +static int g_tlm_count = 0; +void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm) +{ + g_last_tlm = *tlm; + g_tlm_count++; +} + +/* ---- Include implementation directly ---- */ +#include "../src/vesc_can.c" + +/* ---- Test framework ---- */ +#include +#include +#include + +static int g_pass = 0; +static int g_fail = 0; + +#define ASSERT(cond, msg) do { \ + if (cond) { g_pass++; } \ + else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \ +} while(0) + +/* ---- Helpers ---- */ + +static void reset_stubs(void) +{ + g_tick_ms = 0; + g_last_ext_id = 0; + g_last_ext_len = 0; + g_ext_tx_count = 0; + g_tlm_count = 0; + g_registered_cb = NULL; + memset(g_last_ext_data, 0, sizeof(g_last_ext_data)); + memset(&g_last_tlm, 0, sizeof(g_last_tlm)); +} + +/* Build a STATUS frame for vesc_id with given RPM, current_x10, duty_x1000 */ +static void make_status(uint8_t buf[8], int32_t rpm, int16_t cur_x10, int16_t duty) +{ + uint32_t urpm = (uint32_t)rpm; + buf[0] = (uint8_t)(urpm >> 24u); + buf[1] = (uint8_t)(urpm >> 16u); + buf[2] = (uint8_t)(urpm >> 8u); + buf[3] = (uint8_t)(urpm); + buf[4] = (uint8_t)((uint16_t)cur_x10 >> 8u); + buf[5] = (uint8_t)((uint16_t)cur_x10 & 0xFFu); + buf[6] = (uint8_t)((uint16_t)duty >> 8u); + buf[7] = (uint8_t)((uint16_t)duty & 0xFFu); +} + +/* ---- Tests ---- */ + +static void test_init_stores_ids(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + ASSERT(s_id_left == 56u, "init stores left ID"); + ASSERT(s_id_right == 68u, "init stores right ID"); +} + +static void test_init_zeroes_state(void) +{ + reset_stubs(); + /* Dirty the state first */ + s_state[0].rpm = 9999; + s_state[1].rpm = -9999; + vesc_can_init(56u, 68u); + ASSERT(s_state[0].rpm == 0, "init zeroes left RPM"); + ASSERT(s_state[1].rpm == 0, "init zeroes right RPM"); + ASSERT(s_state[0].last_rx_ms == 0u, "init zeroes left last_rx_ms"); +} + +static void test_init_registers_ext_callback(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + ASSERT(g_registered_cb == vesc_can_on_frame, "init registers vesc_can_on_frame as ext_cb"); +} + +static void test_send_rpm_ext_id_left(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_ext_tx_count = 0; + vesc_can_send_rpm(56u, 1000); + /* ext_id = (VESC_PKT_SET_RPM << 8) | vesc_id = (3 << 8) | 56 = 0x0338 */ + ASSERT(g_last_ext_id == 0x0338u, "send_rpm left: correct ext_id"); + ASSERT(g_ext_tx_count == 1, "send_rpm: one TX frame"); + ASSERT(g_last_ext_len == 4u, "send_rpm: DLC=4"); +} + +static void test_send_rpm_ext_id_right(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + vesc_can_send_rpm(68u, 2000); + /* ext_id = (3 << 8) | 68 = 0x0344 */ + ASSERT(g_last_ext_id == 0x0344u, "send_rpm right: correct ext_id"); +} + +static void test_send_rpm_payload_positive(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + vesc_can_send_rpm(56u, 0x01020304); + ASSERT(g_last_ext_data[0] == 0x01u, "send_rpm payload byte0"); + ASSERT(g_last_ext_data[1] == 0x02u, "send_rpm payload byte1"); + ASSERT(g_last_ext_data[2] == 0x03u, "send_rpm payload byte2"); + ASSERT(g_last_ext_data[3] == 0x04u, "send_rpm payload byte3"); +} + +static void test_send_rpm_payload_negative(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + /* -1 as int32 = 0xFFFFFFFF */ + vesc_can_send_rpm(56u, -1); + ASSERT(g_last_ext_data[0] == 0xFFu, "send_rpm -1 byte0"); + ASSERT(g_last_ext_data[1] == 0xFFu, "send_rpm -1 byte1"); + ASSERT(g_last_ext_data[2] == 0xFFu, "send_rpm -1 byte2"); + ASSERT(g_last_ext_data[3] == 0xFFu, "send_rpm -1 byte3"); +} + +static void test_send_rpm_zero(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + vesc_can_send_rpm(56u, 0); + ASSERT(g_last_ext_data[0] == 0u, "send_rpm 0 byte0"); + ASSERT(g_last_ext_data[3] == 0u, "send_rpm 0 byte3"); + ASSERT(g_ext_tx_count == 1, "send_rpm 0: one TX"); +} + +static void test_on_frame_status_rpm(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8]; + make_status(buf, 12345, 150, 500); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 8u); + ASSERT(s_state[0].rpm == 12345, "on_frame STATUS: RPM parsed"); +} + +static void test_on_frame_status_current(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8]; + make_status(buf, 0, 250, 0); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 8u); + ASSERT(s_state[0].current_x10 == 250, "on_frame STATUS: current_x10 parsed"); +} + +static void test_on_frame_status_duty(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8]; + make_status(buf, 0, 0, -300); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 8u); + ASSERT(s_state[0].duty_x1000 == -300, "on_frame STATUS: duty_x1000 parsed"); +} + +static void test_on_frame_status_updates_timestamp(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 5000u; + uint8_t buf[8]; + make_status(buf, 100, 0, 0); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 8u); + ASSERT(s_state[0].last_rx_ms == 5000u, "on_frame STATUS: last_rx_ms updated"); +} + +static void test_on_frame_status_right_node(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8]; + make_status(buf, -9999, 0, 0); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 68u; + vesc_can_on_frame(ext_id, buf, 8u); + ASSERT(s_state[1].rpm == -9999, "on_frame STATUS: right node RPM"); + ASSERT(s_state[0].rpm == 0, "on_frame STATUS: left unaffected"); +} + +static void test_on_frame_status4_temps(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8] = {0x00, 0xF0, 0x01, 0x2C, 0x00, 0x64, 0, 0}; + /* T_fet = 0x00F0 = 240 (24.0°C), T_mot = 0x012C = 300 (30.0°C), I_in = 0x0064 = 100 */ + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS_4 << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 6u); + ASSERT(s_state[0].temp_fet_x10 == 240, "on_frame STATUS_4: temp_fet_x10"); + ASSERT(s_state[0].temp_motor_x10 == 300, "on_frame STATUS_4: temp_motor_x10"); + ASSERT(s_state[0].current_in_x10 == 100, "on_frame STATUS_4: current_in_x10"); +} + +static void test_on_frame_status5_voltage(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + /* tacho at [0..3], V_in×10 at [4..5] = 0x0100 = 256 (25.6 V) */ + uint8_t buf[8] = {0, 0, 0, 0, 0x01, 0x00, 0, 0}; + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS_5 << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 6u); + ASSERT(s_state[0].voltage_x10 == 256, "on_frame STATUS_5: voltage_x10"); +} + +static void test_on_frame_unknown_pkt_type_ignored(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8] = {0}; + uint32_t ext_id = (99u << 8u) | 56u; /* unknown pkt type 99 */ + vesc_can_on_frame(ext_id, buf, 8u); + /* No crash, state unmodified (last_rx_ms stays 0) */ + ASSERT(s_state[0].last_rx_ms == 0u, "on_frame: unknown pkt_type ignored"); +} + +static void test_on_frame_unknown_vesc_id_ignored(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8]; + make_status(buf, 9999, 0, 0); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 99u; /* unknown ID */ + vesc_can_on_frame(ext_id, buf, 8u); + ASSERT(s_state[0].rpm == 0 && s_state[1].rpm == 0, "on_frame: unknown vesc_id ignored"); +} + +static void test_on_frame_short_status_ignored(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + uint8_t buf[8]; + make_status(buf, 1234, 0, 0); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 7u); /* too short: need 8 */ + ASSERT(s_state[0].rpm == 0, "on_frame STATUS: short frame ignored"); +} + +static void test_get_state_unknown_id_returns_false(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + vesc_state_t out; + bool ok = vesc_can_get_state(99u, &out); + ASSERT(!ok, "get_state: unknown id returns false"); +} + +static void test_get_state_no_frame_returns_false(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + vesc_state_t out; + bool ok = vesc_can_get_state(56u, &out); + ASSERT(!ok, "get_state: no frame yet returns false"); +} + +static void test_get_state_after_status_returns_true(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 1000u; + uint8_t buf[8]; + make_status(buf, 4321, 88, -100); + uint32_t ext_id = ((uint32_t)VESC_PKT_STATUS << 8u) | 56u; + vesc_can_on_frame(ext_id, buf, 8u); + + vesc_state_t out; + bool ok = vesc_can_get_state(56u, &out); + ASSERT(ok, "get_state: returns true after STATUS"); + ASSERT(out.rpm == 4321, "get_state: RPM correct"); + ASSERT(out.current_x10 == 88, "get_state: current_x10 correct"); +} + +static void test_is_alive_no_frame(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + ASSERT(!vesc_can_is_alive(56u, 0u), "is_alive: false with no frame"); +} + +static void test_is_alive_within_timeout(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 5000u; + uint8_t buf[8]; + make_status(buf, 100, 0, 0); + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); + + /* Check alive 500 ms later (within 1000 ms timeout) */ + ASSERT(vesc_can_is_alive(56u, 5500u), "is_alive: true within timeout"); +} + +static void test_is_alive_after_timeout(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 1000u; + uint8_t buf[8]; + make_status(buf, 100, 0, 0); + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); + + /* Check alive 1001 ms later — exceeds VESC_ALIVE_TIMEOUT_MS (1000 ms) */ + ASSERT(!vesc_can_is_alive(56u, 2001u), "is_alive: false after timeout"); +} + +static void test_is_alive_at_exact_timeout_boundary(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 1000u; + uint8_t buf[8]; + make_status(buf, 100, 0, 0); + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); + + /* At exactly VESC_ALIVE_TIMEOUT_MS: delta = 1000, condition is < 1000 → false */ + ASSERT(!vesc_can_is_alive(56u, 2000u), "is_alive: false at exact timeout boundary"); +} + +static void test_send_tlm_rate_limited(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tlm_count = 0; + + /* First call at t=0 should fire immediately (pre-wound s_tlm_tick) */ + vesc_can_send_tlm(0u); + ASSERT(g_tlm_count == 1, "send_tlm: fires on first call"); + + /* Second call immediately after: should NOT fire (within 1s window) */ + vesc_can_send_tlm(500u); + ASSERT(g_tlm_count == 1, "send_tlm: rate-limited within 1 s"); + + /* After 1000 ms: should fire again */ + vesc_can_send_tlm(1000u); + ASSERT(g_tlm_count == 2, "send_tlm: fires after 1 s"); +} + +static void test_send_tlm_payload_content(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 100u; + + /* Inject STATUS into left VESC */ + uint8_t buf[8]; + make_status(buf, 5678, 123, 400); + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); + + /* Inject STATUS into right VESC */ + make_status(buf, -1234, -50, -200); + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 68u, buf, 8u); + + /* Inject STATUS_4 into left (for temps) */ + uint8_t buf4[8] = {0x00, 0xC8, 0x01, 0x2C, 0x00, 0x64, 0, 0}; + /* T_fet=200, T_mot=300, I_in=100 */ + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS_4 << 8u) | 56u, buf4, 6u); + + /* Inject STATUS_5 into left (for voltage) */ + uint8_t buf5[8] = {0, 0, 0, 0, 0x01, 0x00, 0, 0}; + /* V_in×10 = 256 (25.6 V) */ + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS_5 << 8u) | 56u, buf5, 6u); + + vesc_can_send_tlm(0u); + + ASSERT(g_tlm_count == 1, "send_tlm: TLM sent"); + ASSERT(g_last_tlm.left_rpm == 5678, "send_tlm: left_rpm"); + ASSERT(g_last_tlm.right_rpm == -1234, "send_tlm: right_rpm"); + ASSERT(g_last_tlm.left_current_x10 == 123, "send_tlm: left_current_x10"); + ASSERT(g_last_tlm.right_current_x10 == -50, "send_tlm: right_current_x10"); + ASSERT(g_last_tlm.left_temp_x10 == 200, "send_tlm: left_temp_x10"); + ASSERT(g_last_tlm.right_temp_x10 == 0, "send_tlm: right_temp_x10 (no STATUS_4)"); + ASSERT(g_last_tlm.voltage_x10 == 256, "send_tlm: voltage_x10"); +} + +static void test_send_tlm_alive_flags(void) +{ + reset_stubs(); + vesc_can_init(56u, 68u); + g_tick_ms = 1000u; + + /* Only send STATUS for left */ + uint8_t buf[8]; + make_status(buf, 100, 0, 0); + vesc_can_on_frame(((uint32_t)VESC_PKT_STATUS << 8u) | 56u, buf, 8u); + + /* TLM at t=1100 (100 ms after last frame — within 1000 ms timeout) */ + vesc_can_send_tlm(0u); /* consume pre-wind */ + g_tlm_count = 0; + vesc_can_send_tlm(1100u); /* but only 100ms have passed — still rate-limited */ + + /* Force TLM at t=1001 to bypass rate limit */ + s_tlm_tick = (uint32_t)(-2000u); /* force next call to send */ + vesc_can_send_tlm(1100u); + + ASSERT(g_last_tlm.left_alive == 1u, "send_tlm: left_alive = 1"); + ASSERT(g_last_tlm.right_alive == 0u, "send_tlm: right_alive = 0 (no STATUS)"); +} + +/* ---- main ---- */ + +int main(void) +{ + test_init_stores_ids(); + test_init_zeroes_state(); + test_init_registers_ext_callback(); + test_send_rpm_ext_id_left(); + test_send_rpm_ext_id_right(); + test_send_rpm_payload_positive(); + test_send_rpm_payload_negative(); + test_send_rpm_zero(); + test_on_frame_status_rpm(); + test_on_frame_status_current(); + test_on_frame_status_duty(); + test_on_frame_status_updates_timestamp(); + test_on_frame_status_right_node(); + test_on_frame_status4_temps(); + test_on_frame_status5_voltage(); + test_on_frame_unknown_pkt_type_ignored(); + test_on_frame_unknown_vesc_id_ignored(); + test_on_frame_short_status_ignored(); + test_get_state_unknown_id_returns_false(); + test_get_state_no_frame_returns_false(); + test_get_state_after_status_returns_true(); + test_is_alive_no_frame(); + test_is_alive_within_timeout(); + test_is_alive_after_timeout(); + test_is_alive_at_exact_timeout_boundary(); + test_send_tlm_rate_limited(); + test_send_tlm_payload_content(); + test_send_tlm_alive_flags(); + + printf("\n%d passed, %d failed\n", g_pass, g_fail); + return g_fail ? 1 : 0; +}