seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat: bxCAN integration for VESC motor control and Orin comms (Issue #674)

Browse Source 
- 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 <noreply@anthropic.com>
This commit is contained in:
sl-firmware 2026-03-17 19:10:11 -04:00 committed by sl-jetson
parent 92c0628c62
commit 5e82878083
11 changed files with 1354 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
include/can_driver.h
View File

@ -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 */

24
include/jlink.h
View File

@ -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 */

103
include/orin_can.h Normal file
View File

@ -0,0 +1,103 @@
#ifndef ORIN_CAN_H
#define ORIN_CAN_H
#include <stdint.h>
#include <stdbool.h>
/*
* 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 */

117
include/vesc_can.h Normal file
View File

@ -0,0 +1,117 @@
#ifndef VESC_CAN_H
#define VESC_CAN_H
#include <stdint.h>
#include <stdbool.h>
/*
* 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 */

127
src/can_driver.c
View File

@ -15,6 +15,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)
{
@ -51,18 +53,16 @@ void can_driver_init(void)
return;
}
/* Filter bank 0: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F
* CAN1 is master; its filter banks start at 0 (SlaveStartFilterBank=14
* reserves banks 14-27 for CAN2, but CAN2 is unused here).
* FilterIdHigh = 0x200 << 5 = 0x4000 (base ID shifted to bit[15:5])
* FilterMaskHigh = 0x7E0 << 5 = 0xFC00 (mask: top 6 bits must match) */
/* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
* CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0
* → mask=0 passes every standard ID. Orin std-frame commands land here. */
CAN_FilterTypeDef flt = {0};
flt.FilterBank = 0u;
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;
@ -73,6 +73,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));
@ -141,7 +163,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];
@ -151,29 +173,104 @@ 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 */
uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) {
continue;
/* 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) {
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();
}
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;
}
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)

25
src/jlink.c
View File

@ -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));
}

94
src/main.c
View File

@ -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 CAN1 BLDC motor controller bus — PB8/PB9, 500 kbps (Issue #597/#676) */
/* Init CAN1 bus — PB8/PB9, 500 kbps (Issues #597/#676/#674).
* Register callbacks BEFORE can_driver_init() so both are ready when
* filter banks activate. */
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;
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);
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;
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.

96
src/orin_can.c Normal file
View File

@ -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 <string.h>
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));
}

141
src/vesc_can.c Normal file
View File

@ -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 <string.h>
#include <stddef.h>
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);
}

126
test/stubs/stm32f7xx_hal.h Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#include <stddef.h>
/* ---- 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 */

518
test/test_vesc_can.c Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#include <stddef.h>
/* 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 <stdio.h>
#include <string.h>
#include <math.h>
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;
}