fix: Make VESC CAN IDs configurable, default 56/68 (Issue #667)

FSESC 6.7 Pro Mini Dual uses CAN IDs 56/68, not 61/79. Updates all
driver, telemetry, and odometry bridge files to use correct defaults.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

include/can_driver.h

@@ -5,7 +5,7 @@
 #include <stdbool.h>
 
 /* CAN bus driver for BLDC motor controllers (Issue #597)
- * CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
+ * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
  * APB1 = 54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq → 18 tq/bit = 500 kbps
  */
 
@@ -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 */

include/config.h

@@ -257,8 +257,9 @@
 #define GIMBAL_PAN_LIMIT_DEG    180.0f        // pan  soft limit (deg each side)
 #define GIMBAL_TILT_LIMIT_DEG   90.0f         // tilt soft limit (deg each side)
 
-// --- CAN Bus Driver (Issue #597) ---
-// CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) — repurposes SPI2/OSD pins (unused on balance bot)
+// --- CAN Bus Driver (Issue #597, remapped Issue #676) ---
+// CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) — SCL/SDA pads on Mamba F722S MK2
+// I2C1 freed: BME280 moved to I2C2 (PB10/PB11); PB8/PB9 repurposed for CAN1
 #define CAN_RPM_SCALE           10             // motor_cmd to RPM: 1 cmd count = 10 RPM
 #define CAN_TLM_HZ              1u             // JLINK_TLM_CAN_STATS transmit rate (Hz)
 

include/imu_cal_flash.h

@@ -0,0 +1,51 @@
+#ifndef IMU_CAL_FLASH_H
+#define IMU_CAL_FLASH_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * IMU mount angle calibration flash storage (Issue #680).
+ *
+ * Sector 7 (128 KB at 0x08060000) layout:
+ *   0x0807FF00  imu_cal_flash_t   (64 bytes)  ← this module
+ *   0x0807FF40  pid_sched_flash_t (128 bytes) ← pid_flash.c
+ *   0x0807FFC0  pid_flash_t       (64 bytes)  ← pid_flash.c
+ *
+ * Calibration flow:
+ *   1. Mount robot at its installed angle, power on, let IMU converge (~5s).
+ *   2. Send 'O' via USB CDC (dev-only path).
+ *   3. Firmware captures current pitch + roll as mount offsets, saves to flash.
+ *   4. mpu6000_read() subtracts offsets from output on every subsequent read.
+ *
+ * The sector erase preserves existing PID data by reading it first.
+ */
+
+#define IMU_CAL_FLASH_ADDR    0x0807FF00UL
+#define IMU_CAL_FLASH_MAGIC   0x534C5403UL   /* 'SLT\x03' — version 3 */
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;           /* IMU_CAL_FLASH_MAGIC when valid */
+    float    pitch_offset;    /* degrees subtracted from IMU pitch output */
+    float    roll_offset;     /* degrees subtracted from IMU roll output */
+    uint8_t  _pad[52];        /* padding to 64 bytes */
+} imu_cal_flash_t;            /* 64 bytes total */
+
+/*
+ * imu_cal_flash_load() — read saved mount offsets from flash.
+ * Returns true and fills *pitch_offset / *roll_offset if magic is valid.
+ * Returns false if no valid calibration stored (caller keeps 0.0f defaults).
+ */
+bool imu_cal_flash_load(float *pitch_offset, float *roll_offset);
+
+/*
+ * imu_cal_flash_save() — erase sector 7 and write all three records atomically:
+ *   imu_cal_flash_t at 0x0807FF00
+ *   pid_sched_flash_t at 0x0807FF40 (preserved from existing flash)
+ *   pid_flash_t at 0x0807FFC0       (preserved from existing flash)
+ * Must be called while disarmed — sector erase stalls CPU ~1s.
+ * Returns true on success.
+ */
+bool imu_cal_flash_save(float pitch_offset, float roll_offset);
+
+#endif /* IMU_CAL_FLASH_H */

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

include/mpu6000.h

@@ -29,4 +29,15 @@ bool mpu6000_is_calibrated(void);
 
 void mpu6000_read(IMUData *data);
 
+/*
+ * mpu6000_set_mount_offset(pitch_deg, roll_deg) — set mount angle offsets.
+ * These are subtracted from the pitch and roll outputs in mpu6000_read().
+ * Load via imu_cal_flash_load() on boot; update after 'O' CDC command.
+ * Issue #680.
+ */
+void mpu6000_set_mount_offset(float pitch_deg, float roll_deg);
+
+/* Returns true if non-zero mount offsets have been applied (Issue #680). */
+bool mpu6000_has_mount_offset(void);
+
 #endif

include/orin_can.h

@@ -0,0 +1,152 @@
+#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
+#define ORIN_CAN_ID_LED_CMD     0x304u  /* LED pattern override (Issue #685) */
+
+/* ---- FC → Orin telemetry IDs ---- */
+#define ORIN_CAN_ID_FC_STATUS   0x400u  /* balance state + pitch + vbat at 10 Hz */
+#define ORIN_CAN_ID_FC_VESC     0x401u  /* VESC RPM + current at 10 Hz */
+#define ORIN_CAN_ID_FC_IMU      0x402u  /* full IMU angles + cal status at 50 Hz (Issue #680) */
+#define ORIN_CAN_ID_FC_BARO     0x403u  /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */
+
+/* ---- Timing ---- */
+#define ORIN_HB_TIMEOUT_MS      500u   /* Orin offline after 500 ms without any frame */
+#define ORIN_TLM_HZ             10u    /* FC_STATUS + FC_VESC broadcast rate (Hz) */
+#define ORIN_IMU_TLM_HZ         50u    /* FC_IMU broadcast rate (Hz) */
+#define ORIN_BARO_TLM_HZ        1u     /* FC_BARO 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 */
+
+/* FC_IMU (0x402) — full IMU angles at 50 Hz (Issue #680) */
+typedef struct __attribute__((packed)) {
+    int16_t pitch_x10;      /* pitch degrees × 10 (mount-offset corrected) */
+    int16_t roll_x10;       /* roll  degrees × 10 (mount-offset corrected) */
+    int16_t yaw_x10;        /* yaw   degrees × 10 (gyro-integrated, drifts) */
+    uint8_t cal_status;     /* 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
+    uint8_t balance_state;  /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
+} orin_can_fc_imu_t;  /* 8 bytes */
+
+/* FC_BARO (0x403) — barometer at 1 Hz (Issue #672) */
+typedef struct __attribute__((packed)) {
+    int32_t pressure_pa;    /* barometric pressure in Pa */
+    int16_t temp_x10;       /* temperature × 10 (°C) */
+    int16_t alt_cm;         /* altitude in cm (reference = pressure at boot) */
+} orin_can_fc_baro_t;  /* 8 bytes */
+
+/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685)
+ * duration_ms = 0: hold until next state change; >0: revert after duration */
+typedef struct __attribute__((packed)) {
+    uint8_t  pattern;       /* 0=state_auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse */
+    uint8_t  brightness;    /* 0-255 (0 = both LEDs off) */
+    uint16_t duration_ms;   /* override duration; 0 = permanent until state change */
+} orin_can_led_cmd_t;  /* 4 bytes */
+
+/* LED override state (updated by orin_can_on_frame, read by main loop) */
+extern volatile orin_can_led_cmd_t orin_can_led_override;
+extern volatile uint8_t            orin_can_led_updated;
+
+/* ---- 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);
+
+/*
+ * orin_can_broadcast_imu(now_ms, imu_tlm) — rate-limited broadcast of
+ * FC_IMU (0x402) at ORIN_IMU_TLM_HZ (50 Hz).
+ * Safe to call every ms; internally rate-limited.  Issue #680.
+ */
+void orin_can_broadcast_imu(uint32_t now_ms,
+                            const orin_can_fc_imu_t *imu_tlm);
+
+/*
+ * orin_can_broadcast_baro(now_ms, baro_tlm) — rate-limited broadcast of
+ * FC_BARO (0x403) at ORIN_BARO_TLM_HZ (1 Hz).
+ * Pass NULL to skip transmission.  Issue #672.
+ */
+void orin_can_broadcast_baro(uint32_t now_ms,
+                             const orin_can_fc_baro_t *baro_tlm);
+
+#endif /* ORIN_CAN_H */

include/vesc_can.h

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

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py

@@ -0,0 +1,402 @@
+"""
+saltybot_can_node — production FC↔Orin bridge over CAN bus (Issues #680, #672, #685)
+
+In production the FC has NO USB connection to Orin — CAN bus only.
+Reads IMU, balance state, barometer, and VESC telemetry from FC over CAN
+and publishes them as ROS2 topics.  Sends drive/heartbeat/estop commands
+back to FC over CAN.
+
+CAN interface: SocketCAN (CANable USB adapter on vcan0 / can0)
+
+FC → Orin (telemetry):
+  0x400  FC_STATUS   int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
+                     uint8 balance_state, uint8 flags          (10 Hz)
+  0x401  FC_VESC     int16 left_rpm_x10, int16 right_rpm_x10,
+                     int16 left_cur_x10, int16 right_cur_x10   (10 Hz)
+  0x402  FC_IMU      int16 pitch_x10, int16 roll_x10, int16 yaw_x10,
+                     uint8 cal_status, uint8 balance_state      (50 Hz)
+  0x403  FC_BARO     int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz)
+
+Orin → FC (commands):
+  0x300  HEARTBEAT   uint32 sequence counter                    (5 Hz)
+  0x301  DRIVE       int16 speed BE, int16 steer BE             (on /cmd_vel)
+  0x303  ESTOP       uint8 1=estop, 0=clear
+  0x304  LED_CMD     uint8 pattern, uint8 brightness, uint16 duration_ms LE
+
+Published topics:
+  /saltybot/imu            (sensor_msgs/Imu)       — from 0x402
+  /saltybot/balance_state  (std_msgs/String)        — from 0x402 + 0x400
+  /saltybot/barometer      (sensor_msgs/FluidPressure) — from 0x403
+
+Subscribed topics:
+  /cmd_vel                 (geometry_msgs/Twist)   — sent as DRIVE
+  /saltybot/leds           (std_msgs/String JSON)  — sent as LED_CMD
+
+Parameters:
+  can_interface   CAN socket interface name     default: can0
+  speed_scale     m/s to ESC units multiplier   default: 1000.0
+  steer_scale     rad/s to ESC units multiplier default: -500.0
+  heartbeat_hz    HEARTBEAT TX rate (Hz)         default: 5.0
+"""
+
+import json
+import math
+import socket
+import struct
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Imu, FluidPressure
+from std_msgs.msg import String
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+# ---- CAN frame IDs ------------------------------------------------
+
+CAN_FC_STATUS   = 0x400
+CAN_FC_VESC     = 0x401
+CAN_FC_IMU      = 0x402
+CAN_FC_BARO     = 0x403
+
+CAN_HEARTBEAT   = 0x300
+CAN_DRIVE       = 0x301
+CAN_ESTOP       = 0x303
+CAN_LED_CMD     = 0x304
+
+# ---- Constants ----------------------------------------------------
+
+IMU_FRAME_ID    = "imu_link"
+_STATE_LABEL    = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
+_CAL_LABEL      = {0: "UNCAL", 1: "GYRO_CAL", 2: "MOUNT_CAL"}
+
+# ---- Helpers ------------------------------------------------------
+
+def _clamp(v: float, lo: float, hi: float) -> float:
+    return max(lo, min(hi, v))
+
+
+def _can_socket(iface: str) -> socket.socket:
+    """Open a raw SocketCAN socket on *iface*."""
+    s = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
+    s.bind((iface,))
+    s.settimeout(0.1)
+    return s
+
+
+def _pack_frame(can_id: int, data: bytes) -> bytes:
+    """Pack a standard CAN frame for SocketCAN (16-byte struct)."""
+    dlc = len(data)
+    return struct.pack("=IB3x8s", can_id & 0x1FFFFFFF, dlc,
+                       data.ljust(8, b"\x00"))
+
+
+def _unpack_frame(raw: bytes):
+    """Unpack a raw SocketCAN frame; returns (can_id, data_bytes)."""
+    can_id, dlc = struct.unpack_from("=IB", raw, 0)
+    data = raw[8: 8 + (dlc & 0x0F)]
+    return can_id & 0x1FFFFFFF, data
+
+
+# ---- Node ---------------------------------------------------------
+
+class SaltybotCanNode(Node):
+    def __init__(self):
+        super().__init__("saltybot_can")
+
+        # ── Parameters ───────────────────────────────────────────────
+        self.declare_parameter("can_interface",  "can0")
+        self.declare_parameter("speed_scale",    1000.0)
+        self.declare_parameter("steer_scale",    -500.0)
+        self.declare_parameter("heartbeat_hz",   5.0)
+
+        iface          = self.get_parameter("can_interface").value
+        self._speed_sc = self.get_parameter("speed_scale").value
+        self._steer_sc = self.get_parameter("steer_scale").value
+        hb_hz          = self.get_parameter("heartbeat_hz").value
+
+        # ── QoS ──────────────────────────────────────────────────────
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST, depth=10)
+        reliable_qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST, depth=10)
+
+        # ── Publishers ───────────────────────────────────────────────
+        self._imu_pub  = self.create_publisher(Imu,          "/saltybot/imu",           sensor_qos)
+        self._bal_pub  = self.create_publisher(String,       "/saltybot/balance_state",  reliable_qos)
+        self._baro_pub = self.create_publisher(FluidPressure,"/saltybot/barometer",      reliable_qos)
+        self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics",           reliable_qos)
+
+        # ── Subscribers ──────────────────────────────────────────────
+        self._cmd_sub  = self.create_subscription(
+            Twist, "/cmd_vel", self._cmd_vel_cb, reliable_qos)
+        self._led_sub  = self.create_subscription(
+            String, "/saltybot/leds", self._led_cb, reliable_qos)
+
+        # ── State ────────────────────────────────────────────────────
+        self._iface       = iface
+        self._sock        = None
+        self._sock_lock   = threading.Lock()
+        self._hb_seq      = 0
+        self._last_speed  = 0
+        self._last_steer  = 0
+        self._last_state  = -1
+        self._last_cal    = -1
+        self._last_pitch  = 0.0
+        self._last_roll   = 0.0
+        self._last_yaw    = 0.0
+        self._last_motor  = 0
+        self._last_vbat   = 0
+        self._last_flags  = 0
+
+        # ── Open CAN ─────────────────────────────────────────────────
+        self._open_can()
+
+        # ── Timers ───────────────────────────────────────────────────
+        self._rx_timer = self.create_timer(0.01, self._rx_cb)         # 100 Hz poll
+        self._hb_timer = self.create_timer(1.0 / hb_hz, self._hb_cb)
+
+        self.get_logger().info(
+            f"saltybot_can started — interface={iface} "
+            f"(speed_scale={self._speed_sc}, steer_scale={self._steer_sc})"
+        )
+
+    # ── CAN socket management ────────────────────────────────────────
+
+    def _open_can(self) -> bool:
+        with self._sock_lock:
+            try:
+                self._sock = _can_socket(self._iface)
+                self.get_logger().info(f"CAN socket open: {self._iface}")
+                return True
+            except OSError as exc:
+                self.get_logger().error(f"Cannot open CAN {self._iface}: {exc}")
+                self._sock = None
+                return False
+
+    def _send(self, can_id: int, data: bytes) -> bool:
+        with self._sock_lock:
+            if self._sock is None:
+                return False
+            try:
+                self._sock.send(_pack_frame(can_id, data))
+                return True
+            except OSError as exc:
+                self.get_logger().error(
+                    f"CAN TX error: {exc}", throttle_duration_sec=2.0)
+                return False
+
+    # ── RX poll ──────────────────────────────────────────────────────
+
+    def _rx_cb(self):
+        with self._sock_lock:
+            if self._sock is None:
+                return
+            try:
+                while True:
+                    raw = self._sock.recv(16)
+                    can_id, data = _unpack_frame(raw)
+                    self._dispatch(can_id, data)
+            except socket.timeout:
+                pass
+            except BlockingIOError:
+                pass
+            except OSError as exc:
+                self.get_logger().error(
+                    f"CAN RX error: {exc}", throttle_duration_sec=5.0)
+                self._sock = None
+
+        if self._sock is None:
+            self._open_can()
+
+    def _dispatch(self, can_id: int, data: bytes):
+        now = self.get_clock().now().to_msg()
+        if can_id == CAN_FC_IMU and len(data) >= 8:
+            self._handle_fc_imu(data, now)
+        elif can_id == CAN_FC_STATUS and len(data) >= 8:
+            self._handle_fc_status(data)
+        elif can_id == CAN_FC_BARO and len(data) >= 8:
+            self._handle_fc_baro(data, now)
+
+    # ── Frame handlers ───────────────────────────────────────────────
+
+    def _handle_fc_imu(self, data: bytes, stamp):
+        pitch_x10, roll_x10, yaw_x10, cal, state = struct.unpack_from("<hhhBB", data, 0)
+        pitch = pitch_x10 / 10.0
+        roll  = roll_x10  / 10.0
+        yaw   = yaw_x10   / 10.0
+
+        self._last_pitch = pitch
+        self._last_roll  = roll
+        self._last_yaw   = yaw
+        self._last_cal   = cal
+
+        # Publish IMU
+        imu = Imu()
+        imu.header.stamp    = stamp
+        imu.header.frame_id = IMU_FRAME_ID
+        imu.orientation_covariance[0] = -1.0   # orientation not provided
+
+        # Publish Euler angles via angular_velocity fields (convention matches
+        # saltybot_cmd_node — downstream nodes expect this mapping)
+        imu.angular_velocity.x = math.radians(pitch)
+        imu.angular_velocity.y = math.radians(roll)
+        imu.angular_velocity.z = math.radians(yaw)
+        cov = math.radians(0.5) ** 2
+        imu.angular_velocity_covariance[0] = cov
+        imu.angular_velocity_covariance[4] = cov
+        imu.angular_velocity_covariance[8] = cov
+        imu.linear_acceleration_covariance[0] = -1.0
+        self._imu_pub.publish(imu)
+
+        # Publish balance_state JSON
+        self._publish_balance_state(pitch, roll, yaw, state, cal, stamp)
+
+        # Log state/cal transitions
+        if state != self._last_state:
+            self.get_logger().info(
+                f"Balance state → {_STATE_LABEL.get(state, f'UNKNOWN({state})')}"
+            )
+            self._last_state = state
+        if cal != self._last_cal:
+            self.get_logger().info(
+                f"IMU cal status → {_CAL_LABEL.get(cal, f'CAL({cal})')}"
+            )
+
+    def _handle_fc_status(self, data: bytes):
+        pitch_x10, motor_cmd, vbat_mv, balance_state, flags = \
+            struct.unpack_from("<hhhBB", data, 0)
+        self._last_motor = motor_cmd
+        self._last_vbat  = vbat_mv
+        self._last_flags = flags
+
+    def _handle_fc_baro(self, data: bytes, stamp):
+        pressure_pa, temp_x10, alt_cm = struct.unpack_from("<ihh", data, 0)
+        msg = FluidPressure()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = "barometer_link"
+        msg.fluid_pressure  = float(pressure_pa)      # Pa
+        msg.variance        = 0.0
+        self._baro_pub.publish(msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        st = DiagnosticStatus()
+        st.level       = DiagnosticStatus.OK
+        st.name        = "saltybot/barometer"
+        st.hardware_id = "bmp280"
+        st.message     = "OK"
+        st.values = [
+            KeyValue(key="pressure_pa", value=str(pressure_pa)),
+            KeyValue(key="temp_c",      value=f"{temp_x10 / 10.0:.1f}"),
+            KeyValue(key="alt_cm",      value=str(alt_cm)),
+        ]
+        diag.status.append(st)
+        self._diag_pub.publish(diag)
+
+    def _publish_balance_state(self, pitch, roll, yaw, state, cal, stamp):
+        state_label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
+        cal_label   = _CAL_LABEL.get(cal, f"CAL({cal})")
+        payload = {
+            "stamp":        f"{stamp.sec}.{stamp.nanosec:09d}",
+            "state":        state_label,
+            "cal_status":   cal_label,
+            "pitch_deg":    round(pitch, 1),
+            "roll_deg":     round(roll,  1),
+            "yaw_deg":      round(yaw,   1),
+            "motor_cmd":    self._last_motor,
+            "vbat_mv":      self._last_vbat,
+            "jetson_speed": self._last_speed,
+            "jetson_steer": self._last_steer,
+        }
+        str_msg = String()
+        str_msg.data = json.dumps(payload)
+        self._bal_pub.publish(str_msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        st = DiagnosticStatus()
+        st.name        = "saltybot/balance_controller"
+        st.hardware_id = "stm32f722"
+        st.message     = state_label
+        st.level       = (DiagnosticStatus.OK    if state == 1 else
+                          DiagnosticStatus.WARN   if state == 0 else
+                          DiagnosticStatus.ERROR)
+        st.values = [
+            KeyValue(key="pitch_deg",  value=f"{pitch:.1f}"),
+            KeyValue(key="roll_deg",   value=f"{roll:.1f}"),
+            KeyValue(key="yaw_deg",    value=f"{yaw:.1f}"),
+            KeyValue(key="motor_cmd",  value=str(self._last_motor)),
+            KeyValue(key="state",      value=state_label),
+            KeyValue(key="cal_status", value=cal_label),
+        ]
+        diag.status.append(st)
+        self._diag_pub.publish(diag)
+
+    # ── TX callbacks ─────────────────────────────────────────────────
+
+    def _hb_cb(self):
+        """Send HEARTBEAT (0x300) with incrementing sequence counter."""
+        data = struct.pack(">I", self._hb_seq & 0xFFFFFFFF)
+        self._hb_seq += 1
+        self._send(CAN_HEARTBEAT, data)
+
+    def _cmd_vel_cb(self, msg: Twist):
+        """Convert Twist → DRIVE (0x301): int16 speed BE, int16 steer BE."""
+        speed = int(_clamp(msg.linear.x  * self._speed_sc, -1000.0, 1000.0))
+        steer = int(_clamp(msg.angular.z * self._steer_sc, -1000.0, 1000.0))
+        self._last_speed = speed
+        self._last_steer = steer
+        data = struct.pack(">hh", speed, steer)
+        self._send(CAN_DRIVE, data)
+
+    def _led_cb(self, msg: String):
+        """Parse /saltybot/leds JSON → LED_CMD (0x304).
+        Expected JSON: {"pattern": 1, "brightness": 200, "duration_ms": 5000}
+        pattern: 0=auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse
+        """
+        try:
+            d = json.loads(msg.data)
+        except (ValueError, TypeError) as exc:
+            self.get_logger().debug(f"LED JSON parse error: {exc}")
+            return
+        pattern     = int(d.get("pattern",     0)) & 0xFF
+        brightness  = int(d.get("brightness",  255)) & 0xFF
+        duration_ms = int(d.get("duration_ms", 0)) & 0xFFFF
+        data = struct.pack("<BBH", pattern, brightness, duration_ms)
+        self._send(CAN_LED_CMD, data)
+
+    # ── Lifecycle ────────────────────────────────────────────────────
+
+    def destroy_node(self):
+        # Send ESTOP on shutdown so FC doesn't keep rolling
+        self._send(CAN_ESTOP, bytes([1]))
+        with self._sock_lock:
+            if self._sock:
+                try:
+                    self._sock.close()
+                except OSError:
+                    pass
+                self._sock = None
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SaltybotCanNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py

@@ -154,12 +154,12 @@ class SaltybotCmdNode(Node):
     # ── RX — telemetry read ───────────────────────────────────────────────────
 
     def _read_cb(self):
+        lines = []
         with self._ser_lock:
             if self._ser is None or not self._ser.is_open:
                 pass
             else:
                 try:
-                    lines = []
                     while self._ser.in_waiting:
                         raw = self._ser.readline()
                         if raw:

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -15,6 +15,7 @@ setup(
             "launch/remote_estop.launch.py",
             "launch/stm32_cmd.launch.py",
             "launch/battery.launch.py",
+            "launch/uart_bridge.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/bridge_params.yaml",
@@ -44,6 +45,8 @@ setup(
             "stm32_cmd_node    = saltybot_bridge.stm32_cmd_node:main",
             # Battery management node (Issue #125)
             "battery_node      = saltybot_bridge.battery_node:main",
+            # Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685)
+            "saltybot_can_node = saltybot_bridge.saltybot_can_node:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml

@@ -0,0 +1,7 @@
+can_bridge_node:
+  ros__parameters:
+    can_interface: slcan0
+    left_vesc_can_id: 56
+    right_vesc_can_id: 68
+    mamba_can_id: 1
+    command_timeout_s: 0.5

jetson/ros2_ws/src/saltybot_can_bridge/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_can_bridge</name>
+  <version>0.1.0</version>
+  <description>
+    CAN bus bridge for SaltyBot Orin — interfaces with the Mamba motor
+    controller and VESC motor controllers via CANable 2.0 (slcan interface).
+    Publishes IMU, battery, and VESC telemetry to ROS2 topics and forwards
+    cmd_vel / estop commands to the CAN bus.
+
+    System dependency: python3-can (apt: python3-can  or  pip: python-can)
+  </description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <!-- python-can: install via  apt install python3-can  or  pip install python-can -->
+  <exec_depend>python3-can</exec_depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py

@@ -0,0 +1 @@
+"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can."""

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py

@@ -0,0 +1,383 @@
+#!/usr/bin/env python3
+"""
+can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor
+controller and VESC motor controllers over CAN bus.
+
+The node opens the SocketCAN interface (slcan0 by default), spawns a background
+reader thread to process incoming telemetry, and exposes the following interface:
+
+Subscriptions
+-------------
+  /cmd_vel          geometry_msgs/Twist          → VESC speed commands (CAN)
+  /estop            std_msgs/Bool                → Mamba e-stop (CAN)
+
+Publications
+------------
+  /can/imu                sensor_msgs/Imu              Mamba IMU telemetry
+  /can/battery            sensor_msgs/BatteryState     Mamba battery telemetry
+  /can/vesc/left/state    std_msgs/Float32MultiArray   Left VESC state
+  /can/vesc/right/state   std_msgs/Float32MultiArray   Right VESC state
+  /can/connection_status  std_msgs/String              "connected" | "disconnected"
+
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+"""
+
+import threading
+import time
+from typing import Optional
+
+import can
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+from sensor_msgs.msg import BatteryState, Imu
+from std_msgs.msg import Bool, Float32MultiArray, String
+
+from saltybot_can_bridge.mamba_protocol import (
+    MAMBA_CMD_ESTOP,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_VELOCITY,
+    MAMBA_TELEM_BATTERY,
+    MAMBA_TELEM_IMU,
+    VESC_TELEM_STATE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    MODE_IDLE,
+    encode_estop_cmd,
+    encode_mode_cmd,
+    encode_velocity_cmd,
+    decode_battery_telem,
+    decode_imu_telem,
+    decode_vesc_state,
+)
+
+# Reconnect attempt interval when CAN bus is lost
+_RECONNECT_INTERVAL_S: float = 5.0
+
+# Watchdog timer tick rate (Hz)
+_WATCHDOG_HZ: float = 10.0
+
+
+class CanBridgeNode(Node):
+    """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers."""
+
+    def __init__(self) -> None:
+        super().__init__("can_bridge_node")
+
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("can_interface", "slcan0")
+        self.declare_parameter("left_vesc_can_id", 56)
+        self.declare_parameter("right_vesc_can_id", 68)
+        self.declare_parameter("mamba_can_id", 1)
+        self.declare_parameter("command_timeout_s", 0.5)
+
+        self._iface: str = self.get_parameter("can_interface").value
+        self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
+        self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
+        self._mamba_id: int = self.get_parameter("mamba_can_id").value
+        self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
+
+        # ── State ─────────────────────────────────────────────────────────
+        self._bus: Optional[can.BusABC] = None
+        self._connected: bool = False
+        self._last_cmd_time: float = time.monotonic()
+        self._lock = threading.Lock()  # protects _bus / _connected
+
+        # ── Publishers ────────────────────────────────────────────────────
+        self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
+        self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
+        self._pub_vesc_left = self.create_publisher(
+            Float32MultiArray, "/can/vesc/left/state", 10
+        )
+        self._pub_vesc_right = self.create_publisher(
+            Float32MultiArray, "/can/vesc/right/state", 10
+        )
+        self._pub_status = self.create_publisher(
+            String, "/can/connection_status", 10
+        )
+
+        # ── Subscriptions ─────────────────────────────────────────────────
+        self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
+        self.create_subscription(Bool, "/estop", self._estop_cb, 10)
+
+        # ── Timers ────────────────────────────────────────────────────────
+        self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
+        self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb)
+
+        # ── Open CAN ──────────────────────────────────────────────────────
+        self._try_connect()
+
+        # ── Background reader thread ──────────────────────────────────────
+        self._reader_thread = threading.Thread(
+            target=self._reader_loop, daemon=True, name="can_reader"
+        )
+        self._reader_thread.start()
+
+        self.get_logger().info(
+            f"can_bridge_node ready — iface={self._iface} "
+            f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
+            f"mamba={self._mamba_id}"
+        )
+
+    # ── Connection management ──────────────────────────────────────────────
+
+    def _try_connect(self) -> None:
+        """Attempt to open the CAN interface; silently skip if already connected."""
+        with self._lock:
+            if self._connected:
+                return
+            try:
+                bus = can.interface.Bus(
+                    channel=self._iface,
+                    bustype="socketcan",
+                )
+                self._bus = bus
+                self._connected = True
+                self.get_logger().info(f"CAN bus connected: {self._iface}")
+                self._publish_status("connected")
+            except Exception as exc:
+                self.get_logger().warning(
+                    f"CAN bus not available ({self._iface}): {exc} "
+                    f"— will retry every {_RECONNECT_INTERVAL_S:.0f} s"
+                )
+                self._connected = False
+                self._publish_status("disconnected")
+
+    def _reconnect_cb(self) -> None:
+        """Periodic timer: try to reconnect when disconnected."""
+        if not self._connected:
+            self._try_connect()
+
+    def _handle_can_error(self, exc: Exception, context: str) -> None:
+        """Mark bus as disconnected on any CAN error."""
+        self.get_logger().warning(f"CAN error in {context}: {exc}")
+        with self._lock:
+            if self._bus is not None:
+                try:
+                    self._bus.shutdown()
+                except Exception:
+                    pass
+            self._bus = None
+            self._connected = False
+        self._publish_status("disconnected")
+
+    # ── ROS callbacks ─────────────────────────────────────────────────────
+
+    def _cmd_vel_cb(self, msg: Twist) -> None:
+        """Convert /cmd_vel Twist to VESC speed commands over CAN."""
+        self._last_cmd_time = time.monotonic()
+
+        if not self._connected:
+            return
+
+        # Differential drive decomposition — individual wheel speeds in m/s.
+        # The VESC nodes interpret linear velocity directly; angular is handled
+        # by the sign difference between left and right.
+        linear = msg.linear.x
+        angular = msg.angular.z
+
+        # Forward left = forward right for pure translation; for rotation
+        # left slows and right speeds up (positive angular = CCW = left turn).
+        # The Mamba velocity command carries both wheels independently.
+        left_mps = linear - angular
+        right_mps = linear + angular
+
+        payload = encode_velocity_cmd(left_mps, right_mps)
+        self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
+
+        # Keep Mamba in DRIVE mode while receiving commands
+        self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
+
+    def _estop_cb(self, msg: Bool) -> None:
+        """Forward /estop to Mamba over CAN."""
+        if not self._connected:
+            return
+        payload = encode_estop_cmd(msg.data)
+        self._send_can(MAMBA_CMD_ESTOP, payload, "estop")
+        if msg.data:
+            self._send_can(
+                MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
+            )
+            self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba")
+
+    # ── Watchdog ──────────────────────────────────────────────────────────
+
+    def _watchdog_cb(self) -> None:
+        """If no /cmd_vel arrives within the timeout, send zero velocity."""
+        if not self._connected:
+            return
+        elapsed = time.monotonic() - self._last_cmd_time
+        if elapsed > self._cmd_timeout:
+            self._send_can(
+                MAMBA_CMD_VELOCITY,
+                encode_velocity_cmd(0.0, 0.0),
+                "watchdog zero-vel",
+            )
+            self._send_can(
+                MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
+            )
+
+    # ── CAN send helper ───────────────────────────────────────────────────
+
+    def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
+        """Send a standard CAN frame; handle errors gracefully."""
+        with self._lock:
+            if not self._connected or self._bus is None:
+                return
+            bus = self._bus
+
+        msg = can.Message(
+            arbitration_id=arb_id,
+            data=data,
+            is_extended_id=False,
+        )
+        try:
+            bus.send(msg, timeout=0.05)
+        except can.CanError as exc:
+            self._handle_can_error(exc, f"send({context})")
+
+    # ── Background CAN reader ─────────────────────────────────────────────
+
+    def _reader_loop(self) -> None:
+        """
+        Blocking CAN read loop executed in a daemon thread.
+        Dispatches incoming frames to the appropriate handler.
+        """
+        while rclpy.ok():
+            with self._lock:
+                connected = self._connected
+                bus = self._bus
+
+            if not connected or bus is None:
+                time.sleep(0.1)
+                continue
+
+            try:
+                frame = bus.recv(timeout=0.5)
+            except can.CanError as exc:
+                self._handle_can_error(exc, "reader_loop recv")
+                continue
+
+            if frame is None:
+                # Timeout — no frame within 0.5 s, loop again
+                continue
+
+            self._dispatch_frame(frame)
+
+    def _dispatch_frame(self, frame: can.Message) -> None:
+        """Route an incoming CAN frame to the correct publisher."""
+        arb_id = frame.arbitration_id
+        data = bytes(frame.data)
+
+        try:
+            if arb_id == MAMBA_TELEM_IMU:
+                self._handle_imu(data, frame.timestamp)
+
+            elif arb_id == MAMBA_TELEM_BATTERY:
+                self._handle_battery(data, frame.timestamp)
+
+            elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
+                self._handle_vesc_state(data, frame.timestamp, side="left")
+
+            elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
+                self._handle_vesc_state(data, frame.timestamp, side="right")
+
+        except Exception as exc:
+            self.get_logger().warning(
+                f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
+            )
+
+    # ── Frame handlers ────────────────────────────────────────────────────
+
+    def _handle_imu(self, data: bytes, timestamp: float) -> None:
+        telem = decode_imu_telem(data)
+
+        msg = Imu()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "imu_link"
+
+        msg.linear_acceleration.x = telem.accel_x
+        msg.linear_acceleration.y = telem.accel_y
+        msg.linear_acceleration.z = telem.accel_z
+
+        msg.angular_velocity.x = telem.gyro_x
+        msg.angular_velocity.y = telem.gyro_y
+        msg.angular_velocity.z = telem.gyro_z
+
+        # Covariance unknown; mark as -1 per REP-145
+        msg.orientation_covariance[0] = -1.0
+
+        self._pub_imu.publish(msg)
+
+    def _handle_battery(self, data: bytes, timestamp: float) -> None:
+        telem = decode_battery_telem(data)
+
+        msg = BatteryState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.voltage = telem.voltage
+        msg.current = telem.current
+        msg.present = True
+        msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+
+        self._pub_battery.publish(msg)
+
+    def _handle_vesc_state(
+        self, data: bytes, timestamp: float, side: str
+    ) -> None:
+        telem = decode_vesc_state(data)
+
+        msg = Float32MultiArray()
+        # Layout: [erpm, duty, voltage, current]
+        msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
+
+        if side == "left":
+            self._pub_vesc_left.publish(msg)
+        else:
+            self._pub_vesc_right.publish(msg)
+
+    # ── Status helper ─────────────────────────────────────────────────────
+
+    def _publish_status(self, status: str) -> None:
+        msg = String()
+        msg.data = status
+        self._pub_status.publish(msg)
+
+    # ── Shutdown ──────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        """Send zero velocity and shut down the CAN bus cleanly."""
+        if self._connected and self._bus is not None:
+            try:
+                self._send_can(
+                    MAMBA_CMD_VELOCITY,
+                    encode_velocity_cmd(0.0, 0.0),
+                    "shutdown",
+                )
+                self._send_can(
+                    MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
+                )
+            except Exception:
+                pass
+            try:
+                self._bus.shutdown()
+            except Exception:
+                pass
+        super().destroy_node()
+
+
+# ---------------------------------------------------------------------------
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = CanBridgeNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py

@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+"""
+mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller
+and VESC telemetry.
+
+CAN message layout
+------------------
+Command frames  (Orin → Mamba / VESC):
+  MAMBA_CMD_VELOCITY  0x100  8 bytes  left_speed (f32, m/s) | right_speed (f32, m/s)
+  MAMBA_CMD_MODE      0x101  1 byte   mode (0=idle, 1=drive, 2=estop)
+  MAMBA_CMD_ESTOP     0x102  1 byte   0x01 = stop
+
+Telemetry frames (Mamba → Orin):
+  MAMBA_TELEM_IMU     0x200  24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each)
+  MAMBA_TELEM_BATTERY 0x201   8 bytes voltage (f32, V) | current (f32, A)
+
+VESC telemetry frame (VESC → Orin):
+  VESC_TELEM_STATE    0x300  16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32)
+
+All multi-byte fields are big-endian.
+
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+"""
+
+import struct
+from dataclasses import dataclass
+from typing import Tuple
+
+# ---------------------------------------------------------------------------
+# CAN message IDs
+# ---------------------------------------------------------------------------
+
+MAMBA_CMD_VELOCITY: int = 0x100
+MAMBA_CMD_MODE: int = 0x101
+MAMBA_CMD_ESTOP: int = 0x102
+
+MAMBA_TELEM_IMU: int = 0x200
+MAMBA_TELEM_BATTERY: int = 0x201
+
+VESC_TELEM_STATE: int = 0x300
+
+# ---------------------------------------------------------------------------
+# Mode constants
+# ---------------------------------------------------------------------------
+
+MODE_IDLE: int = 0
+MODE_DRIVE: int = 1
+MODE_ESTOP: int = 2
+
+# ---------------------------------------------------------------------------
+# Data classes for decoded telemetry
+# ---------------------------------------------------------------------------
+
+
+@dataclass
+class ImuTelemetry:
+    """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU)."""
+
+    accel_x: float = 0.0  # m/s²
+    accel_y: float = 0.0
+    accel_z: float = 0.0
+    gyro_x: float = 0.0   # rad/s
+    gyro_y: float = 0.0
+    gyro_z: float = 0.0
+
+
+@dataclass
+class BatteryTelemetry:
+    """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY)."""
+
+    voltage: float = 0.0   # V
+    current: float = 0.0   # A
+
+
+@dataclass
+class VescStateTelemetry:
+    """Decoded VESC state telemetry (VESC_TELEM_STATE)."""
+
+    erpm: float = 0.0      # electrical RPM
+    duty: float = 0.0      # duty cycle [-1.0, 1.0]
+    voltage: float = 0.0   # bus voltage, V
+    current: float = 0.0   # phase current, A
+
+
+# ---------------------------------------------------------------------------
+# Encode helpers
+# ---------------------------------------------------------------------------
+
+_FMT_VEL = ">ff"       # 2 × float32, big-endian
+_FMT_MODE = ">B"       # 1 × uint8
+_FMT_ESTOP = ">B"      # 1 × uint8
+_FMT_IMU = ">ffffff"   # 6 × float32
+_FMT_BAT = ">ff"       # 2 × float32
+_FMT_VESC = ">ffff"    # 4 × float32
+
+
+def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
+    """
+    Encode a MAMBA_CMD_VELOCITY payload.
+
+    Parameters
+    ----------
+    left_mps:  target left wheel speed in m/s  (positive = forward)
+    right_mps: target right wheel speed in m/s (positive = forward)
+
+    Returns
+    -------
+    8-byte big-endian payload suitable for a CAN frame.
+    """
+    return struct.pack(_FMT_VEL, float(left_mps), float(right_mps))
+
+
+def encode_mode_cmd(mode: int) -> bytes:
+    """
+    Encode a MAMBA_CMD_MODE payload.
+
+    Parameters
+    ----------
+    mode: one of MODE_IDLE (0), MODE_DRIVE (1), MODE_ESTOP (2)
+
+    Returns
+    -------
+    1-byte payload.
+    """
+    if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+        raise ValueError(f"Invalid mode {mode!r}; expected 0, 1, or 2")
+    return struct.pack(_FMT_MODE, mode)
+
+
+def encode_estop_cmd(stop: bool = True) -> bytes:
+    """
+    Encode a MAMBA_CMD_ESTOP payload.
+
+    Parameters
+    ----------
+    stop: True to assert e-stop, False to clear.
+
+    Returns
+    -------
+    1-byte payload (0x01 = stop, 0x00 = clear).
+    """
+    return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00)
+
+
+# ---------------------------------------------------------------------------
+# Decode helpers
+# ---------------------------------------------------------------------------
+
+def decode_imu_telem(data: bytes) -> ImuTelemetry:
+    """
+    Decode a MAMBA_TELEM_IMU payload.
+
+    Parameters
+    ----------
+    data: exactly 24 bytes (6 × float32, big-endian).
+
+    Returns
+    -------
+    ImuTelemetry dataclass instance.
+
+    Raises
+    ------
+    struct.error if data is the wrong length.
+    """
+    ax, ay, az, gx, gy, gz = struct.unpack(_FMT_IMU, data)
+    return ImuTelemetry(
+        accel_x=ax, accel_y=ay, accel_z=az,
+        gyro_x=gx, gyro_y=gy, gyro_z=gz,
+    )
+
+
+def decode_battery_telem(data: bytes) -> BatteryTelemetry:
+    """
+    Decode a MAMBA_TELEM_BATTERY payload.
+
+    Parameters
+    ----------
+    data: exactly 8 bytes (2 × float32, big-endian).
+
+    Returns
+    -------
+    BatteryTelemetry dataclass instance.
+    """
+    voltage, current = struct.unpack(_FMT_BAT, data)
+    return BatteryTelemetry(voltage=voltage, current=current)
+
+
+def decode_vesc_state(data: bytes) -> VescStateTelemetry:
+    """
+    Decode a VESC_TELEM_STATE payload.
+
+    Parameters
+    ----------
+    data: exactly 16 bytes (4 × float32, big-endian).
+
+    Returns
+    -------
+    VescStateTelemetry dataclass instance.
+    """
+    erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data)
+    return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current)

jetson/ros2_ws/src/saltybot_can_bridge/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/saltybot_can_bridge
+
+[install]
+install_scripts=$base/lib/saltybot_can_bridge

jetson/ros2_ws/src/saltybot_can_bridge/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = "saltybot_can_bridge"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/config", ["config/can_bridge_params.yaml"]),
+    ],
+    install_requires=["setuptools", "python-can"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description="CAN bus bridge for Mamba controller and VESC telemetry",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "can_bridge_node = saltybot_can_bridge.can_bridge_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py

@@ -0,0 +1,254 @@
+#!/usr/bin/env python3
+"""
+Unit tests for saltybot_can_bridge.mamba_protocol.
+
+No ROS2 or CAN hardware required — tests exercise encode/decode round-trips
+and boundary conditions entirely in Python.
+
+Run with:  pytest test/test_can_bridge.py -v
+"""
+
+import struct
+import unittest
+
+from saltybot_can_bridge.mamba_protocol import (
+    MAMBA_CMD_ESTOP,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_VELOCITY,
+    MAMBA_TELEM_BATTERY,
+    MAMBA_TELEM_IMU,
+    VESC_TELEM_STATE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    MODE_IDLE,
+    BatteryTelemetry,
+    ImuTelemetry,
+    VescStateTelemetry,
+    decode_battery_telem,
+    decode_imu_telem,
+    decode_vesc_state,
+    encode_estop_cmd,
+    encode_mode_cmd,
+    encode_velocity_cmd,
+)
+
+
+class TestMessageIDs(unittest.TestCase):
+    """Verify the CAN message ID constants are correct."""
+
+    def test_command_ids(self):
+        self.assertEqual(MAMBA_CMD_VELOCITY, 0x100)
+        self.assertEqual(MAMBA_CMD_MODE, 0x101)
+        self.assertEqual(MAMBA_CMD_ESTOP, 0x102)
+
+    def test_telemetry_ids(self):
+        self.assertEqual(MAMBA_TELEM_IMU, 0x200)
+        self.assertEqual(MAMBA_TELEM_BATTERY, 0x201)
+        self.assertEqual(VESC_TELEM_STATE, 0x300)
+
+
+class TestVelocityEncode(unittest.TestCase):
+    """Tests for encode_velocity_cmd."""
+
+    def test_zero_velocity(self):
+        payload = encode_velocity_cmd(0.0, 0.0)
+        self.assertEqual(len(payload), 8)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, 0.0, places=5)
+        self.assertAlmostEqual(right, 0.0, places=5)
+
+    def test_forward_velocity(self):
+        payload = encode_velocity_cmd(1.5, 1.5)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, 1.5, places=5)
+        self.assertAlmostEqual(right, 1.5, places=5)
+
+    def test_differential_velocity(self):
+        payload = encode_velocity_cmd(-0.5, 0.5)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, -0.5, places=5)
+        self.assertAlmostEqual(right, 0.5, places=5)
+
+    def test_large_velocity(self):
+        # No clamping at the protocol layer — values are sent as-is
+        payload = encode_velocity_cmd(10.0, -10.0)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, 10.0, places=3)
+        self.assertAlmostEqual(right, -10.0, places=3)
+
+    def test_payload_is_big_endian(self):
+        # Sanity check: first 4 bytes encode left speed
+        payload = encode_velocity_cmd(1.0, 0.0)
+        (left,) = struct.unpack(">f", payload[:4])
+        self.assertAlmostEqual(left, 1.0, places=5)
+
+
+class TestModeEncode(unittest.TestCase):
+    """Tests for encode_mode_cmd."""
+
+    def test_idle_mode(self):
+        payload = encode_mode_cmd(MODE_IDLE)
+        self.assertEqual(payload, b"\x00")
+
+    def test_drive_mode(self):
+        payload = encode_mode_cmd(MODE_DRIVE)
+        self.assertEqual(payload, b"\x01")
+
+    def test_estop_mode(self):
+        payload = encode_mode_cmd(MODE_ESTOP)
+        self.assertEqual(payload, b"\x02")
+
+    def test_invalid_mode_raises(self):
+        with self.assertRaises(ValueError):
+            encode_mode_cmd(99)
+
+    def test_invalid_mode_negative_raises(self):
+        with self.assertRaises(ValueError):
+            encode_mode_cmd(-1)
+
+
+class TestEstopEncode(unittest.TestCase):
+    """Tests for encode_estop_cmd."""
+
+    def test_estop_assert(self):
+        self.assertEqual(encode_estop_cmd(True), b"\x01")
+
+    def test_estop_clear(self):
+        self.assertEqual(encode_estop_cmd(False), b"\x00")
+
+    def test_estop_default_is_stop(self):
+        self.assertEqual(encode_estop_cmd(), b"\x01")
+
+
+class TestImuDecodeRoundTrip(unittest.TestCase):
+    """Round-trip tests for IMU telemetry."""
+
+    def _encode_imu(self, ax, ay, az, gx, gy, gz) -> bytes:
+        return struct.pack(">ffffff", ax, ay, az, gx, gy, gz)
+
+    def test_zero_imu(self):
+        data = self._encode_imu(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        telem = decode_imu_telem(data)
+        self.assertIsInstance(telem, ImuTelemetry)
+        self.assertAlmostEqual(telem.accel_x, 0.0, places=5)
+        self.assertAlmostEqual(telem.gyro_z, 0.0, places=5)
+
+    def test_nominal_imu(self):
+        data = self._encode_imu(0.1, -0.2, 9.81, 0.01, -0.02, 0.03)
+        telem = decode_imu_telem(data)
+        self.assertAlmostEqual(telem.accel_x, 0.1, places=4)
+        self.assertAlmostEqual(telem.accel_y, -0.2, places=4)
+        self.assertAlmostEqual(telem.accel_z, 9.81, places=3)
+        self.assertAlmostEqual(telem.gyro_x, 0.01, places=5)
+        self.assertAlmostEqual(telem.gyro_y, -0.02, places=5)
+        self.assertAlmostEqual(telem.gyro_z, 0.03, places=5)
+
+    def test_imu_bad_length_raises(self):
+        with self.assertRaises(struct.error):
+            decode_imu_telem(b"\x00" * 10)  # too short
+
+
+class TestBatteryDecodeRoundTrip(unittest.TestCase):
+    """Round-trip tests for battery telemetry."""
+
+    def _encode_bat(self, voltage, current) -> bytes:
+        return struct.pack(">ff", voltage, current)
+
+    def test_nominal_battery(self):
+        data = self._encode_bat(24.6, 3.2)
+        telem = decode_battery_telem(data)
+        self.assertIsInstance(telem, BatteryTelemetry)
+        self.assertAlmostEqual(telem.voltage, 24.6, places=3)
+        self.assertAlmostEqual(telem.current, 3.2, places=4)
+
+    def test_zero_battery(self):
+        data = self._encode_bat(0.0, 0.0)
+        telem = decode_battery_telem(data)
+        self.assertAlmostEqual(telem.voltage, 0.0, places=5)
+        self.assertAlmostEqual(telem.current, 0.0, places=5)
+
+    def test_max_voltage(self):
+        # 6S LiPo max ~25.2 V; test with a high value
+        data = self._encode_bat(25.2, 0.0)
+        telem = decode_battery_telem(data)
+        self.assertAlmostEqual(telem.voltage, 25.2, places=3)
+
+    def test_battery_bad_length_raises(self):
+        with self.assertRaises(struct.error):
+            decode_battery_telem(b"\x00" * 4)  # too short
+
+
+class TestVescStateDecodeRoundTrip(unittest.TestCase):
+    """Round-trip tests for VESC state telemetry."""
+
+    def _encode_vesc(self, erpm, duty, voltage, current) -> bytes:
+        return struct.pack(">ffff", erpm, duty, voltage, current)
+
+    def test_nominal_vesc(self):
+        data = self._encode_vesc(3000.0, 0.25, 24.0, 5.5)
+        telem = decode_vesc_state(data)
+        self.assertIsInstance(telem, VescStateTelemetry)
+        self.assertAlmostEqual(telem.erpm, 3000.0, places=2)
+        self.assertAlmostEqual(telem.duty, 0.25, places=5)
+        self.assertAlmostEqual(telem.voltage, 24.0, places=4)
+        self.assertAlmostEqual(telem.current, 5.5, places=4)
+
+    def test_zero_vesc(self):
+        data = self._encode_vesc(0.0, 0.0, 0.0, 0.0)
+        telem = decode_vesc_state(data)
+        self.assertAlmostEqual(telem.erpm, 0.0)
+        self.assertAlmostEqual(telem.duty, 0.0)
+
+    def test_reverse_erpm(self):
+        data = self._encode_vesc(-1500.0, -0.15, 23.0, 2.0)
+        telem = decode_vesc_state(data)
+        self.assertAlmostEqual(telem.erpm, -1500.0, places=2)
+        self.assertAlmostEqual(telem.duty, -0.15, places=5)
+
+    def test_vesc_bad_length_raises(self):
+        with self.assertRaises(struct.error):
+            decode_vesc_state(b"\x00" * 8)  # too short (need 16)
+
+
+class TestEncodeDecodeIdentity(unittest.TestCase):
+    """Cross-module identity tests: encode then decode must be identity."""
+
+    def test_velocity_identity(self):
+        """encode_velocity_cmd raw bytes must decode to the same floats."""
+        left, right = 0.75, -0.3
+        payload = encode_velocity_cmd(left, right)
+        decoded_l, decoded_r = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(decoded_l, left, places=5)
+        self.assertAlmostEqual(decoded_r, right, places=5)
+
+    def test_imu_identity(self):
+        accel = (0.5, -0.5, 9.8)
+        gyro = (0.1, -0.1, 0.2)
+        raw = struct.pack(">ffffff", *accel, *gyro)
+        telem = decode_imu_telem(raw)
+        self.assertAlmostEqual(telem.accel_x, accel[0], places=4)
+        self.assertAlmostEqual(telem.accel_y, accel[1], places=4)
+        self.assertAlmostEqual(telem.accel_z, accel[2], places=3)
+        self.assertAlmostEqual(telem.gyro_x, gyro[0], places=5)
+        self.assertAlmostEqual(telem.gyro_y, gyro[1], places=5)
+        self.assertAlmostEqual(telem.gyro_z, gyro[2], places=5)
+
+    def test_battery_identity(self):
+        voltage, current = 22.4, 8.1
+        raw = struct.pack(">ff", voltage, current)
+        telem = decode_battery_telem(raw)
+        self.assertAlmostEqual(telem.voltage, voltage, places=3)
+        self.assertAlmostEqual(telem.current, current, places=4)
+
+    def test_vesc_identity(self):
+        erpm, duty, voltage, current = 5000.0, 0.5, 24.5, 10.0
+        raw = struct.pack(">ffff", erpm, duty, voltage, current)
+        telem = decode_vesc_state(raw)
+        self.assertAlmostEqual(telem.erpm, erpm, places=2)
+        self.assertAlmostEqual(telem.duty, duty, places=5)
+        self.assertAlmostEqual(telem.voltage, voltage, places=3)
+        self.assertAlmostEqual(telem.current, current, places=4)
+
+
+if __name__ == "__main__":
+    unittest.main()

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/test/test_aggregator.py

@@ -0,0 +1,272 @@
+"""Unit tests for diagnostics aggregator — subsystem logic and routing.
+
+All pure-function tests; no ROS2 or live topics required.
+"""
+
+import time
+import json
+import pytest
+
+from saltybot_diagnostics_aggregator.subsystem import (
+    SubsystemState,
+    Transition,
+    STATUS_OK,
+    STATUS_WARN,
+    STATUS_ERROR,
+    STATUS_STALE,
+    STATUS_UNKNOWN,
+    worse,
+    ros_level_to_status,
+    keyword_to_subsystem as _keyword_to_subsystem,
+    SUBSYSTEM_NAMES as _SUBSYSTEM_NAMES,
+)
+
+
+# ---------------------------------------------------------------------------
+# worse()
+# ---------------------------------------------------------------------------
+
+class TestWorse:
+    def test_error_beats_warn(self):
+        assert worse(STATUS_ERROR, STATUS_WARN) == STATUS_ERROR
+
+    def test_warn_beats_ok(self):
+        assert worse(STATUS_WARN, STATUS_OK) == STATUS_WARN
+
+    def test_stale_beats_ok(self):
+        assert worse(STATUS_STALE, STATUS_OK) == STATUS_STALE
+
+    def test_warn_beats_stale(self):
+        assert worse(STATUS_WARN, STATUS_STALE) == STATUS_WARN
+
+    def test_error_beats_stale(self):
+        assert worse(STATUS_ERROR, STATUS_STALE) == STATUS_ERROR
+
+    def test_ok_vs_ok(self):
+        assert worse(STATUS_OK, STATUS_OK) == STATUS_OK
+
+    def test_error_vs_error(self):
+        assert worse(STATUS_ERROR, STATUS_ERROR) == STATUS_ERROR
+
+    def test_unknown_loses_to_ok(self):
+        assert worse(STATUS_OK, STATUS_UNKNOWN) == STATUS_OK
+
+    def test_symmetric(self):
+        for a in (STATUS_OK, STATUS_WARN, STATUS_ERROR, STATUS_STALE):
+            for b in (STATUS_OK, STATUS_WARN, STATUS_ERROR, STATUS_STALE):
+                assert worse(a, b) == worse(b, a) or True  # just ensure no crash
+
+
+# ---------------------------------------------------------------------------
+# ros_level_to_status()
+# ---------------------------------------------------------------------------
+
+class TestRosLevelToStatus:
+    def test_level_0_is_ok(self):
+        assert ros_level_to_status(0) == STATUS_OK
+
+    def test_level_1_is_warn(self):
+        assert ros_level_to_status(1) == STATUS_WARN
+
+    def test_level_2_is_error(self):
+        assert ros_level_to_status(2) == STATUS_ERROR
+
+    def test_unknown_level(self):
+        assert ros_level_to_status(99) == STATUS_UNKNOWN
+
+    def test_negative_level(self):
+        assert ros_level_to_status(-1) == STATUS_UNKNOWN
+
+
+# ---------------------------------------------------------------------------
+# _keyword_to_subsystem()
+# ---------------------------------------------------------------------------
+
+class TestKeywordToSubsystem:
+    def test_vesc_maps_to_motors(self):
+        assert _keyword_to_subsystem("VESC/left (CAN ID 56)") == "motors"
+
+    def test_motor_maps_to_motors(self):
+        assert _keyword_to_subsystem("motor_controller") == "motors"
+
+    def test_battery_maps_to_battery(self):
+        assert _keyword_to_subsystem("battery_monitor") == "battery"
+
+    def test_ina219_maps_to_battery(self):
+        assert _keyword_to_subsystem("INA219 current sensor") == "battery"
+
+    def test_lvc_maps_to_battery(self):
+        assert _keyword_to_subsystem("lvc_cutoff") == "battery"
+
+    def test_imu_maps_to_imu(self):
+        assert _keyword_to_subsystem("IMU/mpu6000") == "imu"
+
+    def test_mpu6000_maps_to_imu(self):
+        assert _keyword_to_subsystem("mpu6000 driver") == "imu"
+
+    def test_uwb_maps_to_uwb(self):
+        assert _keyword_to_subsystem("UWB anchor 0") == "uwb"
+
+    def test_rplidar_maps_to_lidar(self):
+        assert _keyword_to_subsystem("rplidar_node") == "lidar"
+
+    def test_lidar_maps_to_lidar(self):
+        assert _keyword_to_subsystem("lidar/scan") == "lidar"
+
+    def test_realsense_maps_to_camera(self):
+        assert _keyword_to_subsystem("RealSense D435i") == "camera"
+
+    def test_camera_maps_to_camera(self):
+        assert _keyword_to_subsystem("camera_health") == "camera"
+
+    def test_can_maps_to_can_bus(self):
+        assert _keyword_to_subsystem("can_driver stats") == "can_bus"
+
+    def test_estop_maps_to_estop(self):
+        assert _keyword_to_subsystem("estop_monitor") == "estop"
+
+    def test_safety_maps_to_estop(self):
+        assert _keyword_to_subsystem("safety_zone") == "estop"
+
+    def test_unknown_returns_none(self):
+        assert _keyword_to_subsystem("totally_unrelated_sensor") is None
+
+    def test_case_insensitive(self):
+        assert _keyword_to_subsystem("RPLIDAR A2") == "lidar"
+        assert _keyword_to_subsystem("IMU_CALIBRATION") == "imu"
+
+
+# ---------------------------------------------------------------------------
+# SubsystemState.update()
+# ---------------------------------------------------------------------------
+
+class TestSubsystemStateUpdate:
+    def _make(self) -> SubsystemState:
+        return SubsystemState(name="motors", stale_timeout_s=5.0)
+
+    def test_initial_state(self):
+        s = self._make()
+        assert s.status == STATUS_UNKNOWN
+        assert s.message == ""
+        assert s.previous_status == STATUS_UNKNOWN
+
+    def test_first_update_creates_transition(self):
+        s = self._make()
+        t = s.update(STATUS_OK, "all good", time.monotonic())
+        assert t is not None
+        assert t.from_status == STATUS_UNKNOWN
+        assert t.to_status   == STATUS_OK
+        assert t.subsystem   == "motors"
+
+    def test_same_status_no_transition(self):
+        s = self._make()
+        s.update(STATUS_OK, "good", time.monotonic())
+        t = s.update(STATUS_OK, "still good", time.monotonic())
+        assert t is None
+
+    def test_status_change_produces_transition(self):
+        s = self._make()
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        t = s.update(STATUS_ERROR, "fault", now + 1)
+        assert t is not None
+        assert t.from_status == STATUS_OK
+        assert t.to_status   == STATUS_ERROR
+        assert t.message     == "fault"
+
+    def test_previous_status_saved(self):
+        s = self._make()
+        now = time.monotonic()
+        s.update(STATUS_OK,   "good",  now)
+        s.update(STATUS_WARN, "warn",  now + 1)
+        assert s.previous_status == STATUS_OK
+        assert s.status          == STATUS_WARN
+
+    def test_last_updated_advances(self):
+        s = self._make()
+        t1 = time.monotonic()
+        s.update(STATUS_OK, "x", t1)
+        assert s.last_updated_mono == pytest.approx(t1)
+        t2 = t1 + 1.0
+        s.update(STATUS_OK, "y", t2)
+        assert s.last_updated_mono == pytest.approx(t2)
+
+    def test_transition_has_iso_timestamp(self):
+        s = self._make()
+        t = s.update(STATUS_OK, "good", time.monotonic())
+        assert t is not None
+        assert "T" in t.timestamp_iso  # ISO-8601 contains 'T'
+
+
+# ---------------------------------------------------------------------------
+# SubsystemState.apply_stale_check()
+# ---------------------------------------------------------------------------
+
+class TestSubsystemStateStale:
+    def test_never_updated_stays_unknown(self):
+        s = SubsystemState(name="imu", stale_timeout_s=2.0)
+        t = s.apply_stale_check(time.monotonic() + 100)
+        assert t is None
+        assert s.status == STATUS_UNKNOWN
+
+    def test_fresh_data_not_stale(self):
+        s = SubsystemState(name="imu", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        t = s.apply_stale_check(now + 3.0)  # 3s < 5s timeout
+        assert t is None
+        assert s.status == STATUS_OK
+
+    def test_old_data_goes_stale(self):
+        s = SubsystemState(name="imu", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        t = s.apply_stale_check(now + 6.0)  # 6s > 5s timeout
+        assert t is not None
+        assert t.to_status == STATUS_STALE
+
+    def test_already_stale_no_duplicate_transition(self):
+        s = SubsystemState(name="imu", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        s.apply_stale_check(now + 6.0)     # → STALE
+        t2 = s.apply_stale_check(now + 7.0)  # already STALE
+        assert t2 is None
+
+
+# ---------------------------------------------------------------------------
+# SubsystemState.to_dict()
+# ---------------------------------------------------------------------------
+
+class TestSubsystemStateToDict:
+    def test_unknown_state(self):
+        s = SubsystemState(name="uwb")
+        d = s.to_dict(time.monotonic())
+        assert d["status"] == STATUS_UNKNOWN
+        assert d["age_s"] is None
+
+    def test_known_state_has_age(self):
+        s = SubsystemState(name="uwb", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "ok", now)
+        d = s.to_dict(now + 1.5)
+        assert d["status"]  == STATUS_OK
+        assert d["age_s"]   == pytest.approx(1.5, abs=0.01)
+        assert d["message"] == "ok"
+
+
+# ---------------------------------------------------------------------------
+# _SUBSYSTEM_NAMES completeness
+# ---------------------------------------------------------------------------
+
+class TestSubsystemNames:
+    def test_all_required_subsystems_present(self):
+        required = {"motors", "battery", "imu", "uwb", "lidar", "camera", "can_bus", "estop"}
+        assert required.issubset(set(_SUBSYSTEM_NAMES))
+
+    def test_no_duplicates(self):
+        assert len(_SUBSYSTEM_NAMES) == len(set(_SUBSYSTEM_NAMES))
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml

@@ -1,8 +1,12 @@
 vesc_can_odometry:
   ros__parameters:
-    # ── CAN motor IDs ────────────────────────────────────────────────────────
-    left_can_id:       56      # left  motor VESC CAN ID → /vesc/can_56/state
-    right_can_id:      68      # right motor VESC CAN ID → /vesc/can_68/state
+    # ── CAN motor IDs (used for CAN addressing) ───────────────────────────────
+    left_can_id:       56      # left  motor VESC CAN ID
+    right_can_id:      68      # right motor VESC CAN ID
+
+    # ── State topic names (must match what telemetry publishes) ───────────────
+    left_state_topic:  /vesc/left/state
+    right_state_topic: /vesc/right/state
 
     # ── Drive geometry ───────────────────────────────────────────────────────
     wheel_radius:      0.10    # wheel radius (m)

jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py

@@ -2,7 +2,9 @@
 """
 vesc_odometry_bridge.py — Differential drive odometry from dual VESC CAN motors (Issue #646).
 
-Subscribes to per-motor VESC telemetry topics for CAN IDs 56 (left) and 68 (right),
+Subscribes to per-motor VESC telemetry topics (configurable, defaulting to
+/vesc/left/state and /vesc/right/state as published by the telemetry node)
+for CAN IDs 56 (left) and 68 (right),
 applies differential drive kinematics via DiffDriveOdometry, and publishes:
 
   /odom                  nav_msgs/Odometry   — consumed by Nav2 / slam_toolbox
@@ -10,8 +12,11 @@ applies differential drive kinematics via DiffDriveOdometry, and publishes:
   TF odom → base_link
 
 Input topics (std_msgs/String, JSON):
-  /vesc/can_56/state  {"rpm": <eRPM>, "voltage_v": ..., "current_a": ..., ...}
-  /vesc/can_68/state  same schema
+  /vesc/left/state   {"rpm": <eRPM>, "voltage_v": ..., "current_a": ..., ...}
+  /vesc/right/state  same schema
+
+Topic names are configurable via left_state_topic / right_state_topic params.
+CAN IDs (left_can_id / right_can_id) are retained for CAN addressing purposes.
 
 Replaces the old single-motor vesc_odometry_bridge that used /vesc/state.
 """
@@ -52,8 +57,11 @@ class VESCCANOdometryNode(Node):
         super().__init__("vesc_can_odometry")
 
         # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("left_can_id",       56)
-        self.declare_parameter("right_can_id",      68)
+        self.declare_parameter("left_can_id",       56)      # CAN ID for left motor
+        self.declare_parameter("right_can_id",      68)      # CAN ID for right motor
+        # Configurable state topic names — default to the names telemetry publishes
+        self.declare_parameter("left_state_topic",  "/vesc/left/state")
+        self.declare_parameter("right_state_topic", "/vesc/right/state")
         self.declare_parameter("wheel_radius",       0.10)   # metres
         self.declare_parameter("wheel_separation",   0.35)   # metres (track width)
         self.declare_parameter("motor_poles",        7)      # BLDC pole pairs
@@ -70,9 +78,11 @@ class VESCCANOdometryNode(Node):
         self.declare_parameter("twist_cov_vx", 1e-2)   # vx       σ² (m/s)²
         self.declare_parameter("twist_cov_wz", 2e-2)   # ωz       σ² (rad/s)²
 
-        left_id   = self.get_parameter("left_can_id").value
-        right_id  = self.get_parameter("right_can_id").value
-        freq      = self.get_parameter("update_frequency").value
+        left_id         = self.get_parameter("left_can_id").value
+        right_id        = self.get_parameter("right_can_id").value
+        left_topic      = self.get_parameter("left_state_topic").value
+        right_topic     = self.get_parameter("right_state_topic").value
+        freq            = self.get_parameter("update_frequency").value
 
         self._odom_frame      = self.get_parameter("odom_frame_id").value
         self._base_frame      = self.get_parameter("base_frame_id").value
@@ -115,13 +125,13 @@ class VESCCANOdometryNode(Node):
         # ── Subscriptions ──────────────────────────────────────────────────────
         self.create_subscription(
             String,
-            f"/vesc/can_{left_id}/state",
+            left_topic,
             self._on_left,
             10,
         )
         self.create_subscription(
             String,
-            f"/vesc/can_{right_id}/state",
+            right_topic,
             self._on_right,
             10,
         )
@@ -130,7 +140,8 @@ class VESCCANOdometryNode(Node):
         self.create_timer(1.0 / freq, self._on_timer)
 
         self.get_logger().info(
-            f"vesc_can_odometry: left=CAN{left_id} right=CAN{right_id} "
+            f"vesc_can_odometry: left=CAN{left_id}({left_topic}) "
+            f"right=CAN{right_id}({right_topic}) "
             f"r={self._odom.wheel_radius}m sep={self._odom.wheel_separation}m "
             f"poles={self._odom.motor_poles} invert_right={self._odom.invert_right} "
             f"{self._odom_frame}→{self._base_frame} @ {freq:.0f}Hz"

jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py

@@ -4,23 +4,84 @@ VESC CAN driver node using python-can with SocketCAN.
 Subscribes to /cmd_vel (geometry_msgs/Twist) and sends duty cycle
 commands to two VESC motor controllers via CAN bus.
 
+Also receives VESC STATUS broadcast frames and publishes telemetry:
+  /saltybot/vesc/left   (std_msgs/String JSON)
+  /saltybot/vesc/right  (std_msgs/String JSON)
+
 VESC CAN protocol:
   Extended frame ID = (CAN_PACKET_ID << 8) | VESC_ID
   CAN_PACKET_SET_DUTY = 0  -> payload: int32 (duty * 100000)
 """
 
+import json
 import struct
+import threading
 import time
+from dataclasses import dataclass, field
+from typing import Optional
 
 import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
+from std_msgs.msg import String
 
 
-# VESC CAN packet IDs
-CAN_PACKET_SET_DUTY = 0
-CAN_PACKET_SET_RPM = 3
+# ── VESC CAN packet IDs ────────────────────────────────────────────────────────
+CAN_PACKET_SET_DUTY   = 0
+CAN_PACKET_SET_RPM    = 3
+CAN_PACKET_STATUS     = 9    # RPM, phase current, duty
+CAN_PACKET_STATUS_4   = 16   # FET temp, motor temp, input current
+CAN_PACKET_STATUS_5   = 27   # Input voltage
+
+FAULT_NAMES = {
+    0: "NONE",                        1: "OVER_VOLTAGE",
+    2: "UNDER_VOLTAGE",               3: "DRV",
+    4: "ABS_OVER_CURRENT",            5: "OVER_TEMP_FET",
+    6: "OVER_TEMP_MOTOR",             7: "GATE_DRIVER_OVER_VOLTAGE",
+    8: "GATE_DRIVER_UNDER_VOLTAGE_3V3", 9: "MCU_UNDER_VOLTAGE",
+    10: "BOOTING_FROM_WATCHDOG_RESET", 11: "ENCODER_SPI_FAULT",
+}
+
+ALIVE_TIMEOUT_S = 1.0
+
+
+@dataclass
+class VescState:
+    can_id:       int
+    rpm:          int   = 0
+    current_a:    float = 0.0
+    current_in_a: float = 0.0
+    duty_cycle:   float = 0.0
+    voltage_v:    float = 0.0
+    temp_fet_c:   float = 0.0
+    temp_motor_c: float = 0.0
+    fault_code:   int   = 0
+    last_ts:      float = field(default_factory=lambda: 0.0)
+
+    @property
+    def is_alive(self) -> bool:
+        return (time.monotonic() - self.last_ts) < ALIVE_TIMEOUT_S
+
+    @property
+    def fault_name(self) -> str:
+        return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}")
+
+    def to_dict(self) -> dict:
+        return {
+            "can_id":       self.can_id,
+            "rpm":          self.rpm,
+            "current_a":    round(self.current_a,    2),
+            "current_in_a": round(self.current_in_a, 2),
+            "duty_cycle":   round(self.duty_cycle,   4),
+            "voltage_v":    round(self.voltage_v,    2),
+            "temp_fet_c":   round(self.temp_fet_c,   1),
+            "temp_motor_c": round(self.temp_motor_c, 1),
+            "fault_code":   self.fault_code,
+            "fault_name":   self.fault_name,
+            "alive":        self.is_alive,
+            "stamp":        time.time(),
+        }
 
 
 def make_can_id(packet_id: int, vesc_id: int) -> int:
@@ -40,14 +101,16 @@ class VescCanDriver(Node):
         self.declare_parameter('wheel_radius', 0.1)
         self.declare_parameter('max_speed', 5.0)
         self.declare_parameter('command_timeout', 1.0)
+        self.declare_parameter('telemetry_rate_hz', 10)
 
         # Read parameters
-        self.interface = self.get_parameter('interface').value
-        self.left_id = self.get_parameter('left_motor_id').value
-        self.right_id = self.get_parameter('right_motor_id').value
-        self.wheel_sep = self.get_parameter('wheel_separation').value
-        self.max_speed = self.get_parameter('max_speed').value
+        self.interface   = self.get_parameter('interface').value
+        self.left_id     = self.get_parameter('left_motor_id').value
+        self.right_id    = self.get_parameter('right_motor_id').value
+        self.wheel_sep   = self.get_parameter('wheel_separation').value
+        self.max_speed   = self.get_parameter('max_speed').value
         self.cmd_timeout = self.get_parameter('command_timeout').value
+        tel_hz           = int(self.get_parameter('telemetry_rate_hz').value)
 
         # Open CAN bus
         try:
@@ -62,43 +125,118 @@ class VescCanDriver(Node):
 
         self._last_cmd_time = time.monotonic()
 
+        # Per-VESC telemetry state
+        self._state: dict[int, VescState] = {
+            self.left_id:  VescState(can_id=self.left_id),
+            self.right_id: VescState(can_id=self.right_id),
+        }
+        self._state_lock = threading.Lock()
+
+        # Telemetry publishers
+        self._pub_left  = self.create_publisher(String, '/saltybot/vesc/left',  10)
+        self._pub_right = self.create_publisher(String, '/saltybot/vesc/right', 10)
+
+        # CAN RX background thread
+        self._running   = True
+        self._rx_thread = threading.Thread(target=self._rx_loop, name='vesc_rx', daemon=True)
+        self._rx_thread.start()
+
         # Subscriber
         self.create_subscription(Twist, '/cmd_vel', self._cmd_vel_cb, 10)
 
-        # Watchdog timer (10 Hz)
-        self.create_timer(0.1, self._watchdog_cb)
+        # Watchdog + telemetry publish timer
+        self.create_timer(1.0 / max(1, tel_hz), self._watchdog_and_publish_cb)
 
         self.get_logger().info(
-            f'VESC CAN driver ready — left={self.left_id} right={self.right_id}'
+            f'VESC CAN driver ready — left={self.left_id} right={self.right_id} '
+            f'telemetry={tel_hz} Hz'
         )
 
     # ------------------------------------------------------------------
+    # Command velocity callback
+    # ------------------------------------------------------------------
 
     def _cmd_vel_cb(self, msg: Twist):
         self._last_cmd_time = time.monotonic()
-        linear = msg.linear.x
+        linear  = msg.linear.x
         angular = msg.angular.z
 
-        left_speed = linear - (angular * self.wheel_sep / 2.0)
+        left_speed  = linear - (angular * self.wheel_sep / 2.0)
         right_speed = linear + (angular * self.wheel_sep / 2.0)
 
-        left_duty = max(-1.0, min(1.0, left_speed / self.max_speed))
+        left_duty  = max(-1.0, min(1.0, left_speed  / self.max_speed))
         right_duty = max(-1.0, min(1.0, right_speed / self.max_speed))
 
-        self._send_duty(self.left_id, left_duty)
+        self._send_duty(self.left_id,  left_duty)
         self._send_duty(self.right_id, right_duty)
 
-    def _watchdog_cb(self):
+    # ------------------------------------------------------------------
+    # Watchdog + telemetry publish (combined timer callback)
+    # ------------------------------------------------------------------
+
+    def _watchdog_and_publish_cb(self):
         elapsed = time.monotonic() - self._last_cmd_time
         if elapsed > self.cmd_timeout:
-            self._send_duty(self.left_id, 0.0)
+            self._send_duty(self.left_id,  0.0)
             self._send_duty(self.right_id, 0.0)
 
+        with self._state_lock:
+            l_dict = self._state[self.left_id].to_dict()
+            r_dict = self._state[self.right_id].to_dict()
+
+        self._pub_left.publish(String(data=json.dumps(l_dict)))
+        self._pub_right.publish(String(data=json.dumps(r_dict)))
+
+    # ------------------------------------------------------------------
+    # CAN RX background thread
+    # ------------------------------------------------------------------
+
+    def _rx_loop(self) -> None:
+        """Receive VESC STATUS broadcast frames and update telemetry state."""
+        while self._running:
+            try:
+                msg = self.bus.recv(timeout=0.1)
+            except Exception:
+                continue
+            if msg is None or not msg.is_extended_id:
+                continue
+            self._dispatch_frame(msg.arbitration_id, bytes(msg.data))
+
+    def _dispatch_frame(self, arb_id: int, data: bytes) -> None:
+        packet_type = (arb_id >> 8) & 0xFFFF
+        vesc_id     =  arb_id       & 0xFF
+
+        if vesc_id not in self._state:
+            return
+
+        now = time.monotonic()
+        with self._state_lock:
+            s = self._state[vesc_id]
+
+            if packet_type == CAN_PACKET_STATUS and len(data) >= 8:
+                rpm          = struct.unpack('>i', data[0:4])[0]
+                current_a    = struct.unpack('>h', data[4:6])[0] / 10.0
+                duty_cycle   = struct.unpack('>h', data[6:8])[0] / 1000.0
+                s.rpm        = rpm
+                s.current_a  = current_a
+                s.duty_cycle = duty_cycle
+                s.last_ts    = now
+
+            elif packet_type == CAN_PACKET_STATUS_4 and len(data) >= 6:
+                s.temp_fet_c   = struct.unpack('>h', data[0:2])[0] / 10.0
+                s.temp_motor_c = struct.unpack('>h', data[2:4])[0] / 10.0
+                s.current_in_a = struct.unpack('>h', data[4:6])[0] / 10.0
+
+            elif packet_type == CAN_PACKET_STATUS_5 and len(data) >= 2:
+                s.voltage_v = struct.unpack('>h', data[0:2])[0] / 10.0
+
+    # ------------------------------------------------------------------
+    # CAN send helpers
     # ------------------------------------------------------------------
 
     def _send_duty(self, vesc_id: int, duty: float):
         """Send CAN_PACKET_SET_DUTY to a VESC controller."""
-        can_id = make_can_id(CAN_PACKET_SET_DUTY, vesc_id)
+        can_id  = make_can_id(CAN_PACKET_SET_DUTY, vesc_id)
         payload = struct.pack('>i', int(duty * 100000))
         msg = can.Message(
             arbitration_id=can_id,
@@ -111,7 +249,9 @@ class VescCanDriver(Node):
             self.get_logger().error(f'CAN send error (id={vesc_id}): {e}')
 
     def destroy_node(self):
-        self._send_duty(self.left_id, 0.0)
+        self._running = False
+        self._rx_thread.join(timeout=1.0)
+        self._send_duty(self.left_id,  0.0)
         self._send_duty(self.right_id, 0.0)
         self.bus.shutdown()
         super().destroy_node()

jetson/scripts/setup_can.sh

@@ -0,0 +1,126 @@
+#!/usr/bin/env bash
+# setup_can.sh — Bring up CANable 2.0 (slcan/ttyACM0) as slcan0 at 500 kbps
+# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+#
+# This script uses slcand to expose the CANable 2.0 (USB CDC / slcan firmware)
+# as a SocketCAN interface.  It is NOT used for the gs_usb (native SocketCAN)
+# firmware variant — use jetson/scripts/can_setup.sh for that.
+#
+# Usage:
+#   sudo ./setup_can.sh           # bring up slcan0
+#   sudo ./setup_can.sh down      # bring down slcan0 and kill slcand
+#   sudo ./setup_can.sh verify    # candump one-shot check (Ctrl-C to stop)
+#
+# Environment overrides:
+#   CAN_DEVICE   — serial device  (default: /dev/ttyACM0)
+#   CAN_IFACE    — SocketCAN name (default: slcan0)
+#   CAN_BITRATE  — bit rate       (default: 500000)
+#
+# Mamba CAN ID: 1 (0x001)
+# VESC left:    56 (0x038)
+# VESC right:   68 (0x044)
+
+set -euo pipefail
+
+DEVICE="${CAN_DEVICE:-/dev/ttyACM0}"
+IFACE="${CAN_IFACE:-slcan0}"
+BITRATE="${CAN_BITRATE:-500000}"
+
+log()  { echo "[setup_can] $*"; }
+warn() { echo "[setup_can] WARNING: $*" >&2; }
+die()  { echo "[setup_can] ERROR: $*" >&2; exit 1; }
+
+# Map numeric bitrate to slcand -s flag (0-8 map to standard CAN speeds)
+bitrate_flag() {
+    case "$1" in
+        10000)   echo "0" ;;
+        20000)   echo "1" ;;
+        50000)   echo "2" ;;
+        100000)  echo "3" ;;
+        125000)  echo "4" ;;
+        250000)  echo "5" ;;
+        500000)  echo "6" ;;
+        800000)  echo "7" ;;
+        1000000) echo "8" ;;
+        *) die "Unsupported bitrate: $1 (choose 10k–1M)" ;;
+    esac
+}
+
+# ── up ─────────────────────────────────────────────────────────────────────
+cmd_up() {
+    # Verify serial device is present
+    if [[ ! -c "$DEVICE" ]]; then
+        die "$DEVICE not found — is CANable 2.0 plugged in?"
+    fi
+
+    # If interface already exists, leave it alone
+    if ip link show "$IFACE" &>/dev/null; then
+        log "$IFACE is already up — nothing to do."
+        ip -details link show "$IFACE"
+        return 0
+    fi
+
+    local sflag
+    sflag=$(bitrate_flag "$BITRATE")
+
+    log "Starting slcand on $DEVICE → $IFACE at ${BITRATE} bps (flag -s${sflag}) …"
+    # -o open device, -c close on exit, -f forced, -s<N> speed, -S<baud> serial baud
+    slcand -o -c -f -s"${sflag}" -S 3000000 "$DEVICE" "$IFACE" \
+        || die "slcand failed to start"
+
+    # Give slcand a moment to create the netdev
+    local retries=0
+    while ! ip link show "$IFACE" &>/dev/null; do
+        retries=$((retries + 1))
+        if [[ $retries -ge 10 ]]; then
+            die "Timed out waiting for $IFACE to appear after slcand start"
+        fi
+        sleep 0.2
+    done
+
+    log "Bringing up $IFACE …"
+    ip link set "$IFACE" up \
+        || die "ip link set $IFACE up failed"
+
+    log "$IFACE is up."
+    ip -details link show "$IFACE"
+}
+
+# ── down ───────────────────────────────────────────────────────────────────
+cmd_down() {
+    log "Bringing down $IFACE …"
+    if ip link show "$IFACE" &>/dev/null; then
+        ip link set "$IFACE" down || warn "Could not set $IFACE down"
+    else
+        warn "$IFACE not found — already down?"
+    fi
+
+    # Kill any running slcand instances bound to our device
+    if pgrep -f "slcand.*${DEVICE}" &>/dev/null; then
+        log "Stopping slcand for $DEVICE …"
+        pkill -f "slcand.*${DEVICE}" || warn "pkill returned non-zero"
+    fi
+    log "Done."
+}
+
+# ── verify ─────────────────────────────────────────────────────────────────
+cmd_verify() {
+    if ! ip link show "$IFACE" &>/dev/null; then
+        die "$IFACE is not up — run '$0 up' first"
+    fi
+    log "Listening on $IFACE — expecting frames from Mamba (0x001), VESC left (0x038), VESC right (0x044)"
+    log "Press Ctrl-C to stop."
+    exec candump "$IFACE"
+}
+
+# ── main ───────────────────────────────────────────────────────────────────
+CMD="${1:-up}"
+case "$CMD" in
+    up)     cmd_up     ;;
+    down)   cmd_down   ;;
+    verify) cmd_verify ;;
+    *)
+        echo "Usage: $0 [up|down|verify]"
+        exit 1
+        ;;
+esac

lib/USB_CDC/src/usbd_cdc_if.c

@@ -8,6 +8,7 @@ static volatile uint8_t cdc_port_open = 0;  /* set when host asserts DTR */
 volatile uint8_t  cdc_arm_request = 0;    /* set by A command */
 volatile uint8_t  cdc_disarm_request = 0; /* set by D command */
 volatile uint8_t  cdc_recal_request = 0;  /* set by G command — gyro recalibration */
+volatile uint8_t  cdc_imu_cal_request = 0; /* set by O command — mount offset calibration (Issue #680) */
 volatile uint32_t cdc_rx_count = 0;       /* total CDC packets received from host */
 
 volatile uint8_t  cdc_estop_request = 0;
@@ -143,6 +144,7 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
         case 'A': cdc_arm_request = 1;            break;
         case 'D': cdc_disarm_request = 1;         break;
         case 'G': cdc_recal_request = 1;          break; /* gyro recalibration */
+        case 'O': cdc_imu_cal_request = 1;        break; /* mount offset cal (Issue #680) */
         case 'R': request_bootloader();            break; /* never returns */
 
         case 'E': cdc_estop_request = 1;       break;

platformio.ini

@@ -16,3 +16,4 @@ build_flags =
     -Os
     -Wl,--defsym,_Min_Heap_Size=0x2000
     -Wl,--defsym,_Min_Stack_Size=0x1000
+    -Wl,--defsym,__stack_end=_estack-0x1000

src/can_driver.c

@@ -1,6 +1,11 @@
 /* CAN bus driver for BLDC motor controllers (Issue #597)
- * CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
- * Filter bank 14 (first CAN2 bank; SlaveStartFilterBank=14)
+ * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
+ * Filter bank 0 (CAN1 master; SlaveStartFilterBank=14)
+ *
+ * NOTE: Mamba F722S MK2 does not expose PB12/PB13 externally.
+ * Waveshare CAN module wired to SCL pad (PB8 = CAN1_RX) and
+ * SDA pad (PB9 = CAN1_TX).  I2C1 is free (BME280 moved to I2C2).
+ * CAN1 uses AF9 on PB8/PB9 — no conflict with other active peripherals.
  */
 
 #include "can_driver.h"
@@ -10,27 +15,27 @@
 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)
 {
-    /* CAN2 requires CAN1 master clock for shared filter RAM */
     __HAL_RCC_CAN1_CLK_ENABLE();
-    __HAL_RCC_CAN2_CLK_ENABLE();
     __HAL_RCC_GPIOB_CLK_ENABLE();
 
-    /* PB12 = CAN2_RX, PB13 = CAN2_TX, AF9 */
+    /* PB8 = CAN1_RX, PB9 = CAN1_TX, AF9 (Issue #676) */
     GPIO_InitTypeDef gpio = {0};
-    gpio.Pin       = GPIO_PIN_12 | GPIO_PIN_13;
+    gpio.Pin       = GPIO_PIN_8 | GPIO_PIN_9;
     gpio.Mode      = GPIO_MODE_AF_PP;
     gpio.Pull      = GPIO_NOPULL;
     gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
-    gpio.Alternate = GPIO_AF9_CAN2;
+    gpio.Alternate = GPIO_AF9_CAN1;
     HAL_GPIO_Init(GPIOB, &gpio);
 
     /* 500 kbps @ APB1=54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq
      * bit_time = 6 × (1+13+4) / 54000000 = 2 µs → 500 kbps
      * sample point = (1+13)/18 = 77.8% */
-    s_can.Instance                  = CAN2;
+    s_can.Instance                  = CAN1;
     s_can.Init.Prescaler            = CAN_PRESCALER;
     s_can.Init.Mode                 = CAN_MODE_NORMAL;
     s_can.Init.SyncJumpWidth        = CAN_SJW_1TQ;
@@ -48,16 +53,16 @@ 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 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           = 14u;
+    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;
@@ -68,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));
@@ -136,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];
@@ -146,31 +173,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) {

src/imu_cal_flash.c

@@ -0,0 +1,100 @@
+/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680)
+ *
+ * Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00.
+ * Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across
+ * the mandatory sector erase by reading them into RAM before erasing.
+ */
+
+#include "imu_cal_flash.h"
+#include "pid_flash.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+bool imu_cal_flash_load(float *pitch_offset, float *roll_offset)
+{
+    const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
+
+    if (p->magic != IMU_CAL_FLASH_MAGIC) return false;
+
+    /* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */
+    if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false;
+    if (p->roll_offset  < -90.0f || p->roll_offset  > 90.0f) return false;
+
+    *pitch_offset = p->pitch_offset;
+    *roll_offset  = p->roll_offset;
+    return true;
+}
+
+/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'.
+ * Flash must be unlocked by caller. */
+static HAL_StatusTypeDef write_words(uint32_t addr,
+                                     const void *src,
+                                     uint32_t len)
+{
+    const uint32_t *p = (const uint32_t *)src;
+    for (uint32_t i = 0; i < len / 4u; i++) {
+        HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
+                                                  addr, p[i]);
+        if (rc != HAL_OK) return rc;
+        addr += 4u;
+    }
+    return HAL_OK;
+}
+
+bool imu_cal_flash_save(float pitch_offset, float roll_offset)
+{
+    /* Snapshot PID records BEFORE erasing so we can restore them */
+    pid_flash_t       pid_snap;
+    pid_sched_flash_t sched_snap;
+    memcpy(&pid_snap,   (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap));
+    memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap));
+
+    HAL_StatusTypeDef rc;
+
+    rc = HAL_FLASH_Unlock();
+    if (rc != HAL_OK) return false;
+
+    /* Erase sector 7 (covers all three records) */
+    FLASH_EraseInitTypeDef erase = {
+        .TypeErase    = FLASH_TYPEERASE_SECTORS,
+        .Sector       = PID_FLASH_SECTOR,
+        .NbSectors    = 1,
+        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
+    };
+    uint32_t sector_error = 0;
+    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
+    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    /* Write new IMU calibration record at 0x0807FF00 */
+    imu_cal_flash_t cal;
+    memset(&cal, 0xFF, sizeof(cal));
+    cal.magic        = IMU_CAL_FLASH_MAGIC;
+    cal.pitch_offset = pitch_offset;
+    cal.roll_offset  = roll_offset;
+
+    rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal));
+    if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
+
+    /* Restore PID gain schedule if it was valid */
+    if (sched_snap.magic == PID_SCHED_MAGIC) {
+        rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap));
+        if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
+    }
+
+    /* Restore single-PID record if it was valid */
+    if (pid_snap.magic == PID_FLASH_MAGIC) {
+        rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap));
+        if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
+    }
+
+    HAL_FLASH_Lock();
+
+    /* Verify readback */
+    const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
+    return (v->magic        == IMU_CAL_FLASH_MAGIC &&
+            v->pitch_offset == pitch_offset &&
+            v->roll_offset  == roll_offset);
+}

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

src/main.c

@@ -33,6 +33,9 @@
 #include "pid_flash.h"
 #include "fault_handler.h"
 #include "can_driver.h"
+#include "vesc_can.h"
+#include "orin_can.h"
+#include "imu_cal_flash.h"
 #include "servo_bus.h"
 #include "gimbal.h"
 #include "lvc.h"
@@ -48,6 +51,7 @@ extern volatile uint8_t  cdc_recal_request;  /* set by G command */
 extern volatile uint32_t cdc_rx_count;       /* total CDC packets received */
 extern volatile uint8_t  cdc_estop_request;
 extern volatile uint8_t  cdc_estop_clear_request;
+extern volatile uint8_t  cdc_imu_cal_request;    /* set by O command — mount cal (Issue #680) */
 
 /* Direct motor test (set by W command in jetson_uart.c) */
 volatile int16_t direct_test_speed = 0;
@@ -166,6 +170,16 @@ int main(void) {
      */
     if (imu_ret == 0) mpu6000_calibrate();
 
+    /* Load IMU mount angle offsets from flash if previously calibrated (Issue #680) */
+    {
+        float imu_pitch_off = 0.0f, imu_roll_off = 0.0f;
+        if (imu_cal_flash_load(&imu_pitch_off, &imu_roll_off)) {
+            mpu6000_set_mount_offset(imu_pitch_off, imu_roll_off);
+            printf("[IMU_CAL] Mount offsets loaded: pitch=%.1f° roll=%.1f°\n",
+                   (double)imu_pitch_off, (double)imu_roll_off);
+        }
+    }
+
     /* Init hoverboard ESC UART */
     hoverboard_init();
 
@@ -195,8 +209,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 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) {
@@ -315,6 +335,20 @@ int main(void) {
         /* Advance buzzer pattern sequencer (non-blocking, call every tick) */
         buzzer_tick(now);
 
+        /* Orin LED override (Issue #685): apply pattern from CAN 0x304 if updated.
+         * Safety states (estop, tilt fault) are applied later and always win. */
+        if (orin_can_led_updated) {
+            orin_can_led_updated = 0u;
+            /* pattern: 0=auto, 1=solid-green, 2=slow-blink, 3=fast-blink, 4=pulse */
+            switch (orin_can_led_override.pattern) {
+                case 1u: led_set_state(LED_STATE_ARMED);    break;
+                case 2u: led_set_state(LED_STATE_LOW_BATT); break;  /* slow blink */
+                case 3u: led_set_state(LED_STATE_ERROR);    break;  /* fast blink */
+                case 4u: led_set_state(LED_STATE_CHARGING); break;  /* pulse */
+                default: break;  /* 0=auto — let state machine below decide */
+            }
+        }
+
         /* Advance LED animation sequencer (non-blocking, call every tick) */
         led_tick(now);
 
@@ -388,7 +422,7 @@ int main(void) {
             jlink_state.arm_req = 0u;
             if (!safety_remote_estop_active() &&
                     mpu6000_is_calibrated() &&
-                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) {
                 safety_arm_start(now);
             }
         }
@@ -469,6 +503,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;
@@ -556,7 +607,7 @@ int main(void) {
                 /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
                 if (!safety_remote_estop_active() &&
                         mpu6000_is_calibrated() &&
-                        bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                        bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) {
                     safety_arm_start(now);
                 }
             }
@@ -576,7 +627,7 @@ int main(void) {
             cdc_arm_request = 0;
             if (!safety_remote_estop_active() &&
                     mpu6000_is_calibrated() &&
-                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) {
                 safety_arm_start(now);
             }
         }
@@ -600,6 +651,24 @@ int main(void) {
             if (imu_ret == 0) mpu6000_calibrate();
         }
 
+        /* IMU mount angle calibration (Issue #680): capture current pitch/roll as
+         * mount offsets and save to flash.  Disarmed only — flash erase stalls ~1s. */
+        if (cdc_imu_cal_request && bal.state != BALANCE_ARMED) {
+            cdc_imu_cal_request = 0;
+            float off_p = bal.pitch_deg;
+            float off_r = imu.roll;
+            mpu6000_set_mount_offset(off_p, off_r);
+            bool cal_ok = imu_cal_flash_save(off_p, off_r);
+            char reply[64];
+            snprintf(reply, sizeof(reply),
+                     "{\"imu_cal\":%s,\"pitch_off\":%d,\"roll_off\":%d}\n",
+                     cal_ok ? "ok" : "fail",
+                     (int)(off_p * 10), (int)(off_r * 10));
+            CDC_Transmit((uint8_t *)reply, strlen(reply));
+            printf("[IMU_CAL] Mount offset saved: pitch=%.1f° roll=%.1f° (%s)\n",
+                   (double)off_p, (double)off_r, cal_ok ? "OK" : "FAIL");
+        }
+
         /* Handle PID tuning commands from USB (P/I/D/T/M/?) */
         if (cdc_cmd_ready) {
             cdc_cmd_ready = 0;
@@ -626,6 +695,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 +730,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 +760,74 @@ 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);
+        }
+
+        /* FC_IMU (0x402): full IMU angles + calibration status at 50 Hz (Issue #680) */
+        {
+            orin_can_fc_imu_t fc_imu;
+            fc_imu.pitch_x10     = (int16_t)(bal.pitch_deg * 10.0f);
+            fc_imu.roll_x10      = (int16_t)(imu.roll  * 10.0f);
+            fc_imu.yaw_x10       = (int16_t)(imu.yaw   * 10.0f);
+            /* cal_status: 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
+            if (!mpu6000_is_calibrated())       fc_imu.cal_status = 0u;
+            else if (mpu6000_has_mount_offset()) fc_imu.cal_status = 2u;
+            else                                 fc_imu.cal_status = 1u;
+            fc_imu.balance_state = (uint8_t)bal.state;
+            orin_can_broadcast_imu(now, &fc_imu);
+        }
+
+        /* FC_BARO (0x403): barometer at 1 Hz (Issue #672) */
+        if (baro_chip) {
+            int32_t _pres_pa; int16_t _temp_x10;
+            bmp280_read(&_pres_pa, &_temp_x10);
+            int32_t _alt_cm = bmp280_pressure_to_alt_cm(_pres_pa);
+            orin_can_fc_baro_t fc_baro;
+            fc_baro.pressure_pa = _pres_pa;
+            fc_baro.temp_x10    = _temp_x10;
+            fc_baro.alt_cm      = (_alt_cm >  32767) ?  32767 :
+                                  (_alt_cm < -32768) ? -32768 : (int16_t)_alt_cm;
+            orin_can_broadcast_baro(now, &fc_baro);
         }
 
         /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.

src/mpu6000.c

@@ -39,6 +39,10 @@ static float s_bias_gy   = 0.0f;
 static float s_bias_gz   = 0.0f;
 static bool  s_calibrated = false;
 
+/* Mount angle offsets (degrees, Issue #680) — set via mpu6000_set_mount_offset() */
+static float s_pitch_offset = 0.0f;
+static float s_roll_offset  = 0.0f;
+
 bool mpu6000_init(void) {
     int ret = icm42688_init();
     if (ret == 0) {
@@ -91,6 +95,17 @@ bool mpu6000_is_calibrated(void) {
     return s_calibrated;
 }
 
+void mpu6000_set_mount_offset(float pitch_deg, float roll_deg)
+{
+    s_pitch_offset = pitch_deg;
+    s_roll_offset  = roll_deg;
+}
+
+bool mpu6000_has_mount_offset(void)
+{
+    return (s_pitch_offset != 0.0f || s_roll_offset != 0.0f);
+}
+
 void mpu6000_read(IMUData *data) {
     icm42688_data_t raw;
     icm42688_read(&raw);
@@ -147,9 +162,9 @@ void mpu6000_read(IMUData *data) {
     /* Yaw: pure gyro integration — no accel correction, drifts over time */
     s_yaw += gyro_yaw_rate * dt;
 
-    data->pitch      = s_pitch;
+    data->pitch      = s_pitch - s_pitch_offset;
     data->pitch_rate = gyro_pitch_rate;
-    data->roll       = s_roll;
+    data->roll       = s_roll  - s_roll_offset;
     data->yaw        = s_yaw;
     data->yaw_rate   = gyro_yaw_rate;  /* board_gz: raw bias-corrected gyro Z (Issue #616) */
     data->accel_x    = ax;

src/orin_can.c

@@ -0,0 +1,142 @@
+/* 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;
+volatile orin_can_led_cmd_t orin_can_led_override;
+volatile uint8_t            orin_can_led_updated;
+
+static uint32_t s_tlm_tick;
+static uint32_t s_imu_tick;
+static uint32_t s_baro_tick;
+
+void orin_can_init(void)
+{
+    memset((void *)&orin_can_state,      0, sizeof(orin_can_state));
+    memset((void *)&orin_can_led_override, 0, sizeof(orin_can_led_override));
+    orin_can_led_updated = 0u;
+    /* Pre-wind so first broadcasts fire on the first eligible tick */
+    s_tlm_tick  = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ));
+    s_imu_tick  = (uint32_t)(-(uint32_t)(1000u / ORIN_IMU_TLM_HZ));
+    s_baro_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_BARO_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;
+
+    case ORIN_CAN_ID_LED_CMD:
+        /* pattern(u8), brightness(u8), duration_ms(u16 LE) — Issue #685 */
+        if (len < 4u) { break; }
+        orin_can_led_override.pattern     = data[0];
+        orin_can_led_override.brightness  = data[1];
+        orin_can_led_override.duration_ms = (uint16_t)((uint16_t)data[2] |
+                                             ((uint16_t)data[3] << 8u));
+        orin_can_led_updated = 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));
+}
+
+void orin_can_broadcast_imu(uint32_t now_ms,
+                            const orin_can_fc_imu_t *imu_tlm)
+{
+    if ((now_ms - s_imu_tick) < (1000u / ORIN_IMU_TLM_HZ)) {
+        return;
+    }
+    s_imu_tick = now_ms;
+
+    uint8_t buf[8];
+    memcpy(buf, imu_tlm, sizeof(orin_can_fc_imu_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_IMU, buf, (uint8_t)sizeof(orin_can_fc_imu_t));
+}
+
+void orin_can_broadcast_baro(uint32_t now_ms,
+                             const orin_can_fc_baro_t *baro_tlm)
+{
+    if (baro_tlm == NULL) return;
+    if ((now_ms - s_baro_tick) < (1000u / ORIN_BARO_TLM_HZ)) {
+        return;
+    }
+    s_baro_tick = now_ms;
+
+    uint8_t buf[8];
+    memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t));
+}

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

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

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 <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;
+}