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

- can_driver: add filter bank 15 (all ext IDs → FIFO1) and widen bank 14 to accept all standard IDs; add can_driver_send_ext/std and ext/std frame callbacks (can_driver_set_ext_cb / can_driver_set_std_cb) - vesc_can: VESC 29-bit extended CAN protocol driver — send RPM to IDs 56 and 68 (FSESC 6.7 Pro Mini Dual), parse STATUS/STATUS_4/STATUS_5 big-endian payloads, alive timeout, JLINK_TLM_VESC_STATE at 1 Hz - orin_can: Orin↔FC standard CAN protocol — HEARTBEAT/DRIVE/MODE/ESTOP commands in, FC_STATUS + FC_VESC broadcast at 10 Hz - jlink: add JLINK_TLM_VESC_STATE (0x8E), jlink_tlm_vesc_state_t (22 bytes), jlink_send_vesc_state_tlm() - main: wire vesc_can_init/orin_can_init; replace can_driver_send_cmd with vesc_can_send_rpm; inject Orin CAN speed/steer into balance PID; add Orin CAN estop/clear handling; add orin_can_broadcast at 10 Hz - test: 56-test host-side suite for vesc_can; test/stubs/stm32f7xx_hal.h minimal HAL stub for all future host-side tests Safety: balance PID runs independently on Mamba — if Orin CAN link drops (orin_can_is_alive() == false) the robot continues balancing in-place. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
feat: Orin CANable 2.0 bridge for Mamba and VESC CAN bus (Issue #674 )
2026-03-17 19:10:11 -04:00 · 2026-03-17 18:54:20 -04:00
22 changed files with 2393 additions and 47 deletions
--- a/include/can_driver.h
+++ b/include/can_driver.h
@ -78,4 +78,24 @@ void can_driver_get_stats(can_stats_t *out);
 /* Drain RX FIFO0; call every main-loop tick */
 void can_driver_process(void);

+/* ---- Extended / standard frame support (Issue #674) ---- */
+
+/* Callback for extended-ID (29-bit) frames arriving in FIFO1 (VESC STATUS) */
+typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len);
+
+/* Callback for standard-ID (11-bit) frames arriving in FIFO0 (Orin commands) */
+typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len);
+
+/* Register callback for 29-bit extended frames (register before can_driver_init) */
+void can_driver_set_ext_cb(can_ext_frame_cb_t cb);
+
+/* Register callback for 11-bit standard frames (register before can_driver_init) */
+void can_driver_set_std_cb(can_std_frame_cb_t cb);
+
+/* Transmit a 29-bit extended-ID data frame (VESC RPM/current commands) */
+void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len);
+
+/* Transmit an 11-bit standard-ID data frame (Orin telemetry broadcast) */
+void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len);
+
 #endif /* CAN_DRIVER_H */
--- a/include/jlink.h
+++ b/include/jlink.h
@ -99,6 +99,7 @@
 #define JLINK_TLM_STEERING      0x8Au  /* jlink_tlm_steering_t (8 bytes, Issue #616) */
 #define JLINK_TLM_LVC           0x8Bu  /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
 #define JLINK_TLM_ODOM          0x8Cu  /* jlink_tlm_odom_t (16 bytes, Issue #632) */
+#define JLINK_TLM_VESC_STATE    0x8Eu  /* jlink_tlm_vesc_state_t (22 bytes, Issue #674) */

 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
@ -239,6 +240,22 @@ typedef struct __attribute__((packed)) {
    int16_t  speed_mmps;        /* linear speed of centre point (mm/s)        */
 } jlink_tlm_odom_t;  /* 16 bytes */

+/* ---- Telemetry VESC_STATE payload (22 bytes, packed) Issue #674 ---- */
+/* Sent at VESC_TLM_HZ (1 Hz) by vesc_can_send_tlm(). */
+typedef struct __attribute__((packed)) {
+    int32_t  left_rpm;           /* left  VESC actual RPM                       */
+    int32_t  right_rpm;          /* right VESC actual RPM                       */
+    int16_t  left_current_x10;   /* left  phase current (A × 10)                */
+    int16_t  right_current_x10;  /* right phase current (A × 10)                */
+    int16_t  left_temp_x10;      /* left  FET temperature (°C × 10)             */
+    int16_t  right_temp_x10;     /* right FET temperature (°C × 10)             */
+    int16_t  voltage_x10;        /* input voltage (V × 10; from STATUS_5)       */
+    uint8_t  left_fault;         /* left  VESC fault code (0 = none)            */
+    uint8_t  right_fault;        /* right VESC fault code (0 = none)            */
+    uint8_t  left_alive;         /* 1 = left  VESC alive (STATUS within 1 s)    */
+    uint8_t  right_alive;        /* 1 = right VESC alive (STATUS within 1 s)    */
+} jlink_tlm_vesc_state_t;  /* 22 bytes */
+
 /* ---- Volatile state (read from main loop) ---- */
 typedef struct {
    /* Drive command - updated on JLINK_CMD_DRIVE */
@ -377,4 +394,11 @@ void jlink_send_lvc_tlm(const jlink_tlm_lvc_t *tlm);
 */
 void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm);

+/*
+ * jlink_send_vesc_state_tlm(tlm) - transmit JLINK_TLM_VESC_STATE (0x8E) frame
+ * (28 bytes total) at VESC_TLM_HZ (1 Hz).  Issue #674.
+ * Rate-limiting handled by vesc_can_send_tlm(); call from there only.
+ */
+void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm);
+
 #endif /* JLINK_H */
--- a/include/orin_can.h
+++ b/include/orin_can.h
@ -0,0 +1,103 @@
+#ifndef ORIN_CAN_H
+#define ORIN_CAN_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * orin_can — Orin↔FC CAN protocol driver (Issue #674).
+ *
+ * Standard 11-bit CAN IDs on CAN2, FIFO0.
+ *
+ * Orin → FC commands:
+ *   0x300 HEARTBEAT : uint32 sequence counter (4 bytes)
+ *   0x301 DRIVE     : int16 speed (−1000..+1000), int16 steer (−1000..+1000)
+ *   0x302 MODE      : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS)
+ *   0x303 ESTOP     : uint8 action (1=ESTOP, 0=CLEAR)
+ *
+ * FC → Orin telemetry (broadcast at ORIN_TLM_HZ):
+ *   0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
+ *                     uint8 balance_state, uint8 flags [bit0=estop, bit1=armed]
+ *   0x401 FC_VESC   : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10,
+ *                     int16 left_current_x10, int16 right_current_x10
+ *
+ * Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC
+ * continues balancing in-place — Orin commands are simply not injected.
+ * The balance PID loop runs entirely on Mamba and never depends on Orin.
+ */
+
+/* ---- Orin → FC command IDs ---- */
+#define ORIN_CAN_ID_HEARTBEAT   0x300u
+#define ORIN_CAN_ID_DRIVE       0x301u
+#define ORIN_CAN_ID_MODE        0x302u
+#define ORIN_CAN_ID_ESTOP       0x303u
+
+/* ---- FC → Orin telemetry IDs ---- */
+#define ORIN_CAN_ID_FC_STATUS   0x400u
+#define ORIN_CAN_ID_FC_VESC     0x401u
+
+/* ---- Timing ---- */
+#define ORIN_HB_TIMEOUT_MS      500u   /* Orin offline after 500 ms without any frame */
+#define ORIN_TLM_HZ             10u    /* FC → Orin broadcast rate (Hz) */
+
+/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */
+typedef struct {
+    volatile int16_t  speed;           /* DRIVE: −1000..+1000 */
+    volatile int16_t  steer;           /* DRIVE: −1000..+1000 */
+    volatile uint8_t  mode;            /* MODE:  robot_mode_t value */
+    volatile uint8_t  drive_updated;   /* set on DRIVE, cleared by main */
+    volatile uint8_t  mode_updated;    /* set on MODE,  cleared by main */
+    volatile uint8_t  estop_req;       /* set on ESTOP(1), cleared by main */
+    volatile uint8_t  estop_clear_req; /* set on ESTOP(0), cleared by main */
+    volatile uint32_t last_rx_ms;      /* HAL_GetTick() of last received frame */
+} OrinCanState;
+
+extern volatile OrinCanState orin_can_state;
+
+/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */
+
+typedef struct __attribute__((packed)) {
+    int16_t  pitch_x10;      /* pitch degrees × 10 */
+    int16_t  motor_cmd;      /* balance PID output −1000..+1000 */
+    uint16_t vbat_mv;        /* battery voltage (mV) */
+    uint8_t  balance_state;  /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */
+    uint8_t  flags;          /* bit0=estop_active, bit1=armed */
+} orin_can_fc_status_t;  /* 8 bytes */
+
+typedef struct __attribute__((packed)) {
+    int16_t left_rpm_x10;        /* left  wheel RPM / 10 (±32767 × 10 = ±327k RPM) */
+    int16_t right_rpm_x10;       /* right wheel RPM / 10 */
+    int16_t left_current_x10;    /* left  phase current × 10 (A) */
+    int16_t right_current_x10;   /* right phase current × 10 (A) */
+} orin_can_fc_vesc_t;  /* 8 bytes */
+
+/* ---- API ---- */
+
+/*
+ * orin_can_init() — zero state, register orin_can_on_frame as std_cb with
+ * can_driver.  Call after can_driver_init().
+ */
+void orin_can_init(void);
+
+/*
+ * orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each
+ * standard-ID frame in FIFO0.  Updates orin_can_state.
+ */
+void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len);
+
+/*
+ * orin_can_is_alive(now_ms) — true if a frame from Orin arrived within
+ * ORIN_HB_TIMEOUT_MS of now_ms.
+ */
+bool orin_can_is_alive(uint32_t now_ms);
+
+/*
+ * orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of
+ * FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz).
+ * Safe to call every ms; internally rate-limited.
+ */
+void orin_can_broadcast(uint32_t now_ms,
+                        const orin_can_fc_status_t *status,
+                        const orin_can_fc_vesc_t   *vesc);
+
+#endif /* ORIN_CAN_H */
--- a/include/vesc_can.h
+++ b/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 */
--- a/jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml
+++ b/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
--- a/jetson/ros2_ws/src/saltybot_can_bridge/package.xml
+++ b/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>
--- a/jetson/ros2_ws/src/saltybot_can_bridge/resource/saltybot_can_bridge
+++ b/jetson/ros2_ws/src/saltybot_can_bridge/resource/saltybot_can_bridge
--- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/init.py
+++ b/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."""
--- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
+++ b/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()
--- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py
+++ b/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)
--- a/jetson/ros2_ws/src/saltybot_can_bridge/setup.cfg
+++ b/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
--- a/jetson/ros2_ws/src/saltybot_can_bridge/setup.py
+++ b/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",
+        ],
+    },
+)
--- a/jetson/ros2_ws/src/saltybot_can_bridge/test/init.py
+++ b/jetson/ros2_ws/src/saltybot_can_bridge/test/init.py
--- a/jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py
+++ b/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()
--- a/jetson/scripts/setup_can.sh
+++ b/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
--- a/src/can_driver.c
+++ b/src/can_driver.c
@ -10,6 +10,8 @@
 static CAN_HandleTypeDef        s_can;
 static volatile can_feedback_t  s_feedback[CAN_NUM_MOTORS];
 static volatile can_stats_t     s_stats;
+static can_ext_frame_cb_t       s_ext_cb = NULL;
+static can_std_frame_cb_t       s_std_cb = NULL;

 void can_driver_init(void)
 {
@ -48,16 +50,17 @@ void can_driver_init(void)
        return;
    }

-    /* Filter bank 14: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F
-     * FilterIdHigh   = 0x200 << 5 = 0x4000  (base ID shifted to bit[15:5])
-     * FilterMaskHigh = 0x7E0 << 5 = 0xFC00  (mask: top 6 bits must match) */
+    /* Filter bank 14: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
+     * FilterIdHigh=0, FilterMaskHigh=0 → mask=0 passes every standard ID.
+     * FilterIdLow[2]=0 (IDE=0) → only standard frames; mask bit[2]=0 → don't
+     * check IDE bit, but standard frames have IDE=0 by design so all pass. */
    CAN_FilterTypeDef flt = {0};
    flt.FilterBank           = 14u;
    flt.FilterMode           = CAN_FILTERMODE_IDMASK;
    flt.FilterScale          = CAN_FILTERSCALE_32BIT;
-    flt.FilterIdHigh         = (uint16_t)(CAN_FILTER_STDID << 5u);
+    flt.FilterIdHigh         = 0u;
    flt.FilterIdLow          = 0u;
-    flt.FilterMaskIdHigh     = (uint16_t)(CAN_FILTER_MASK  << 5u);
+    flt.FilterMaskIdHigh     = 0u;
    flt.FilterMaskIdLow      = 0u;
    flt.FilterFIFOAssignment = CAN_RX_FIFO0;
    flt.FilterActivation     = CAN_FILTER_ENABLE;
@ -68,6 +71,28 @@ void can_driver_init(void)
        return;
    }

+    /* Filter bank 15: 32-bit mask, FIFO1, accept ALL extended 29-bit frames.
+     * For extended frames, the IDE bit sits at FilterIdLow[2].
+     * FilterIdLow  = 0x0004 (IDE=1) → match extended frames.
+     * FilterMaskIdLow = 0x0004 → only the IDE bit is checked; all ext IDs pass.
+     * This routes every VESC STATUS / STATUS_4 / STATUS_5 frame to FIFO1. */
+    CAN_FilterTypeDef flt2 = {0};
+    flt2.FilterBank           = 15u;
+    flt2.FilterMode           = CAN_FILTERMODE_IDMASK;
+    flt2.FilterScale          = CAN_FILTERSCALE_32BIT;
+    flt2.FilterIdHigh         = 0u;
+    flt2.FilterIdLow          = 0x0004u;  /* IDE = 1 */
+    flt2.FilterMaskIdHigh     = 0u;
+    flt2.FilterMaskIdLow      = 0x0004u;  /* only check IDE bit */
+    flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
+    flt2.FilterActivation     = CAN_FILTER_ENABLE;
+    flt2.SlaveStartFilterBank = 14u;
+
+    if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) {
+        s_stats.bus_off = 1u;
+        return;
+    }
+
    HAL_CAN_Start(&s_can);

    memset((void *)s_feedback, 0, sizeof(s_feedback));
@ -136,7 +161,7 @@ void can_driver_process(void)
    }
    s_stats.bus_off = 0u;

-    /* Drain FIFO0 */
+    /* Drain FIFO0 (standard-ID frames: Orin commands + legacy feedback) */
    while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
        CAN_RxHeaderTypeDef rxhdr;
        uint8_t rxdata[8];
@ -146,31 +171,106 @@ void can_driver_process(void)
            break;
        }

-        /* Only process data frames with standard IDs */
        if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
            continue;
        }

-        /* Decode feedback frames: base 0x200, one per node */
+        /* Dispatch to registered standard-frame callback (Orin CAN protocol) */
+        if (s_std_cb != NULL) {
+            s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
+        }
+
+        /* Legacy: decode feedback frames at 0x200-0x21F (Issue #597) */
        uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
-        if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) {
+        if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
+            uint8_t nid = (uint8_t)nid_u;
+            s_feedback[nid].velocity_rpm  = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
+            s_feedback[nid].current_ma    = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
+            s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
+            s_feedback[nid].temperature_c = (int8_t)rxdata[6];
+            s_feedback[nid].fault         = rxdata[7];
+            s_feedback[nid].last_rx_ms    = HAL_GetTick();
+        }
+
+        s_stats.rx_count++;
+    }
+
+    /* Drain FIFO1 (extended-ID frames: VESC STATUS/STATUS_4/STATUS_5) */
+    while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
+        CAN_RxHeaderTypeDef rxhdr;
+        uint8_t rxdata[8];
+
+        if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
+            s_stats.err_count++;
+            break;
+        }
+
+        if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) {
            continue;
        }

-        uint8_t nid = (uint8_t)nid_u;
-
-        /* Layout: [0-1] vel_rpm  [2-3] current_ma  [4-5] pos_x100  [6] temp_c  [7] fault */
-        s_feedback[nid].velocity_rpm  = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
-        s_feedback[nid].current_ma    = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
-        s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
-        s_feedback[nid].temperature_c = (int8_t)rxdata[6];
-        s_feedback[nid].fault         = rxdata[7];
-        s_feedback[nid].last_rx_ms    = HAL_GetTick();
+        if (s_ext_cb != NULL) {
+            s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
+        }

        s_stats.rx_count++;
    }
 }

+void can_driver_set_ext_cb(can_ext_frame_cb_t cb)
+{
+    s_ext_cb = cb;
+}
+
+void can_driver_set_std_cb(can_std_frame_cb_t cb)
+{
+    s_std_cb = cb;
+}
+
+void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
+{
+    if (s_stats.bus_off || len > 8u) {
+        return;
+    }
+
+    CAN_TxHeaderTypeDef hdr = {0};
+    hdr.ExtId = ext_id;
+    hdr.IDE   = CAN_ID_EXT;
+    hdr.RTR   = CAN_RTR_DATA;
+    hdr.DLC   = len;
+
+    uint32_t mailbox;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
+            s_stats.tx_count++;
+        } else {
+            s_stats.err_count++;
+        }
+    }
+}
+
+void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
+{
+    if (s_stats.bus_off || len > 8u) {
+        return;
+    }
+
+    CAN_TxHeaderTypeDef hdr = {0};
+    hdr.StdId = std_id;
+    hdr.IDE   = CAN_ID_STD;
+    hdr.RTR   = CAN_RTR_DATA;
+    hdr.DLC   = len;
+
+    uint32_t mailbox;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
+            s_stats.tx_count++;
+        } else {
+            s_stats.err_count++;
+        }
+    }
+}
+
 bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
 {
    if (node_id >= CAN_NUM_MOTORS || out == NULL) {
--- a/src/jlink.c
+++ b/src/jlink.c
@ -662,3 +662,28 @@ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm)

    jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_vesc_state_tlm() -- Issue #674 ---- */
+void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x8E][22 bytes VESC_STATE][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 22 (payload) = 23; total frame = 28 bytes.
+     * At 921600 baud: 28×10/921600 ≈ 0.30 ms — safe to block.
+     */
+    static uint8_t frame[28];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_vesc_state_t);  /* 22 */
+    const uint8_t  len  = 1u + plen;                                 /* 23 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_VESC_STATE;
+    memcpy(&frame[3], tlm, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, sizeof(frame));
+}
--- a/src/main.c
+++ b/src/main.c
@ -33,6 +33,8 @@
 #include "pid_flash.h"
 #include "fault_handler.h"
 #include "can_driver.h"
+#include "vesc_can.h"
+#include "orin_can.h"
 #include "servo_bus.h"
 #include "gimbal.h"
 #include "lvc.h"
@ -195,8 +197,14 @@ int main(void) {
    /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
    jlink_init();

-    /* Init CAN2 BLDC motor controller bus — PB12/PB13, 500 kbps (Issue #597) */
+    /* Init CAN2 bus — PB12/PB13, 500 kbps.
+     * Register VESC and Orin CAN callbacks BEFORE can_driver_init() so filter
+     * banks are configured with both callbacks ready. (Issue #674) */
+    can_driver_set_ext_cb(vesc_can_on_frame);   /* VESC STATUS → FIFO1 */
+    can_driver_set_std_cb(orin_can_on_frame);   /* Orin cmds  → FIFO0 */
    can_driver_init();
+    vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT);
+    orin_can_init();

    /* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
    if (fault_get_last_type() != FAULT_NONE) {
@ -469,6 +477,23 @@ int main(void) {
            }
        }

+        /* Orin CAN: handle commands from Orin over CAN bus (Issue #674).
+         * Estop and clear are latching; drive/mode are consumed each tick.
+         * Balance independence: if orin_can_is_alive() == false the loop
+         * simply does not inject any commands and holds upright in-place. */
+        if (orin_can_state.estop_req) {
+            orin_can_state.estop_req = 0u;
+            safety_remote_estop(ESTOP_REMOTE);
+            safety_arm_cancel();
+            balance_disarm(&bal);
+            motor_driver_estop(&motors);
+        }
+        if (orin_can_state.estop_clear_req) {
+            orin_can_state.estop_clear_req = 0u;
+            if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
+                safety_remote_estop_clear();
+        }
+
        /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
        if (jlink_state.fault_log_req) {
            jlink_state.fault_log_req = 0u;
@ -626,6 +651,8 @@ int main(void) {
            float base_sp = bal.setpoint;
            if (jlink_is_active(now))
                bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
+            else if (orin_can_is_alive(now))
+                bal.setpoint += ((float)orin_can_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
            else if (jetson_cmd_is_active(now))
                bal.setpoint += jetson_cmd_sp_offset();
            balance_update(&bal, &imu, dt);
@ -659,6 +686,8 @@ int main(void) {
         * mode_manager_get_steer() blends it with RC steer per active mode. */
        if (jlink_is_active(now))
            mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
+        else if (orin_can_is_alive(now))
+            mode_manager_set_auto_cmd(&mode, orin_can_state.steer, 0);
        else if (jetson_cmd_is_active(now))
            mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);

@ -687,38 +716,47 @@ int main(void) {
            }
        }

-        /* CAN BLDC: enable/disable follows arm state (Issue #597) */
-        {
-            static bool s_can_enabled = false;
-            bool armed_now = (bal.state == BALANCE_ARMED);
-            if (armed_now && !s_can_enabled) {
-                can_driver_send_enable(CAN_NODE_LEFT,  true);
-                can_driver_send_enable(CAN_NODE_RIGHT, true);
-                s_can_enabled = true;
-            } else if (!armed_now && s_can_enabled) {
-                can_driver_send_enable(CAN_NODE_LEFT,  false);
-                can_driver_send_enable(CAN_NODE_RIGHT, false);
-                s_can_enabled = false;
-            }
-        }
-
-        /* CAN BLDC: send velocity/torque commands at CAN_TX_RATE_HZ (100 Hz) (Issue #597)
-         * Converts balance PID output + blended steer to per-wheel RPM.
+        /* VESC: send RPM commands at CAN_TX_RATE_HZ (100 Hz) via 29-bit extended CAN.
+         * VESC does not need enable/disable frames — RPM=0 while disarmed holds brakes.
         * Left  wheel: speed_rpm + steer_rpm (differential drive)
-         * Right wheel: speed_rpm - steer_rpm */
+         * Right wheel: speed_rpm − steer_rpm  (Issue #674) */
        if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
            can_cmd_tick = now;
-            int16_t can_steer = mode_manager_get_steer(&mode);
-            int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
-            int32_t steer_rpm = (int32_t)can_steer    * CAN_RPM_SCALE / 2;
-            can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 };
-            can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 };
-            if (bal.state != BALANCE_ARMED) {
-                cmd_l.velocity_rpm = 0;
-                cmd_r.velocity_rpm = 0;
+            int32_t speed_rpm = 0;
+            int32_t steer_rpm = 0;
+            if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
+                int16_t can_steer = mode_manager_get_steer(&mode);
+                speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
+                steer_rpm = (int32_t)can_steer     * CAN_RPM_SCALE / 2;
            }
-            can_driver_send_cmd(CAN_NODE_LEFT,  &cmd_l);
-            can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r);
+            vesc_can_send_rpm(VESC_CAN_ID_LEFT,  speed_rpm + steer_rpm);
+            vesc_can_send_rpm(VESC_CAN_ID_RIGHT, speed_rpm - steer_rpm);
+        }
+
+        /* VESC telemetry: send JLINK_TLM_VESC_STATE at 1 Hz (Issue #674) */
+        vesc_can_send_tlm(now);
+
+        /* Orin CAN broadcast: FC_STATUS + FC_VESC at ORIN_TLM_HZ (10 Hz) (Issue #674) */
+        {
+            orin_can_fc_status_t fc_st;
+            fc_st.pitch_x10     = (int16_t)(bal.pitch_deg * 10.0f);
+            fc_st.motor_cmd     = bal.motor_cmd;
+            uint32_t _vbat      = battery_read_mv();
+            fc_st.vbat_mv       = (_vbat > 65535u) ? 65535u : (uint16_t)_vbat;
+            fc_st.balance_state = (uint8_t)bal.state;
+            fc_st.flags         = (safety_remote_estop_active() ? 0x01u : 0u) |
+                                  (bal.state == BALANCE_ARMED   ? 0x02u : 0u);
+
+            orin_can_fc_vesc_t fc_vesc;
+            vesc_state_t vl, vr;
+            bool vl_ok = vesc_can_get_state(VESC_CAN_ID_LEFT,  &vl);
+            bool vr_ok = vesc_can_get_state(VESC_CAN_ID_RIGHT, &vr);
+            fc_vesc.left_rpm_x10      = vl_ok ? (int16_t)(vl.rpm / 10) : 0;
+            fc_vesc.right_rpm_x10     = vr_ok ? (int16_t)(vr.rpm / 10) : 0;
+            fc_vesc.left_current_x10  = vl_ok ? vl.current_x10 : 0;
+            fc_vesc.right_current_x10 = vr_ok ? vr.current_x10 : 0;
+
+            orin_can_broadcast(now, &fc_st, &fc_vesc);
        }

        /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
--- a/src/orin_can.c
+++ b/src/orin_can.c
@ -0,0 +1,96 @@
+/* orin_can.c — Orin↔FC CAN protocol driver (Issue #674)
+ *
+ * Receives high-level drive/mode/estop commands from Orin over CAN.
+ * Broadcasts FC status and VESC telemetry back to Orin at ORIN_TLM_HZ.
+ *
+ * Registered as the standard-ID callback with can_driver via
+ * can_driver_set_std_cb(orin_can_on_frame).
+ *
+ * Balance independence: if Orin link drops (orin_can_is_alive() == false),
+ * main loop stops injecting Orin commands and the balance PID holds
+ * upright in-place.  No action required here — the safety is in main.c.
+ */
+
+#include "orin_can.h"
+#include "can_driver.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+volatile OrinCanState orin_can_state;
+static uint32_t s_tlm_tick;
+
+void orin_can_init(void)
+{
+    memset((void *)&orin_can_state, 0, sizeof(orin_can_state));
+    /* Pre-wind so first broadcast fires on the first eligible tick */
+    s_tlm_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ));
+    can_driver_set_std_cb(orin_can_on_frame);
+}
+
+void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len)
+{
+    /* Any frame from Orin refreshes the heartbeat */
+    orin_can_state.last_rx_ms = HAL_GetTick();
+
+    switch (std_id) {
+    case ORIN_CAN_ID_HEARTBEAT:
+        /* Heartbeat payload (sequence counter) ignored — timestamp is enough */
+        break;
+
+    case ORIN_CAN_ID_DRIVE:
+        /* int16 speed (BE), int16 steer (BE) */
+        if (len < 4u) { break; }
+        orin_can_state.speed = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
+        orin_can_state.steer = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
+        orin_can_state.drive_updated = 1u;
+        break;
+
+    case ORIN_CAN_ID_MODE:
+        /* uint8 mode */
+        if (len < 1u) { break; }
+        orin_can_state.mode = data[0];
+        orin_can_state.mode_updated = 1u;
+        break;
+
+    case ORIN_CAN_ID_ESTOP:
+        /* uint8: 1 = assert estop, 0 = clear estop */
+        if (len < 1u) { break; }
+        if (data[0] != 0u) {
+            orin_can_state.estop_req = 1u;
+        } else {
+            orin_can_state.estop_clear_req = 1u;
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
+bool orin_can_is_alive(uint32_t now_ms)
+{
+    if (orin_can_state.last_rx_ms == 0u) {
+        return false;
+    }
+    return (now_ms - orin_can_state.last_rx_ms) < ORIN_HB_TIMEOUT_MS;
+}
+
+void orin_can_broadcast(uint32_t now_ms,
+                        const orin_can_fc_status_t *status,
+                        const orin_can_fc_vesc_t   *vesc)
+{
+    if ((now_ms - s_tlm_tick) < (1000u / ORIN_TLM_HZ)) {
+        return;
+    }
+    s_tlm_tick = now_ms;
+
+    uint8_t buf[8];
+
+    /* FC_STATUS (0x400): 8 bytes */
+    memcpy(buf, status, sizeof(orin_can_fc_status_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_STATUS, buf, (uint8_t)sizeof(orin_can_fc_status_t));
+
+    /* FC_VESC (0x401): 8 bytes */
+    memcpy(buf, vesc, sizeof(orin_can_fc_vesc_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_VESC, buf, (uint8_t)sizeof(orin_can_fc_vesc_t));
+}
--- a/src/vesc_can.c
+++ b/src/vesc_can.c
@ -0,0 +1,141 @@
+/* vesc_can.c — VESC CAN protocol driver (Issue #674)
+ *
+ * Registers vesc_can_on_frame as the extended-frame callback with can_driver.
+ * VESC uses 29-bit arb IDs: (pkt_type << 8) | vesc_node_id.
+ * All wire values are big-endian (VESC FW 6.x).
+ */
+
+#include "vesc_can.h"
+#include "can_driver.h"
+#include "jlink.h"
+#include "stm32f7xx_hal.h"
+#include <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);
+}
--- a/test/stubs/stm32f7xx_hal.h
+++ b/test/stubs/stm32f7xx_hal.h
@ -0,0 +1,126 @@
+/* test/stubs/stm32f7xx_hal.h — minimal HAL stub for host-side unit tests.
+ *
+ * Provides just enough types and functions so that the firmware source files
+ * compile on a host (Linux/macOS) with -DTEST_HOST.
+ * Each test file must define the actual behavior of HAL_GetTick() etc.
+ */
+
+#ifndef STM32F7XX_HAL_H
+#define STM32F7XX_HAL_H
+
+#include <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 */
--- a/test/test_vesc_can.c
+++ b/test/test_vesc_can.c
@ -0,0 +1,518 @@
+/*
+ * test_vesc_can.c — Unit tests for VESC CAN protocol driver (Issue #674).
+ *
+ * Build (host, no hardware):
+ *   gcc -I include -I test/stubs -DTEST_HOST -lm \
+ *       -o /tmp/test_vesc_can test/test_vesc_can.c
+ *
+ * All tests are self-contained; no HAL, no CAN peripheral required.
+ * vesc_can.c calls can_driver_send_ext / can_driver_set_ext_cb and
+ * jlink_send_vesc_state_tlm — all stubbed below.
+ */
+
+/* ---- Block HAL and board-specific headers ---- */
+/* Must appear before any board include is transitively pulled */
+#define STM32F7XX_HAL_H        /* skip stm32f7xx_hal.h */
+#define STM32F722xx            /* satisfy any chip guard */
+#define JLINK_H                /* skip jlink.h (pid_flash / HAL deps) */
+#define CAN_DRIVER_H           /* skip can_driver.h body (we stub functions below) */
+
+#include <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;
+}
				`@ -0,0 +1 @@`
				`"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can."""`