7 changed files with 1295 additions and 430 deletions
--- a/docs/wiring-diagram.md
+++ b/docs/wiring-diagram.md
@ -14,123 +14,125 @@
 │                        ORIN NANO SUPER                              │
 │                     (Top Plate — 25W)                               │
 │                                                                     │
-│  USB-A ──── CANable2 USB-CAN adapter (slcan0, 500 kbps)           │
+│  USB-C ──── STM32 CDC (/dev/stm32-bridge, 921600 baud)            │
 │  USB-A ──── ESP32-S3 IO (/dev/esp32-io, 460800 baud)              │
 │  USB-A1 ─── RealSense D435i (USB 3.1)                             │
 │  USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200)             │
 │  USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP)     │
 │  USB ─────── Leap Motion Controller (hand/gesture tracking)        │
-│  CSI-A ──── ArduCam adapter → 2x IMX219 (front + left)            │
+│  CSI-A ──── ArduCam adapter → 2× IMX219 (front + left)            │
-│  CSI-B ──── ArduCam adapter → 2x IMX219 (rear + right)            │
+│  CSI-B ──── ArduCam adapter → 2× IMX219 (rear + right)            │
 │  M.2 ───── 1TB NVMe SSD                                            │
 │  40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec)         │
 │  Pin 8 ──┐                                                         │
-│  Pin 10 ─┤ UART fallback to ESP32-S3 BALANCE (ttyTHS0, 460800)    │
+│  Pin 10 ─┤ UART fallback to FC (ttyTHS0, 921600)                  │
 │  Pin 6 ──┘ GND                                                     │
 │                                                                     │
 └─────────────────────────────────────────────────────────────────────┘
-         │ USB-A (CANable2)               │ UART fallback (3 wires)
+         │ USB-C (data only)              │ UART fallback (3 wires)
-         │ SocketCAN slcan0               │ 460800 baud, 3.3V
+         │ 921600 baud                    │ 921600 baud, 3.3V
         │ 500 kbps                       │
         ▼                                ▼
 ┌─────────────────────────────────────────────────────────────────────┐
-│                  ESP32-S3 BALANCE                                    │
+│                     MAMBA F722S (FC)                                 │
-│          (Waveshare Touch LCD 1.28, Middle Plate)                   │
+│                  (Middle Plate — foam mounted)                       │
 │                                                                     │
-│  CAN bus ──── CANable2 → Orin (primary link, ISO 11898)            │
+│  USB-C ──── Orin (CDC serial, primary link)                        │
-│  UART0 ──── Orin UART fallback (460800 baud, 3.3V)                │
+│                                                                     │
-│  UART1 ──── VESC Left  (CAN ID 56) via UART/CAN bridge            │
+│  USART2 (PA2=TX, PA3=RX) ──── Hoverboard ESC (26400 baud)         │
-│  UART2 ──── VESC Right (CAN ID 68) via UART/CAN bridge            │
+│  UART4  (PA0=TX, PA1=RX) ──── ELRS RX (CRSF, 420000 baud)        │
-│  I2C   ──── QMI8658 IMU (onboard, 6-DOF accel+gyro)               │
+│  USART6 (PC6=TX, PC7=RX) ──── Orin UART fallback                  │
-│  SPI   ──── GC9A01 LCD (onboard, 240x240 round display)           │
+│  UART5  (PC12=TX, PD2=RX) ─── Debug (optional)                    │
-│  GPIO  ──── WS2812B LED strip                                      │
+│                                                                     │
-│  GPIO  ──── Buzzer                                                  │
+│  SPI1 ─── MPU6000 IMU (on-board, CW270)                           │
-│  ADC   ──── Battery voltage divider                                │
+│  I2C1 ─── BMP280 baro (on-board, disabled)                        │
 │  ADC ──── Battery voltage (PC1) + Current (PC3)                    │
 │  PB3 ──── WS2812B LED strip                                        │
 │  PB2 ──── Buzzer                                                    │
 │                                                                     │
 └─────────────────────────────────────────────────────────────────────┘
-         │ CAN bus (ISO 11898)            │ UART (460800 baud)
+         │ USART2                         │ UART4
-         │ 500 kbps                       │
+         │ PA2=TX → ESC RX               │ PA0=TX → ELRS TX
         │ PA3=RX ← ESC TX               │ PA1=RX ← ELRS RX
         │ GND ─── GND                   │ GND ─── GND
         ▼                                ▼
 ┌────────────────────────┐    ┌──────────────────────────┐
-│   VESC Left (ID 56)    │    │   VESC Right (ID 68)     │
+│   HOVERBOARD ESC       │    │   ELRS 2.4GHz RX         │
-│   (Bottom Plate)       │    │   (Bottom Plate)          │
+│   (Bottom Plate)       │    │   (beside FC)             │
 │                        │    │                            │
 │  BLDC hub motor        │    │  BLDC hub motor            │
 │  CAN 500 kbps          │    │  CAN 500 kbps             │
 │  FOC current control   │    │  FOC current control      │
 │  VESC Status 1 (0x900) │    │  VESC Status 1 (0x910)   │
 │                        │    │                            │
 │  2× BLDC hub motors    │    │  CRSF protocol            │
 │  26400 baud UART       │    │  420000 baud              │
 │  Frame: [0xABCD]       │    │  BetaFPV 1W TX → RX      │
 │  [steer][speed][csum]  │    │  CH3=speed CH4=steer      │
 │                        │    │  CH5=arm   CH6=mode       │
 └────────────────────────┘    └──────────────────────────┘
-         │                              │
+         │                              
-    LEFT MOTOR                    RIGHT MOTOR
+    ┌────┴────┐                         
-```
+    ▼         ▼                         
  🛞 LEFT   RIGHT 🛞                    
  MOTOR     MOTOR                       
 ## Wire-by-Wire Connections
-### 1. Orin <-> ESP32-S3 BALANCE (Primary: CAN Bus via CANable2)
+### 1. Orin ↔ FC (Primary: USB CDC)
-| From | To | Wire | Notes |
+| From | To | Wire Color | Notes |
-|------|----|------|-------|
+|------|----|-----------|-------|
-| Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps |
+| Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus |
 | CANable2 CAN-H | ESP32-S3 BALANCE CAN-H | twisted pair | ISO 11898 differential |
 | CANable2 CAN-L | ESP32-S3 BALANCE CAN-L | twisted pair | ISO 11898 differential |
- Interface: SocketCAN `slcan0`, 500 kbps
+- Device: `/dev/ttyACM0` → symlink `/dev/stm32-bridge`
- Device node: `/dev/canable2` (via udev, symlink to ttyUSBx)
+- Baud: 921600, 8N1
- Protocol: CAN frames --- ORIN_CMD_DRIVE (0x300), ORIN_CMD_MODE (0x301), ORIN_CMD_ESTOP (0x302)
+- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
 - Telemetry: BALANCE_STATUS (0x400), BALANCE_VESC (0x401), BALANCE_IMU (0x402), BALANCE_BATTERY (0x403)
-### 2. Orin <-> ESP32-S3 BALANCE (Fallback: Hardware UART)
+### 2. Orin ↔ FC (Fallback: Hardware UART)
-| Orin Pin | Signal | ESP32-S3 Pin | Notes |
+| Orin Pin | Signal | FC Pin | FC Signal |
-|----------|--------|--------------|-------|
+|----------|--------|--------|-----------|
-| Pin 8 | TXD0 | GPIO17 (UART0 RX) | Orin TX -> BALANCE RX |
+| Pin 8 | TXD0 | PC7 | USART6 RX |
-| Pin 10 | RXD0 | GPIO18 (UART0 TX) | Orin RX <- BALANCE TX |
+| Pin 10 | RXD0 | PC6 | USART6 TX |
-| Pin 6 | GND | GND | Common ground |
+| Pin 6 | GND | GND | GND |
 - Jetson device: `/dev/ttyTHS0`
- Baud: 460800, 8N1
+- Baud: 921600, 8N1
 - Voltage: 3.3V both sides (no level shifter needed)
- Cross-connect: Orin TX -> BALANCE RX, Orin RX <- BALANCE TX
+- **Cross-connect:** Orin TX → FC RX, Orin RX ← FC TX
-### 3. Orin <-> ESP32-S3 IO (USB Serial)
+### 3. FC ↔ Hoverboard ESC
-| From | To | Notes |
+| FC Pin | Signal | ESC Pin | Notes |
-|------|----|-------|
+|--------|--------|---------|-------|
-| Orin USB-A | ESP32-S3 IO USB-C | USB cable, /dev/esp32-io |
+| PA2 | USART2 TX | RX | FC sends speed/steer commands |
-
+| PA3 | USART2 RX | TX | ESC sends feedback (optional) |
 - Device node: `/dev/esp32-io` (udev symlink)
 - Baud: 460800, 8N1
 - Protocol: Binary frames `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
 - Use: IO expansion, GPIO control, sensor polling
 ### 4. ESP32-S3 BALANCE <-> VESC Motors (CAN Bus)
 | BALANCE Pin | Signal | VESC Pin | Notes |
 |-------------|--------|----------|-------|
 | GPIO21 | CAN-H | CAN-H | ISO 11898 differential pair |
 | GPIO22 | CAN-L | CAN-L | ISO 11898 differential pair |
 | GND | GND | GND | Common ground |
- Baud: 500 kbps CAN
+- Baud: 26400, 8N1
- VESC Left: CAN ID 56, VESC Right: CAN ID 68
+- Protocol: Binary frame — `[0xABCD][steer:int16][speed:int16][checksum:uint16]`
- Commands: COMM_SET_RPM, COMM_SET_CURRENT, COMM_SET_DUTY
+- Speed range: -1000 to +1000
- Telemetry: VESC Status 1 at 50 Hz (RPM, current, duty)
+- **Keep wires short and twisted** (EMI from ESC)
 ### 4. FC ↔ ELRS Receiver
 | FC Pin | Signal | ELRS Pin | Notes |
 |--------|--------|----------|-------|
 | PA0 | UART4 TX | RX | Telemetry to TX (optional) |
 | PA1 | UART4 RX | TX | CRSF frames from RX |
 | GND | GND | GND | Common ground |
 | 5V | — | VCC | Power ELRS from 5V bus |
 - Baud: 420000 (CRSF protocol)
 - Failsafe: disarm after 300ms without frame
 ### 5. Power Distribution
 ```
-BATTERY (36V) ──┬── VESC Left  (36V direct -> BLDC left motor)
+BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
                ├── VESC Right (36V direct -> BLDC right motor)
                │
                ├── 5V BEC/regulator ──┬── Orin (USB-C PD or barrel jack)
-                │                      ├── ESP32-S3 BALANCE (5V via USB-C)
+                │                      ├── FC (via USB or 5V pad)
-                │                      ├── ESP32-S3 IO (5V via USB-C)
+                │                      ├── ELRS RX (5V)
                │                      ├── WS2812B LEDs (5V)
                │                      └── RPLIDAR (5V via USB)
                │
-                └── Battery monitor ──── ESP32-S3 BALANCE ADC (voltage divider)
+                └── Battery monitor ──── FC ADC (PC1=voltage, PC3=current)
 ```
 ### 6. Sensors on Orin (USB/CSI)
@ -141,36 +143,20 @@ BATTERY (36V) ──┬── VESC Left  (36V direct -> BLDC left motor)
 | RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` |
 | IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` |
 | IMX219 rear+right | MIPI CSI-2 | CSI-B (J8) | `/dev/video4,6` |
-| 1TB NVMe | PCIe Gen3 x4 | M.2 Key M | `/dev/nvme0n1` |
+| 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
 | CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
 ## FC UART Summary (MAMBA F722S — OBSOLETE)
-| Interface | Pins | Baud/Rate | Assignment | Notes |
+| UART | Pins | Baud | Assignment | Notes |
-|-----------|------|-----------|------------|-------|
+|------|------|------|------------|-------|
-| UART0 | GPIO17=RX, GPIO18=TX | 460800 | Orin UART fallback | 3.3V, cross-connect |
+| USART1 | PB6=TX, PB7=RX | — | SmartAudio/VTX | Unused in SaltyLab |
-| UART1 | GPIO19=RX, GPIO20=TX | 115200 | Debug serial | Optional |
+| USART2 | PA2=TX, PA3=RX | 26400 | Hoverboard ESC | Binary motor commands |
-| CAN (TWAI) | GPIO21=H, GPIO22=L | 500 kbps | CAN bus (VESCs + Orin) | SN65HVD230 transceiver |
+| USART3 | PB10=TX, PB11=RX | — | Available | Was SBUS default |
-| I2C | GPIO4=SDA, GPIO5=SCL | 400 kHz | QMI8658 IMU (addr 0x6B) | Onboard |
+| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control |
-| SPI | GPIO36=MOSI, GPIO37=SCLK, GPIO35=CS | 40 MHz | GC9A01 LCD (onboard) | 240x240 round |
+| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
-| USB CDC | USB-C | 460800 | Orin USB fallback | /dev/esp32-balance |
+| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
-
+| USB CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` |
 ## CAN Frame ID Map
 | CAN ID | Direction | Name | Contents |
 |--------|-----------|------|----------|
 | 0x300 | Orin -> BALANCE | ORIN_CMD_DRIVE | left_rpm_f32, right_rpm_f32 (8 bytes LE) |
 | 0x301 | Orin -> BALANCE | ORIN_CMD_MODE | mode byte (0=IDLE, 1=DRIVE, 2=ESTOP) |
 | 0x302 | Orin -> BALANCE | ORIN_CMD_ESTOP | flags byte (bit0=stop, bit1=clear) |
 | 0x400 | BALANCE -> Orin | BALANCE_STATUS | pitch x10:i16, motor_cmd:u16, vbat_mv:u16, state:u8, flags:u8 |
 | 0x401 | BALANCE -> Orin | BALANCE_VESC | l_rpm x10:i16, r_rpm x10:i16, l_cur x10:i16, r_cur x10:i16 |
 | 0x402 | BALANCE -> Orin | BALANCE_IMU | pitch x100:i16, roll x100:i16, yaw x100:i16, ax x100:i16, ay x100:i16, az x100:i16 |
 | 0x403 | BALANCE -> Orin | BALANCE_BATTERY | vbat_mv:u16, current_ma:i16, soc_pct:u8 |
 | 0x900+ID | VESC Left -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 |
 | 0x910+ID | VESC Right -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 |
 VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44).
 ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
@ -188,59 +174,57 @@ VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44).
 | Pin 2, 4 | 5V | Power |
 | Pin 6, 9 | GND | Ground |
- Codec: Wolfson WM8960 (I2C addr 0x1A)
+- **Codec:** Wolfson WM8960 (I2C addr 0x1A)
- Mics: 2x MEMS (left + right) --- basic stereo / sound localization
+- **Mics:** 2× MEMS (left + right) — basic stereo / sound localization
- Speaker: 3W class-D amp output (JST connector)
+- **Speaker:** 3W class-D amp output (JST connector)
- Headset: 3.5mm TRRS jack
+- **Headset:** 3.5mm TRRS jack
- Requires: WM8960 device tree overlay for Jetson (community port)
+- **Requires:** WM8960 device tree overlay for Jetson (community port)
- Use: Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
+- **Use:** Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
 ### 8. SIM7600A 4G/LTE HAT (via USB)
 | Connection | Detail |
 |-----------|--------|
-| Interface | USB (micro-B on HAT -> USB-A/C on Orin) |
+| Interface | USB (micro-B on HAT → USB-A/C on Orin) |
 | Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) |
 | Power | 5V from USB or separate 5V supply (peak 2A during TX) |
 | SIM | Nano-SIM slot on HAT |
-| Antenna | 4G LTE + GPS/GNSS (external SMA antennas --- mount high on chassis) |
+| Antenna | 4G LTE + GPS/GNSS (external SMA antennas — mount high on chassis) |
- Data: PPP or QMI for internet connectivity
+- **Data:** PPP or QMI for internet connectivity
- GPS/GNSS: Built-in receiver, NMEA sentences on ttyUSB2 --- outdoor positioning
+- **GPS/GNSS:** Built-in receiver, NMEA sentences on ttyUSB2 — outdoor positioning
- AT commands: `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
+- **AT commands:** `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
- Connected via USB (not 40-pin) --- avoids UART conflict with BALANCE fallback, flexible antenna placement
+- **Connected via USB** (not 40-pin) — avoids UART conflict with FC fallback, flexible antenna placement
- Use: Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
+- **Use:** Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
-### 9. Leap Motion Controller (USB)
+### 10. Leap Motion Controller (USB)
 | Connection | Detail |
 |-----------|--------|
-| Interface | USB 3.0 (micro-B on controller -> USB-A on Orin) |
+| Interface | USB 3.0 (micro-B on controller → USB-A on Orin) |
 | Power | ~0.5W |
-| Range | ~80cm, 150 deg FOV |
+| Range | ~80cm, 150° FOV |
 | SDK | Ultraleap Gemini V5+ (Linux ARM64 support) |
 | ROS2 | `leap_motion_ros2` wrapper available |
- 2x IR cameras + 3x IR LEDs --- tracks all 10 fingers in 3D, sub-mm precision
+- **2× IR cameras + 3× IR LEDs** — tracks all 10 fingers in 3D, sub-mm precision
- Mount: Forward-facing on sensor tower or upward on Orin plate
+- **Mount:** Forward-facing on sensor tower or upward on Orin plate
- Use: Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
+- **Use:** Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
- Combined with ReSpeaker: Voice + gesture control with zero hardware in hand
+- **Combined with ReSpeaker:** Voice + gesture control with zero hardware in hand
-### 10. Power Budget (USB)
+### 11. Power Budget (USB)
 | Device | Interface | Power Draw |
 |--------|-----------|------------|
-| CANable2 USB-CAN | USB-A | ~0.5W |
+| STM32 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
 | ESP32-S3 BALANCE | USB-C | ~0.8W (WiFi off) |
 | ESP32-S3 IO | USB-C | ~0.5W |
 | RealSense D435i | USB-A | ~1.5W (3.5W peak) |
 | RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
 | SIM7600A | USB | ~1W idle, 3W TX peak |
-| Leap Motion | USB-A | ~0.5W |
+| Leap Motion | USB | ~0.5W |
 | ReSpeaker HAT | 40-pin | ~0.5W |
-| **Total USB** | | **~7.9W typical, ~11W peak** |
+| **Total USB** | | **~6.5W typical, ~10.5W peak** |
-Orin Nano Super delivers up to 25W --- USB peripherals are well within budget.
+Orin Nano Super delivers up to 25W — USB peripherals are well within budget.
 ---
@ -248,37 +232,38 @@ Orin Nano Super delivers up to 25W --- USB peripherals are well within budget.
 ```
                    ┌──────────────┐
-                    │  RC TX       │ (in your hand)
+                    │  ELRS TX     │ (in your hand)
                    │  (2.4GHz)    │
                    └──────┬───────┘
                           │ radio
                    ┌──────▼───────┐
-                    │  RC RX       │ CRSF 420kbaud (future)
+                    │  ELRS RX     │ CRSF 420kbaud
                    └──────┬───────┘
-                           │ UART
+                           │ UART4
              ┌────────────▼────────────┐
-              │   ESP32-S3 BALANCE      │
+              │      MAMBA F722S        │
              │  (Waveshare LCD 1.28)   │
              │                         │
-              │  QMI8658 -> Balance PID │
+              │  MPU6000 → Balance PID  │
-              │  RC -> Mode Manager     │
+              │  CRSF → Mode Manager   │
              │  Safety Monitor         │
              │                         │
              └──┬──────────┬───────────┘
-    CAN 500kbps─┘          └───── CAN bus / UART fallback
+    USART2 ─────┘          └───── USB CDC / USART6
    26400 baud                    921600 baud
         │                              │
-    ┌────┴────────────┐                 ▼
+         ▼                              ▼
-    │ CAN bus (500k)  │        ┌───────────────────┐
+┌────────────────┐          ┌───────────────────┐
-    ├─ VESC Left  56  │        │   Orin Nano Super  │
+│ Hoverboard ESC │          │   Orin Nano Super  │
-    └─ VESC Right 68  │        │                     │
+│                │          │                     │
-         │    │                │  SLAM / Nav2 / AI  │
+│ L motor  R motor│         │  SLAM / Nav2 / AI  │
-         ▼    ▼                │  Person following   │
+│    🛞      🛞   │         │  Person following   │
-       LEFT  RIGHT             │  Voice commands     │
+└────────────────┘          │  Voice commands     │
-       MOTOR MOTOR             │  4G telemetry       │
+                            │  4G telemetry       │
                            └──┬──────────┬───────┘
                               │          │
                    ┌──────────▼─┐   ┌────▼──────────┐
                    │ ReSpeaker  │   │  SIM7600A     │
                    │ 2-Mic HAT  │   │  4G/LTE + GPS │
                    │ 🎤 🔊      │   │  📡 🛰️        │
                    └────────────┘   └───────────────┘
 ```
--- a/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml
@ -1,16 +1,30 @@
-# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
+# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
-# Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud.
+# Binary-framed Jetson↔ESP32 bridge at 921600 baud.
 # Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]
 # Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
 # ── Serial port ────────────────────────────────────────────────────────────────
-# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
+# Use /dev/esp32-bridge if the udev rule is applied:
-# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
+#   SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
-serial_port:      /dev/esp32-io
+#   SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
-baud_rate:        460800
+serial_port:      /dev/ttyACM0
-reconnect_delay:  2.0        # seconds between reconnect attempts
+baud_rate:        921600
 reconnect_delay:  2.0        # seconds between USB reconnect attempts
 # ── Heartbeat ─────────────────────────────────────────────────────────────────
-# HEARTBEAT (0x20) sent every heartbeat_period.
+# HEARTBEAT frame sent every heartbeat_period seconds.
-# ESP32 IO watchdog fires if no heartbeat for ~500 ms.
+# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
-heartbeat_period: 0.2        # 200 ms → well within 500 ms watchdog
+heartbeat_period: 0.2        # 200ms → well within 500ms ESP32 BALANCE watchdog
 # ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
 # If no /cmd_vel message received for watchdog_timeout seconds,
 # send SPEED_STEER(0,0) to stop the robot.
 watchdog_timeout: 0.5        # 500ms
 # ── Twist velocity scaling ────────────────────────────────────────────────────
 # speed = clamp(linear.x  * speed_scale, -1000, 1000)  (m/s → ESC units)
 # steer = clamp(angular.z * steer_scale, -1000, 1000)  (rad/s → ESC units)
 #
 # Default: 1 m/s → 1000 ESC units, ±2 rad/s → ±1000 steer.
 # Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction.
 # Tune speed_scale to set the physical top speed.
 speed_scale:  1000.0
 steer_scale:  -500.0
--- a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
+++ b/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
@ -1,14 +1,14 @@
-"""stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node.
+"""stm32_cmd.launch.py — Launch the binary-framed ESP32 BALANCE command node (Issue #119).
 Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol).
 Handles RC monitoring, sensor data, LED/output commands.
 Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node.
 Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
 Usage:
  # Default (binary protocol, bidirectional):
  ros2 launch saltybot_bridge stm32_cmd.launch.py
-  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
+
  # Override serial port:
  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
  # Custom velocity scales:
  ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
 """
 import os
@ -24,8 +24,11 @@ def generate_launch_description() -> LaunchDescription:
    params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
    return LaunchDescription([
-        DeclareLaunchArgument("serial_port",     default_value="/dev/esp32-io"),
+        DeclareLaunchArgument("serial_port",      default_value="/dev/ttyACM0"),
-        DeclareLaunchArgument("baud_rate",        default_value="460800"),
+        DeclareLaunchArgument("baud_rate",         default_value="921600"),
        DeclareLaunchArgument("speed_scale",       default_value="1000.0"),
        DeclareLaunchArgument("steer_scale",       default_value="-500.0"),
        DeclareLaunchArgument("watchdog_timeout",  default_value="0.5"),
        DeclareLaunchArgument("heartbeat_period",  default_value="0.2"),
        Node(
@ -39,6 +42,9 @@ def generate_launch_description() -> LaunchDescription:
                {
                    "serial_port":      LaunchConfiguration("serial_port"),
                    "baud_rate":        LaunchConfiguration("baud_rate"),
                    "speed_scale":      LaunchConfiguration("speed_scale"),
                    "steer_scale":      LaunchConfiguration("steer_scale"),
                    "watchdog_timeout": LaunchConfiguration("watchdog_timeout"),
                    "heartbeat_period": LaunchConfiguration("heartbeat_period"),
                },
            ],
--- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
+++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
@ -1,32 +1,45 @@
-"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.
+"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge.
-Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
+Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
-inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
+framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
-This node is NOT the primary drive path (that is CAN via can_bridge_node).
+TX commands (Jetson → STM32):
-It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
+  SPEED_STEER  — 50 Hz from /cmd_vel subscription
  HEARTBEAT    — 200 ms timer (ESP32 BALANCE watchdog fires at 500 ms)
  ARM          — via /saltybot/arm service
  SET_MODE     — via /saltybot/set_mode service
  PID_UPDATE   — via /saltybot/pid_update topic
-Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]  @ 460800 baud
+Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
-RX from ESP32 IO:
+RX telemetry (STM32 → Jetson):
-  RC_CHANNELS (0x01) → /saltybot/rc_channels  (std_msgs/String JSON)
+  IMU          → /saltybot/imu           (sensor_msgs/Imu)
-  SENSORS     (0x02) → /saltybot/sensors       (std_msgs/String JSON)
+  BATTERY      → /saltybot/telemetry/battery (std_msgs/String JSON)
  MOTOR_RPM    → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
  ARM_STATE    → /saltybot/arm_state     (std_msgs/String JSON)
  ERROR        → /saltybot/error         (std_msgs/String JSON)
  All frames   → /diagnostics            (diagnostic_msgs/DiagnosticArray)
-TX to ESP32 IO:
+Auto-reconnect: USB disconnect is detected when serial.read() raises; node
-  LED_CMD     (0x10) ← /saltybot/leds          (std_msgs/String JSON)
+continuously retries at reconnect_delay interval.
-  OUTPUT_CMD  (0x11) ← /saltybot/outputs       (std_msgs/String JSON)
+
-  HEARTBEAT   (0x20)  — sent every heartbeat_period (keep IO watchdog alive)
+This node owns /dev/ttyACM0 exclusively — do NOT run alongside
 serial_bridge_node or saltybot_cmd_node on the same port.
 Parameters (config/stm32_cmd_params.yaml):
-  serial_port       /dev/esp32-io
+  serial_port       /dev/ttyACM0
-  baud_rate         460800
+  baud_rate         921600
-  reconnect_delay   2.0
+  reconnect_delay   2.0   (seconds)
-  heartbeat_period  0.2   (ESP32 IO watchdog fires at ~500 ms)
+  heartbeat_period  0.2   (seconds)
  watchdog_timeout  0.5   (seconds — no /cmd_vel → send zero-speed)
  speed_scale       1000.0 (linear.x m/s → ESC units)
  steer_scale      -500.0  (angular.z rad/s → ESC units, neg to flip convention)
 """
 from __future__ import annotations
 import json
 import math
 import threading
 import time
@ -37,69 +50,119 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 import serial
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import Imu
 from std_msgs.msg import String
 from std_srvs.srv import SetBool, Trigger
 from .stm32_protocol import (
    BAUD_RATE,
    FrameParser,
-    RcChannels,
+    ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
-    SensorData,
+    encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
-    encode_heartbeat,
+    encode_pid_update,
    encode_led_cmd,
    encode_output_cmd,
 )
 # ── Constants ─────────────────────────────────────────────────────────────────
 IMU_FRAME_ID = "imu_link"
 _ARM_LABEL   = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
 def _clamp(v: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, v))
 # ── Node ──────────────────────────────────────────────────────────────────────
 class Stm32CmdNode(Node):
-    """Orin ↔ ESP32-S3 IO auxiliary bridge node."""
+    """Binary-framed Jetson↔ESP32 bridge node."""
    def __init__(self) -> None:
        super().__init__("stm32_cmd_node")
-        # ── Parameters ────────────────────────────────────────────────────
+        # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("serial_port",      "/dev/esp32-io")
+        self.declare_parameter("serial_port",      "/dev/ttyACM0")
-        self.declare_parameter("baud_rate",        BAUD_RATE)
+        self.declare_parameter("baud_rate",        921600)
        self.declare_parameter("reconnect_delay",  2.0)
        self.declare_parameter("heartbeat_period", 0.2)
        self.declare_parameter("watchdog_timeout", 0.5)
        self.declare_parameter("speed_scale",      1000.0)
        self.declare_parameter("steer_scale",      -500.0)
-        self._port_name       = self.get_parameter("serial_port").value
+        port                  = self.get_parameter("serial_port").value
-        self._baud            = self.get_parameter("baud_rate").value
+        baud                  = self.get_parameter("baud_rate").value
        self._reconnect_delay = self.get_parameter("reconnect_delay").value
        self._hb_period       = self.get_parameter("heartbeat_period").value
        self._wd_timeout      = self.get_parameter("watchdog_timeout").value
        self._speed_scale     = self.get_parameter("speed_scale").value
        self._steer_scale     = self.get_parameter("steer_scale").value
-        # ── QoS ───────────────────────────────────────────────────────────
+        # ── QoS ───────────────────────────────────────────────────────────────
        sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST, depth=10,
        )
        rel_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST, depth=10,
        )
-        # ── Publishers ────────────────────────────────────────────────────
+        # ── Publishers ────────────────────────────────────────────────────────
-        self._rc_pub   = self.create_publisher(String,          "/saltybot/rc_channels", rel_qos)
+        self._imu_pub      = self.create_publisher(Imu,    "/saltybot/imu",           sensor_qos)
-        self._sens_pub = self.create_publisher(String,          "/saltybot/sensors",     rel_qos)
+        self._arm_pub      = self.create_publisher(String, "/saltybot/arm_state",      rel_qos)
        self._error_pub    = self.create_publisher(String, "/saltybot/error",          rel_qos)
        self._battery_pub  = self.create_publisher(String, "/saltybot/telemetry/battery",  rel_qos)
        self._rpm_pub      = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
        self._diag_pub     = self.create_publisher(DiagnosticArray, "/diagnostics",    rel_qos)
-        # ── Subscriptions ─────────────────────────────────────────────────
+        # ── Subscribers ───────────────────────────────────────────────────────
-        self.create_subscription(String, "/saltybot/leds",    self._on_leds,    rel_qos)
+        self._cmd_vel_sub = self.create_subscription(
-        self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
+            Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
        )
        self._pid_sub = self.create_subscription(
            String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
        )
-        # ── Serial state ──────────────────────────────────────────────────
+        # ── Services ──────────────────────────────────────────────────────────
        self._arm_srv  = self.create_service(SetBool, "/saltybot/arm",      self._svc_arm)
        self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode)
        # ── Serial state ──────────────────────────────────────────────────────
        self._port_name = port
        self._baud      = baud
        self._ser: serial.Serial | None = None
        self._ser_lock  = threading.Lock()
        self._parser    = FrameParser()
        self._rx_count = 0
-        # ── Open serial and start timers ──────────────────────────────────
+        # ── TX state ──────────────────────────────────────────────────────────
        self._last_speed    = 0
        self._last_steer    = 0
        self._last_cmd_t    = time.monotonic()
        self._watchdog_sent = False     # tracks whether we already sent zero
        # ── Diagnostics state ──────────────────────────────────────────────────
        self._last_arm_state   = -1
        self._last_battery_mv  = 0
        self._rx_frame_count   = 0
        # ── Open serial and start timers ──────────────────────────────────────
        self._open_serial()
        # Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean)
        self._read_timer = self.create_timer(0.005, self._read_cb)
        # Heartbeat TX
        self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
        # Watchdog check (fires at 2× watchdog_timeout for quick detection)
        self._wd_timer   = self.create_timer(self._wd_timeout / 2, self._watchdog_cb)
        # Periodic diagnostics
        self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
        self.get_logger().info(
-            f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
+            f"stm32_cmd_node started — {port} @ {baud} baud | "
            f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
        )
-    # ── Serial management ─────────────────────────────────────────────────
+    # ── Serial management ─────────────────────────────────────────────────────
    def _open_serial(self) -> bool:
        with self._ser_lock:
@ -107,7 +170,7 @@ class Stm32CmdNode(Node):
                self._ser = serial.Serial(
                    port=self._port_name,
                    baudrate=self._baud,
-                    timeout=0.005,
+                    timeout=0.005,          # non-blocking reads
                    write_timeout=0.1,
                )
                self._ser.reset_input_buffer()
@ -122,7 +185,17 @@ class Stm32CmdNode(Node):
                self._ser = None
                return False
    def _close_serial(self) -> None:
        with self._ser_lock:
            if self._ser and self._ser.is_open:
                try:
                    self._ser.close()
                except Exception:
                    pass
            self._ser = None
    def _write(self, data: bytes) -> bool:
        """Thread-safe serial write. Returns False if port is not open."""
        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
                return False
@ -134,15 +207,16 @@ class Stm32CmdNode(Node):
                self._ser = None
                return False
-    # ── RX ────────────────────────────────────────────────────────────────
+    # ── RX — read callback ────────────────────────────────────────────────────
    def _read_cb(self) -> None:
        """Read bytes from serial and feed them to the frame parser."""
        raw: bytes | None = None
-        reconnect = False
+        reconnect_needed  = False
        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
-                reconnect = True
+                reconnect_needed = True
            else:
                try:
                    n = self._ser.in_waiting
@ -151,9 +225,9 @@ class Stm32CmdNode(Node):
                except serial.SerialException as exc:
                    self.get_logger().error(f"Serial read error: {exc}")
                    self._ser = None
-                    reconnect = True
+                    reconnect_needed = True
-        if reconnect:
+        if reconnect_needed:
            self.get_logger().warn(
                "Serial disconnected — will retry",
                throttle_duration_sec=self._reconnect_delay,
@ -166,105 +240,230 @@ class Stm32CmdNode(Node):
            return
        for byte in raw:
-            msg = self._parser.feed(byte)
+            frame = self._parser.feed(byte)
-            if msg is not None:
+            if frame is not None:
-                self._rx_count += 1
+                self._rx_frame_count += 1
-                self._dispatch(msg)
+                self._dispatch_frame(frame)
-    def _dispatch(self, msg) -> None:
+    def _dispatch_frame(self, frame) -> None:
        """Route a decoded frame to the appropriate publisher."""
        now = self.get_clock().now().to_msg()
        ts  = f"{now.sec}.{now.nanosec:09d}"
-        if isinstance(msg, RcChannels):
+        if isinstance(frame, ImuFrame):
-            out = String()
+            self._publish_imu(frame, now)
            out.data = json.dumps({
                "channels": msg.channels,
                "source":   msg.source,
                "ts":       ts,
            })
            self._rc_pub.publish(out)
-        elif isinstance(msg, SensorData):
+        elif isinstance(frame, BatteryFrame):
-            out = String()
+            self._publish_battery(frame, now)
            out.data = json.dumps({
                "pressure_pa":   msg.pressure_pa,
                "temperature_c": msg.temperature_c,
                "tof_mm":        msg.tof_mm,
                "ts":            ts,
            })
            self._sens_pub.publish(out)
-        elif isinstance(msg, tuple):
+        elif isinstance(frame, MotorRpmFrame):
-            type_code, _ = msg
+            self._publish_motor_rpm(frame, now)
            self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
-    # ── TX ────────────────────────────────────────────────────────────────
+        elif isinstance(frame, ArmStateFrame):
            self._publish_arm_state(frame, now)
        elif isinstance(frame, ErrorFrame):
            self._publish_error(frame, now)
        elif isinstance(frame, tuple):
            type_code, payload = frame
            self.get_logger().debug(
                f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}"
            )
    # ── Telemetry publishers ──────────────────────────────────────────────────
    def _publish_imu(self, frame: ImuFrame, stamp) -> None:
        msg = Imu()
        msg.header.stamp    = stamp
        msg.header.frame_id = IMU_FRAME_ID
        # orientation: unknown — signal with -1 in first covariance
        msg.orientation_covariance[0] = -1.0
        msg.angular_velocity.x = math.radians(frame.pitch_deg)
        msg.angular_velocity.y = math.radians(frame.roll_deg)
        msg.angular_velocity.z = math.radians(frame.yaw_deg)
        cov = math.radians(0.3) ** 2    # ±0.3° noise estimate from ESP32 BMI088
        msg.angular_velocity_covariance[0] = cov
        msg.angular_velocity_covariance[4] = cov
        msg.angular_velocity_covariance[8] = cov
        msg.linear_acceleration.x = frame.accel_x
        msg.linear_acceleration.y = frame.accel_y
        msg.linear_acceleration.z = frame.accel_z
        acov = 0.05 ** 2    # ±0.05 m/s² noise
        msg.linear_acceleration_covariance[0] = acov
        msg.linear_acceleration_covariance[4] = acov
        msg.linear_acceleration_covariance[8] = acov
        self._imu_pub.publish(msg)
    def _publish_battery(self, frame: BatteryFrame, stamp) -> None:
        payload = {
            "voltage_v":  round(frame.voltage_mv / 1000.0, 3),
            "voltage_mv": frame.voltage_mv,
            "current_ma": frame.current_ma,
            "soc_pct":    frame.soc_pct,
            "charging":   frame.current_ma < -100,
            "ts":         f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        self._last_battery_mv = frame.voltage_mv
        msg = String()
        msg.data = json.dumps(payload)
        self._battery_pub.publish(msg)
    def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None:
        payload = {
            "left_rpm":  frame.left_rpm,
            "right_rpm": frame.right_rpm,
            "ts":        f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._rpm_pub.publish(msg)
    def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None:
        label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})")
        if frame.state != self._last_arm_state:
            self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})")
            self._last_arm_state = frame.state
        payload = {
            "state":       frame.state,
            "state_label": label,
            "error_flags": frame.error_flags,
            "ts":          f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._arm_pub.publish(msg)
    def _publish_error(self, frame: ErrorFrame, stamp) -> None:
        self.get_logger().error(
            f"ESP32 BALANCE error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
        )
        payload = {
            "error_code": frame.error_code,
            "subcode":    frame.subcode,
            "ts":         f"{stamp.sec}.{stamp.nanosec:09d}",
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._error_pub.publish(msg)
    # ── TX — command send ─────────────────────────────────────────────────────
    def _on_cmd_vel(self, msg: Twist) -> None:
        """Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz."""
        speed = int(_clamp(msg.linear.x  * self._speed_scale, -1000.0, 1000.0))
        steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0))
        self._last_speed    = speed
        self._last_steer    = steer
        self._last_cmd_t    = time.monotonic()
        self._watchdog_sent = False
        frame = encode_speed_steer(speed, steer)
        if not self._write(frame):
            self.get_logger().warn(
                "SPEED_STEER dropped — serial not open",
                throttle_duration_sec=2.0,
            )
    def _heartbeat_cb(self) -> None:
        """Send HEARTBEAT every heartbeat_period (default 200ms)."""
        self._write(encode_heartbeat())
-    def _on_leds(self, msg: String) -> None:
+    def _watchdog_cb(self) -> None:
-        """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
+        """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
-        try:
+        if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
-            d = json.loads(msg.data)
+            if not self._watchdog_sent:
-            frame = encode_led_cmd(
+                self.get_logger().warn(
-                int(d.get("pattern", 0)),
+                    f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
                int(d.get("r", 0)),
                int(d.get("g", 0)),
                int(d.get("b", 0)),
                )
-        except (ValueError, KeyError, json.JSONDecodeError) as exc:
+                self._watchdog_sent = True
-            self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
+            self._last_speed = 0
-            return
+            self._last_steer = 0
-        self._write(frame)
+            self._write(encode_speed_steer(0, 0))
-    def _on_outputs(self, msg: String) -> None:
+    def _on_pid_update(self, msg: String) -> None:
-        """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
+        """Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
        try:
-            d = json.loads(msg.data)
+            data = json.loads(msg.data)
-            frame = encode_output_cmd(
+            kp = float(data["kp"])
-                bool(d.get("horn",     False)),
+            ki = float(data["ki"])
-                bool(d.get("buzzer",   False)),
+            kd = float(data["kd"])
                int(d.get("headlight", 0)),
                int(d.get("fan",       0)),
            )
        except (ValueError, KeyError, json.JSONDecodeError) as exc:
-            self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}")
+            self.get_logger().error(f"Bad PID update JSON: {exc}")
            return
-        self._write(frame)
+        frame = encode_pid_update(kp, ki, kd)
        if self._write(frame):
            self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
        else:
            self.get_logger().warn("PID_UPDATE dropped — serial not open")
-    # ── Diagnostics ───────────────────────────────────────────────────────
+    # ── Services ──────────────────────────────────────────────────────────────
    def _svc_arm(self, request: SetBool.Request, response: SetBool.Response):
        """SetBool(True) = arm, SetBool(False) = disarm."""
        arm = request.data
        frame = encode_arm(arm)
        ok = self._write(frame)
        response.success = ok
        response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open"
        self.get_logger().info(
            f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}"
        )
        return response
    def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response):
        """SetBool: data maps to mode byte (True=1, False=0)."""
        mode = 1 if request.data else 0
        frame = encode_set_mode(mode)
        ok = self._write(frame)
        response.success = ok
        response.message = f"mode={mode}" if ok else "serial not open"
        return response
    # ── Diagnostics ───────────────────────────────────────────────────────────
    def _publish_diagnostics(self) -> None:
        diag = DiagnosticArray()
        diag.header.stamp = self.get_clock().now().to_msg()
        status = DiagnosticStatus()
-        status.name        = "saltybot/esp32_io_bridge"
+        status.name        = "saltybot/stm32_cmd_node"
-        status.hardware_id = "esp32-s3-io"
+        status.hardware_id = "esp32"
        port_ok = self._ser is not None and self._ser.is_open
-        status.level   = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
+        if port_ok:
-        status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
+            status.level   = DiagnosticStatus.OK
            status.message = "Serial OK"
        else:
            status.level   = DiagnosticStatus.ERROR
            status.message = f"Serial disconnected: {self._port_name}"
        wd_age = time.monotonic() - self._last_cmd_t
        status.values = [
            KeyValue(key="serial_port",    value=self._port_name),
            KeyValue(key="baud_rate",   value=str(self._baud)),
            KeyValue(key="port_open",      value=str(port_ok)),
-            KeyValue(key="rx_frames",   value=str(self._rx_count)),
+            KeyValue(key="rx_frames",      value=str(self._rx_frame_count)),
            KeyValue(key="rx_errors",      value=str(self._parser.frames_error)),
            KeyValue(key="last_speed",     value=str(self._last_speed)),
            KeyValue(key="last_steer",     value=str(self._last_steer)),
            KeyValue(key="cmd_vel_age_s",  value=f"{wd_age:.2f}"),
            KeyValue(key="battery_mv",     value=str(self._last_battery_mv)),
            KeyValue(key="arm_state",      value=_ARM_LABEL.get(self._last_arm_state, "?")),
        ]
        diag.status.append(status)
        self._diag_pub.publish(diag)
-    # ── Lifecycle ─────────────────────────────────────────────────────────
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
    def destroy_node(self) -> None:
-        self._write(encode_heartbeat(state=0))
+        # Send zero-speed + disarm on shutdown
-        with self._ser_lock:
+        self._write(encode_speed_steer(0, 0))
-            if self._ser and self._ser.is_open:
+        self._write(encode_arm(False))
-                try:
+        self._close_serial()
                    self._ser.close()
                except Exception:
                    pass
            self._ser = None
        super().destroy_node()
--- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py
+++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py
@ -0,0 +1,336 @@
 """stm32_protocol.py — Binary frame codec for Jetson↔ESP32 BALANCE communication.
 # TODO(esp32-migration): This protocol was designed for STM32F722 USB CDC.
 # When ESP32 BALANCE protocol is defined, update frame layout and baud rate.
 Issue #119: defines the binary serial protocol between the Jetson Nano and the
 ESP32 BALANCE over USB CDC @ 921600 baud.
 # TODO(esp32-migration): update when ESP32 BALANCE protocol is defined.
 Frame layout (all multi-byte fields are big-endian):
  ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
  │  STX │ TYPE │  LEN │     PAYLOAD      │  CRC16    │  ETX │
  │ 0x02 │  1B  │  1B  │    LEN bytes     │   2B BE   │ 0x03 │
  └──────┴──────┴──────┴──────────────────┴───────────┴──────┘
 CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
 CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
 Command types (Jetson → ESP32 BALANCE):
  0x01  HEARTBEAT   — no payload              (len=0)
  0x02  SPEED_STEER — int16 speed + int16 steer (len=4)  range: -1000..+1000
  0x03  ARM         — uint8 (0=disarm, 1=arm)  (len=1)
  0x04  SET_MODE    — uint8 mode               (len=1)
  0x05  PID_UPDATE  — float32 kp + ki + kd     (len=12)
 Telemetry types (ESP32 BALANCE → Jetson):
  0x10  IMU         — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12)
  0x11  BATTERY     — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5)
  0x12  MOTOR_RPM   — int16 left_rpm + int16 right_rpm     (len=4)
  0x13  ARM_STATE   — uint8 state + uint8 error_flags       (len=2)
  0x14  ERROR       — uint8 error_code + uint8 subcode      (len=2)
 Usage:
  # Encoding (Jetson → STM32)
  frame = encode_speed_steer(300, -150)
  ser.write(frame)
  # Decoding (ESP32 BALANCE → Jetson), one byte at a time
  parser = FrameParser()
  for byte in incoming_bytes:
      result = parser.feed(byte)
      if result is not None:
          handle_frame(result)
 """
 from __future__ import annotations
 import struct
 from dataclasses import dataclass
 from enum import IntEnum
 from typing import Optional
 # ── Frame constants ───────────────────────────────────────────────────────────
 STX = 0x02
 ETX = 0x03
 MAX_PAYLOAD_LEN = 64       # hard limit; any frame larger is corrupt
 # ── Command / telemetry type codes ────────────────────────────────────────────
 class CmdType(IntEnum):
    HEARTBEAT   = 0x01
    SPEED_STEER = 0x02
    ARM         = 0x03
    SET_MODE    = 0x04
    PID_UPDATE  = 0x05
 class TelType(IntEnum):
    IMU       = 0x10
    BATTERY   = 0x11
    MOTOR_RPM = 0x12
    ARM_STATE = 0x13
    ERROR     = 0x14
 # ── Parsed telemetry objects ──────────────────────────────────────────────────
@dataclass
 class ImuFrame:
    pitch_deg: float      # degrees (positive = forward tilt)
    roll_deg:  float
    yaw_deg:   float
    accel_x:   float      # m/s²
    accel_y:   float
    accel_z:   float
@dataclass
 class BatteryFrame:
    voltage_mv:  int      # millivolts (e.g. 11100 = 11.1 V)
    current_ma:  int      # milliamps (negative = charging)
    soc_pct:     int      # state of charge 0–100 (from STM32 fuel gauge or lookup)
@dataclass
 class MotorRpmFrame:
    left_rpm:   int
    right_rpm:  int
@dataclass
 class ArmStateFrame:
    state:       int      # 0=DISARMED 1=ARMED 2=TILT_FAULT
    error_flags: int      # bitmask
@dataclass
 class ErrorFrame:
    error_code: int
    subcode:    int
 # Union type for decoded results
 TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame
 # ── CRC16 CCITT ───────────────────────────────────────────────────────────────
 def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
    """CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR."""
    crc = init
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            if crc & 0x8000:
                crc = (crc << 1) ^ 0x1021
            else:
                crc <<= 1
        crc &= 0xFFFF
    return crc
 # ── Frame encoder ─────────────────────────────────────────────────────────────
 def _build_frame(cmd_type: int, payload: bytes) -> bytes:
    """Assemble a complete binary frame with CRC16."""
    assert len(payload) <= MAX_PAYLOAD_LEN, "Payload too large"
    length = len(payload)
    header = bytes([cmd_type, length])
    crc = _crc16_ccitt(header + payload)
    return bytes([STX, cmd_type, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
 def encode_heartbeat() -> bytes:
    """HEARTBEAT frame — no payload."""
    return _build_frame(CmdType.HEARTBEAT, b"")
 def encode_speed_steer(speed: int, steer: int) -> bytes:
    """SPEED_STEER frame — int16 speed + int16 steer, both in -1000..+1000."""
    speed = max(-1000, min(1000, int(speed)))
    steer = max(-1000, min(1000, int(steer)))
    return _build_frame(CmdType.SPEED_STEER, struct.pack(">hh", speed, steer))
 def encode_arm(arm: bool) -> bytes:
    """ARM frame — 0=disarm, 1=arm."""
    return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
 def encode_set_mode(mode: int) -> bytes:
    """SET_MODE frame — mode byte."""
    return _build_frame(CmdType.SET_MODE, struct.pack("B", mode & 0xFF))
 def encode_pid_update(kp: float, ki: float, kd: float) -> bytes:
    """PID_UPDATE frame — three float32 values."""
    return _build_frame(CmdType.PID_UPDATE, struct.pack(">fff", kp, ki, kd))
 # ── Frame decoder (state-machine parser) ─────────────────────────────────────
 class ParserState(IntEnum):
    WAIT_STX  = 0
    WAIT_TYPE = 1
    WAIT_LEN  = 2
    PAYLOAD   = 3
    CRC_HI    = 4
    CRC_LO    = 5
    WAIT_ETX  = 6
 class ParseError(Exception):
    pass
 class FrameParser:
    """Byte-by-byte streaming parser for ESP32 BALANCE telemetry frames.
    Feed individual bytes via feed(); returns a decoded TelemetryFrame (or raw
    bytes tuple) when a complete valid frame is received.
    Thread-safety: single-threaded — wrap in a lock if shared across threads.
    Usage::
        parser = FrameParser()
        for b in incoming:
            result = parser.feed(b)
            if result is not None:
                process(result)
    """
    def __init__(self) -> None:
        self._state    = ParserState.WAIT_STX
        self._type     = 0
        self._length   = 0
        self._payload  = bytearray()
        self._crc_rcvd = 0
        self.frames_ok    = 0
        self.frames_error = 0
    def reset(self) -> None:
        """Reset parser to initial state (call after error or port reconnect)."""
        self._state   = ParserState.WAIT_STX
        self._payload = bytearray()
    def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
        """Process one byte. Returns decoded frame on success, None otherwise.
        On CRC error, increments frames_error and resets. The return value on
        success is a dataclass (ImuFrame, BatteryFrame, etc.) or a
        (type_code, raw_payload) tuple for unknown type codes.
        """
        s = self._state
        if s == ParserState.WAIT_STX:
            if byte == STX:
                self._state = ParserState.WAIT_TYPE
            return None
        if s == ParserState.WAIT_TYPE:
            self._type  = byte
            self._state = ParserState.WAIT_LEN
            return None
        if s == ParserState.WAIT_LEN:
            self._length  = byte
            self._payload = bytearray()
            if self._length > MAX_PAYLOAD_LEN:
                # Corrupt frame — too big; reset
                self.frames_error += 1
                self.reset()
                return None
            if self._length == 0:
                self._state = ParserState.CRC_HI
            else:
                self._state = ParserState.PAYLOAD
            return None
        if s == ParserState.PAYLOAD:
            self._payload.append(byte)
            if len(self._payload) == self._length:
                self._state = ParserState.CRC_HI
            return None
        if s == ParserState.CRC_HI:
            self._crc_rcvd = byte << 8
            self._state    = ParserState.CRC_LO
            return None
        if s == ParserState.CRC_LO:
            self._crc_rcvd |= byte
            self._state     = ParserState.WAIT_ETX
            return None
        if s == ParserState.WAIT_ETX:
            self.reset()   # always reset so we look for next STX
            if byte != ETX:
                self.frames_error += 1
                return None
            # Verify CRC
            crc_data = bytes([self._type, self._length]) + self._payload
            expected = _crc16_ccitt(crc_data)
            if expected != self._crc_rcvd:
                self.frames_error += 1
                return None
            # Decode
            self.frames_ok += 1
            return _decode_telemetry(self._type, bytes(self._payload))
        # Should never reach here
        self.reset()
        return None
 # ── Telemetry decoder ─────────────────────────────────────────────────────────
 def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
    """Decode a validated telemetry payload into a typed dataclass."""
    try:
        if type_code == TelType.IMU:
            if len(payload) < 12:
                return None
            p, r, y, ax, ay, az = struct.unpack_from(">hhhhhh", payload)
            return ImuFrame(
                pitch_deg=p  / 100.0,
                roll_deg=r   / 100.0,
                yaw_deg=y    / 100.0,
                accel_x=ax   / 100.0,
                accel_y=ay   / 100.0,
                accel_z=az   / 100.0,
            )
        if type_code == TelType.BATTERY:
            if len(payload) < 5:
                return None
            v_mv, i_ma, soc = struct.unpack_from(">HhB", payload)
            return BatteryFrame(voltage_mv=v_mv, current_ma=i_ma, soc_pct=soc)
        if type_code == TelType.MOTOR_RPM:
            if len(payload) < 4:
                return None
            left, right = struct.unpack_from(">hh", payload)
            return MotorRpmFrame(left_rpm=left, right_rpm=right)
        if type_code == TelType.ARM_STATE:
            if len(payload) < 2:
                return None
            state, flags = struct.unpack_from("BB", payload)
            return ArmStateFrame(state=state, error_flags=flags)
        if type_code == TelType.ERROR:
            if len(payload) < 2:
                return None
            code, sub = struct.unpack_from("BB", payload)
            return ErrorFrame(error_code=code, subcode=sub)
    except struct.error:
        return None
    # Unknown telemetry type — return raw
    return (type_code, payload)
--- 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
@ -1,34 +1,27 @@
 #!/usr/bin/env python3
 """
-can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
+can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32 IO motor
-board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps).
+controller and VESC motor controllers over CAN bus.
-Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
+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     → ORIN_CMD_DRIVE (0x300)
+  /cmd_vel          geometry_msgs/Twist          → VESC speed commands (CAN)
-  /estop                std_msgs/Bool           → ORIN_CMD_ESTOP (0x303)
+  /estop            std_msgs/Bool                → ESP32 IO e-stop (CAN)
  /saltybot/arm         std_msgs/Bool           → ORIN_CMD_ARM   (0x301)
 Publications
 ------------
-  /saltybot/attitude      std_msgs/String (JSON)   pitch, speed, yaw_rate, state
+  /can/imu                sensor_msgs/Imu              ESP32 IO IMU telemetry
-  /saltybot/balance_state std_msgs/String (JSON)   alias of /saltybot/attitude
+  /can/battery            sensor_msgs/BatteryState     ESP32 IO battery telemetry
-  /can/battery            sensor_msgs/BatteryState  vbat_mv, fault, rssi
+  /can/vesc/left/state    std_msgs/Float32MultiArray   Left VESC state
-  /can/vesc/left/state    std_msgs/Float32MultiArray  VESC STATUS_1 left
+  /can/vesc/right/state   std_msgs/Float32MultiArray   Right VESC state
  /can/vesc/right/state   std_msgs/Float32MultiArray  VESC STATUS_1 right
  /can/connection_status  std_msgs/String              "connected" | "disconnected"
-Parameters
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
 ----------
  can_interface     str    CAN socket name (default: slcan0)
  speed_scale       float  /cmd_vel linear.x (m/s) → motor units  (default: 1000.0)
  steer_scale       float  /cmd_vel angular.z (rad/s) → motor units (default: -500.0)
  command_timeout_s float  watchdog zero-vel threshold (default: 0.5)
 """
 import json
 import threading
 import time
 from typing import Optional
@ -37,74 +30,87 @@ import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
-from sensor_msgs.msg import BatteryState
+from rcl_interfaces.msg import SetParametersResult
 from sensor_msgs.msg import BatteryState, Imu
 from std_msgs.msg import Bool, Float32MultiArray, String
-from saltybot_can_bridge.balance_protocol import (
+from saltybot_can_bridge.mamba_protocol import (
-    ORIN_CMD_DRIVE,
+    MAMBA_CMD_ESTOP,
-    ORIN_CMD_ARM,
+    MAMBA_CMD_MODE,
-    ORIN_CMD_ESTOP,
+    MAMBA_CMD_VELOCITY,
-    ESP32_TELEM_ATTITUDE,
+    MAMBA_TELEM_BATTERY,
-    ESP32_TELEM_BATTERY,
+    MAMBA_TELEM_IMU,
-    VESC_LEFT_ID,
+    VESC_TELEM_STATE,
-    VESC_RIGHT_ID,
+    ORIN_CAN_ID_FC_PID_ACK,
-    VESC_STATUS_1,
+    ORIN_CAN_ID_PID_SET,
    MODE_DRIVE,
    MODE_ESTOP,
    MODE_IDLE,
    encode_drive_cmd,
    encode_arm_cmd,
    encode_estop_cmd,
-    decode_attitude,
+    encode_mode_cmd,
-    decode_battery,
+    encode_velocity_cmd,
-    decode_vesc_status1,
+    encode_pid_set_cmd,
    decode_battery_telem,
    decode_imu_telem,
    decode_pid_ack,
    decode_vesc_state,
 )
 # Reconnect attempt interval when CAN bus is lost
 _RECONNECT_INTERVAL_S: float = 5.0
-# Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent
+# Watchdog timer tick rate (Hz)
 _WATCHDOG_HZ: float = 10.0
 class CanBridgeNode(Node):
-    """CAN bus bridge between Orin ROS2 and ESP32 BALANCE / VESC controllers."""
+    """CAN bus bridge between Orin ROS2 and ESP32 IO / 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",  VESC_LEFT_ID)
+        self.declare_parameter("left_vesc_can_id", 56)
-        self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
+        self.declare_parameter("right_vesc_can_id", 68)
-        self.declare_parameter("speed_scale",       1000.0)
+        self.declare_parameter("mamba_can_id", 1)
        self.declare_parameter("steer_scale",       -500.0)
        self.declare_parameter("command_timeout_s", 0.5)
        self.declare_parameter("pid/kp", 0.0)
        self.declare_parameter("pid/ki", 0.0)
        self.declare_parameter("pid/kd", 0.0)
-        self._iface         = self.get_parameter("can_interface").value
+        self._iface: str = self.get_parameter("can_interface").value
-        self._left_vesc_id  = self.get_parameter("left_vesc_can_id").value
+        self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
-        self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
+        self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
-        self._speed_scale   = self.get_parameter("speed_scale").value
+        self._mamba_id: int = self.get_parameter("mamba_can_id").value
-        self._steer_scale   = self.get_parameter("steer_scale").value
+        self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
-        self._cmd_timeout   = self.get_parameter("command_timeout_s").value
+        self._pid_kp: float = self.get_parameter("pid/kp").value
        self._pid_ki: float = self.get_parameter("pid/ki").value
        self._pid_kd: float = self.get_parameter("pid/kd").value
        # ── State ─────────────────────────────────────────────────────────
        self._bus: Optional[can.BusABC] = None
        self._connected: bool = False
        self._last_cmd_time: float = time.monotonic()
-        self._lock = threading.Lock()
+        self._lock = threading.Lock()  # protects _bus / _connected
        # ── Publishers ────────────────────────────────────────────────────
-        self._pub_attitude  = self.create_publisher(String,           "/saltybot/attitude",      10)
+        self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
        self._pub_balance   = self.create_publisher(String,           "/saltybot/balance_state", 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_left = self.create_publisher(
-        self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state",   10)
+            Float32MultiArray, "/can/vesc/left/state", 10
-        self._pub_status    = self.create_publisher(String,           "/can/connection_status",  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)
-        self.create_subscription(Bool,  "/saltybot/arm",  self._arm_cb,     10)
+        self.add_on_set_parameters_callback(self._on_set_parameters)
        # ── Timers ────────────────────────────────────────────────────────
        self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
@ -122,17 +128,46 @@ class CanBridgeNode(Node):
        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"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
+            f"mamba={self._mamba_id}"
        )
    # -- PID parameter callback (Issue #693) --
    def _on_set_parameters(self, params) -> SetParametersResult:
        """Send new PID gains over CAN when pid/* params change."""
        for p in params:
            if p.name == "pid/kp":
                self._pid_kp = float(p.value)
            elif p.name == "pid/ki":
                self._pid_ki = float(p.value)
            elif p.name == "pid/kd":
                self._pid_kd = float(p.value)
            else:
                continue
            try:
                payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
                self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
                self.get_logger().info(
                    f"PID gains sent: Kp={self._pid_kp:.2f} "
                    f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
                )
            except ValueError as exc:
                return SetParametersResult(successful=False, reason=str(exc))
        return SetParametersResult(successful=True)
    # ── 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:
-                self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
+                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")
@ -145,10 +180,12 @@ class CanBridgeNode(Node):
                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:
@ -163,51 +200,73 @@ class CanBridgeNode(Node):
    # ── ROS callbacks ─────────────────────────────────────────────────────
    def _cmd_vel_cb(self, msg: Twist) -> None:
-        """Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN."""
+        """Convert /cmd_vel Twist to VESC speed commands over CAN."""
        self._last_cmd_time = time.monotonic()
        if not self._connected:
            return
-        speed = int(max(-1000.0, min(1000.0, msg.linear.x  * self._speed_scale)))
+
-        steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
+        # Differential drive decomposition — individual wheel speeds in m/s.
-        self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
+        # 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 ESP32 IO 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 ESP32 IO 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 ESP32 BALANCE over CAN."""
+        """Forward /estop to ESP32 IO 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(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
+            self._send_can(
-            self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
+                MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
-        else:
+            )
-            # Clear estop: send DISARM then re-ARM (let operator decide to re-arm)
+            self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 IO")
            self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "estop_clear")
    def _arm_cb(self, msg: Bool) -> None:
        """Forward /saltybot/arm to ORIN_CMD_ARM."""
        if not self._connected:
            return
        self._send_can(ORIN_CMD_ARM, encode_arm_cmd(msg.data), "arm")
        self.get_logger().info(f"ARM command: {'ARM' if msg.data else 'DISARM'}")
    # ── Watchdog ──────────────────────────────────────────────────────────
    def _watchdog_cb(self) -> None:
-        """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive)."""
+        """If no /cmd_vel arrives within the timeout, send zero velocity."""
        if not self._connected:
            return
-        if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
+        elapsed = time.monotonic() - self._last_cmd_time
-            self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
+        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,
+    def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
-                  extended: bool = False) -> 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=extended)
+        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:
@ -216,41 +275,55 @@ class CanBridgeNode(Node):
    # ── 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, bus = self._connected, self._bus
+                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)
-        vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
+
        vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
        try:
-            if arb_id == ESP32_TELEM_ATTITUDE:
+            if arb_id == MAMBA_TELEM_IMU:
-                self._handle_attitude(data)
+                self._handle_imu(data, frame.timestamp)
-            elif arb_id == ESP32_TELEM_BATTERY:
+
-                self._handle_battery(data)
+            elif arb_id == MAMBA_TELEM_BATTERY:
-            elif arb_id == vesc_l:
+                self._handle_battery(data, frame.timestamp)
-                t = decode_vesc_status1(self._left_vesc_id, data)
+
-                m = Float32MultiArray()
+            elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
-                m.data = [t.erpm, t.duty, 0.0, t.current]
+                self._handle_vesc_state(data, frame.timestamp, side="left")
-                self._pub_vesc_left.publish(m)
+
-            elif arb_id == vesc_r:
+            elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
-                t = decode_vesc_status1(self._right_vesc_id, data)
+                self._handle_vesc_state(data, frame.timestamp, side="right")
-                m = Float32MultiArray()
+
-                m.data = [t.erpm, t.duty, 0.0, t.current]
+            elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
-                self._pub_vesc_right.publish(m)
+                gains = decode_pid_ack(data)
                self.get_logger().debug(
                    f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
                )
        except Exception as exc:
            self.get_logger().warning(
                f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
@ -258,36 +331,52 @@ class CanBridgeNode(Node):
    # ── Frame handlers ────────────────────────────────────────────────────
-    _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
+    def _handle_imu(self, data: bytes, timestamp: float) -> None:
        telem = decode_imu_telem(data)
-    def _handle_attitude(self, data: bytes) -> None:
+        msg = Imu()
-        """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
+        msg.header.stamp = self.get_clock().now().to_msg()
-        t = decode_attitude(data)
+        msg.header.frame_id = "imu_link"
-        now = self.get_clock().now().to_msg()
+
-        payload = {
+        msg.linear_acceleration.x = telem.accel_x
-            "pitch_deg":  round(t.pitch_deg, 2),
+        msg.linear_acceleration.y = telem.accel_y
-            "speed_mps":  round(t.speed, 3),
+        msg.linear_acceleration.z = telem.accel_z
-            "yaw_rate":   round(t.yaw_rate, 3),
+
-            "state":      t.state,
+        msg.angular_velocity.x = telem.gyro_x
-            "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
+        msg.angular_velocity.y = telem.gyro_y
-            "flags":      t.flags,
+        msg.angular_velocity.z = telem.gyro_z
-            "ts":         f"{now.sec}.{now.nanosec:09d}",
+
-        }
+        # Covariance unknown; mark as -1 per REP-145
-        msg = String()
+        msg.orientation_covariance[0] = -1.0
-        msg.data = json.dumps(payload)
+
-        self._pub_attitude.publish(msg)
+        self._pub_imu.publish(msg)
-        self._pub_balance.publish(msg)   # keep /saltybot/balance_state alive
+
    def _handle_battery(self, data: bytes, timestamp: float) -> None:
        telem = decode_battery_telem(data)
    def _handle_battery(self, data: bytes) -> None:
        """BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
        t = decode_battery(data)
        msg = BatteryState()
        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.voltage = t.vbat_mv / 1000.0
+        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:
@ -298,10 +387,17 @@ class CanBridgeNode(Node):
    # ── 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(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
+                self._send_can(
-                self._send_can(ORIN_CMD_ARM,   encode_arm_cmd(False), "shutdown")
+                    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:
@ -311,6 +407,8 @@ class CanBridgeNode(Node):
        super().destroy_node()
 # ---------------------------------------------------------------------------
 def main(args=None) -> None:
    rclpy.init(args=args)
    node = CanBridgeNode()
--- 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,227 @@
 #!/usr/bin/env python3
 """
 mamba_protocol.py — CAN message encoding/decoding for the ESP32 IO motor controller
 and VESC telemetry.
 # TODO(esp32-migration): CAN IDs and struct layouts below are for the legacy Mamba
 # controller. When ESP32 IO CAN protocol is defined, update CAN IDs and frame formats.
 CAN message layout
 ------------------
 Command frames  (Orin → ESP32 IO / 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 (ESP32 IO → 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
 ORIN_CAN_ID_PID_SET: int = 0x305
 ORIN_CAN_ID_FC_PID_ACK: int = 0x405
 # ---------------------------------------------------------------------------
 # 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 ESP32 IO (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 ESP32 IO (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
@dataclass
 class PidGains:
    """Balance PID gains (Issue #693)."""
    kp: float = 0.0
    ki: float = 0.0
    kd: float = 0.0
 # ---------------------------------------------------------------------------
 # 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)
 def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes:
    """Encode ORIN_CAN_ID_PID_SET (6 bytes, uint16 BE x3). Issue #693."""
    if kp < 0.0 or ki < 0.0 or kd < 0.0:
        raise ValueError("PID gains must be non-negative")
    return struct.pack(_FMT_PID, round(min(kp,_PID_KP_MAX)*100), round(min(ki,_PID_KI_MAX)*100), round(min(kd,_PID_KD_MAX)*100))
 # ---------------------------------------------------------------------------
 # 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)
 def decode_pid_ack(data: bytes) -> PidGains:
    """Decode ORIN_CAN_ID_FC_PID_ACK (6 bytes). Issue #693."""
    kp_x100, ki_x100, kd_x100 = struct.unpack(_FMT_PID, data)
    return PidGains(kp=kp_x100/100.0, ki=ki_x100/100.0, kd=kd_x100/100.0)