feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec

Update CAN and serial bridge code to match authoritative protocol spec from docs/SAUL-TEE-SYSTEM-REFERENCE.md §5-6 (hal, 2026-04-04). mamba_protocol.py (CAN, Orin ↔ ESP32 BALANCE): - 0x300 DRIVE: [speed:i16][steer:i16][mode:u8][flags:u8][_:u16] — combined frame - 0x301 ARM: [arm:u8] - 0x302 PID: [kp:f16][ki:f16][kd:f16][_:u16] — half-float gains - 0x303 ESTOP: [0xE5] — magic byte cut - 0x400 ATTITUDE: [pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8] - 0x401 BATTERY: [vbat_mv:u16][fault_code:u8][rssi:i8] - Add VESC STATUS1/4/5 decode helpers; VESC IDs 56 (left) / 68 (right) can_bridge_node.py: - /cmd_vel → encode_drive_cmd (speed/steer int16, MODE_DRIVE) - /estop → encode_estop_cmd (magic 0xE5); clear → DISARM - /saltybot/arm → encode_arm_cmd (new subscription) - Watchdog sends DRIVE(0,0,MODE_IDLE) when /cmd_vel silent - ATTITUDE (0x400) → /saltybot/attitude + /saltybot/balance_state JSON - BATTERY (0x401) → /can/battery BatteryState - VESC STATUS1 frames → /can/vesc/left|right/state stm32_cmd_node.py — rewritten for inter-board protocol API: - Imports from updated stm32_protocol (BAUD_RATE=460800, new frame types) - RX: RcChannels → /saltybot/rc_channels, SensorData → /saltybot/sensors - TX: encode_led_cmd, encode_output_cmd from /saltybot/leds + /saltybot/outputs - HEARTBEAT (0x20) timer replaces old SPEED_STEER/ARM logic stm32_cmd_params.yaml: serial_port=/dev/esp32-io, baud=460800 stm32_cmd.launch.py: updated defaults and description Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
feat(arch): implement SAUL-TEE ESP32 protocol specs from hal reference doc
2026-04-04 08:32:00 -04:00 · 2026-04-04 08:25:24 -04:00
9 changed files with 1075 additions and 1006 deletions
--- a/CLAUDE.md
+++ b/CLAUDE.md
@ -1,17 +1,31 @@
 # SaltyLab Firmware — Agent Playbook

 ## Project
-Self-balancing two-wheeled robot: STM32F722 flight controller, hoverboard hub motors, Jetson Nano for AI/SLAM.
+**SAUL-TEE** — 4-wheel wagon robot (870×510×550 mm, 23 kg).
+Two ESP32-S3 boards + Jetson Orin for AI/ROS2.
+
+> **Full hardware spec:** `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
+
+### Embedded boards
+| Board | Hardware | Role |
+|-------|----------|------|
+| **ESP32-S3 BALANCE** | Waveshare Touch LCD 1.28 (CH343 USB) | QMI8658 IMU, PID balance loop, CAN→VESCs (SN65HVD230, 500 kbps) |
+| **ESP32-S3 IO** | Bare DevKit (JTAG USB) | BTS7960 motors, TBS Crossfire (UART0) + ELRS failover (UART2), NFC/baro/ToF (I2C), WS2812 LEDs, horn/headlight/fan/buzzer |
+
+### Key protocols
+- **Orin ↔ BALANCE:** CAN 500 kbps via CANable2 (slcan0). Cmds 0x300–0x303, telemetry 0x400–0x401
+- **BALANCE ↔ IO:** UART 460800 baud, frame `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
+- **VESC IDs:** Left = 56, Right = 68

 ## Team
 | Agent | Role | Focus |
 |-------|------|-------|
-| **sl-firmware** | Embedded Firmware Lead | STM32 HAL, USB CDC debugging, SPI/UART, PlatformIO, DFU bootloader |
+| **sl-firmware** | Embedded Firmware Lead | ESP32-S3 firmware (PlatformIO), QMI8658 IMU, PID, VESC CAN, inter-board UART |
 | **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
-| **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 |
+| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 |

 ## Status
-USB CDC TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).
+Architecture migrated from Mamba F722S/BlackPill → ESP32-S3 BALANCE + IO (PR #712, 2026-04-04).

 ## Repo Structure
 - `projects/saltybot/SALTYLAB.md` — Design doc
--- a/docs/SAUL-TEE-SYSTEM-REFERENCE.md
+++ b/docs/SAUL-TEE-SYSTEM-REFERENCE.md
@ -0,0 +1,444 @@
+# SAUL-TEE — System Reference
+**Rev A — 2026-04-04**
+_Authoritative architecture reference for all agents. Source: hal (Orin), max._
+
+---
+
+## 1. Robot Overview
+
+| Parameter | Value |
+|-----------|-------|
+| **Name** | SAUL-TEE |
+| **Config** | 4-wheel wagon (replaces 2-wheel self-balancing bot) |
+| **Dimensions** | 870 × 510 × 550 mm (L × W × H) |
+| **Mass** | ~23 kg |
+| **Drive** | 4× hub motors via 2× VESC 6.7 (dual channel each) |
+| **Power** | 36V battery bus |
+| **AI brain** | Jetson Orin Nano Super (25W) |
+| **CAN bus** | 500 kbps, CANable 2.0 USB↔CAN on Orin |
+
+> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32F722 / BlackPill are retired.
+> New embedded stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** (see §2–3 below).
+
+---
+
+## 2. ESP32-S3 BALANCE Board
+
+### Hardware
+| Item | Detail |
+|------|--------|
+| **Module** | Waveshare ESP32-S3 Touch LCD 1.28 |
+| **MCU** | ESP32-S3 (dual-core 240 MHz, 512KB SRAM, 8MB flash, 8MB PSRAM) |
+| **USB** | CH343G USB-UART bridge (UART0 / GPIO43 TX, GPIO44 RX) |
+| **Display** | 1.28" round GC9A01 240×240 LCD (SPI) |
+| **Touch** | CST816S capacitive touch (I2C) |
+| **IMU** | QMI8658 6-axis (gyro + accel), I2C on-board |
+| **CAN transceiver** | SN65HVD230, external, wired to ESP32-S3 TWAI peripheral |
+
+### Role
+- Runs the **PID balance / drive loop** (or stability assist for wagon mode)
+- Reads QMI8658 IMU at high rate for tilt / attitude
+- Sends drive commands to VESCs over **500 kbps CAN** (VESC native protocol)
+- Receives high-level velocity commands from Orin over CAN (0x300–0x303)
+- Publishes telemetry to Orin over CAN (0x400–0x401)
+- Bridges ESP32-IO sensor data over **inter-board UART** @ 460800 baud
+
+### On-Board Pin Map (fixed by Waveshare PCB)
+
+| Signal | GPIO | Notes |
+|--------|------|-------|
+| LCD SCLK | 10 | GC9A01 SPI clock |
+| LCD MOSI | 11 | GC9A01 SPI data |
+| LCD CS | 9 | GC9A01 chip select |
+| LCD DC | 8 | GC9A01 data/cmd |
+| LCD BL | 2 | Backlight PWM |
+| Touch SDA | 6 | CST816S / I2C-0 |
+| Touch SCL | 7 | CST816S / I2C-0 |
+| Touch INT | 5 | Active-low interrupt |
+| Touch RST | 4 | Active-low reset |
+| IMU SDA | 6 | QMI8658 shares I2C-0 |
+| IMU SCL | 7 | QMI8658 shares I2C-0 |
+| IMU INT1 | 3 | Data-ready interrupt |
+| CH343 USB TX | 43 | UART0 TX (USB serial) |
+| CH343 USB RX | 44 | UART0 RX (USB serial) |
+
+### External Wiring (user-soldered, confirm with calipers/multimeter)
+
+| Signal | GPIO | Notes |
+|--------|------|-------|
+| CAN TX | **TBD** | → SN65HVD230 D pin; use free header GPIO |
+| CAN RX | **TBD** | ← SN65HVD230 R pin; use free header GPIO |
+| Inter-board UART TX | **TBD** | → ESP32-IO UART RX @ 460800 |
+| Inter-board UART RX | **TBD** | ← ESP32-IO UART TX @ 460800 |
+
+> ⚠️ **Verify GPIO assignments in `esp32/balance/src/config.h` before writing firmware.**
+
+### QMI8658 Details
+| Item | Value |
+|------|-------|
+| I2C address | 0x6A (SA0 pin low) or 0x6B (SA0 high) |
+| Gyro full-scale | ±2048 °/s (configurable ±16/32/64/128/256/512/1024/2048) |
+| Accel full-scale | ±16 g (configurable ±2/4/8/16) |
+| Output data rate | Up to 7174.4 Hz |
+| Interface | I2C or SPI (board routes I2C) |
+
+---
+
+## 3. ESP32-S3 IO Board
+
+### Hardware
+| Item | Detail |
+|------|--------|
+| **Module** | ESP32-S3 bare dev board (JTAG USB) |
+| **MCU** | ESP32-S3 (dual-core 240 MHz) |
+| **USB** | Built-in USB-JTAG/Serial (no external bridge) |
+
+### Role
+- All **physical I/O**: motors, RC, sensors, indicators, accessories
+- TBS Crossfire primary RC (UART0)
+- ELRS failover RC (UART2)
+- BTS7960 half-bridge motor drivers (4-wheel PWM drive)
+- NFC, barometer, ToF range sensors (I2C)
+- WS2812B LED strip
+- Horn, headlight, cooling fan, buzzer
+
+### Pin Map
+
+| Signal | GPIO | Protocol | Notes |
+|--------|------|----------|-------|
+| **TBS Crossfire RX** UART TX | 43 | CRSF @ 420000 baud | Telemetry out to TBS TX module |
+| **TBS Crossfire RX** UART RX | 44 | CRSF @ 420000 baud | UART0 — RC frames in |
+| **ELRS failover** UART TX | **TBD** | CRSF @ 420000 baud | UART2 |
+| **ELRS failover** UART RX | **TBD** | CRSF @ 420000 baud | UART2 |
+| **Inter-board UART TX** | **TBD** | Binary @ 460800 | → BALANCE UART RX |
+| **Inter-board UART RX** | **TBD** | Binary @ 460800 | ← BALANCE UART TX |
+| **BTS7960 — FL** RPWM | **TBD** | PWM | Front-left forward |
+| **BTS7960 — FL** LPWM | **TBD** | PWM | Front-left reverse |
+| **BTS7960 — FL** R_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — FL** L_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — FR** RPWM | **TBD** | PWM | Front-right forward |
+| **BTS7960 — FR** LPWM | **TBD** | PWM | Front-right reverse |
+| **BTS7960 — FR** R_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — FR** L_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — RL** RPWM | **TBD** | PWM | Rear-left forward |
+| **BTS7960 — RL** LPWM | **TBD** | PWM | Rear-left reverse |
+| **BTS7960 — RL** R_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — RL** L_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — RR** RPWM | **TBD** | PWM | Rear-right forward |
+| **BTS7960 — RR** LPWM | **TBD** | PWM | Rear-right reverse |
+| **BTS7960 — RR** R_EN | **TBD** | GPIO | Enable H |
+| **BTS7960 — RR** L_EN | **TBD** | GPIO | Enable H |
+| **I2C SDA** | **TBD** | I2C | NFC + baro + ToF shared bus |
+| **I2C SCL** | **TBD** | I2C | NFC + baro + ToF shared bus |
+| **WS2812B data** | **TBD** | RMT | LED strip |
+| **Horn** | **TBD** | GPIO/PWM | MOSFET or relay |
+| **Headlight** | **TBD** | GPIO/PWM | MOSFET |
+| **Fan** | **TBD** | PWM | ESC/electronics cooling |
+| **Buzzer** | **TBD** | GPIO/PWM | Piezo or active buzzer |
+
+> ⚠️ **TBD pin assignments** — to be confirmed in `esp32/io/src/config.h` once wiring is set.
+
+### I2C Peripherals
+
+| Device | Address | Function |
+|--------|---------|----------|
+| NFC module | 0x24 (PN532) or 0x28 | NFC tag read/write |
+| Barometer | 0x76 (BMP280/BMP388) | Altitude + temp |
+| ToF range | 0x29 (VL53L0X) or 0x52 (VL53L4CD) | Proximity/obstacle |
+
+---
+
+## 4. Inter-Board UART Protocol (BALANCE ↔ IO)
+
+### Link Parameters
+| Parameter | Value |
+|-----------|-------|
+| Baud rate | 460800 |
+| Format | 8N1 |
+| Direction | Full-duplex |
+
+### Frame Format
+
+```
+Byte 0:   0xAA          (start-of-frame magic)
+Byte 1:   LEN           (payload length in bytes, uint8)
+Byte 2:   TYPE          (message type, uint8)
+Byte 3…N: PAYLOAD       (LEN bytes)
+Byte N+1: CRC8          (CRC-8/MAXIM over bytes 1..N)
+```
+
+### Message Types (draft — confirm in firmware)
+
+| Type | Direction | Payload | Description |
+|------|-----------|---------|-------------|
+| 0x01 | IO → BAL | `[ch1:u16][ch2:u16]…[ch16:u16][flags:u8]` | RC channels (16× uint16, 1000–2000 µs) + status flags |
+| 0x02 | IO → BAL | `[sensor_id:u8][value:f32]` | Sensor reading (ToF, baro, etc.) |
+| 0x03 | IO → BAL | `[nfc_uid:u8×7][len:u8]` | NFC tag detected |
+| 0x10 | BAL → IO | `[leds:u8][horn:u8][light:u8][fan:u8][buzz:u8]` | Actuator commands |
+| 0x11 | BAL → IO | `[state:u8][pitch:f32][speed:f32]` | Status for LED animations |
+| 0xFF | both | `[uptime_ms:u32]` | Heartbeat (watchdog reset) |
+
+> Frame types are **draft** — refer to `esp32/shared/protocol.h` for authoritative definitions.
+
+### CRC-8 Polynomial
+```
+Poly: 0x31 (CRC-8/MAXIM, also called CRC-8/1-Wire)
+Init: 0x00
+RefIn/RefOut: true
+XorOut: 0x00
+```
+
+---
+
+## 5. CAN Bus
+
+### Bus Parameters
+| Parameter | Value |
+|-----------|-------|
+| Bit rate | 500 kbps |
+| Topology | Single bus; all nodes share CANH/CANL + GND |
+| Termination | 120 Ω at each end of the bus |
+| Orin interface | CANable 2.0 USB↔CAN → `/dev/canable0` (or `can0` after `ip link`) |
+
+### Node Addresses
+
+| Node | CAN ID / Role |
+|------|--------------|
+| VESC left motor | ID **68** (0x44) — FSESC 6.7 Pro Mini Dual, left channel |
+| VESC right motor | ID **56** (0x38) — FSESC 6.7 Pro Mini Dual, right channel |
+| ESP32-S3 BALANCE | transmits telemetry 0x400–0x401; receives Orin cmds 0x300–0x303 |
+| Orin (CANable2) | transmits cmds 0x300–0x303; receives telemetry 0x400–0x401 |
+
+### Orin → Robot Command Frames (0x300–0x303)
+
+| CAN ID | DLC | Payload | Description |
+|--------|-----|---------|-------------|
+| **0x300** | 8 | `[speed:i16][steer:i16][mode:u8][flags:u8][_:u16]` | Drive command (speed/steer ±1000, mode byte) |
+| **0x301** | 1 | `[arm:u8]` | Arm/disarm (0x01 = arm, 0x00 = disarm) |
+| **0x302** | 8 | `[kp:f16][ki:f16][kd:f16][_:u16]` | PID update (half-float) |
+| **0x303** | 1 | `[0xE5]` | Emergency stop (magic byte, cuts all motors) |
+
+### Robot → Orin Telemetry Frames (0x400–0x401)
+
+| CAN ID | DLC | Payload | Description |
+|--------|-----|---------|-------------|
+| **0x400** | 8 | `[pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8]` | Attitude + drive state |
+| **0x401** | 4 | `[vbat:u16][fault_code:u8][rssi:i8]` | Battery voltage (mV), fault, RC RSSI |
+
+### VESC Native CAN (standard VESC protocol)
+
+ESP32-S3 BALANCE sends VESC commands using the standard VESC CAN protocol:
+
+| Frame type | CAN ID formula | Notes |
+|------------|---------------|-------|
+| SET_DUTY | `(0x00 << 8) | VESC_ID` | Duty cycle −1.0..+1.0 × 100000 |
+| SET_CURRENT | `(0x01 << 8) | VESC_ID` | Current in mA |
+| SET_RPM | `(0x03 << 8) | VESC_ID` | Electrical RPM |
+| SET_CURRENT_BRAKE | `(0x02 << 8) | VESC_ID` | Braking current in mA |
+| STATUS_1 | `(0x09 << 8) | VESC_ID` | ERPMs + current + duty (rx) |
+| STATUS_4 | `(0x10 << 8) | VESC_ID` | Temp + input voltage + input current (rx) |
+
+---
+
+## 6. RC Channel Mapping
+
+### Primary: TBS Crossfire (UART0 on ESP32-IO, CRSF @ 420000 baud)
+
+| Channel | Function | Range | Notes |
+|---------|----------|-------|-------|
+| CH1 | Roll / Steer | 1000–2000 µs | Left stick X (mode 2) |
+| CH2 | Pitch / Speed | 1000–2000 µs | Left stick Y |
+| CH3 | Throttle | 1000–2000 µs | Right stick Y |
+| CH4 | Yaw | 1000–2000 µs | Right stick X |
+| CH5 | Arm | 1000 / 2000 µs | 2-pos switch — <1200=DISARM, >1800=ARM |
+| CH6 | Drive mode | 1000/1500/2000 µs | 3-pos: RC / Assisted / Autonomous |
+| CH7 | Speed limit | 1000–2000 µs | Analog knob, scales max speed |
+| CH8 | Aux / Horn | 1000 / 2000 µs | Momentary for horn; held = klaxon |
+
+### Failover: ELRS (UART2 on ESP32-IO, CRSF @ 420000 baud)
+Same channel mapping as TBS Crossfire. IO board switches to ELRS automatically if Crossfire link lost for > 300 ms.
+
+### Failsafe
+- **RC lost > 300 ms**: motors stop, DISARM state, wait for link restoration
+- **Arm switch** must be cycled to DISARM→ARM to re-arm after failsafe
+
+---
+
+## 7. Serial Commands (Orin → ESP32-S3 BALANCE via CAN 0x300)
+
+All high-level commands from the Orin go over CAN. The BALANCE board also accepts direct USB-serial commands on its CH343 port for bench debugging:
+
+```
+# Arm / disarm
+ARM
+DISARM
+
+# Set drive (speed -1000..1000, steer -1000..1000)
+DRIVE <speed> <steer>
+
+# Set mode
+MODE RC           # full RC passthrough
+MODE ASSISTED     # Orin sends velocity, BALANCE does stability
+MODE AUTO         # full autonomous (Orin CAN commands only)
+
+# PID update
+PID <kp> <ki> <kd>
+
+# Emergency stop (same as CAN 0x303)
+ESTOP
+
+# Status query
+STATUS            # returns JSON telemetry line
+
+# IMU calibration
+IMU CAL
+```
+
+---
+
+## 8. System Wiring Diagram
+
+```
+                    ┌──────────────────────────────────────────────────────────┐
+                    │               JETSON ORIN NANO SUPER                     │
+                    │                  (Top plate, 25W)                        │
+                    │                                                          │
+                    │  USB-A ─── CANable 2.0 USB↔CAN (can0, 500 kbps)        │
+                    │  USB-A ─── RealSense D435i (USB 3.1)                   │
+                    │  USB-A ─── RPLIDAR A1M8 (CP2102, 115200)               │
+                    │  USB-C ─── SIM7600A 4G/LTE modem                       │
+                    │  CSI-A ─── 2× IMX219 cameras                           │
+                    │  CSI-B ─── 2× IMX219 cameras                           │
+                    │  40-pin ── ReSpeaker 2-Mic HAT                          │
+                    └──────────────────────┬───────────────────────────────────┘
+                                           │ USB-A
+                                           │ CANable 2.0
+                                           │ 500 kbps CAN
+          ┌────────────────────────────────┴──────────────────────────────────┐
+          │                         CAN BUS (CANH / CANL)                     │
+          │         120Ω ───┤  (all nodes)  ├─── 120Ω                        │
+          └───┬──────────────────────────┬───────────────────┬────────────────┘
+              │                          │                   │
+              ▼                          ▼                   ▼
+  ┌─────────────────────┐   ┌─────────────────────┐   (CAN ID 56/68)
+  │  ESP32-S3 BALANCE   │   │   VESC left  (ID 68)│   VESC right (ID 56)
+  │  Waveshare LCD 1.28 │   │   FSESC 6.7 Pro Mini│
+  │                     │   │   Dual              │
+  │  QMI8658 IMU (I2C)  │   └────────┬────────────┘
+  │  SN65HVD230 (CAN)   │            │  Phase wires
+  │  CH343 USB (debug)  │      ┌─────┴────────────────┐
+  │                     │      │  Hub motors (4×)      │
+  │  UART ─────────────────┐   │  FL / FR / RL / RR   │
+  └─────────────────────┘  │   └──────────────────────┘
+    ↑ 460800 baud binary    │
+    ↓ inter-board protocol  │
+  ┌─────────────────────────┘
+  │  ESP32-S3 IO (bare board)
+  │  JTAG USB (debug)
+  │
+  │  UART0 ── TBS Crossfire RX (CRSF, 420000)
+  │  UART2 ── ELRS receiver (CRSF failover, 420000)
+  │  PWM ──── 4× BTS7960 motor drivers
+  │  I2C ──── NFC + Baro + ToF sensors
+  │  GPIO ─── WS2812B LEDs
+  │  GPIO ─── Horn / Headlight / Fan / Buzzer
+  └─────────────────────────────────────────────
+```
+
+---
+
+## 9. Power Architecture
+
+```
+36V BATTERY
+  │
+  ├── VESCs (36V direct) ──── 4× Hub motors
+  │
+  ├── BTS7960 boards (36V → motor logic 5V via onboard reg)
+  │
+  ├── DC-DC 12V ──── Fan / Headlight / Accessories
+  │
+  └── DC-DC 5V ─┬── Jetson Orin (USB-C PD or barrel)
+                 ├── ESP32-S3 BALANCE (USB 5V or VIN)
+                 ├── ESP32-S3 IO (USB 5V or VIN)
+                 ├── TBS Crossfire RX (5V)
+                 ├── ELRS RX (5V)
+                 ├── WS2812B strip (5V)
+                 ├── RPLIDAR A1M8 (5V via USB)
+                 └── Sensors / NFC / ToF / Baro (3.3V LDO from ESP32)
+```
+
+---
+
+## 10. Device Nodes (Orin)
+
+| Device | Node | Notes |
+|--------|------|-------|
+| CANable 2.0 | `can0` | `ip link set can0 up type can bitrate 500000` |
+| RealSense D435i | `/dev/bus/usb/...` | realsense-ros driver |
+| RPLIDAR A1M8 | `/dev/rplidar` | udev symlink from ttyUSB* |
+| SIM7600A 4G | `/dev/ttyUSB0–2` | AT, PPP, GNSS NMEA |
+| IMX219 cameras | `/dev/video0,2,4,6` | CSI-A: 0/2, CSI-B: 4/6 |
+| ESP32-S3 BAL debug | `/dev/ttyACM0` or `/dev/esp32-balance` | CH343 CDC |
+| ESP32-S3 IO debug | `/dev/ttyACM1` or `/dev/esp32-io` | JTAG/CDC |
+
+---
+
+## 11. Safety Systems
+
+| System | Trigger | Action |
+|--------|---------|--------|
+| **HW kill switch** | Big red button (NC inline with 36V) | Cuts all power instantly |
+| **Tilt cutoff** | `|pitch| > 25°` | Motors off → DISARM, manual re-arm required |
+| **RC failsafe** | No RC frame > 300 ms | Motors off → DISARM |
+| **CAN watchdog** | No Orin heartbeat > 500 ms | Drop to RC-only mode |
+| **ESTOP CAN** | CAN frame 0x303 (magic 0xE5) | Immediate motor off, DISARM |
+| **ESTOP inter-board** | Type 0xFF heartbeat missing > 200 ms | IO board stops BTS7960 enables |
+| **Startup arming** | Cold power-on | Motors NEVER spin; need deliberate ARM |
+
+---
+
+## 12. Firmware Repository Layout
+
+```
+esp32/
+├── balance/          — ESP32-S3 BALANCE firmware (PlatformIO)
+│   ├── src/
+│   │   ├── main.cpp
+│   │   ├── config.h      ← GPIO assignments live here
+│   │   ├── imu_qmi8658.cpp/.h
+│   │   ├── can_vesc.cpp/.h
+│   │   └── protocol.cpp/.h
+│   └── platformio.ini
+├── io/               — ESP32-S3 IO firmware (PlatformIO)
+│   ├── src/
+│   │   ├── main.cpp
+│   │   ├── config.h      ← GPIO assignments live here
+│   │   ├── rc_crsf.cpp/.h
+│   │   ├── motor_bts7960.cpp/.h
+│   │   └── protocol.cpp/.h
+│   └── platformio.ini
+└── shared/
+    └── protocol.h    — inter-board frame types (authoritative)
+
+src/                  — LEGACY STM32 firmware (ARCHIVED — do not extend)
+include/              — LEGACY STM32 headers (ARCHIVED — do not extend)
+```
+
+---
+
+## 13. Open Questions / TBDs
+
+| Item | Owner | Status |
+|------|-------|--------|
+| GPIO assignments for CAN TX/RX on BALANCE board | sl-firmware | **TBD** |
+| GPIO assignments for IO board (all external pins) | sl-firmware | **TBD** |
+| Inter-board protocol message type table (finalized) | sl-firmware | **TBD** |
+| BTS7960 wiring — 4WD vs 2WD mode (all 4 or front 2 only) | max | **TBD** |
+| UWB ESP-NOW receiver — BALANCE or IO board? | sl-uwb + max | **TBD** |
+| VESC CAN IDs confirmed (68/56 left/right) | max | ✅ Confirmed |
+| Robot dimensions 870×510×550 mm, 23 kg | max | ✅ Confirmed |
+
+---
+
+_Last updated: 2026-04-04. Contact max or hal on MQTT for corrections._
--- a/docs/wiring-diagram.md
+++ b/docs/wiring-diagram.md
@ -1,6 +1,13 @@
-# SaltyLab Wiring Diagram
+# SaltyLab / SAUL-TEE Wiring Reference

-## System Overview
+> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired.
+> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
+> **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md)
+> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only.
+
+---
+
+## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md)

 ```
 ┌─────────────────────────────────────────────────────────────────────┐
@ -139,7 +146,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
 | 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |


-## FC UART Summary (MAMBA F722S)
+## FC UART Summary (MAMBA F722S — OBSOLETE)

 | UART | Pins | Baud | Assignment | Notes |
 |------|------|------|------------|-------|
--- 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,30 +1,16 @@
-# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
-# Binary-framed Jetson↔STM32 bridge at 921600 baud.
+# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
+# Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud.
+# Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]
+# Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5

 # ── Serial port ────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if the udev rule is applied:
-#   SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
-#   SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
-serial_port:      /dev/ttyACM0
-baud_rate:        921600
-reconnect_delay:  2.0        # seconds between USB reconnect attempts
+# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
+# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
+serial_port:      /dev/esp32-io
+baud_rate:        460800
+reconnect_delay:  2.0        # seconds between reconnect attempts

 # ── Heartbeat ─────────────────────────────────────────────────────────────────
-# HEARTBEAT frame sent every heartbeat_period seconds.
-# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
-heartbeat_period: 0.2        # 200ms → well within 500ms STM32 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
+# HEARTBEAT (0x20) sent every heartbeat_period.
+# ESP32 IO watchdog fires if no heartbeat for ~500 ms.
+heartbeat_period: 0.2        # 200 ms → well within 500 ms watchdog
--- 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 binary-framed STM32 command node (Issue #119).
+"""stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node.
+
+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
-
-  # 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
+  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
 """

 import os
@ -24,12 +24,9 @@ def generate_launch_description() -> LaunchDescription:
    params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")

    return LaunchDescription([
-        DeclareLaunchArgument("serial_port",      default_value="/dev/ttyACM0"),
-        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"),
+        DeclareLaunchArgument("serial_port",     default_value="/dev/esp32-io"),
+        DeclareLaunchArgument("baud_rate",        default_value="460800"),
+        DeclareLaunchArgument("heartbeat_period", default_value="0.2"),

        Node(
            package="saltybot_bridge",
@ -42,9 +39,6 @@ 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,45 +1,32 @@
-"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge.
+"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.

-Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
-framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
+Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
+inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).

-TX commands (Jetson → STM32):
-  SPEED_STEER  — 50 Hz from /cmd_vel subscription
-  HEARTBEAT    — 200 ms timer (STM32 watchdog fires at 500 ms)
-  ARM          — via /saltybot/arm service
-  SET_MODE     — via /saltybot/set_mode service
-  PID_UPDATE   — via /saltybot/pid_update topic
+This node is NOT the primary drive path (that is CAN via can_bridge_node).
+It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.

-Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
+Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]  @ 460800 baud

-RX telemetry (STM32 → Jetson):
-  IMU          → /saltybot/imu           (sensor_msgs/Imu)
-  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)
+RX from ESP32 IO:
+  RC_CHANNELS (0x01) → /saltybot/rc_channels  (std_msgs/String JSON)
+  SENSORS     (0x02) → /saltybot/sensors       (std_msgs/String JSON)

-Auto-reconnect: USB disconnect is detected when serial.read() raises; node
-continuously retries at reconnect_delay interval.
-
-This node owns /dev/ttyACM0 exclusively — do NOT run alongside
-serial_bridge_node or saltybot_cmd_node on the same port.
+TX to ESP32 IO:
+  LED_CMD     (0x10) ← /saltybot/leds          (std_msgs/String JSON)
+  OUTPUT_CMD  (0x11) ← /saltybot/outputs       (std_msgs/String JSON)
+  HEARTBEAT   (0x20)  — sent every heartbeat_period (keep IO watchdog alive)

 Parameters (config/stm32_cmd_params.yaml):
-  serial_port       /dev/ttyACM0
-  baud_rate         921600
-  reconnect_delay   2.0   (seconds)
-  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)
+  serial_port       /dev/esp32-io
+  baud_rate         460800
+  reconnect_delay   2.0
+  heartbeat_period  0.2   (ESP32 IO watchdog fires at ~500 ms)
 """

 from __future__ import annotations

 import json
-import math
 import threading
 import time

@ -50,119 +37,69 @@ 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,
-    ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
-    encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
-    encode_pid_update,
+    RcChannels,
+    SensorData,
+    encode_heartbeat,
+    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):
-    """Binary-framed Jetson↔STM32 bridge node."""
+    """Orin ↔ ESP32-S3 IO auxiliary bridge node."""

    def __init__(self) -> None:
        super().__init__("stm32_cmd_node")

-        # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("serial_port",      "/dev/ttyACM0")
-        self.declare_parameter("baud_rate",        921600)
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",      "/dev/esp32-io")
+        self.declare_parameter("baud_rate",        BAUD_RATE)
        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)

-        port                  = self.get_parameter("serial_port").value
-        baud                  = self.get_parameter("baud_rate").value
+        self._port_name       = self.get_parameter("serial_port").value
+        self._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 ───────────────────────────────────────────────────────────────
-        sensor_qos = QoSProfile(
-            reliability=ReliabilityPolicy.BEST_EFFORT,
-            history=HistoryPolicy.KEEP_LAST, depth=10,
-        )
+        # ── QoS ───────────────────────────────────────────────────────────
        rel_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST, depth=10,
        )

-        # ── Publishers ────────────────────────────────────────────────────────
-        self._imu_pub      = self.create_publisher(Imu,    "/saltybot/imu",           sensor_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)
+        # ── Publishers ────────────────────────────────────────────────────
+        self._rc_pub   = self.create_publisher(String,          "/saltybot/rc_channels", rel_qos)
+        self._sens_pub = self.create_publisher(String,          "/saltybot/sensors",     rel_qos)
+        self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics",          rel_qos)

-        # ── Subscribers ───────────────────────────────────────────────────────
-        self._cmd_vel_sub = self.create_subscription(
-            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,
-        )
+        # ── Subscriptions ─────────────────────────────────────────────────
+        self.create_subscription(String, "/saltybot/leds",    self._on_leds,    rel_qos)
+        self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)

-        # ── 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
+        # ── Serial state ──────────────────────────────────────────────────
        self._ser: serial.Serial | None = None
-        self._ser_lock  = threading.Lock()
-        self._parser    = FrameParser()
+        self._ser_lock = threading.Lock()
+        self._parser   = FrameParser()
+        self._rx_count = 0

-        # ── 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 ──────────────────────────────────────
+        # ── 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._read_timer = self.create_timer(0.005,           self._read_cb)
        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._diag_timer = self.create_timer(1.0,             self._publish_diagnostics)

        self.get_logger().info(
-            f"stm32_cmd_node started — {port} @ {baud} baud | "
-            f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
+            f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
        )

-    # ── Serial management ─────────────────────────────────────────────────────
+    # ── Serial management ─────────────────────────────────────────────────

    def _open_serial(self) -> bool:
        with self._ser_lock:
@ -170,7 +107,7 @@ class Stm32CmdNode(Node):
                self._ser = serial.Serial(
                    port=self._port_name,
                    baudrate=self._baud,
-                    timeout=0.005,          # non-blocking reads
+                    timeout=0.005,
                    write_timeout=0.1,
                )
                self._ser.reset_input_buffer()
@ -185,17 +122,7 @@ 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
@ -207,16 +134,15 @@ class Stm32CmdNode(Node):
                self._ser = None
                return False

-    # ── RX — read callback ────────────────────────────────────────────────────
+    # ── RX ────────────────────────────────────────────────────────────────

    def _read_cb(self) -> None:
-        """Read bytes from serial and feed them to the frame parser."""
        raw: bytes | None = None
-        reconnect_needed  = False
+        reconnect = False

        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
-                reconnect_needed = True
+                reconnect = True
            else:
                try:
                    n = self._ser.in_waiting
@ -225,9 +151,9 @@ class Stm32CmdNode(Node):
                except serial.SerialException as exc:
                    self.get_logger().error(f"Serial read error: {exc}")
                    self._ser = None
-                    reconnect_needed = True
+                    reconnect = True

-        if reconnect_needed:
+        if reconnect:
            self.get_logger().warn(
                "Serial disconnected — will retry",
                throttle_duration_sec=self._reconnect_delay,
@ -240,230 +166,105 @@ class Stm32CmdNode(Node):
            return

        for byte in raw:
-            frame = self._parser.feed(byte)
-            if frame is not None:
-                self._rx_frame_count += 1
-                self._dispatch_frame(frame)
+            msg = self._parser.feed(byte)
+            if msg is not None:
+                self._rx_count += 1
+                self._dispatch(msg)

-    def _dispatch_frame(self, frame) -> None:
-        """Route a decoded frame to the appropriate publisher."""
+    def _dispatch(self, msg) -> None:
        now = self.get_clock().now().to_msg()
+        ts  = f"{now.sec}.{now.nanosec:09d}"

-        if isinstance(frame, ImuFrame):
-            self._publish_imu(frame, now)
+        if isinstance(msg, RcChannels):
+            out = String()
+            out.data = json.dumps({
+                "channels": msg.channels,
+                "source":   msg.source,
+                "ts":       ts,
+            })
+            self._rc_pub.publish(out)

-        elif isinstance(frame, BatteryFrame):
-            self._publish_battery(frame, now)
+        elif isinstance(msg, SensorData):
+            out = String()
+            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(frame, MotorRpmFrame):
-            self._publish_motor_rpm(frame, now)
+        elif isinstance(msg, tuple):
+            type_code, _ = msg
+            self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")

-        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 STM32 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"STM32 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,
-            )
+    # ── TX ────────────────────────────────────────────────────────────────

    def _heartbeat_cb(self) -> None:
-        """Send HEARTBEAT every heartbeat_period (default 200ms)."""
        self._write(encode_heartbeat())

-    def _watchdog_cb(self) -> None:
-        """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
-        if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
-            if not self._watchdog_sent:
-                self.get_logger().warn(
-                    f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
-                )
-                self._watchdog_sent = True
-            self._last_speed = 0
-            self._last_steer = 0
-            self._write(encode_speed_steer(0, 0))
-
-    def _on_pid_update(self, msg: String) -> None:
-        """Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
+    def _on_leds(self, msg: String) -> None:
+        """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
        try:
-            data = json.loads(msg.data)
-            kp = float(data["kp"])
-            ki = float(data["ki"])
-            kd = float(data["kd"])
+            d = json.loads(msg.data)
+            frame = encode_led_cmd(
+                int(d.get("pattern", 0)),
+                int(d.get("r", 0)),
+                int(d.get("g", 0)),
+                int(d.get("b", 0)),
+            )
        except (ValueError, KeyError, json.JSONDecodeError) as exc:
-            self.get_logger().error(f"Bad PID update JSON: {exc}")
+            self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
            return
-        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")
+        self._write(frame)

-    # ── Services ──────────────────────────────────────────────────────────────
+    def _on_outputs(self, msg: String) -> None:
+        """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
+        try:
+            d = json.loads(msg.data)
+            frame = encode_output_cmd(
+                bool(d.get("horn",     False)),
+                bool(d.get("buzzer",   False)),
+                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}")
+            return
+        self._write(frame)

-    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 ───────────────────────────────────────────────────────────
+    # ── Diagnostics ───────────────────────────────────────────────────────

    def _publish_diagnostics(self) -> None:
        diag = DiagnosticArray()
        diag.header.stamp = self.get_clock().now().to_msg()
-
        status = DiagnosticStatus()
-        status.name        = "saltybot/stm32_cmd_node"
-        status.hardware_id = "stm32f722"
-
+        status.name        = "saltybot/esp32_io_bridge"
+        status.hardware_id = "esp32-s3-io"
        port_ok = self._ser is not None and self._ser.is_open
-        if port_ok:
-            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="port_open",      value=str(port_ok)),
-            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, "?")),
+        status.level   = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
+        status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
+        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_errors",   value=str(self._parser.frames_error)),
        ]
        diag.status.append(status)
        self._diag_pub.publish(diag)

-    # ── Lifecycle ─────────────────────────────────────────────────────────────
+    # ── Lifecycle ─────────────────────────────────────────────────────────

    def destroy_node(self) -> None:
-        # Send zero-speed + disarm on shutdown
-        self._write(encode_speed_steer(0, 0))
-        self._write(encode_arm(False))
-        self._close_serial()
+        self._write(encode_heartbeat(state=0))
+        with self._ser_lock:
+            if self._ser and self._ser.is_open:
+                try:
+                    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
@ -1,332 +1,260 @@
-"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
+"""stm32_protocol.py — Inter-board UART frame codec (ESP32 BALANCE ↔ ESP32 IO).

-Issue #119: defines the binary serial protocol between the Jetson Nano and the
-STM32F722 flight controller over USB CDC @ 921600 baud.
+File name retained for import compatibility. This module implements the binary
+serial protocol that runs between the two ESP32-S3 embedded boards.

-Frame layout (all multi-byte fields are big-endian):
-  ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
-  │  STX │ TYPE │  LEN │     PAYLOAD      │  CRC16    │  ETX │
-  │ 0x02 │  1B  │  1B  │    LEN bytes     │   2B BE   │ 0x03 │
-  └──────┴──────┴──────┴──────────────────┴───────────┴──────┘
+Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 (2026-04-04)
+Physical: UART @ 460800 baud, 8N1

-CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
-CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
+Frame layout:
+  ┌────────┬───────┬───────┬──────────────────┬───────┐
+  │ HEADER │  LEN  │  TYPE │    PAYLOAD       │ CRC8  │
+  │  0xAA  │  1 B  │  1 B  │   LEN bytes      │  1 B  │
+  └────────┴───────┴───────┴──────────────────┴───────┘

-Command types (Jetson → STM32):
-  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)
+CRC8 covers: LEN + TYPE + PAYLOAD  (polynomial 0x07, init 0x00).
+Max payload: 64 bytes.  No ETX byte.

-Telemetry types (STM32 → 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)
+Note: the Orin communicates with ESP32 BALANCE via CAN (CANable2/slcan0),
+NOT via this serial protocol.  See mamba_protocol.py for the CAN frame codec.

-Usage:
-  # Encoding (Jetson → STM32)
-  frame = encode_speed_steer(300, -150)
-  ser.write(frame)
+Message types — IO → BALANCE:
+  0x01  RC_CHANNELS  — raw RC channel values (CRSF or ELRS)
+  0x02  SENSORS      — barometer + ToF data

-  # Decoding (STM32 → 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)
+Message types — BALANCE → IO:
+  0x10  LED_CMD      — LED strip pattern  [pattern u8][r u8][g u8][b u8]
+  0x11  OUTPUT_CMD   — horn/buzzer/headlight/fan  [flags u8][headlight u8][fan u8]
+  0x12  MOTOR_CMD    — BTS7960 aux motor  [motor_a i16 BE][motor_b i16 BE]
+  0x20  HEARTBEAT    — status keepalive   [state u8][error_flags u8]
+
+RC channel mapping (CH1–CH8, CRSF range 172–1811, centre 992):
+  CH1 Steer   CH2 Drive   CH3 Throttle  CH4 Yaw
+  CH5 Arm     CH6 Mode    CH7 E-stop    CH8 Spare
 """

 from __future__ import annotations

 import struct
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 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
+FRAME_HEADER    = 0xAA
+MAX_PAYLOAD_LEN = 64
+BAUD_RATE       = 460800   # inter-board UART
+
+# ── Message type codes ────────────────────────────────────────────────────────
+
+class IOMsg(IntEnum):
+    RC_CHANNELS = 0x01
+    SENSORS     = 0x02


-# ── Command / telemetry type codes ────────────────────────────────────────────
-
-class CmdType(IntEnum):
-    HEARTBEAT   = 0x01
-    SPEED_STEER = 0x02
-    ARM         = 0x03
-    SET_MODE    = 0x04
-    PID_UPDATE  = 0x05
+class BalMsg(IntEnum):
+    LED_CMD    = 0x10
+    OUTPUT_CMD = 0x11
+    MOTOR_CMD  = 0x12
+    HEARTBEAT  = 0x20


-class TelType(IntEnum):
-    IMU       = 0x10
-    BATTERY   = 0x11
-    MOTOR_RPM = 0x12
-    ARM_STATE = 0x13
-    ERROR     = 0x14
-
-
-# ── Parsed telemetry objects ──────────────────────────────────────────────────
+# ── Parsed message dataclasses ────────────────────────────────────────────────

@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
+class RcChannels:
+    """RC channel values from TBS Crossfire (primary) or ELRS (failover).
+    CRSF range: 172–1811 (centre 992).
+    """
+    channels: list = field(default_factory=lambda: [992] * 8)
+    source: int = 0   # 0 = CRSF, 1 = ELRS failover


@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)
+class SensorData:
+    pressure_pa:   float = 0.0   # Pascal (barometer)
+    temperature_c: float = 0.0   # °C
+    tof_mm:        int   = 0     # Time-of-flight distance mm


@dataclass
-class MotorRpmFrame:
-    left_rpm:   int
-    right_rpm:  int
+class LedCmd:
+    pattern: int = 0
+    r: int = 0
+    g: int = 0
+    b: int = 0


@dataclass
-class ArmStateFrame:
-    state:       int      # 0=DISARMED 1=ARMED 2=TILT_FAULT
-    error_flags: int      # bitmask
+class OutputCmd:
+    horn:      bool = False
+    buzzer:    bool = False
+    headlight: int  = 0    # 0–255 PWM
+    fan:       int  = 0    # 0–255 PWM


@dataclass
-class ErrorFrame:
-    error_code: int
-    subcode:    int
+class MotorCmd:
+    motor_a: int = 0   # -255..+255 (BTS7960 channel A)
+    motor_b: int = 0   # -255..+255 (BTS7960 channel B)


-# Union type for decoded results
-TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame
+@dataclass
+class Heartbeat:
+    state:       int = 0    # 0=IDLE  1=RUNNING  2=FAULT
+    error_flags: int = 0


-# ── CRC16 CCITT ───────────────────────────────────────────────────────────────
+InterboardMsg = RcChannels | SensorData | LedCmd | OutputCmd | MotorCmd | Heartbeat

-def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
-    """CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR."""
-    crc = init
+
+# ── CRC-8 (polynomial 0x07, init 0x00) ───────────────────────────────────────
+
+def _crc8(data: bytes) -> int:
+    crc = 0
    for byte in data:
-        crc ^= byte << 8
+        crc ^= byte
        for _ in range(8):
-            if crc & 0x8000:
-                crc = (crc << 1) ^ 0x1021
-            else:
-                crc <<= 1
-        crc &= 0xFFFF
+            crc = ((crc << 1) ^ 0x07) if (crc & 0x80) else (crc << 1)
+            crc &= 0xFF
    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 _build_frame(msg_type: int, payload: bytes) -> bytes:
+    assert len(payload) <= MAX_PAYLOAD_LEN
+    crc_data = bytes([len(payload), msg_type]) + payload
+    return bytes([FRAME_HEADER, len(payload), msg_type]) + payload + bytes([_crc8(crc_data)])


-def encode_heartbeat() -> bytes:
-    """HEARTBEAT frame — no payload."""
-    return _build_frame(CmdType.HEARTBEAT, b"")
+def encode_heartbeat(state: int = 0, error_flags: int = 0) -> bytes:
+    return _build_frame(BalMsg.HEARTBEAT, struct.pack("BB", state & 0xFF, error_flags & 0xFF))


-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_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes:
+    return _build_frame(BalMsg.LED_CMD, struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF))


-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_output_cmd(horn: bool, buzzer: bool, headlight: int, fan: int) -> bytes:
+    flags = (int(horn) & 1) | ((int(buzzer) & 1) << 1)
+    return _build_frame(BalMsg.OUTPUT_CMD, struct.pack("BBB", flags, headlight & 0xFF, fan & 0xFF))


-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_motor_cmd(motor_a: int, motor_b: int) -> bytes:
+    a = max(-255, min(255, int(motor_a)))
+    b = max(-255, min(255, int(motor_b)))
+    return _build_frame(BalMsg.MOTOR_CMD, struct.pack(">hh", a, b))


-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))
+# ── Streaming frame parser ────────────────────────────────────────────────────

-
-# ── 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 _St(IntEnum):
+    HDR  = 0
+    LEN  = 1
+    TYPE = 2
+    PAY  = 3
+    CRC  = 4


 class FrameParser:
-    """Byte-by-byte streaming parser for STM32 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.
+    """Byte-by-byte streaming parser for inter-board frames.

    Usage::
        parser = FrameParser()
        for b in incoming:
-            result = parser.feed(b)
-            if result is not None:
-                process(result)
+            msg = parser.feed(b)
+            if msg is not None:
+                handle(msg)
    """

-    def __init__(self) -> None:
-        self._state    = ParserState.WAIT_STX
-        self._type     = 0
-        self._length   = 0
-        self._payload  = bytearray()
-        self._crc_rcvd = 0
+    def __init__(self):
+        self._s    = _St.HDR
+        self._len  = 0
+        self._type = 0
+        self._pay  = bytearray()
        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 reset(self):
+        self._s   = _St.HDR
+        self._pay = bytearray()

-    def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
-        """Process one byte. Returns decoded frame on success, None otherwise.
+    def feed(self, byte: int) -> Optional[InterboardMsg | tuple]:
+        s = self._s

-        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
+        if s == _St.HDR:
+            if byte == FRAME_HEADER:
+                self._s = _St.LEN
            return None

-        if s == ParserState.WAIT_TYPE:
-            self._type  = byte
-            self._state = ParserState.WAIT_LEN
+        if s == _St.LEN:
+            if byte > MAX_PAYLOAD_LEN:
+                self.frames_error += 1; self.reset(); return None
+            self._len = byte
+            self._s   = _St.TYPE
            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
+        if s == _St.TYPE:
+            self._type = byte
+            self._pay  = bytearray()
+            self._s    = _St.PAY if self._len > 0 else _St.CRC
            return None

-        if s == ParserState.PAYLOAD:
-            self._payload.append(byte)
-            if len(self._payload) == self._length:
-                self._state = ParserState.CRC_HI
+        if s == _St.PAY:
+            self._pay.append(byte)
+            if len(self._pay) == self._len:
+                self._s = _St.CRC
            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
+        if s == _St.CRC:
+            self.reset()
+            expected = _crc8(bytes([self._len, self._type]) + self._pay)
+            if byte != expected:
+                self.frames_error += 1; return None
            self.frames_ok += 1
-            return _decode_telemetry(self._type, bytes(self._payload))
+            return _decode(self._type, bytes(self._pay))

-        # Should never reach here
-        self.reset()
-        return None
+        self.reset(); return None


-# ── Telemetry decoder ─────────────────────────────────────────────────────────
+# ── Message decoder ───────────────────────────────────────────────────────────

-def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
-    """Decode a validated telemetry payload into a typed dataclass."""
+def _decode(type_code: int, payload: bytes) -> Optional[InterboardMsg | tuple]:
    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 == IOMsg.RC_CHANNELS:
+            if len(payload) < 17: return None
+            ch = list(struct.unpack_from("<8H", payload))
+            return RcChannels(channels=ch, source=payload[16])

-        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 == IOMsg.SENSORS:
+            if len(payload) < 10: return None
+            pres, temp, tof = struct.unpack_from("<ffH", payload)
+            return SensorData(pressure_pa=pres, temperature_c=temp, tof_mm=tof)

-        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 == BalMsg.LED_CMD:
+            if len(payload) < 4: return None
+            pat, r, g, b = struct.unpack_from("BBBB", payload)
+            return LedCmd(pattern=pat, r=r, g=g, b=b)

-        if type_code == TelType.ARM_STATE:
-            if len(payload) < 2:
-                return None
+        if type_code == BalMsg.OUTPUT_CMD:
+            if len(payload) < 3: return None
+            flags, headlight, fan = struct.unpack_from("BBB", payload)
+            return OutputCmd(horn=bool(flags & 1), buzzer=bool(flags & 2),
+                             headlight=headlight, fan=fan)
+
+        if type_code == BalMsg.MOTOR_CMD:
+            if len(payload) < 4: return None
+            a, b = struct.unpack_from(">hh", payload)
+            return MotorCmd(motor_a=a, motor_b=b)
+
+        if type_code == BalMsg.HEARTBEAT:
+            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)
+            return Heartbeat(state=state, error_flags=flags)

    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,27 +1,34 @@
 #!/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.
+can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
+board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps).

-The node opens the SocketCAN interface (slcan0 by default), spawns a background
-reader thread to process incoming telemetry, and exposes the following interface:
+Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)

 Subscriptions
 -------------
-  /cmd_vel          geometry_msgs/Twist          → VESC speed commands (CAN)
-  /estop            std_msgs/Bool                → Mamba e-stop (CAN)
+  /cmd_vel              geometry_msgs/Twist     → ORIN_CMD_DRIVE (0x300)
+  /estop                std_msgs/Bool           → ORIN_CMD_ESTOP (0x303)
+  /saltybot/arm         std_msgs/Bool           → ORIN_CMD_ARM   (0x301)

 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"
+  /saltybot/attitude      std_msgs/String (JSON)   pitch, speed, yaw_rate, state
+  /saltybot/balance_state std_msgs/String (JSON)   alias of /saltybot/attitude
+  /can/battery            sensor_msgs/BatteryState  vbat_mv, fault, rssi
+  /can/vesc/left/state    std_msgs/Float32MultiArray  VESC STATUS_1 left
+  /can/vesc/right/state   std_msgs/Float32MultiArray  VESC STATUS_1 right
+  /can/connection_status  std_msgs/String            "connected" | "disconnected"

-Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+Parameters
+----------
+  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
@ -30,90 +37,77 @@ import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
-from rcl_interfaces.msg import SetParametersResult
-from sensor_msgs.msg import BatteryState, Imu
+from sensor_msgs.msg import BatteryState
 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,
-    ORIN_CAN_ID_FC_PID_ACK,
-    ORIN_CAN_ID_PID_SET,
+    ORIN_CMD_DRIVE,
+    ORIN_CMD_ARM,
+    ORIN_CMD_ESTOP,
+    ESP32_TELEM_ATTITUDE,
+    ESP32_TELEM_BATTERY,
+    VESC_LEFT_ID,
+    VESC_RIGHT_ID,
+    VESC_STATUS_1,
    MODE_DRIVE,
-    MODE_ESTOP,
    MODE_IDLE,
+    encode_drive_cmd,
+    encode_arm_cmd,
    encode_estop_cmd,
-    encode_mode_cmd,
-    encode_velocity_cmd,
-    encode_pid_set_cmd,
-    decode_battery_telem,
-    decode_imu_telem,
-    decode_pid_ack,
-    decode_vesc_state,
+    decode_attitude,
+    decode_battery,
+    decode_vesc_status1,
 )

 # Reconnect attempt interval when CAN bus is lost
 _RECONNECT_INTERVAL_S: float = 5.0

-# Watchdog timer tick rate (Hz)
+# Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent
 _WATCHDOG_HZ: float = 10.0


 class CanBridgeNode(Node):
-    """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers."""
+    """CAN bus bridge between Orin ROS2 and ESP32 BALANCE / 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("can_interface",     "slcan0")
+        self.declare_parameter("left_vesc_can_id",  VESC_LEFT_ID)
+        self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
+        self.declare_parameter("speed_scale",       1000.0)
+        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: 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
-        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
+        self._iface         = self.get_parameter("can_interface").value
+        self._left_vesc_id  = self.get_parameter("left_vesc_can_id").value
+        self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
+        self._speed_scale   = self.get_parameter("speed_scale").value
+        self._steer_scale   = self.get_parameter("steer_scale").value
+        self._cmd_timeout   = 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
+        self._lock = threading.Lock()

        # ── 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
-        )
+        self._pub_attitude  = self.create_publisher(String,           "/saltybot/attitude",      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_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.add_on_set_parameters_callback(self._on_set_parameters)
+        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)

        # ── Timers ────────────────────────────────────────────────────────
-        self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
+        self.create_timer(1.0 / _WATCHDOG_HZ,  self._watchdog_cb)
        self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb)

        # ── Open CAN ──────────────────────────────────────────────────────
@ -128,46 +122,17 @@ 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"mamba={self._mamba_id}"
+            f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
        )

-    # -- 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:
-                bus = can.interface.Bus(
-                    channel=self._iface,
-                    bustype="socketcan",
-                )
-                self._bus = bus
+                self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
                self._connected = True
                self.get_logger().info(f"CAN bus connected: {self._iface}")
                self._publish_status("connected")
@ -180,12 +145,10 @@ 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:
@ -200,73 +163,51 @@ class CanBridgeNode(Node):
    # ── ROS callbacks ─────────────────────────────────────────────────────

    def _cmd_vel_cb(self, msg: Twist) -> None:
-        """Convert /cmd_vel Twist to VESC speed commands over CAN."""
+        """Convert /cmd_vel Twist to ORIN_CMD_DRIVE 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")
+        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)))
+        self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")

    def _estop_cb(self, msg: Bool) -> None:
-        """Forward /estop to Mamba over CAN."""
+        """Forward /estop to ESP32 BALANCE 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")
+            self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
+            self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
+        else:
+            # Clear estop: send DISARM then re-ARM (let operator decide to re-arm)
+            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 no /cmd_vel arrives within the timeout, send zero velocity."""
+        """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive)."""
        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"
-            )
+        if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
+            self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")

    # ── CAN send helper ───────────────────────────────────────────────────

-    def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
-        """Send a standard CAN frame; handle errors gracefully."""
+    def _send_can(self, arb_id: int, data: bytes, context: str,
+                  extended: bool = False) -> None:
        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,
-        )
+        msg = can.Message(arbitration_id=arb_id, data=data,
+                          is_extended_id=extended)
        try:
            bus.send(msg, timeout=0.05)
        except can.CanError as exc:
@ -275,55 +216,41 @@ 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 = self._connected
-                bus = self._bus
-
+                connected, bus = self._connected, 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)
-
+        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 == 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")
-
-            elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
-                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}"
-                )
-
+            if arb_id == ESP32_TELEM_ATTITUDE:
+                self._handle_attitude(data)
+            elif arb_id == ESP32_TELEM_BATTERY:
+                self._handle_battery(data)
+            elif arb_id == vesc_l:
+                t = decode_vesc_status1(self._left_vesc_id, data)
+                m = Float32MultiArray()
+                m.data = [t.erpm, t.duty, 0.0, t.current]
+                self._pub_vesc_left.publish(m)
+            elif arb_id == vesc_r:
+                t = decode_vesc_status1(self._right_vesc_id, data)
+                m = Float32MultiArray()
+                m.data = [t.erpm, t.duty, 0.0, t.current]
+                self._pub_vesc_right.publish(m)
        except Exception as exc:
            self.get_logger().warning(
                f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
@ -331,52 +258,36 @@ class CanBridgeNode(Node):

    # ── Frame handlers ────────────────────────────────────────────────────

-    def _handle_imu(self, data: bytes, timestamp: float) -> None:
-        telem = decode_imu_telem(data)
+    _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}

-        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)
+    def _handle_attitude(self, data: bytes) -> None:
+        """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
+        t = decode_attitude(data)
+        now = self.get_clock().now().to_msg()
+        payload = {
+            "pitch_deg":  round(t.pitch_deg, 2),
+            "speed_mps":  round(t.speed, 3),
+            "yaw_rate":   round(t.yaw_rate, 3),
+            "state":      t.state,
+            "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
+            "flags":      t.flags,
+            "ts":         f"{now.sec}.{now.nanosec:09d}",
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._pub_attitude.publish(msg)
+        self._pub_balance.publish(msg)   # keep /saltybot/balance_state alive

+    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 = telem.voltage
-        msg.current = telem.current
+        msg.voltage = t.vbat_mv / 1000.0
        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:
@ -387,17 +298,10 @@ 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(
-                    MAMBA_CMD_VELOCITY,
-                    encode_velocity_cmd(0.0, 0.0),
-                    "shutdown",
-                )
-                self._send_can(
-                    MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
-                )
+                self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
+                self._send_can(ORIN_CMD_ARM,   encode_arm_cmd(False), "shutdown")
            except Exception:
                pass
            try:
@ -407,8 +311,6 @@ 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
@ -1,224 +1,217 @@
 #!/usr/bin/env python3
-"""
-mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller
-and VESC telemetry.
+"""mamba_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE.

-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
+Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
+File name retained for import compatibility.

-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)
+CAN bus: 500 kbps, standard 11-bit IDs, CANable 2.0 (slcan0 / can0) on Orin.

-VESC telemetry frame (VESC → Orin):
-  VESC_TELEM_STATE    0x300  16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32)
+── Orin → ESP32 BALANCE (commands) ───────────────────────────────────────────
+  0x300  DRIVE    8 B  [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16]
+  0x301  ARM      1 B  [arm:u8]  0x00=DISARM  0x01=ARM
+  0x302  PID_SET  8 B  [kp:f16 BE][ki:f16 BE][kd:f16 BE][_:u16]
+  0x303  ESTOP    1 B  [0xE5]   magic byte — cuts all motors immediately

-All multi-byte fields are big-endian.
+── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
+  0x400  ATTITUDE  8 B  [pitch:f16 BE][speed:f16 BE][yaw_rate:f16 BE][state:u8][flags:u8]
+  0x401  BATTERY   4 B  [vbat_mv:u16 BE][fault_code:u8][rssi:i8]

-Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+speed/steer range: −1000..+1000 (motor units).  f16 = IEEE 754 half-precision.
+
+VESC standard extended CAN (29-bit IDs = packet_type<<8 | node_id):
+  Left VESC  node ID = 56 (0x38)
+  Right VESC node ID = 68 (0x44)
+  STATUS_1   cmd=9   erpm i32 BE, current i16 (/10 A), duty i16 (/1000)
+  STATUS_4   cmd=16  temp_fet i16 (/10 °C), temp_mot i16 (/10 °C), cur_in i16 (/10 A)
+  STATUS_5   cmd=27  tacho i32, vbat i16 (/10 V)
 """

 import struct
 from dataclasses import dataclass
-from typing import Tuple

-# ---------------------------------------------------------------------------
-# CAN message IDs
-# ---------------------------------------------------------------------------
+# ── CAN IDs ───────────────────────────────────────────────────────────────────

-MAMBA_CMD_VELOCITY: int = 0x100
-MAMBA_CMD_MODE: int = 0x101
-MAMBA_CMD_ESTOP: int = 0x102
+# Orin → ESP32 BALANCE
+ORIN_CMD_DRIVE: int  = 0x300
+ORIN_CMD_ARM:   int  = 0x301
+ORIN_CMD_PID:   int  = 0x302
+ORIN_CMD_ESTOP: int  = 0x303

-MAMBA_TELEM_IMU: int = 0x200
-MAMBA_TELEM_BATTERY: int = 0x201
+# ESP32 BALANCE → Orin
+ESP32_TELEM_ATTITUDE: int = 0x400
+ESP32_TELEM_BATTERY:  int = 0x401

-VESC_TELEM_STATE: int = 0x300
-ORIN_CAN_ID_PID_SET: int = 0x305
-ORIN_CAN_ID_FC_PID_ACK: int = 0x405
+# Backward-compat aliases used by other nodes
+FC_STATUS: int = ESP32_TELEM_ATTITUDE
+FC_VESC:   int = ESP32_TELEM_BATTERY

-# ---------------------------------------------------------------------------
-# Mode constants
-# ---------------------------------------------------------------------------
+# VESC node IDs
+VESC_LEFT_ID:  int = 56
+VESC_RIGHT_ID: int = 68

-MODE_IDLE: int = 0
-MODE_DRIVE: int = 1
-MODE_ESTOP: int = 2
+# VESC packet types
+VESC_STATUS_1: int = 9
+VESC_STATUS_4: int = 16
+VESC_STATUS_5: int = 27

-# ---------------------------------------------------------------------------
-# Data classes for decoded telemetry
-# ---------------------------------------------------------------------------
+# ── Mode constants (DRIVE frame mode byte) ─────────────────────────────────────

+MODE_IDLE:       int = 0   # RC passthrough, Orin not injecting
+MODE_DRIVE:      int = 1   # Orin velocity commands
+MODE_AUTONOMOUS: int = 2   # full autonomy
+MODE_ESTOP:      int = 2   # alias
+
+# ESTOP magic byte
+_ESTOP_MAGIC: int = 0xE5
+
+# ── Struct formats (big-endian) ────────────────────────────────────────────────
+
+_FMT_DRIVE    = ">hhBBH"   # i16 speed, i16 steer, u8 mode, u8 flags, u16 pad
+_FMT_PID      = ">eeeH"    # f16 kp, f16 ki, f16 kd, u16 pad
+_FMT_ATTITUDE = ">eeeBB"   # f16 pitch, f16 speed, f16 yaw_rate, u8 state, u8 flags
+_FMT_BATTERY  = ">HBb"     # u16 vbat_mv, u8 fault_code, i8 rssi
+
+
+# ── Data classes ──────────────────────────────────────────────────────────────

@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
+class AttitudeTelemetry:
+    """Decoded ATTITUDE (0x400) from ESP32 BALANCE."""
+    pitch_deg:  float = 0.0   # degrees, half-float
+    speed:      float = 0.0   # m/s, half-float
+    yaw_rate:   float = 0.0   # rad/s, half-float
+    state:      int   = 0     # 0=IDLE  1=RUNNING  2=FAULT
+    flags:      int   = 0     # error bitmask


@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
+    """Decoded BATTERY (0x401) from ESP32 BALANCE."""
+    vbat_mv:    int = 0       # millivolts
+    fault_code: int = 0       # 0 = OK
+    rssi:       int = 0       # RC signal dBm (signed)


@dataclass
 class PidGains:
-    """Balance PID gains (Issue #693)."""
+    """Balance PID gains."""
    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
+@dataclass
+class VescStatus1:
+    """Decoded VESC STATUS (cmd=9) — direct from VESC."""
+    node_id: int   = 0
+    erpm:    float = 0.0
+    current: float = 0.0   # A
+    duty:    float = 0.0   # -1.0..+1.0


-def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
+@dataclass
+class VescStatus4:
+    """Decoded VESC STATUS_4 (cmd=16)."""
+    node_id:      int   = 0
+    temp_fet_c:   float = 0.0
+    temp_motor_c: float = 0.0
+    current_in:   float = 0.0
+
+
+@dataclass
+class VescStatus5:
+    """Decoded VESC STATUS_5 (cmd=27)."""
+    node_id: int   = 0
+    tacho:   int   = 0
+    vbat_v:  float = 0.0
+
+
+# ── Orin → BALANCE encoders ───────────────────────────────────────────────────
+
+def encode_drive_cmd(speed: int, steer: int,
+                     mode: int = MODE_DRIVE, flags: int = 0) -> bytes:
+    """Encode ORIN_CMD_DRIVE (0x300) — 8 bytes.
+
+    speed: −1000..+1000 motor units (positive = forward)
+    steer: −1000..+1000 motor units (positive = right)
+    mode:  MODE_IDLE / MODE_DRIVE / MODE_AUTONOMOUS
    """
-    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))
+    speed = max(-1000, min(1000, int(speed)))
+    steer = max(-1000, min(1000, int(steer)))
+    return struct.pack(_FMT_DRIVE, speed, steer, mode & 0xFF, flags & 0xFF, 0)


-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_arm_cmd(arm: bool) -> bytes:
+    """Encode ORIN_CMD_ARM (0x301) — 1 byte."""
+    return struct.pack("B", 0x01 if arm else 0x00)


-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_cmd(kp: float, ki: float, kd: float) -> bytes:
+    """Encode ORIN_CMD_PID (0x302) — 8 bytes (3× half-float + 2-byte pad)."""
+    return struct.pack(_FMT_PID, float(kp), float(ki), float(kd), 0)


-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))
+def encode_estop_cmd() -> bytes:
+    """Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5."""
+    return struct.pack("B", _ESTOP_MAGIC)


-# ---------------------------------------------------------------------------
-# Decode helpers
-# ---------------------------------------------------------------------------
+# ── BALANCE → Orin decoders ───────────────────────────────────────────────────

-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_attitude(data: bytes) -> AttitudeTelemetry:
+    """Decode ATTITUDE (0x400) — 8 bytes."""
+    if len(data) < 8:
+        raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}")
+    pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data)
+    return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate,
+                             state=state, flags=flags)


-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_battery(data: bytes) -> BatteryTelemetry:
+    """Decode BATTERY (0x401) — 4 bytes."""
+    if len(data) < 4:
+        raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}")
+    vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data)
+    return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi)


-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)
+# Backward-compat aliases
+def decode_fc_status(data: bytes) -> AttitudeTelemetry:
+    return decode_attitude(data)


-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)
+def decode_fc_vesc(data: bytes) -> BatteryTelemetry:
+    return decode_battery(data)
+
+
+# ── VESC CAN helpers ─────────────────────────────────────────────────────────
+
+def decode_vesc_can_id(can_id: int) -> tuple:
+    """Split a VESC extended CAN ID into (packet_type, node_id)."""
+    return (can_id >> 8) & 0xFF, can_id & 0xFF
+
+
+def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1:
+    """Decode VESC STATUS (cmd=9): erpm i32, current i16(/10), duty i16(/1000)."""
+    erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8])
+    return VescStatus1(node_id=node_id, erpm=float(erpm),
+                       current=cur_x10 / 10.0, duty=duty_x1000 / 1000.0)
+
+
+def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4:
+    """Decode VESC STATUS_4 (cmd=16): temp_fet, temp_mot, cur_in (all /10)."""
+    tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8])
+    return VescStatus4(node_id=node_id, temp_fet_c=tfet / 10.0,
+                       temp_motor_c=tmot / 10.0, current_in=cur_in / 10.0)
+
+
+def decode_vesc_status5(node_id: int, data: bytes) -> VescStatus5:
+    """Decode VESC STATUS_5 (cmd=27): tacho i32, vbat i16 (/10 V)."""
+    tacho, vbat_x10 = struct.unpack_from(">ih", data[:6])
+    return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0)
+
+
+def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple:
+    """Encode VESC SET_RPM command. Returns (extended_can_id, payload)."""
+    can_id = (3 << 8) | (node_id & 0xFF)
+    return can_id, struct.pack(">i", int(rpm))