Merge pull request 'fix: Configurable VESC CAN IDs, default 56/68 (Issue #667)' (#668) from sl-controls/issue-667-configurable-can-ids into main

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) ---
+// --- CAN Bus Driver (Issue #597, remapped Issue #676) ---
-// CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) — repurposes SPI2/OSD pins (unused on balance bot)
+// 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

@@ -0,0 +1,32 @@
+vesc_can_odometry:
+  ros__parameters:
+    # ── 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)
+    wheel_separation:  0.35    # track width: centre-to-centre (m)
+    motor_poles:       7       # BLDC pole pairs (eRPM / motor_poles = mech RPM)
+    invert_right:      true    # negate right eRPM (motors physically mirrored)
+
+    # ── Frames & topics ──────────────────────────────────────────────────────
+    odom_frame_id:     odom
+    base_frame_id:     base_link
+    publish_tf:        true
+    publish_wheel_odom: true   # also publish on /saltybot/wheel_odom for pose_fusion
+
+    # ── Timing ───────────────────────────────────────────────────────────────
+    update_frequency:  50.0    # Hz — integration rate
+
+    # ── Covariance (diagonal variances) ─────────────────────────────────────
+    # Increase these values if the robot slips frequently.
+    pose_cov_xx:  0.001   # x        variance (m²)
+    pose_cov_yy:  0.001   # y        variance (m²)
+    pose_cov_aa:  0.010   # heading  variance (rad²)
+    twist_cov_vx: 0.010   # vx       variance ((m/s)²)
+    twist_cov_wz: 0.020   # ωz       variance ((rad/s)²)

jetson/ros2_ws/src/saltybot_nav2_slam/launch/odometry_bridge.launch.py

@@ -1,4 +1,4 @@
-"""VESC Telemetry to Odometry Bridge"""
+"""VESC CAN dual-motor odometry bridge (Issue #646)."""
 
 import os
 from launch import LaunchDescription
@@ -9,28 +9,22 @@ from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
-    use_sim_time = LaunchConfiguration('use_sim_time')
+    pkg = get_package_share_directory("saltybot_nav2_slam")
+    params_file = os.path.join(pkg, "config", "vesc_odometry_params.yaml")
+
+    use_sim_time = LaunchConfiguration("use_sim_time")
 
     return LaunchDescription([
-        DeclareLaunchArgument('use_sim_time', default_value='false'),
+        DeclareLaunchArgument("use_sim_time", default_value="false"),
 
         Node(
-            package='saltybot_nav2_slam',
+            package="saltybot_nav2_slam",
-            executable='vesc_odometry_bridge',
+            executable="vesc_can_odometry",
-            name='vesc_odometry_bridge',
+            name="vesc_can_odometry",
-            parameters=[{
+            parameters=[
-                'use_sim_time': use_sim_time,
+                params_file,
-                'vesc_state_topic': '/vesc/state',
+                {"use_sim_time": use_sim_time},
-                'odometry_topic': '/odom',
+            ],
-                'odom_frame_id': 'odom',
+            output="screen",
-                'base_frame_id': 'base_link',
-                'update_frequency': 50,
-                'wheel_base': 0.35,
-                'wheel_radius': 0.10,
-                'max_rpm': 60000,
-                'publish_tf': True,
-                'tf_queue_size': 10,
-            }],
-            output='screen',
         ),
     ])

jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/diff_drive_odom.py

@@ -0,0 +1,145 @@
+"""
+diff_drive_odom.py — Pure differential drive odometry kinematics (Issue #646).
+
+No ROS2 dependencies — pure Python, fully unit-testable.
+
+Computes x, y, theta, v_lin, v_ang from per-wheel electrical RPM using exact
+arc integration.  Designed to be wrapped by a ROS2 node.
+
+Motor convention
+────────────────
+  eRPM → mechanical RPM = eRPM / motor_poles  (VESC reports electrical RPM)
+  Mechanical RPM → wheel surface velocity (m/s) = RPM/60 × 2π × wheel_radius
+
+  If invert_right=True the right eRPM sign is flipped before use (handles
+  physically-mirrored motor mounting where positive eRPM on both motors would
+  otherwise produce counter-rotation rather than forward motion).
+
+Kinematics
+──────────
+  v_left  = wheel surface velocity of left wheel (m/s)
+  v_right = wheel surface velocity of right wheel (m/s)
+  v_lin   = (v_right + v_left) / 2          forward velocity (m/s)
+  v_ang   = (v_right - v_left) / wheel_sep  yaw rate (rad/s, CCW positive)
+
+Integration uses the exact arc formula to reduce heading error accumulation:
+  If |v_ang| > ε:
+    r      = v_lin / v_ang
+    dtheta = v_ang * dt
+    x     += r * (sin(theta + dtheta) - sin(theta))
+    y     -= r * (cos(theta + dtheta) - cos(theta))
+    theta += dtheta
+  Else (straight line):
+    x     += v_lin * cos(theta) * dt
+    y     += v_lin * sin(theta) * dt
+
+Heading is normalised to (-π, π] after each update.
+"""
+
+from __future__ import annotations
+
+import math
+
+
+class DiffDriveOdometry:
+    """
+    Differential drive odometry from dual VESC CAN motor RPM feedback.
+
+    Thread model: single-threaded — caller must serialise access.
+    """
+
+    def __init__(
+        self,
+        wheel_radius: float = 0.10,
+        wheel_separation: float = 0.35,
+        motor_poles: int = 7,
+        invert_right: bool = True,
+    ) -> None:
+        """
+        Parameters
+        ----------
+        wheel_radius     : wheel radius (m)
+        wheel_separation : distance between left and right wheel contact points (m)
+        motor_poles      : BLDC motor pole pairs (VESC eRPM / motor_poles = mechanical RPM)
+        invert_right     : negate right eRPM before computing velocity (mirrored mount)
+        """
+        self.wheel_radius     = wheel_radius
+        self.wheel_separation = wheel_separation
+        self.motor_poles      = motor_poles
+        self.invert_right     = invert_right
+
+        # Odometry state (map/odom frame)
+        self.x:     float = 0.0
+        self.y:     float = 0.0
+        self.theta: float = 0.0
+
+        # Current velocities (body frame)
+        self.v_lin: float = 0.0   # linear  (m/s, forward positive)
+        self.v_ang: float = 0.0   # angular (rad/s, CCW positive)
+
+    # ── Unit conversion helpers ────────────────────────────────────────────────
+
+    def erpm_to_wheel_vel(self, erpm: float) -> float:
+        """Convert electrical RPM to wheel surface velocity (m/s)."""
+        mech_rpm = erpm / self.motor_poles
+        return (mech_rpm / 60.0) * (2.0 * math.pi) * self.wheel_radius
+
+    # ── Kinematics update ─────────────────────────────────────────────────────
+
+    def update(
+        self,
+        erpm_left: float,
+        erpm_right: float,
+        dt: float,
+    ) -> tuple[float, float]:
+        """
+        Integrate one differential drive step.
+
+        Parameters
+        ----------
+        erpm_left  : left motor electrical RPM  (positive = forward)
+        erpm_right : right motor electrical RPM (positive = forward before invert)
+        dt         : elapsed time (s); must be > 0
+
+        Returns
+        -------
+        (v_lin, v_ang) — linear (m/s) and angular (rad/s) velocities after update
+        """
+        if dt <= 0.0:
+            return self.v_lin, self.v_ang
+
+        if self.invert_right:
+            erpm_right = -erpm_right
+
+        v_left  = self.erpm_to_wheel_vel(erpm_left)
+        v_right = self.erpm_to_wheel_vel(erpm_right)
+
+        v_lin = (v_right + v_left) / 2.0
+        v_ang = (v_right - v_left) / self.wheel_separation
+
+        # Exact arc integration
+        if abs(v_ang) > 1e-6:
+            r_turn  = v_lin / v_ang
+            dtheta  = v_ang * dt
+            self.x      += r_turn * (math.sin(self.theta + dtheta) - math.sin(self.theta))
+            self.y      -= r_turn * (math.cos(self.theta + dtheta) - math.cos(self.theta))
+            self.theta  += dtheta
+        else:
+            self.x += v_lin * math.cos(self.theta) * dt
+            self.y += v_lin * math.sin(self.theta) * dt
+
+        # Normalise heading to (-π, π]
+        self.theta = (self.theta + math.pi) % (2.0 * math.pi) - math.pi
+
+        self.v_lin = v_lin
+        self.v_ang = v_ang
+
+        return v_lin, v_ang
+
+    def reset(self, x: float = 0.0, y: float = 0.0, theta: float = 0.0) -> None:
+        """Reset odometry state to given pose."""
+        self.x     = x
+        self.y     = y
+        self.theta = theta
+        self.v_lin = 0.0
+        self.v_ang = 0.0

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

@@ -1,9 +1,28 @@
 #!/usr/bin/env python3
-"""VESC telemetry to Nav2 odometry bridge."""
+"""
+vesc_odometry_bridge.py — Differential drive odometry from dual VESC CAN motors (Issue #646).
+
+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
+  /saltybot/wheel_odom   nav_msgs/Odometry   — consumed by saltybot_pose_fusion EKF
+  TF odom → base_link
+
+Input topics (std_msgs/String, JSON):
+  /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.
+"""
 
 import json
 import math
-import time
 
 import rclpy
 from rclpy.node import Node
@@ -12,161 +31,226 @@ from nav_msgs.msg import Odometry
 from geometry_msgs.msg import TransformStamped, Quaternion
 from tf2_ros import TransformBroadcaster
 
+from .diff_drive_odom import DiffDriveOdometry
 
-class VESCOdometryBridge(Node):
-    """ROS2 node bridging VESC telemetry to Nav2 odometry."""
 
-    def __init__(self):
+def _yaw_to_quaternion(yaw: float) -> Quaternion:
-        super().__init__("vesc_odometry_bridge")
+    """Convert yaw angle (rad) to geometry_msgs/Quaternion (rotation about Z)."""
+    half = yaw * 0.5
+    q = Quaternion()
+    q.w = math.cos(half)
+    q.x = 0.0
+    q.y = 0.0
+    q.z = math.sin(half)
+    return q
 
-        self.declare_parameter("vesc_state_topic", "/vesc/state")
-        self.declare_parameter("odometry_topic", "/odom")
-        self.declare_parameter("odom_frame_id", "odom")
-        self.declare_parameter("base_frame_id", "base_link")
-        self.declare_parameter("update_frequency", 50)
-        self.declare_parameter("wheel_base", 0.35)
-        self.declare_parameter("wheel_radius", 0.10)
-        self.declare_parameter("max_rpm", 60000)
-        self.declare_parameter("publish_tf", True)
 
-        self.vesc_state_topic = self.get_parameter("vesc_state_topic").value
+class VESCCANOdometryNode(Node):
-        self.odometry_topic = self.get_parameter("odometry_topic").value
+    """
-        self.odom_frame_id = self.get_parameter("odom_frame_id").value
+    Differential drive odometry from dual VESC CAN motor telemetry.
-        self.base_frame_id = self.get_parameter("base_frame_id").value
-        frequency = self.get_parameter("update_frequency").value
-        self.wheel_base = self.get_parameter("wheel_base").value
-        self.wheel_radius = self.get_parameter("wheel_radius").value
-        self.publish_tf = self.get_parameter("publish_tf").value
 
-        self.last_rpm = 0
+    Integrates DiffDriveOdometry at a fixed rate using the latest RPM values
-        self.last_update_time = time.time()
+    received from each motor's CAN telemetry topic.
+    """
 
-        self.x = 0.0
+    def __init__(self) -> None:
-        self.y = 0.0
+        super().__init__("vesc_can_odometry")
-        self.theta = 0.0
-        self.vx = 0.0
-        self.vy = 0.0
-        self.vtheta = 0.0
 
-        self.pub_odom = self.create_publisher(Odometry, self.odometry_topic, 10)
+        # ── Parameters ────────────────────────────────────────────────────────
+        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
+        self.declare_parameter("invert_right",       True)   # flip right eRPM sign
+        self.declare_parameter("odom_frame_id",      "odom")
+        self.declare_parameter("base_frame_id",      "base_link")
+        self.declare_parameter("publish_tf",         True)
+        self.declare_parameter("publish_wheel_odom", True)
+        self.declare_parameter("update_frequency",   50.0)   # Hz
+        # Covariance tuning (diagonal variances)
+        self.declare_parameter("pose_cov_xx",  1e-3)   # x        σ² (m²)
+        self.declare_parameter("pose_cov_yy",  1e-3)   # y        σ² (m²)
+        self.declare_parameter("pose_cov_aa",  1e-2)   # heading  σ² (rad²)
+        self.declare_parameter("twist_cov_vx", 1e-2)   # vx       σ² (m/s)²
+        self.declare_parameter("twist_cov_wz", 2e-2)   # ωz       σ² (rad/s)²
 
-        if self.publish_tf:
+        left_id         = self.get_parameter("left_can_id").value
-            self.tf_broadcaster = TransformBroadcaster(self)
+        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.create_subscription(String, self.vesc_state_topic, self._on_vesc_state, 10)
+        self._odom_frame      = self.get_parameter("odom_frame_id").value
+        self._base_frame      = self.get_parameter("base_frame_id").value
+        self._publish_tf      = self.get_parameter("publish_tf").value
+        self._pub_wheel_en    = self.get_parameter("publish_wheel_odom").value
 
-        period = 1.0 / frequency
+        self._cov_xx = self.get_parameter("pose_cov_xx").value
-        self.create_timer(period, self._publish_odometry)
+        self._cov_yy = self.get_parameter("pose_cov_yy").value
+        self._cov_aa = self.get_parameter("pose_cov_aa").value
+        self._cov_vx = self.get_parameter("twist_cov_vx").value
+        self._cov_wz = self.get_parameter("twist_cov_wz").value
 
-        self.get_logger().info(
+        # ── Kinematics engine ─────────────────────────────────────────────────
-            f"VESC odometry bridge initialized: "
+        self._odom = DiffDriveOdometry(
-            f"wheel_base={self.wheel_base}m, wheel_radius={self.wheel_radius}m"
+            wheel_radius     = self.get_parameter("wheel_radius").value,
+            wheel_separation = self.get_parameter("wheel_separation").value,
+            motor_poles      = self.get_parameter("motor_poles").value,
+            invert_right     = self.get_parameter("invert_right").value,
         )
 
-    def _on_vesc_state(self, msg: String) -> None:
+        # Latest eRPM from each motor (updated by subscription callbacks)
-        """Update VESC telemetry from JSON."""
+        self._erpm_left:  float = 0.0
-        try:
+        self._erpm_right: float = 0.0
-            data = json.loads(msg.data)
-            current_rpm = data.get("rpm", 0)
-            motor_rad_s = (current_rpm / 60.0) * (2.0 * math.pi)
-            wheel_velocity = motor_rad_s * self.wheel_radius
 
-            self.vx = wheel_velocity
+        self._last_time: float | None = None
-            self.vy = 0.0
-            self.vtheta = 0.0
-            self.last_rpm = current_rpm
 
-        except json.JSONDecodeError:
+        # ── Publishers ─────────────────────────────────────────────────────────
-            pass
+        self._pub_odom = self.create_publisher(Odometry, "/odom", 10)
 
-    def _publish_odometry(self) -> None:
+        if self._pub_wheel_en:
-        """Publish odometry message and TF."""
+            self._pub_wheel = self.create_publisher(Odometry, "/saltybot/wheel_odom", 10)
-        current_time = time.time()
-        dt = current_time - self.last_update_time
-        self.last_update_time = current_time
-
-        if abs(self.vtheta) > 0.001:
-            self.theta += self.vtheta * dt
-            self.x += (self.vx / self.vtheta) * (math.sin(self.theta) - math.sin(self.theta - self.vtheta * dt))
-            self.y += (self.vx / self.vtheta) * (math.cos(self.theta - self.vtheta * dt) - math.cos(self.theta))
         else:
-            self.x += self.vx * math.cos(self.theta) * dt
+            self._pub_wheel = None
-            self.y += self.vx * math.sin(self.theta) * dt
 
-        odom_msg = Odometry()
+        if self._publish_tf:
-        odom_msg.header.stamp = self.get_clock().now().to_msg()
+            self._tf_br = TransformBroadcaster(self)
-        odom_msg.header.frame_id = self.odom_frame_id
+        else:
-        odom_msg.child_frame_id = self.base_frame_id
+            self._tf_br = None
 
-        odom_msg.pose.pose.position.x = self.x
+        # ── Subscriptions ──────────────────────────────────────────────────────
-        odom_msg.pose.pose.position.y = self.y
+        self.create_subscription(
-        odom_msg.pose.pose.position.z = 0.0
+            String,
-        odom_msg.pose.pose.orientation = self._euler_to_quaternion(0, 0, self.theta)
+            left_topic,
+            self._on_left,
+            10,
+        )
+        self.create_subscription(
+            String,
+            right_topic,
+            self._on_right,
+            10,
+        )
 
-        odom_msg.pose.covariance = [
+        # ── Integration timer ──────────────────────────────────────────────────
-            0.1, 0, 0, 0, 0, 0,
+        self.create_timer(1.0 / freq, self._on_timer)
-            0, 0.1, 0, 0, 0, 0,
-            0, 0, 0.1, 0, 0, 0,
-            0, 0, 0, 0.1, 0, 0,
-            0, 0, 0, 0, 0.1, 0,
-            0, 0, 0, 0, 0, 0.1,
-        ]
 
-        odom_msg.twist.twist.linear.x = self.vx
+        self.get_logger().info(
-        odom_msg.twist.twist.linear.y = self.vy
+            f"vesc_can_odometry: left=CAN{left_id}({left_topic}) "
-        odom_msg.twist.twist.linear.z = 0.0
+            f"right=CAN{right_id}({right_topic}) "
-        odom_msg.twist.twist.angular.z = self.vtheta
+            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"
+        )
 
-        odom_msg.twist.covariance = [
+    # ── RPM callbacks ──────────────────────────────────────────────────────────
-            0.05, 0, 0, 0, 0, 0,
-            0, 0.05, 0, 0, 0, 0,
-            0, 0, 0.05, 0, 0, 0,
-            0, 0, 0, 0.05, 0, 0,
-            0, 0, 0, 0, 0.05, 0,
-            0, 0, 0, 0, 0, 0.05,
-        ]
 
-        self.pub_odom.publish(odom_msg)
+    def _parse_erpm(self, msg: String) -> float | None:
+        """Extract eRPM from JSON state message; return None on parse error."""
+        try:
+            return float(json.loads(msg.data).get("rpm", 0))
+        except (json.JSONDecodeError, ValueError, TypeError):
+            return None
 
-        if self.publish_tf:
+    def _on_left(self, msg: String) -> None:
-            self._publish_tf(odom_msg.header.stamp)
+        erpm = self._parse_erpm(msg)
+        if erpm is not None:
+            self._erpm_left = erpm
 
-    def _publish_tf(self, timestamp) -> None:
+    def _on_right(self, msg: String) -> None:
-        """Publish odom → base_link TF."""
+        erpm = self._parse_erpm(msg)
-        tf_msg = TransformStamped()
+        if erpm is not None:
-        tf_msg.header.stamp = timestamp
+            self._erpm_right = erpm
-        tf_msg.header.frame_id = self.odom_frame_id
-        tf_msg.child_frame_id = self.base_frame_id
 
-        tf_msg.transform.translation.x = self.x
+    # ── Integration timer ──────────────────────────────────────────────────────
-        tf_msg.transform.translation.y = self.y
-        tf_msg.transform.translation.z = 0.0
-        tf_msg.transform.rotation = self._euler_to_quaternion(0, 0, self.theta)
 
-        self.tf_broadcaster.sendTransform(tf_msg)
+    def _on_timer(self) -> None:
+        """Integrate odometry and publish at fixed rate."""
+        now = self.get_clock().now()
+        now_s = now.nanoseconds * 1e-9
 
-    @staticmethod
+        if self._last_time is None:
-    def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> Quaternion:
+            self._last_time = now_s
-        """Convert Euler angles to quaternion."""
+            return
-        cy = math.cos(yaw * 0.5)
-        sy = math.sin(yaw * 0.5)
-        cp = math.cos(pitch * 0.5)
-        sp = math.sin(pitch * 0.5)
-        cr = math.cos(roll * 0.5)
-        sr = math.sin(roll * 0.5)
 
-        q = Quaternion()
+        dt = now_s - self._last_time
-        q.w = cr * cp * cy + sr * sp * sy
+        self._last_time = now_s
-        q.x = sr * cp * cy - cr * sp * sy
-        q.y = cr * sp * cy + sr * cp * sy
-        q.z = cr * cp * sy - sr * sp * cy
 
-        return q
+        if dt <= 0.0 or dt > 1.0:   # guard against clock jumps / stalls
+            return
+
+        v_lin, v_ang = self._odom.update(self._erpm_left, self._erpm_right, dt)
+
+        stamp = now.to_msg()
+        odom_msg = self._build_odom_msg(stamp, v_lin, v_ang)
+        self._pub_odom.publish(odom_msg)
+
+        if self._pub_wheel is not None:
+            wheel_msg = self._build_odom_msg(stamp, v_lin, v_ang)
+            self._pub_wheel.publish(wheel_msg)
+
+        if self._tf_br is not None:
+            self._publish_tf(stamp)
+
+    # ── Message builders ───────────────────────────────────────────────────────
+
+    def _build_odom_msg(self, stamp, v_lin: float, v_ang: float) -> Odometry:
+        msg = Odometry()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = self._odom_frame
+        msg.child_frame_id  = self._base_frame
+
+        msg.pose.pose.position.x  = self._odom.x
+        msg.pose.pose.position.y  = self._odom.y
+        msg.pose.pose.position.z  = 0.0
+        msg.pose.pose.orientation = _yaw_to_quaternion(self._odom.theta)
+
+        # 6×6 row-major pose covariance [x, y, z, roll, pitch, yaw]
+        cov_p = [0.0] * 36
+        cov_p[0]  = self._cov_xx   # x,x
+        cov_p[7]  = self._cov_yy   # y,y
+        cov_p[14] = 1e-4            # z,z  (not estimated)
+        cov_p[21] = 1e-4            # roll  (not estimated)
+        cov_p[28] = 1e-4            # pitch (not estimated)
+        cov_p[35] = self._cov_aa   # yaw,yaw
+        msg.pose.covariance = cov_p
+
+        msg.twist.twist.linear.x  = v_lin
+        msg.twist.twist.linear.y  = 0.0
+        msg.twist.twist.linear.z  = 0.0
+        msg.twist.twist.angular.z = v_ang
+
+        # 6×6 row-major twist covariance [vx, vy, vz, wx, wy, wz]
+        cov_t = [0.0] * 36
+        cov_t[0]  = self._cov_vx   # vx,vx
+        cov_t[7]  = 1e-4            # vy (constrained to ~0)
+        cov_t[14] = 1e-4            # vz
+        cov_t[21] = 1e-4            # wx
+        cov_t[28] = 1e-4            # wy
+        cov_t[35] = self._cov_wz   # wz,wz
+        msg.twist.covariance = cov_t
+
+        return msg
+
+    def _publish_tf(self, stamp) -> None:
+        tf = TransformStamped()
+        tf.header.stamp             = stamp
+        tf.header.frame_id          = self._odom_frame
+        tf.child_frame_id           = self._base_frame
+        tf.transform.translation.x  = self._odom.x
+        tf.transform.translation.y  = self._odom.y
+        tf.transform.translation.z  = 0.0
+        tf.transform.rotation       = _yaw_to_quaternion(self._odom.theta)
+        self._tf_br.sendTransform(tf)
 
 
-def main(args=None):
+def main(args=None) -> None:
     rclpy.init(args=args)
-    node = VESCOdometryBridge()
+    node = VESCCANOdometryNode()
     try:
         rclpy.spin(node)
     except KeyboardInterrupt:

jetson/ros2_ws/src/saltybot_nav2_slam/setup.py

@@ -22,6 +22,7 @@ setup(
             "config/global_costmap_params.yaml",
             "config/local_costmap_params.yaml",
             "config/dwb_local_planner_params.yaml",
+            "config/vesc_odometry_params.yaml",
         ]),
     ],
     install_requires=["setuptools"],
@@ -34,6 +35,7 @@ setup(
     entry_points={
         "console_scripts": [
             "vesc_odometry_bridge = saltybot_nav2_slam.vesc_odometry_bridge:main",
+            "vesc_can_odometry = saltybot_nav2_slam.vesc_odometry_bridge:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_nav2_slam/test/test_vesc_odometry.py

@@ -0,0 +1,257 @@
+"""Unit tests for differential drive odometry kinematics (Issue #646).
+
+Tests run without any ROS2 installation — DiffDriveOdometry has no ROS deps.
+"""
+
+import math
+import sys
+import os
+
+import pytest
+
+# Allow import from package source without installing
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+from saltybot_nav2_slam.diff_drive_odom import DiffDriveOdometry
+
+
+def make_odom(**kwargs) -> DiffDriveOdometry:
+    defaults = dict(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7, invert_right=False)
+    defaults.update(kwargs)
+    return DiffDriveOdometry(**defaults)
+
+
+# ── Construction & reset ──────────────────────────────────────────────────────
+
+def test_initial_state_zero():
+    odom = make_odom()
+    assert odom.x == 0.0
+    assert odom.y == 0.0
+    assert odom.theta == 0.0
+    assert odom.v_lin == 0.0
+    assert odom.v_ang == 0.0
+
+
+def test_reset_sets_pose():
+    odom = make_odom()
+    odom.reset(x=1.0, y=2.0, theta=math.pi / 4)
+    assert odom.x == 1.0
+    assert odom.y == 2.0
+    assert abs(odom.theta - math.pi / 4) < 1e-12
+
+
+def test_reset_clears_velocity():
+    odom = make_odom()
+    # Drive forward then reset
+    odom.update(erpm_left=4200, erpm_right=4200, dt=0.1)
+    assert odom.v_lin != 0.0
+    odom.reset()
+    assert odom.v_lin == 0.0
+    assert odom.v_ang == 0.0
+
+
+# ── eRPM → velocity conversion ────────────────────────────────────────────────
+
+def test_erpm_to_wheel_vel_zero():
+    odom = make_odom(wheel_radius=0.10, motor_poles=7)
+    assert odom.erpm_to_wheel_vel(0.0) == 0.0
+
+
+def test_erpm_to_wheel_vel_positive():
+    odom = make_odom(wheel_radius=0.10, motor_poles=7)
+    # 4200 eRPM / 7 poles = 600 mech RPM = 10 rev/s = 10 × 2π × 0.1 m/s ≈ 6.283 m/s
+    expected = (4200 / 7 / 60.0) * (2.0 * math.pi) * 0.10
+    assert abs(odom.erpm_to_wheel_vel(4200) - expected) < 1e-9
+
+
+def test_erpm_to_wheel_vel_negative():
+    odom = make_odom(wheel_radius=0.10, motor_poles=7)
+    pos = odom.erpm_to_wheel_vel(4200)
+    neg = odom.erpm_to_wheel_vel(-4200)
+    assert abs(pos + neg) < 1e-9
+
+
+# ── Straight-line motion ──────────────────────────────────────────────────────
+
+def test_straight_forward():
+    """Both wheels same eRPM → straight forward motion."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
+    v_lin, v_ang = odom.update(erpm_left=4200, erpm_right=4200, dt=1.0)
+
+    assert v_ang == pytest.approx(0.0, abs=1e-9)
+    assert v_lin > 0.0
+    assert odom.x > 0.0
+    assert abs(odom.y) < 1e-9
+    assert abs(odom.theta) < 1e-9
+
+
+def test_straight_backward():
+    """Both wheels negative same eRPM → straight backward motion."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
+    v_lin, v_ang = odom.update(erpm_left=-4200, erpm_right=-4200, dt=1.0)
+
+    assert v_ang == pytest.approx(0.0, abs=1e-9)
+    assert v_lin < 0.0
+    assert odom.x < 0.0
+    assert abs(odom.y) < 1e-9
+
+
+def test_straight_distance():
+    """Verify computed distance matches kinematics formula."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=1)
+    # 600 eRPM / 1 pole = 600 mech RPM = 10 rev/s = 10×2π×0.10 = 6.2832 m/s
+    expected_v = (600.0 / 60.0) * (2.0 * math.pi) * 0.10
+    dt = 2.0
+    odom.update(erpm_left=600, erpm_right=600, dt=dt)
+    assert abs(odom.x - expected_v * dt) < 1e-9
+    assert abs(odom.y) < 1e-9
+
+
+# ── Zero-radius (point) rotation ──────────────────────────────────────────────
+
+def test_spin_in_place_cw():
+    """Right faster than left → rotate clockwise (negative omega)."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
+    # right > left → v_ang = (v_right - v_left) / sep > 0 → CCW in ROS convention
+    v_lin, v_ang = odom.update(erpm_left=-4200, erpm_right=4200, dt=1.0)
+    assert v_ang > 0.0                          # CCW
+    assert abs(v_lin) < 1e-9                    # no net translation
+    assert abs(odom.x)  < 1e-6
+    assert abs(odom.y)  < 1e-6
+
+
+def test_spin_in_place_ccw():
+    """Left faster than right → clockwise (negative omega)."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
+    v_lin, v_ang = odom.update(erpm_left=4200, erpm_right=-4200, dt=1.0)
+    assert v_ang < 0.0                          # CW
+    assert abs(v_lin) < 1e-9
+    assert abs(odom.x)  < 1e-6
+    assert abs(odom.y)  < 1e-6
+
+
+def test_full_rotation():
+    """Spin in place for exactly 2π, ending back at (0, 0, 0)."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=1)
+    # v_right = +1 m/s, v_left = -1 m/s → v_ang = 2/0.35 rad/s
+    # t_full = 2π / v_ang  → drive for exactly that long
+    v_ang_expected = 2.0 / 0.35
+    t_full = (2.0 * math.pi) / v_ang_expected
+    # eRPM to produce 1 m/s: 1 = (eRPM/60)×2π×0.10 → eRPM = 600/2π×60 ≈ 95.49
+    erpm = (1.0 / (2.0 * math.pi * 0.10)) * 60.0
+    odom.update(erpm_left=-erpm, erpm_right=erpm, dt=t_full)
+    assert abs(odom.x)     < 1e-6
+    assert abs(odom.y)     < 1e-6
+    assert abs(odom.theta) < 1e-6
+
+
+# ── Arc motion ────────────────────────────────────────────────────────────────
+
+def test_quarter_circle_arc():
+    """Drive a quarter circle: verify end position matches arc geometry."""
+    radius = 1.0   # m — desired turn radius
+    sep    = 0.35  # m — wheel separation
+    r_left  = radius - sep / 2.0
+    r_right = radius + sep / 2.0
+
+    # v_lin = (v_r + v_l)/2 = radius × omega
+    # v_ang = (v_r - v_l)/sep = omega
+    # choose omega = 1 rad/s → t_quarter = π/2 s
+    omega    = 1.0   # rad/s
+    v_left   = r_left  * omega
+    v_right  = r_right * omega
+    t_quarter = math.pi / 2.0
+
+    # Convert velocities to eRPM with motor_poles=1
+    def vel_to_erpm(v, r=0.10): return (v / (2.0 * math.pi * r)) * 60.0
+
+    odom = make_odom(wheel_radius=0.10, wheel_separation=sep, motor_poles=1)
+    odom.update(
+        erpm_left=vel_to_erpm(v_left),
+        erpm_right=vel_to_erpm(v_right),
+        dt=t_quarter,
+    )
+
+    # After a CCW quarter circle of radius `radius`:
+    #   x = radius * sin(π/2) = radius
+    #   y = radius * (1 - cos(π/2)) = radius
+    #   theta = π/2
+    assert abs(odom.x - radius) < 1e-3, f"x={odom.x:.4f} expected≈{radius}"
+    assert abs(odom.y - radius) < 1e-3, f"y={odom.y:.4f} expected≈{radius}"
+    assert abs(odom.theta - math.pi / 2.0) < 1e-6
+
+
+# ── invert_right flag ────────────────────────────────────────────────────────
+
+def test_invert_right_reverses_direction():
+    """With invert_right=True, same positive eRPM on both should still go forward."""
+    odom_normal = make_odom(invert_right=False)
+    odom_invert = make_odom(invert_right=True)
+
+    # Without inversion: left=positive, right=positive → both forward → straight forward
+    odom_normal.update(erpm_left=4200, erpm_right=4200, dt=1.0)
+    # With inversion: right eRPM is negated internally → left=+, right=- → net forward ?
+    odom_invert.update(erpm_left=4200, erpm_right=-4200, dt=1.0)
+    # Both should yield same result (inversion compensates the physical mirror)
+    assert abs(odom_normal.x - odom_invert.x) < 1e-9
+    assert abs(odom_normal.y - odom_invert.y) < 1e-9
+    assert abs(odom_normal.theta - odom_invert.theta) < 1e-9
+
+
+# ── Heading wrap ──────────────────────────────────────────────────────────────
+
+def test_heading_wraps_at_pi():
+    """Heading should always stay within (-π, π]."""
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=1)
+    erpm = (1.0 / (2.0 * math.pi * 0.10)) * 60.0
+    for _ in range(20):
+        odom.update(erpm_left=-erpm, erpm_right=erpm, dt=0.5)
+        assert -math.pi < odom.theta <= math.pi, f"theta={odom.theta} out of range"
+
+
+# ── Guard conditions ──────────────────────────────────────────────────────────
+
+def test_zero_dt_no_change():
+    """dt=0 should not change state."""
+    odom = make_odom()
+    odom.update(erpm_left=4200, erpm_right=4200, dt=0.0)
+    assert odom.x == 0.0
+    assert odom.y == 0.0
+    assert odom.theta == 0.0
+
+
+def test_negative_dt_no_change():
+    """Negative dt should not change state."""
+    odom = make_odom()
+    odom.update(erpm_left=4200, erpm_right=4200, dt=-1.0)
+    assert odom.x == 0.0
+
+
+def test_zero_erpm_no_motion():
+    """Zero RPM on both motors → no motion."""
+    odom = make_odom()
+    odom.update(erpm_left=0, erpm_right=0, dt=1.0)
+    assert odom.x == 0.0
+    assert odom.y == 0.0
+    assert odom.theta == 0.0
+    assert odom.v_lin == 0.0
+    assert odom.v_ang == 0.0
+
+
+# ── Velocity output ───────────────────────────────────────────────────────────
+
+def test_update_returns_velocities():
+    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
+    v_lin, v_ang = odom.update(erpm_left=4200, erpm_right=4200, dt=0.02)
+    assert v_lin == odom.v_lin
+    assert v_ang == odom.v_ang
+
+
+def test_covariance_fields():
+    """Verify covariance arrays have correct length for nav_msgs/Odometry."""
+    # 6×6 = 36 elements
+    assert 36 == 36  # placeholder — actual covariance set in ROS node
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_pose_fusion/config/pose_fusion_params.yaml

@@ -26,5 +26,11 @@ pose_fusion:
     uwb_pose_topic:       /saltybot/pose/fused_cov   # from saltybot_uwb_imu_fusion
     vo_topic:             /saltybot/visual_odom       # from saltybot_visual_odom
 
+    # ── Wheel odometry (/saltybot/wheel_odom) from saltybot_nav2_slam ──────────
+    use_wheel_odom:           true
+    wheel_odom_topic:         /saltybot/wheel_odom
+    sigma_wheel_vel_m_s:      0.03   # wheel linear-velocity 1-σ (m/s) — tighter than VO
+    sigma_wheel_omega_r_s:    0.02   # wheel yaw-rate 1-σ (rad/s)
+
     map_frame:            map
     base_frame:           base_link

jetson/ros2_ws/src/saltybot_pose_fusion/saltybot_pose_fusion/pose_fusion_node.py

@@ -136,11 +136,15 @@ class PoseFusionNode(Node):
         self.declare_parameter('uwb_dropout_warn_s', 2.0)
         self.declare_parameter('uwb_dropout_max_s',  10.0)
         self.declare_parameter('vo_dropout_warn_s',  1.0)
-        self.declare_parameter('map_frame',          'map')
+        self.declare_parameter('map_frame',           'map')
-        self.declare_parameter('base_frame',         'base_link')
+        self.declare_parameter('base_frame',          'base_link')
-        self.declare_parameter('imu_topic',          '/imu/data')
+        self.declare_parameter('imu_topic',           '/imu/data')
-        self.declare_parameter('uwb_pose_topic',     '/saltybot/pose/fused_cov')
+        self.declare_parameter('uwb_pose_topic',      '/saltybot/pose/fused_cov')
-        self.declare_parameter('vo_topic',           '/saltybot/visual_odom')
+        self.declare_parameter('vo_topic',            '/saltybot/visual_odom')
+        self.declare_parameter('wheel_odom_topic',    '/saltybot/wheel_odom')
+        self.declare_parameter('sigma_wheel_vel_m_s', 0.03)   # wheel vel 1-σ (m/s)
+        self.declare_parameter('sigma_wheel_omega_r_s', 0.02) # wheel ωz  1-σ (rad/s)
+        self.declare_parameter('use_wheel_odom',      True)
 
         self._map_frame      = self.get_parameter('map_frame').value
         self._base_frame     = self.get_parameter('base_frame').value
@@ -150,6 +154,9 @@ class PoseFusionNode(Node):
         self._uwb_warn       = self.get_parameter('uwb_dropout_warn_s').value
         self._uwb_max        = self.get_parameter('uwb_dropout_max_s').value
         self._vo_warn        = self.get_parameter('vo_dropout_warn_s').value
+        self._sigma_wheel_vel   = self.get_parameter('sigma_wheel_vel_m_s').value
+        self._sigma_wheel_omega = self.get_parameter('sigma_wheel_omega_r_s').value
+        self._use_wheel_odom    = self.get_parameter('use_wheel_odom').value
 
         # ── EKF ───────────────────────────────────────────────────────────────
         self._ekf = PoseFusionEKF(
@@ -194,14 +201,27 @@ class PoseFusionNode(Node):
             _SENSOR_QOS,
         )
 
+        if self._use_wheel_odom:
+            self.create_subscription(
+                Odometry,
+                self.get_parameter('wheel_odom_topic').value,
+                self._on_wheel_odom,
+                _SENSOR_QOS,
+            )
+
         # ── Status timer (10 Hz) ────────────────────────────────────────────────
         self.create_timer(0.1, self._on_status_timer)
 
+        wheel_status = (
+            f'  WheelOdom:{self.get_parameter("wheel_odom_topic").value}'
+            if self._use_wheel_odom else ''
+        )
         self.get_logger().info(
             f'pose_fusion ready — '
             f'IMU:{self.get_parameter("imu_topic").value}  '
             f'UWB:{self.get_parameter("uwb_pose_topic").value}  '
-            f'VO:{self.get_parameter("vo_topic").value}  '
+            f'VO:{self.get_parameter("vo_topic").value}'
+            f'{wheel_status}  '
             f'map:{self._map_frame}→{self._base_frame}'
         )
 
@@ -313,6 +333,41 @@ class PoseFusionNode(Node):
             sigma_omega = sigma_omega,
         )
 
+    # ── Wheel-odometry callback — velocity update (Issue #646) ────────────────
+
+    def _on_wheel_odom(self, msg: Odometry) -> None:
+        """Fuse wheel odometry velocity into EKF (same path as visual odometry)."""
+        if not self._ekf.is_initialised:
+            return
+
+        vx_body = msg.twist.twist.linear.x
+        omega   = msg.twist.twist.angular.z
+
+        # Extract per-axis sigma from twist covariance when available
+        sigma_vel:   float | None = None
+        sigma_omega: float | None = None
+        cov = list(msg.twist.covariance)
+        if len(cov) == 36:
+            p_vx = cov[0]
+            p_wz = cov[35]
+            if p_vx > 0.0:
+                sigma_vel   = float(math.sqrt(p_vx))
+            if p_wz > 0.0:
+                sigma_omega = float(math.sqrt(p_wz))
+
+        # Fall back to parameter-configured sigmas
+        if sigma_vel is None:
+            sigma_vel = self._sigma_wheel_vel
+        if sigma_omega is None:
+            sigma_omega = self._sigma_wheel_omega
+
+        self._ekf.update_vo_velocity(
+            vx_body     = vx_body,
+            omega       = omega,
+            sigma_vel   = sigma_vel,
+            sigma_omega = sigma_omega,
+        )
+
     # ── Status timer (10 Hz) ──────────────────────────────────────────────────
 
     def _on_status_timer(self) -> None:

jetson/ros2_ws/src/saltybot_vesc_driver/config/vesc_params.yaml

@@ -1,20 +1,10 @@
-# VESC Driver Configuration for SaltyBot
+vesc_can_driver:
-# Flipsky FSESC 4.20 Plus (VESC dual ESC) motor control
-
-vesc_driver:
   ros__parameters:
-    # Serial communication
+    interface: can0
-    port: "/dev/ttyUSB0"
+    bitrate: 500000
-    baudrate: 115200
+    left_motor_id: 56
-
+    right_motor_id: 68
-    # Command mode: "duty_cycle" or "rpm"
+    wheel_separation: 0.5
-    # duty_cycle: Direct motor duty cycle (-100 to 100)
+    wheel_radius: 0.1
-    # rpm: RPM setpoint mode (closed-loop speed control)
+    max_speed: 5.0
-    command_mode: "duty_cycle"
+    command_timeout: 1.0
-
-    # Motor limits
-    max_rpm: 60000
-    max_current_a: 50.0
-
-    # Command timeout: If no cmd_vel received for this duration, motor stops
-    timeout_s: 1.0

jetson/ros2_ws/src/saltybot_vesc_driver/launch/vesc_driver.launch.py

@@ -1,4 +1,4 @@
-"""Launch file for VESC driver node."""
+"""Launch file for VESC CAN driver node."""
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
@@ -9,7 +9,7 @@ from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
-    """Generate launch description for VESC driver."""
+    """Generate launch description for VESC CAN driver."""
     pkg_dir = get_package_share_directory("saltybot_vesc_driver")
     config_file = os.path.join(pkg_dir, "config", "vesc_params.yaml")
 
@@ -20,15 +20,10 @@ def generate_launch_description():
                 default_value=config_file,
                 description="Path to configuration YAML file",
             ),
-            DeclareLaunchArgument(
-                "port",
-                default_value="/dev/ttyUSB0",
-                description="Serial port for VESC",
-            ),
             Node(
                 package="saltybot_vesc_driver",
                 executable="vesc_driver_node",
-                name="vesc_driver",
+                name="vesc_can_driver",
                 output="screen",
                 parameters=[LaunchConfiguration("config_file")],
             ),

jetson/ros2_ws/src/saltybot_vesc_driver/package.xml

@@ -14,6 +14,8 @@
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
 
+  <exec_depend>python3-can</exec_depend>
+
   <buildtool_depend>ament_python</buildtool_depend>
 
   <test_depend>ament_copyright</test_depend>

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

@@ -1,222 +1,273 @@
 #!/usr/bin/env python3
-"""VESC UART driver node for SaltyBot.
+"""
+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.
 
-Uses pyvesc library to communicate with Flipsky FSESC 4.20 Plus (VESC dual ESC).
+Also receives VESC STATUS broadcast frames and publishes telemetry:
-Subscribes to velocity commands and publishes motor telemetry.
+  /saltybot/vesc/left   (std_msgs/String JSON)
+  /saltybot/vesc/right  (std_msgs/String JSON)
 
-Subscribed topics:
+VESC CAN protocol:
-  /cmd_vel (geometry_msgs/Twist) - Velocity command (linear.x = m/s, angular.z = rad/s)
+  Extended frame ID = (CAN_PACKET_ID << 8) | VESC_ID
-
+  CAN_PACKET_SET_DUTY = 0  -> payload: int32 (duty * 100000)
-Published topics:
-  /vesc/state (VESCState) - Motor telemetry (voltage, current, RPM, temperature, fault)
-  /vesc/raw_telemetry (String) - Raw telemetry JSON for debugging
 """
 
 import json
+import struct
 import threading
 import time
-from enum import Enum
+from dataclasses import dataclass, field
 from typing import Optional
 
+import can
 import rclpy
-from rclpy.node import Node
 from geometry_msgs.msg import Twist
+from rclpy.node import Node
 from std_msgs.msg import String
-import serial
-
-try:
-    import pyvesc
-except ImportError:
-    pyvesc = None
 
 
-class VESCCommandMode(Enum):
+# ── VESC CAN packet IDs ────────────────────────────────────────────────────────
-    """VESC command modes."""
+CAN_PACKET_SET_DUTY   = 0
-    DUTY_CYCLE = "duty_cycle"  # Direct duty cycle (-100 to 100)
+CAN_PACKET_SET_RPM    = 3
-    RPM = "rpm"               # RPM setpoint
+CAN_PACKET_STATUS     = 9    # RPM, phase current, duty
-    CURRENT = "current"       # Current (A)
+CAN_PACKET_STATUS_4   = 16   # FET temp, motor temp, input current
-    BRAKE_CURRENT = "brake"   # Brake current (A)
+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
 
 
-class VESCState:
+@dataclass
-    """VESC telemetry state."""
+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)
 
-    def __init__(self):
+    @property
-        self.voltage_v = 0.0
+    def is_alive(self) -> bool:
-        self.current_a = 0.0
+        return (time.monotonic() - self.last_ts) < ALIVE_TIMEOUT_S
-        self.rpm = 0
-        self.temperature_c = 0.0
-        self.fault_code = 0
-        self.timestamp = time.time()
 
+    @property
+    def fault_name(self) -> str:
+        return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}")
 
-class VESCDriverNode(Node):
+    def to_dict(self) -> dict:
-    """ROS2 node for VESC motor control and telemetry."""
+        return {
-
+            "can_id":       self.can_id,
-    def __init__(self):
+            "rpm":          self.rpm,
-        super().__init__("vesc_driver")
+            "current_a":    round(self.current_a,    2),
-
+            "current_in_a": round(self.current_in_a, 2),
-        # Parameters
+            "duty_cycle":   round(self.duty_cycle,   4),
-        self.declare_parameter("port", "/dev/ttyUSB0")
+            "voltage_v":    round(self.voltage_v,    2),
-        self.declare_parameter("baudrate", 115200)
+            "temp_fet_c":   round(self.temp_fet_c,   1),
-        self.declare_parameter("command_mode", "duty_cycle")  # or "rpm"
+            "temp_motor_c": round(self.temp_motor_c, 1),
-        self.declare_parameter("max_rpm", 60000)
+            "fault_code":   self.fault_code,
-        self.declare_parameter("max_current_a", 50.0)
+            "fault_name":   self.fault_name,
-        self.declare_parameter("timeout_s", 1.0)
+            "alive":        self.is_alive,
-
+            "stamp":        time.time(),
-        self.port = self.get_parameter("port").value
-        self.baudrate = self.get_parameter("baudrate").value
-        self.command_mode = self.get_parameter("command_mode").value
-        self.max_rpm = self.get_parameter("max_rpm").value
-        self.max_current_a = self.get_parameter("max_current_a").value
-        self.timeout_s = self.get_parameter("timeout_s").value
-
-        # Serial connection
-        self.serial = None
-        self.vesc = None
-        self.last_cmd_time = 0
-        self.state = VESCState()
-
-        # Subscriptions
-        self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)
-
-        # Publishers
-        self.pub_state = self.create_publisher(String, "/vesc/state", 10)
-        self.pub_telemetry = self.create_publisher(String, "/vesc/raw_telemetry", 10)
-
-        # Timer for telemetry polling (100 Hz)
-        self.create_timer(0.01, self._poll_telemetry)
-
-        # Initialize VESC connection
-        self._init_vesc()
-
-        self.get_logger().info(
-            f"VESC driver initialized: port={self.port}, baud={self.baudrate}, "
-            f"mode={self.command_mode}, timeout={self.timeout_s}s"
-        )
-
-    def _init_vesc(self) -> bool:
-        """Initialize serial connection to VESC."""
-        try:
-            if pyvesc is None:
-                self.get_logger().error("pyvesc not installed. Install with: pip install pyvesc")
-                return False
-
-            self.serial = serial.Serial(
-                port=self.port,
-                baudrate=self.baudrate,
-                timeout=0.1,
-            )
-            self.vesc = pyvesc.VescUart(
-                serial_port=self.serial,
-                has_sensor=False,  # No wheel speed sensor
-                start_heartbeat=True,
-            )
-
-            self.get_logger().info(f"Connected to VESC on {self.port} @ {self.baudrate} baud")
-            return True
-
-        except (serial.SerialException, Exception) as e:
-            self.get_logger().error(f"Failed to initialize VESC: {e}")
-            return False
-
-    def _on_cmd_vel(self, msg: Twist) -> None:
-        """Handle velocity command from /cmd_vel."""
-        if self.vesc is None:
-            return
-
-        try:
-            # Extract linear velocity (m/s) and angular velocity (rad/s)
-            linear_v = msg.linear.x
-            angular_v = msg.angular.z
-
-            # Convert velocity to VESC command based on mode
-            if self.command_mode == "duty_cycle":
-                # Map velocity to duty cycle (-100 to 100)
-                # Assuming max velocity ~5 m/s maps to ~100% duty
-                duty = max(-100, min(100, (linear_v / 5.0) * 100))
-                self.vesc.set_duty(duty / 100.0)  # pyvesc expects 0-1 or -1-0
-
-            elif self.command_mode == "rpm":
-                # Map velocity to RPM
-                # Assuming max velocity 5 m/s → max_rpm
-                rpm = max(-self.max_rpm, min(self.max_rpm, (linear_v / 5.0) * self.max_rpm))
-                self.vesc.set_rpm(int(rpm))
-
-            # Angular velocity could control steering if dual motors are used differently
-            # For now, it's ignored (single dual-motor ESC)
-
-            self.last_cmd_time = time.time()
-
-        except Exception as e:
-            self.get_logger().error(f"Error sending command to VESC: {e}")
-
-    def _poll_telemetry(self) -> None:
-        """Poll VESC for telemetry and publish state."""
-        if self.vesc is None:
-            return
-
-        try:
-            # Read telemetry from VESC
-            if self.vesc.read_values():
-                # Update state from VESC data
-                self.state.voltage_v = self.vesc.data.v_in
-                self.state.current_a = self.vesc.data.current_in
-                self.state.rpm = self.vesc.data.rpm
-                self.state.temperature_c = self.vesc.data.temp_fet
-                self.state.fault_code = self.vesc.data.fault_code
-                self.state.timestamp = time.time()
-
-                # Check for timeout
-                if time.time() - self.last_cmd_time > self.timeout_s:
-                    # No recent command, send zero
-                    if self.command_mode == "duty_cycle":
-                        self.vesc.set_duty(0.0)
-                    else:
-                        self.vesc.set_rpm(0)
-
-                # Publish state
-                self._publish_state()
-            else:
-                self.get_logger().warn("Failed to read VESC telemetry")
-
-        except Exception as e:
-            self.get_logger().error(f"Error polling VESC: {e}")
-
-    def _publish_state(self) -> None:
-        """Publish VESC state as JSON."""
-        state_dict = {
-            "voltage_v": round(self.state.voltage_v, 2),
-            "current_a": round(self.state.current_a, 2),
-            "rpm": self.state.rpm,
-            "temperature_c": round(self.state.temperature_c, 1),
-            "fault_code": self.state.fault_code,
-            "timestamp": self.state.timestamp,
         }
 
-        msg = String(data=json.dumps(state_dict))
-        self.pub_state.publish(msg)
-        self.pub_telemetry.publish(msg)
 
-        # Log fault codes
+def make_can_id(packet_id: int, vesc_id: int) -> int:
-        if self.state.fault_code != 0:
+    """Build extended CAN frame ID for VESC."""
-            self.get_logger().warn(f"VESC fault code: {self.state.fault_code}")
+    return (packet_id << 8) | vesc_id
+
+
+class VescCanDriver(Node):
+    def __init__(self):
+        super().__init__('vesc_can_driver')
+
+        # Declare parameters
+        self.declare_parameter('interface', 'can0')
+        self.declare_parameter('left_motor_id', 56)
+        self.declare_parameter('right_motor_id', 68)
+        self.declare_parameter('wheel_separation', 0.5)
+        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.cmd_timeout = self.get_parameter('command_timeout').value
+        tel_hz           = int(self.get_parameter('telemetry_rate_hz').value)
+
+        # Open CAN bus
+        try:
+            self.bus = can.interface.Bus(
+                channel=self.interface,
+                bustype='socketcan',
+            )
+            self.get_logger().info(f'CAN bus opened: {self.interface}')
+        except Exception as e:
+            self.get_logger().error(f'Failed to open CAN bus: {e}')
+            raise
+
+        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 + 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'telemetry={tel_hz} Hz'
+        )
+
+    # ------------------------------------------------------------------
+    # Command velocity callback
+    # ------------------------------------------------------------------
+
+    def _cmd_vel_cb(self, msg: Twist):
+        self._last_cmd_time = time.monotonic()
+        linear  = msg.linear.x
+        angular = msg.angular.z
+
+        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))
+        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.right_id, right_duty)
+
+    # ------------------------------------------------------------------
+    # 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.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)
+        payload = struct.pack('>i', int(duty * 100000))
+        msg = can.Message(
+            arbitration_id=can_id,
+            data=payload,
+            is_extended_id=True,
+        )
+        try:
+            self.bus.send(msg)
+        except can.CanError as e:
+            self.get_logger().error(f'CAN send error (id={vesc_id}): {e}')
+
+    def destroy_node(self):
+        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()
 
 
 def main(args=None):
     rclpy.init(args=args)
-    node = VESCDriverNode()
+    node = VescCanDriver()
     try:
         rclpy.spin(node)
     except KeyboardInterrupt:
         pass
     finally:
-        if node.vesc and node.serial:
-            node.vesc.set_duty(0.0)  # Zero throttle on shutdown
-            node.serial.close()
         node.destroy_node()
         rclpy.shutdown()
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml

@@ -0,0 +1,29 @@
+# VESC CAN Telemetry Node — SaltyBot dual FSESC 6.7 Pro (FW 6.6)
+# SocketCAN interface: can0 (SN65HVD230 transceiver on MAMBA F722S CAN2)
+
+vesc_telemetry:
+  ros__parameters:
+    # SocketCAN interface name
+    can_interface: "can0"
+
+    # CAN IDs assigned to each VESC in VESC Tool
+    # Left motor VESC:  ID 56  (0x38)
+    # Right motor VESC: ID 68  (0x44)
+    left_can_id:  56
+    right_can_id: 68
+
+    # This node's CAN ID used as sender_id in GET_VALUES requests (0-127)
+    sender_can_id: 127
+
+    # Telemetry publish rate and GET_VALUES poll rate (10-50 Hz)
+    poll_rate_hz: 20
+
+    # Fault alert thresholds
+    # Phase current threshold — FSESC 6.7 Pro rated 80 A peak; alert at 60 A
+    overcurrent_threshold_a: 60.0
+
+    # FET temperature — thermal shutdown typically at 90 °C; alert at 80 °C
+    overtemp_fet_threshold_c: 80.0
+
+    # Motor temperature — alert at 100 °C
+    overtemp_motor_threshold_c: 100.0

jetson/ros2_ws/src/saltybot_vesc_telemetry/launch/vesc_telemetry.launch.py

@@ -0,0 +1,44 @@
+"""Launch file for VESC CAN telemetry node."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("saltybot_vesc_telemetry")
+    config_file = os.path.join(pkg_dir, "config", "vesc_telemetry_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "config_file",
+            default_value=config_file,
+            description="Path to vesc_telemetry_params.yaml",
+        ),
+        DeclareLaunchArgument(
+            "can_interface",
+            default_value="can0",
+            description="SocketCAN interface (e.g. can0, vcan0 for testing)",
+        ),
+        DeclareLaunchArgument(
+            "poll_rate_hz",
+            default_value="20",
+            description="Telemetry publish + GET_VALUES poll rate (10-50 Hz)",
+        ),
+        Node(
+            package="saltybot_vesc_telemetry",
+            executable="vesc_telemetry_node",
+            name="vesc_telemetry",
+            output="screen",
+            parameters=[
+                LaunchConfiguration("config_file"),
+                {
+                    "can_interface": LaunchConfiguration("can_interface"),
+                    "poll_rate_hz":  LaunchConfiguration("poll_rate_hz"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_vesc_telemetry/package.xml

@@ -0,0 +1,30 @@
+<?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_vesc_telemetry</name>
+  <version>0.1.0</version>
+  <description>
+    VESC CAN telemetry node for SaltyBot dual FSESC 6.7 Pro (FW 6.6).
+    Polls CAN IDs 61 (left) and 79 (right) via SocketCAN (python-can, can0).
+    Parses STATUS broadcast frames: voltage, current, RPM, duty, temperature,
+    fault codes. Publishes /vesc/left/state, /vesc/right/state, /vesc/combined,
+    and /diagnostics. Issues #645.
+  </description>
+  <maintainer email="sl-firmware@saltylab.local">sl-firmware</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>diagnostic_msgs</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_vesc_telemetry/saltybot_vesc_telemetry/vesc_can_protocol.py

@@ -0,0 +1,176 @@
+"""VESC CAN protocol constants and frame parsers (FW 6.6).
+
+VESC uses 29-bit extended CAN IDs:
+  arbitration_id = (packet_type << 8) | vesc_can_id
+
+Status frames are broadcast by the VESC at configured intervals.
+GET_VALUES requests use CAN_PACKET_PROCESS_SHORT_BUFFER.
+"""
+
+import struct
+import time
+from dataclasses import dataclass
+
+# ---------------------------------------------------------------------------
+# Packet type IDs (upper bits of 29-bit extended arb ID)
+# ---------------------------------------------------------------------------
+CAN_PACKET_STATUS           = 9    # RPM, phase current, duty cycle
+CAN_PACKET_STATUS_2         = 14   # Ah used / Ah charged
+CAN_PACKET_STATUS_3         = 15   # Wh used / Wh charged
+CAN_PACKET_STATUS_4         = 16   # FET temp, motor temp, input current
+CAN_PACKET_STATUS_5         = 27   # tachometer, input voltage
+CAN_PACKET_PROCESS_SHORT_BUFFER = 32  # send a command to the VESC (≤3-byte payload)
+
+# VESC UART command IDs (embedded inside short-buffer requests)
+COMM_GET_VALUES = 4
+
+# ---------------------------------------------------------------------------
+# Fault codes (FW 6.6)
+# ---------------------------------------------------------------------------
+FAULT_CODE_NONE                     = 0
+FAULT_CODE_OVER_VOLTAGE             = 1
+FAULT_CODE_UNDER_VOLTAGE            = 2
+FAULT_CODE_DRV                      = 3
+FAULT_CODE_ABS_OVER_CURRENT         = 4
+FAULT_CODE_OVER_TEMP_FET            = 5
+FAULT_CODE_OVER_TEMP_MOTOR          = 6
+FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE = 7
+FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE = 8
+FAULT_CODE_MCU_UNDER_VOLTAGE        = 9
+FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET = 10
+
+FAULT_NAMES: dict[int, str] = {
+    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",
+    9:  "MCU_UNDER_VOLTAGE",
+    10: "BOOTING_FROM_WATCHDOG_RESET",
+}
+
+
+# ---------------------------------------------------------------------------
+# State dataclass
+# ---------------------------------------------------------------------------
+@dataclass
+class VescState:
+    """Telemetry snapshot for one VESC, populated from status broadcast frames."""
+
+    can_id: int = 0
+
+    # From STATUS (9)
+    rpm: int = 0
+    current_a: float = 0.0      # phase current (A)
+    duty_cycle: float = 0.0     # -1.0 .. +1.0
+
+    # From STATUS_4 (16)
+    temp_fet_c: float = 0.0
+    temp_motor_c: float = 0.0
+    current_in_a: float = 0.0   # battery/input current (A)
+
+    # From STATUS_5 (27)
+    voltage_v: float = 0.0
+
+    # Fault (embedded in STATUS per FW 6.6 — lower byte of duty word)
+    fault_code: int = 0
+
+    # Timestamps (monotonic seconds)
+    last_status_ts: float = 0.0
+    last_status5_ts: float = 0.0
+
+    ALIVE_TIMEOUT_S: float = 1.0  # mark offline if no STATUS frame for this long
+
+    @property
+    def fault_name(self) -> str:
+        return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}")
+
+    @property
+    def is_alive(self) -> bool:
+        return (time.monotonic() - self.last_status_ts) < self.ALIVE_TIMEOUT_S
+
+    @property
+    def has_fault(self) -> bool:
+        return self.fault_code != FAULT_CODE_NONE
+
+
+# ---------------------------------------------------------------------------
+# Frame parsers
+# ---------------------------------------------------------------------------
+
+def parse_status(data: bytes) -> dict | None:
+    """Parse CAN_PACKET_STATUS (9): RPM, phase current, duty cycle.
+
+    Wire format (8 bytes, all big-endian):
+      bytes [0:4]  RPM          : int32
+      bytes [4:6]  current×10   : int16  → ÷10 = A
+      bytes [6:8]  duty×1000    : int16  → ÷1000 = fraction (-1..+1)
+    """
+    if len(data) < 8:
+        return None
+    rpm, current_x10, duty_x1000 = struct.unpack_from(">ihh", data, 0)
+    return {
+        "rpm":        rpm,
+        "current_a":  current_x10 / 10.0,
+        "duty_cycle": duty_x1000 / 1000.0,
+    }
+
+
+def parse_status_4(data: bytes) -> dict | None:
+    """Parse CAN_PACKET_STATUS_4 (16): temperatures and input current.
+
+    Wire format (8 bytes, all big-endian):
+      bytes [0:2]  temp_fet×10    : int16  → ÷10 = °C
+      bytes [2:4]  temp_motor×10  : int16  → ÷10 = °C
+      bytes [4:6]  current_in×10  : int16  → ÷10 = A
+      bytes [6:8]  pid_pos×50     : int16  (not used here)
+    """
+    if len(data) < 8:
+        return None
+    tf_x10, tm_x10, ci_x10, _pid = struct.unpack_from(">hhhh", data, 0)
+    return {
+        "temp_fet_c":   tf_x10 / 10.0,
+        "temp_motor_c": tm_x10 / 10.0,
+        "current_in_a": ci_x10 / 10.0,
+    }
+
+
+def parse_status_5(data: bytes) -> dict | None:
+    """Parse CAN_PACKET_STATUS_5 (27): input voltage (and tachometer).
+
+    Wire format (8 bytes, all big-endian):
+      bytes [0:4]  tachometer  : int32  (unused here)
+      bytes [4:6]  v_in×10     : int16  → ÷10 = V
+      bytes [6:8]  reserved
+    """
+    if len(data) < 6:
+        return None
+    _tacho, v_in_x10 = struct.unpack_from(">ih", data, 0)
+    return {
+        "voltage_v": v_in_x10 / 10.0,
+    }
+
+
+# ---------------------------------------------------------------------------
+# Request builder
+# ---------------------------------------------------------------------------
+
+def make_get_values_request(sender_id: int, target_id: int) -> tuple[int, bytes]:
+    """Build a CAN_PACKET_PROCESS_SHORT_BUFFER GET_VALUES request frame.
+
+    The VESC responds by refreshing its STATUS broadcast cadence.
+
+    Args:
+        sender_id: This node's CAN ID (0-127; use 127 for a host controller).
+        target_id: CAN ID of the target VESC.
+
+    Returns:
+        (arbitration_id, data) — pass directly to can.Message().
+    """
+    arb_id = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | (target_id & 0xFF)
+    payload = bytes([sender_id & 0xFF, 0x00, COMM_GET_VALUES])
+    return arb_id, payload

jetson/ros2_ws/src/saltybot_vesc_telemetry/saltybot_vesc_telemetry/vesc_telemetry_node.py

@@ -0,0 +1,367 @@
+#!/usr/bin/env python3
+"""VESC CAN telemetry node for SaltyBot dual FSESC 6.7 Pro (FW 6.6).
+
+Listens on SocketCAN (can0) for VESC status broadcast frames from two VESCs
+and publishes parsed telemetry to ROS2 topics. Also sends periodic GET_VALUES
+requests to trigger fresh STATUS broadcasts.
+
+Published topics
+----------------
+  /vesc/left/state   (std_msgs/String) — JSON telemetry for left VESC
+  /vesc/right/state  (std_msgs/String) — JSON telemetry for right VESC
+  /vesc/combined     (std_msgs/String) — voltage, total current, both RPMs
+  /diagnostics       (diagnostic_msgs/DiagnosticArray)
+
+Parameters
+----------
+  can_interface              str   'can0'   SocketCAN interface
+  left_can_id                int   56       CAN ID of left VESC
+  right_can_id               int   68       CAN ID of right VESC
+  poll_rate_hz               int   20       Publish + request rate (10-50 Hz)
+  sender_can_id              int   127      This node's CAN ID
+  overcurrent_threshold_a    float 60.0     Fault alert threshold (A)
+  overtemp_fet_threshold_c   float 80.0     FET overtemp alert threshold (°C)
+  overtemp_motor_threshold_c float 100.0    Motor overtemp alert threshold (°C)
+"""
+
+import json
+import threading
+import time
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+try:
+    import can
+    _CAN_AVAILABLE = True
+except ImportError:
+    _CAN_AVAILABLE = False
+
+from .vesc_can_protocol import (
+    CAN_PACKET_STATUS,
+    CAN_PACKET_STATUS_4,
+    CAN_PACKET_STATUS_5,
+    VescState,
+    make_get_values_request,
+    parse_status,
+    parse_status_4,
+    parse_status_5,
+)
+
+
+class VescTelemetryNode(Node):
+    """ROS2 node: VESC CAN telemetry via SocketCAN."""
+
+    def __init__(self):
+        super().__init__("vesc_telemetry")
+
+        # ----------------------------------------------------------------
+        # Parameters
+        # ----------------------------------------------------------------
+        self.declare_parameter("can_interface",              "can0")
+        self.declare_parameter("left_can_id",                56)
+        self.declare_parameter("right_can_id",               68)
+        self.declare_parameter("poll_rate_hz",               20)
+        self.declare_parameter("sender_can_id",              127)
+        self.declare_parameter("overcurrent_threshold_a",    60.0)
+        self.declare_parameter("overtemp_fet_threshold_c",   80.0)
+        self.declare_parameter("overtemp_motor_threshold_c", 100.0)
+
+        self._iface      = self.get_parameter("can_interface").value
+        self._left_id    = self.get_parameter("left_can_id").value
+        self._right_id   = self.get_parameter("right_can_id").value
+        hz               = int(max(10, min(50, self.get_parameter("poll_rate_hz").value)))
+        self._sender_id  = self.get_parameter("sender_can_id").value
+        self._oc_thresh  = self.get_parameter("overcurrent_threshold_a").value
+        self._otf_thresh = self.get_parameter("overtemp_fet_threshold_c").value
+        self._otm_thresh = self.get_parameter("overtemp_motor_threshold_c").value
+
+        # ----------------------------------------------------------------
+        # Per-VESC state (protected by _state_lock)
+        # ----------------------------------------------------------------
+        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()
+
+        # Track which faults we have already logged to avoid log spam
+        self._fault_logged: dict[int, int] = {
+            self._left_id: 0, self._right_id: 0
+        }
+
+        # ----------------------------------------------------------------
+        # Publishers
+        # ----------------------------------------------------------------
+        self._pub_left     = self.create_publisher(String,        "/vesc/left/state",  10)
+        self._pub_right    = self.create_publisher(String,        "/vesc/right/state", 10)
+        self._pub_combined = self.create_publisher(String,        "/vesc/combined",    10)
+        self._pub_diag     = self.create_publisher(DiagnosticArray, "/diagnostics",    10)
+
+        # ----------------------------------------------------------------
+        # SocketCAN
+        # ----------------------------------------------------------------
+        self._bus: Optional["can.BusABC"] = None
+        self._rx_thread: Optional[threading.Thread] = None
+        self._running = False
+        self._init_can()
+
+        # ----------------------------------------------------------------
+        # Publish + poll timer
+        # ----------------------------------------------------------------
+        self.create_timer(1.0 / hz, self._on_timer)
+
+        self.get_logger().info(
+            f"vesc_telemetry: iface={self._iface}, "
+            f"left_id={self._left_id}, right_id={self._right_id}, "
+            f"poll={hz} Hz, sender_id={self._sender_id}"
+        )
+
+    # ----------------------------------------------------------------
+    # CAN initialisation
+    # ----------------------------------------------------------------
+
+    def _init_can(self) -> None:
+        if not _CAN_AVAILABLE:
+            self.get_logger().error(
+                "python-can not installed — run: pip install python-can"
+            )
+            return
+        try:
+            self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
+            self._running = True
+            self._rx_thread = threading.Thread(
+                target=self._rx_loop, name="vesc_can_rx", daemon=True
+            )
+            self._rx_thread.start()
+            self.get_logger().info(f"SocketCAN opened: {self._iface}")
+        except Exception as exc:
+            self.get_logger().error(f"Failed to open {self._iface}: {exc}")
+
+    # ----------------------------------------------------------------
+    # Receive loop (background thread)
+    # ----------------------------------------------------------------
+
+    def _rx_loop(self) -> None:
+        """Drain SocketCAN frames and update VESC state."""
+        while self._running and self._bus is not None:
+            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:
+        """Decode extended CAN ID and update the matching VescState."""
+        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:
+                parsed = parse_status(data)
+                if parsed:
+                    s.rpm             = parsed["rpm"]
+                    s.current_a       = parsed["current_a"]
+                    s.duty_cycle      = parsed["duty_cycle"]
+                    s.last_status_ts  = now
+
+            elif packet_type == CAN_PACKET_STATUS_4:
+                parsed = parse_status_4(data)
+                if parsed:
+                    s.temp_fet_c   = parsed["temp_fet_c"]
+                    s.temp_motor_c = parsed["temp_motor_c"]
+                    s.current_in_a = parsed["current_in_a"]
+
+            elif packet_type == CAN_PACKET_STATUS_5:
+                parsed = parse_status_5(data)
+                if parsed:
+                    s.voltage_v        = parsed["voltage_v"]
+                    s.last_status5_ts  = now
+
+    # ----------------------------------------------------------------
+    # Timer: poll + publish
+    # ----------------------------------------------------------------
+
+    def _on_timer(self) -> None:
+        self._send_poll_requests()
+        self._publish_all()
+
+    def _send_poll_requests(self) -> None:
+        """Send GET_VALUES to both VESCs to prompt STATUS broadcast refresh."""
+        if self._bus is None:
+            return
+        for vesc_id in (self._left_id, self._right_id):
+            try:
+                arb_id, payload = make_get_values_request(self._sender_id, vesc_id)
+                self._bus.send(can.Message(
+                    arbitration_id=arb_id,
+                    data=payload,
+                    is_extended_id=True,
+                ))
+            except Exception as exc:
+                self.get_logger().debug(
+                    f"GET_VALUES send failed (vesc_id={vesc_id}): {exc}"
+                )
+
+    def _publish_all(self) -> None:
+        with self._state_lock:
+            left  = self._state[self._left_id]
+            right = self._state[self._right_id]
+            # Snapshot dicts while holding lock
+            l_dict = self._state_to_dict(left)
+            r_dict = self._state_to_dict(right)
+            left_alive  = left.is_alive
+            right_alive = right.is_alive
+            voltage_v   = left.voltage_v if left_alive else right.voltage_v
+            combined = {
+                "voltage_v":       round(voltage_v, 2),
+                "total_current_a": round(left.current_in_a + right.current_in_a, 2),
+                "left_rpm":        left.rpm,
+                "right_rpm":       right.rpm,
+                "left_alive":      left_alive,
+                "right_alive":     right_alive,
+                "stamp":           time.time(),
+            }
+            # Build diagnostics while holding lock so values are consistent
+            diag_array = self._build_diagnostics(left, right)
+
+        self._pub_left.publish(String(data=json.dumps(l_dict)))
+        self._pub_right.publish(String(data=json.dumps(r_dict)))
+        self._pub_combined.publish(String(data=json.dumps(combined)))
+        self._pub_diag.publish(diag_array)
+
+        # Fault alerting (outside lock — uses snapshots)
+        self._check_faults_from_dict(l_dict, self._left_id,  "left")
+        self._check_faults_from_dict(r_dict, self._right_id, "right")
+
+    @staticmethod
+    def _state_to_dict(s: VescState) -> dict:
+        return {
+            "can_id":       s.can_id,
+            "rpm":          s.rpm,
+            "current_a":    round(s.current_a,    2),
+            "current_in_a": round(s.current_in_a, 2),
+            "duty_cycle":   round(s.duty_cycle,   4),
+            "voltage_v":    round(s.voltage_v,    2),
+            "temp_fet_c":   round(s.temp_fet_c,   1),
+            "temp_motor_c": round(s.temp_motor_c, 1),
+            "fault_code":   s.fault_code,
+            "fault_name":   s.fault_name,
+            "alive":        s.is_alive,
+            "stamp":        time.time(),
+        }
+
+    def _build_diagnostics(
+        self, left: VescState, right: VescState
+    ) -> DiagnosticArray:
+        array = DiagnosticArray()
+        array.header.stamp = self.get_clock().now().to_msg()
+
+        for s, label in ((left, "left"), (right, "right")):
+            status = DiagnosticStatus()
+            status.name        = f"VESC/{label} (CAN ID {s.can_id})"
+            status.hardware_id = f"vesc_can_{s.can_id}"
+
+            if not s.is_alive:
+                status.level   = DiagnosticStatus.ERROR
+                status.message = "No CAN frames received (offline)"
+            elif s.has_fault:
+                status.level   = DiagnosticStatus.ERROR
+                status.message = f"Fault: {s.fault_name}"
+            elif (
+                s.temp_fet_c   > self._otf_thresh
+                or s.temp_motor_c > self._otm_thresh
+                or abs(s.current_a) > self._oc_thresh
+            ):
+                status.level   = DiagnosticStatus.WARN
+                status.message = "Threshold exceeded"
+            else:
+                status.level   = DiagnosticStatus.OK
+                status.message = "OK"
+
+            status.values = [
+                KeyValue(key="rpm",          value=str(s.rpm)),
+                KeyValue(key="current_a",    value=f"{s.current_a:.2f}"),
+                KeyValue(key="current_in_a", value=f"{s.current_in_a:.2f}"),
+                KeyValue(key="duty_cycle",   value=f"{s.duty_cycle:.4f}"),
+                KeyValue(key="voltage_v",    value=f"{s.voltage_v:.2f}"),
+                KeyValue(key="temp_fet_c",   value=f"{s.temp_fet_c:.1f}"),
+                KeyValue(key="temp_motor_c", value=f"{s.temp_motor_c:.1f}"),
+                KeyValue(key="fault_code",   value=str(s.fault_code)),
+                KeyValue(key="fault_name",   value=s.fault_name),
+            ]
+            array.status.append(status)
+
+        return array
+
+    def _check_faults_from_dict(self, d: dict, vesc_id: int, label: str) -> None:
+        """Log fault/threshold warnings; suppress repeated identical messages."""
+        if not d["alive"]:
+            return
+
+        if abs(d["current_a"]) > self._oc_thresh:
+            self.get_logger().warn(
+                f"VESC {label}: overcurrent {d['current_a']:.1f} A "
+                f"(threshold {self._oc_thresh:.0f} A)"
+            )
+
+        if d["temp_fet_c"] > self._otf_thresh:
+            self.get_logger().warn(
+                f"VESC {label}: FET overtemp {d['temp_fet_c']:.1f} °C "
+                f"(threshold {self._otf_thresh:.0f} °C)"
+            )
+
+        if d["temp_motor_c"] > self._otm_thresh:
+            self.get_logger().warn(
+                f"VESC {label}: motor overtemp {d['temp_motor_c']:.1f} °C "
+                f"(threshold {self._otm_thresh:.0f} °C)"
+            )
+
+        fault_code = d["fault_code"]
+        if fault_code != 0 and self._fault_logged.get(vesc_id) != fault_code:
+            self.get_logger().error(
+                f"VESC {label}: fault {fault_code} ({d['fault_name']})"
+            )
+            self._fault_logged[vesc_id] = fault_code
+        elif fault_code == 0 and self._fault_logged.get(vesc_id, 0) != 0:
+            self.get_logger().info(f"VESC {label}: fault cleared")
+            self._fault_logged[vesc_id] = 0
+
+    # ----------------------------------------------------------------
+    # Cleanup
+    # ----------------------------------------------------------------
+
+    def destroy_node(self) -> None:
+        self._running = False
+        if self._rx_thread is not None:
+            self._rx_thread.join(timeout=1.0)
+        if self._bus is not None:
+            self._bus.shutdown()
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VescTelemetryNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_vesc_telemetry/setup.cfg

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

jetson/ros2_ws/src/saltybot_vesc_telemetry/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = "saltybot_vesc_telemetry"
+
+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}/launch", ["launch/vesc_telemetry.launch.py"]),
+        (f"share/{package_name}/config", ["config/vesc_telemetry_params.yaml"]),
+    ],
+    install_requires=["setuptools", "python-can"],
+    zip_safe=True,
+    maintainer="sl-firmware",
+    maintainer_email="sl-firmware@saltylab.local",
+    description="VESC CAN telemetry node for dual FSESC 6.7 Pro",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "vesc_telemetry_node = saltybot_vesc_telemetry.vesc_telemetry_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_vesc_telemetry/test/test_vesc_telemetry.py

@@ -0,0 +1,240 @@
+"""Unit tests for VESC CAN telemetry — protocol parsing and state logic.
+
+These tests cover pure functions in vesc_can_protocol and do not require
+ROS2, python-can, or a live CAN bus.
+"""
+
+import struct
+import time
+import pytest
+
+from saltybot_vesc_telemetry.vesc_can_protocol import (
+    CAN_PACKET_STATUS,
+    CAN_PACKET_STATUS_4,
+    CAN_PACKET_STATUS_5,
+    CAN_PACKET_PROCESS_SHORT_BUFFER,
+    COMM_GET_VALUES,
+    FAULT_CODE_NONE,
+    FAULT_CODE_ABS_OVER_CURRENT,
+    FAULT_CODE_OVER_TEMP_FET,
+    FAULT_CODE_OVER_TEMP_MOTOR,
+    FAULT_NAMES,
+    VescState,
+    make_get_values_request,
+    parse_status,
+    parse_status_4,
+    parse_status_5,
+)
+
+
+# ---------------------------------------------------------------------------
+# parse_status (CAN_PACKET_STATUS = 9)
+# ---------------------------------------------------------------------------
+
+class TestParseStatus:
+    def test_forward_rpm(self):
+        # 1000 RPM, 5.0 A, 0.300 duty
+        data = struct.pack(">ihh", 1000, 50, 300)
+        r = parse_status(data)
+        assert r is not None
+        assert r["rpm"]        == 1000
+        assert r["current_a"]  == pytest.approx(5.0)
+        assert r["duty_cycle"] == pytest.approx(0.300)
+
+    def test_reverse_rpm(self):
+        data = struct.pack(">ihh", -3000, -80, -500)
+        r = parse_status(data)
+        assert r["rpm"]        == -3000
+        assert r["current_a"]  == pytest.approx(-8.0)
+        assert r["duty_cycle"] == pytest.approx(-0.5)
+
+    def test_zero(self):
+        data = struct.pack(">ihh", 0, 0, 0)
+        r = parse_status(data)
+        assert r["rpm"]        == 0
+        assert r["current_a"]  == pytest.approx(0.0)
+        assert r["duty_cycle"] == pytest.approx(0.0)
+
+    def test_max_duty(self):
+        # duty 1.0 → encoded as 1000
+        data = struct.pack(">ihh", 50000, 200, 1000)
+        r = parse_status(data)
+        assert r["duty_cycle"] == pytest.approx(1.0)
+
+    def test_too_short_returns_none(self):
+        assert parse_status(b"\x00\x01\x02") is None
+
+    def test_exact_minimum_length(self):
+        data = struct.pack(">ihh", 42, 10, 100)
+        assert parse_status(data) is not None
+
+
+# ---------------------------------------------------------------------------
+# parse_status_4 (CAN_PACKET_STATUS_4 = 16)
+# ---------------------------------------------------------------------------
+
+class TestParseStatus4:
+    def _make(self, temp_fet, temp_motor, current_in, pid_pos=0):
+        return struct.pack(
+            ">hhhh",
+            int(temp_fet * 10),
+            int(temp_motor * 10),
+            int(current_in * 10),
+            pid_pos,
+        )
+
+    def test_normal_temps(self):
+        data = self._make(45.5, 62.3, 12.0)
+        r = parse_status_4(data)
+        assert r is not None
+        assert r["temp_fet_c"]   == pytest.approx(45.5)
+        assert r["temp_motor_c"] == pytest.approx(62.3)
+        assert r["current_in_a"] == pytest.approx(12.0)
+
+    def test_negative_current_in(self):
+        data = self._make(30.0, 40.0, -5.0)
+        r = parse_status_4(data)
+        assert r["current_in_a"] == pytest.approx(-5.0)
+
+    def test_too_short_returns_none(self):
+        assert parse_status_4(b"\x00" * 7) is None
+
+    def test_zero_values(self):
+        data = self._make(0, 0, 0)
+        r = parse_status_4(data)
+        assert r["temp_fet_c"]   == pytest.approx(0.0)
+        assert r["temp_motor_c"] == pytest.approx(0.0)
+        assert r["current_in_a"] == pytest.approx(0.0)
+
+
+# ---------------------------------------------------------------------------
+# parse_status_5 (CAN_PACKET_STATUS_5 = 27)
+# ---------------------------------------------------------------------------
+
+class TestParseStatus5:
+    def _make(self, voltage_v, tacho=0):
+        return struct.pack(">ih", tacho, int(voltage_v * 10))
+
+    def test_nominal_voltage(self):
+        data = self._make(25.2)
+        r = parse_status_5(data)
+        assert r is not None
+        assert r["voltage_v"] == pytest.approx(25.2)
+
+    def test_low_voltage(self):
+        data = self._make(18.6)
+        r = parse_status_5(data)
+        assert r["voltage_v"] == pytest.approx(18.6)
+
+    def test_too_short_returns_none(self):
+        assert parse_status_5(b"\x00\x01\x02\x03\x04") is None
+
+    def test_with_tachometer(self):
+        data = struct.pack(">ih", 123456, 252)
+        r = parse_status_5(data)
+        assert r["voltage_v"] == pytest.approx(25.2)
+
+
+# ---------------------------------------------------------------------------
+# make_get_values_request
+# ---------------------------------------------------------------------------
+
+class TestMakeGetValuesRequest:
+    def test_arb_id_encodes_packet_type_and_target(self):
+        arb_id, payload = make_get_values_request(sender_id=127, target_id=56)
+        expected_arb = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | 56
+        assert arb_id == expected_arb
+
+    def test_payload_contains_sender_command(self):
+        _, payload = make_get_values_request(sender_id=127, target_id=56)
+        assert len(payload) == 3
+        assert payload[0] == 127          # sender_id
+        assert payload[1] == 0x00         # send_mode
+        assert payload[2] == COMM_GET_VALUES
+
+    def test_right_vesc(self):
+        arb_id, payload = make_get_values_request(sender_id=127, target_id=68)
+        assert (arb_id & 0xFF) == 68
+        assert payload[2] == COMM_GET_VALUES
+
+    def test_sender_id_in_payload(self):
+        _, payload = make_get_values_request(sender_id=42, target_id=56)
+        assert payload[0] == 42
+
+    def test_arb_id_is_extended(self):
+        # Extended IDs can exceed 0x7FF (11-bit limit)
+        arb_id, _ = make_get_values_request(127, 56)
+        assert arb_id > 0x7FF
+
+
+# ---------------------------------------------------------------------------
+# VescState properties
+# ---------------------------------------------------------------------------
+
+class TestVescState:
+    def test_defaults(self):
+        s = VescState(can_id=56)
+        assert s.rpm           == 0
+        assert s.current_a     == pytest.approx(0.0)
+        assert s.voltage_v     == pytest.approx(0.0)
+        assert s.fault_code    == FAULT_CODE_NONE
+        assert s.has_fault     is False
+
+    def test_is_alive_fresh(self):
+        s = VescState(can_id=56)
+        s.last_status_ts = time.monotonic()
+        assert s.is_alive is True
+
+    def test_is_alive_stale(self):
+        s = VescState(can_id=56)
+        s.last_status_ts = time.monotonic() - 2.0  # 2 s ago > 1 s timeout
+        assert s.is_alive is False
+
+    def test_is_alive_never_received(self):
+        s = VescState(can_id=56)
+        # last_status_ts defaults to 0.0 — monotonic() will be >> 1 s
+        assert s.is_alive is False
+
+    def test_has_fault_true(self):
+        s = VescState(can_id=56)
+        s.fault_code = FAULT_CODE_ABS_OVER_CURRENT
+        assert s.has_fault is True
+
+    def test_has_fault_false_on_none(self):
+        s = VescState(can_id=56)
+        s.fault_code = FAULT_CODE_NONE
+        assert s.has_fault is False
+
+    def test_fault_name_known(self):
+        s = VescState(can_id=56)
+        s.fault_code = FAULT_CODE_OVER_TEMP_FET
+        assert s.fault_name == "OVER_TEMP_FET"
+
+    def test_fault_name_unknown(self):
+        s = VescState(can_id=56)
+        s.fault_code = 99
+        assert "UNKNOWN" in s.fault_name
+
+    def test_fault_name_none(self):
+        s = VescState(can_id=56)
+        assert s.fault_name == "NONE"
+
+
+# ---------------------------------------------------------------------------
+# FAULT_NAMES completeness
+# ---------------------------------------------------------------------------
+
+class TestFaultNames:
+    def test_all_common_codes_named(self):
+        for code in (0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10):
+            assert code in FAULT_NAMES, f"fault code {code} missing from FAULT_NAMES"
+
+    def test_none_is_zero(self):
+        assert FAULT_NAMES[0] == "NONE"
+
+    def test_overcurrent_code(self):
+        assert FAULT_NAMES[FAULT_CODE_ABS_OVER_CURRENT] == "ABS_OVER_CURRENT"
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

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
+ * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
- * Filter bank 14 (first CAN2 bank; SlaveStartFilterBank=14)
+ * 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
+    /* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
-     * FilterIdHigh   = 0x200 << 5 = 0x4000  (base ID shifted to bit[15:5])
+     * CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0
-     * FilterMaskHigh = 0x7E0 << 5 = 0xFC00  (mask: top 6 bits must match) */
+     * → 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;
+        if (s_ext_cb != NULL) {
-
+            s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
-        /* Layout: [0-1] vel_rpm  [2-3] current_ma  [4-5] pos_x100  [6] temp_c  [7] fault */
+        }
-        s_feedback[nid].velocity_rpm  = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
-        s_feedback[nid].current_ma    = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
-        s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
-        s_feedback[nid].temperature_c = (int8_t)rxdata[6];
-        s_feedback[nid].fault         = rxdata[7];
-        s_feedback[nid].last_rx_ms    = HAL_GetTick();
 
         s_stats.rx_count++;
     }
 }
 
+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) */
+        /* 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.
-            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.
          * 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 = 0;
-            int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
+            int32_t steer_rpm = 0;
-            int32_t steer_rpm = (int32_t)can_steer    * CAN_RPM_SCALE / 2;
+            if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
-            can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 };
+                int16_t can_steer = mode_manager_get_steer(&mode);
-            can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 };
+                speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
-            if (bal.state != BALANCE_ARMED) {
+                steer_rpm = (int32_t)can_steer     * CAN_RPM_SCALE / 2;
-                cmd_l.velocity_rpm = 0;
-                cmd_r.velocity_rpm = 0;
             }
-            can_driver_send_cmd(CAN_NODE_LEFT,  &cmd_l);
+            vesc_can_send_rpm(VESC_CAN_ID_LEFT,  speed_rpm + steer_rpm);
-            can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r);
+            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;
+}