9 changed files with 1014 additions and 1083 deletions
--- a/CLAUDE.md
+++ b/CLAUDE.md
@ -1,31 +1,17 @@
 # SaltyLab Firmware — Agent Playbook
 ## Project
-**SAUL-TEE** — 4-wheel wagon robot (870×510×550 mm, 23 kg).
+Self-balancing two-wheeled robot: STM32F722 flight controller, hoverboard hub motors, Jetson Nano for AI/SLAM.
 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 | ESP32-S3 firmware (PlatformIO), QMI8658 IMU, PID, VESC CAN, inter-board UART |
+| **sl-firmware** | Embedded Firmware Lead | STM32 HAL, USB CDC debugging, SPI/UART, PlatformIO, DFU bootloader |
 | **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
-| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 |
+| **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 |
 ## Status
-Architecture migrated from Mamba F722S/BlackPill → ESP32-S3 BALANCE + IO (PR #712, 2026-04-04).
+USB CDC TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).
 ## Repo Structure
 - `projects/saltybot/SALTYLAB.md` — Design doc
--- a/docs/SAUL-TEE-SYSTEM-REFERENCE.md
+++ b/docs/SAUL-TEE-SYSTEM-REFERENCE.md
@ -1,444 +0,0 @@
 # 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,13 +1,6 @@
-# SaltyLab / SAUL-TEE Wiring Reference
+# SaltyLab Wiring Diagram
-> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired.
+## System Overview
 > 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)
 ```
 ┌─────────────────────────────────────────────────────────────────────┐
@ -146,7 +139,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
 | 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
-## FC UART Summary (MAMBA F722S — OBSOLETE)
+## FC UART Summary (MAMBA F722S)
 | 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,16 +1,30 @@
-# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
+# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
-# Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud.
+# Binary-framed Jetson↔STM32 bridge at 921600 baud.
 # Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]
 # Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
 # ── Serial port ────────────────────────────────────────────────────────────────
-# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
+# Use /dev/stm32-bridge if the udev rule is applied:
-# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
+#   SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
-serial_port:      /dev/esp32-io
+#   SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
-baud_rate:        460800
+serial_port:      /dev/ttyACM0
-reconnect_delay:  2.0        # seconds between reconnect attempts
+baud_rate:        921600
 reconnect_delay:  2.0        # seconds between USB reconnect attempts
 # ── Heartbeat ─────────────────────────────────────────────────────────────────
-# HEARTBEAT (0x20) sent every heartbeat_period.
+# HEARTBEAT frame sent every heartbeat_period seconds.
-# ESP32 IO watchdog fires if no heartbeat for ~500 ms.
+# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
-heartbeat_period: 0.2        # 200 ms → well within 500 ms watchdog
+heartbeat_period: 0.2        # 200ms → well within 500ms 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
--- a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
+++ b/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
@ -1,14 +1,14 @@
-"""stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node.
+"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119).
 Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol).
 Handles RC monitoring, sensor data, LED/output commands.
 Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node.
 Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
 Usage:
  # Default (binary protocol, bidirectional):
  ros2 launch saltybot_bridge stm32_cmd.launch.py
-  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
+
  # Override serial port:
  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
  # Custom velocity scales:
  ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
 """
 import os
@ -24,8 +24,11 @@ def generate_launch_description() -> LaunchDescription:
    params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
    return LaunchDescription([
-        DeclareLaunchArgument("serial_port",     default_value="/dev/esp32-io"),
+        DeclareLaunchArgument("serial_port",      default_value="/dev/ttyACM0"),
-        DeclareLaunchArgument("baud_rate",        default_value="460800"),
+        DeclareLaunchArgument("baud_rate",         default_value="921600"),
        DeclareLaunchArgument("speed_scale",       default_value="1000.0"),
        DeclareLaunchArgument("steer_scale",       default_value="-500.0"),
        DeclareLaunchArgument("watchdog_timeout",  default_value="0.5"),
        DeclareLaunchArgument("heartbeat_period",  default_value="0.2"),
        Node(
@ -39,6 +42,9 @@ def generate_launch_description() -> LaunchDescription:
                {
                    "serial_port":      LaunchConfiguration("serial_port"),
                    "baud_rate":        LaunchConfiguration("baud_rate"),
                    "speed_scale":      LaunchConfiguration("speed_scale"),
                    "steer_scale":      LaunchConfiguration("steer_scale"),
                    "watchdog_timeout": LaunchConfiguration("watchdog_timeout"),
                    "heartbeat_period": LaunchConfiguration("heartbeat_period"),
                },
            ],
--- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
+++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
@ -1,32 +1,45 @@
-"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.
+"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge.
-Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
+Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
-inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
+framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
-This node is NOT the primary drive path (that is CAN via can_bridge_node).
+TX commands (Jetson → STM32):
-It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
+  SPEED_STEER  — 50 Hz from /cmd_vel subscription
  HEARTBEAT    — 200 ms timer (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
-Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]  @ 460800 baud
+Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
-RX from ESP32 IO:
+RX telemetry (STM32 → Jetson):
-  RC_CHANNELS (0x01) → /saltybot/rc_channels  (std_msgs/String JSON)
+  IMU          → /saltybot/imu           (sensor_msgs/Imu)
-  SENSORS     (0x02) → /saltybot/sensors       (std_msgs/String JSON)
+  BATTERY      → /saltybot/telemetry/battery (std_msgs/String JSON)
  MOTOR_RPM    → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
  ARM_STATE    → /saltybot/arm_state     (std_msgs/String JSON)
  ERROR        → /saltybot/error         (std_msgs/String JSON)
  All frames   → /diagnostics            (diagnostic_msgs/DiagnosticArray)
-TX to ESP32 IO:
+Auto-reconnect: USB disconnect is detected when serial.read() raises; node
-  LED_CMD     (0x10) ← /saltybot/leds          (std_msgs/String JSON)
+continuously retries at reconnect_delay interval.
-  OUTPUT_CMD  (0x11) ← /saltybot/outputs       (std_msgs/String JSON)
+
-  HEARTBEAT   (0x20)  — sent every heartbeat_period (keep IO watchdog alive)
+This node owns /dev/ttyACM0 exclusively — do NOT run alongside
 serial_bridge_node or saltybot_cmd_node on the same port.
 Parameters (config/stm32_cmd_params.yaml):
-  serial_port       /dev/esp32-io
+  serial_port       /dev/ttyACM0
-  baud_rate         460800
+  baud_rate         921600
-  reconnect_delay   2.0
+  reconnect_delay   2.0   (seconds)
-  heartbeat_period  0.2   (ESP32 IO watchdog fires at ~500 ms)
+  heartbeat_period  0.2   (seconds)
  watchdog_timeout  0.5   (seconds — no /cmd_vel → send zero-speed)
  speed_scale       1000.0 (linear.x m/s → ESC units)
  steer_scale      -500.0  (angular.z rad/s → ESC units, neg to flip convention)
 """
 from __future__ import annotations
 import json
 import math
 import threading
 import time
@ -37,69 +50,119 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 import serial
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import Imu
 from std_msgs.msg import String
 from std_srvs.srv import SetBool, Trigger
 from .stm32_protocol import (
    BAUD_RATE,
    FrameParser,
-    RcChannels,
+    ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
-    SensorData,
+    encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
-    encode_heartbeat,
+    encode_pid_update,
    encode_led_cmd,
    encode_output_cmd,
 )
 # ── Constants ─────────────────────────────────────────────────────────────────
 IMU_FRAME_ID = "imu_link"
 _ARM_LABEL   = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
 def _clamp(v: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, v))
 # ── Node ──────────────────────────────────────────────────────────────────────
 class Stm32CmdNode(Node):
-    """Orin ↔ ESP32-S3 IO auxiliary bridge node."""
+    """Binary-framed Jetson↔STM32 bridge node."""
    def __init__(self) -> None:
        super().__init__("stm32_cmd_node")
-        # ── Parameters ────────────────────────────────────────────────────
+        # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("serial_port",      "/dev/esp32-io")
+        self.declare_parameter("serial_port",      "/dev/ttyACM0")
-        self.declare_parameter("baud_rate",        BAUD_RATE)
+        self.declare_parameter("baud_rate",        921600)
        self.declare_parameter("reconnect_delay",  2.0)
        self.declare_parameter("heartbeat_period", 0.2)
        self.declare_parameter("watchdog_timeout", 0.5)
        self.declare_parameter("speed_scale",      1000.0)
        self.declare_parameter("steer_scale",      -500.0)
-        self._port_name       = self.get_parameter("serial_port").value
+        port                  = self.get_parameter("serial_port").value
-        self._baud            = self.get_parameter("baud_rate").value
+        baud                  = self.get_parameter("baud_rate").value
        self._reconnect_delay = self.get_parameter("reconnect_delay").value
        self._hb_period       = self.get_parameter("heartbeat_period").value
        self._wd_timeout      = self.get_parameter("watchdog_timeout").value
        self._speed_scale     = self.get_parameter("speed_scale").value
        self._steer_scale     = self.get_parameter("steer_scale").value
-        # ── QoS ───────────────────────────────────────────────────────────
+        # ── QoS ───────────────────────────────────────────────────────────────
        sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST, depth=10,
        )
        rel_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST, depth=10,
        )
-        # ── Publishers ────────────────────────────────────────────────────
+        # ── Publishers ────────────────────────────────────────────────────────
-        self._rc_pub   = self.create_publisher(String,          "/saltybot/rc_channels", rel_qos)
+        self._imu_pub      = self.create_publisher(Imu,    "/saltybot/imu",           sensor_qos)
-        self._sens_pub = self.create_publisher(String,          "/saltybot/sensors",     rel_qos)
+        self._arm_pub      = self.create_publisher(String, "/saltybot/arm_state",      rel_qos)
        self._error_pub    = self.create_publisher(String, "/saltybot/error",          rel_qos)
        self._battery_pub  = self.create_publisher(String, "/saltybot/telemetry/battery",  rel_qos)
        self._rpm_pub      = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
        self._diag_pub     = self.create_publisher(DiagnosticArray, "/diagnostics",    rel_qos)
-        # ── Subscriptions ─────────────────────────────────────────────────
+        # ── Subscribers ───────────────────────────────────────────────────────
-        self.create_subscription(String, "/saltybot/leds",    self._on_leds,    rel_qos)
+        self._cmd_vel_sub = self.create_subscription(
-        self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
+            Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
        )
        self._pid_sub = self.create_subscription(
            String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
        )
-        # ── Serial state ──────────────────────────────────────────────────
+        # ── Services ──────────────────────────────────────────────────────────
        self._arm_srv  = self.create_service(SetBool, "/saltybot/arm",      self._svc_arm)
        self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode)
        # ── Serial state ──────────────────────────────────────────────────────
        self._port_name = port
        self._baud      = baud
        self._ser: serial.Serial | None = None
        self._ser_lock  = threading.Lock()
        self._parser    = FrameParser()
        self._rx_count = 0
-        # ── Open serial and start timers ──────────────────────────────────
+        # ── TX state ──────────────────────────────────────────────────────────
        self._last_speed    = 0
        self._last_steer    = 0
        self._last_cmd_t    = time.monotonic()
        self._watchdog_sent = False     # tracks whether we already sent zero
        # ── Diagnostics state ──────────────────────────────────────────────────
        self._last_arm_state   = -1
        self._last_battery_mv  = 0
        self._rx_frame_count   = 0
        # ── Open serial and start timers ──────────────────────────────────────
        self._open_serial()
        # Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean)
        self._read_timer = self.create_timer(0.005, self._read_cb)
        # Heartbeat TX
        self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
        # Watchdog check (fires at 2× watchdog_timeout for quick detection)
        self._wd_timer   = self.create_timer(self._wd_timeout / 2, self._watchdog_cb)
        # Periodic diagnostics
        self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
        self.get_logger().info(
-            f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
+            f"stm32_cmd_node started — {port} @ {baud} baud | "
            f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
        )
-    # ── Serial management ─────────────────────────────────────────────────
+    # ── Serial management ─────────────────────────────────────────────────────
    def _open_serial(self) -> bool:
        with self._ser_lock:
@ -107,7 +170,7 @@ class Stm32CmdNode(Node):
                self._ser = serial.Serial(
                    port=self._port_name,
                    baudrate=self._baud,
-                    timeout=0.005,
+                    timeout=0.005,          # non-blocking reads
                    write_timeout=0.1,
                )
                self._ser.reset_input_buffer()
@ -122,7 +185,17 @@ class Stm32CmdNode(Node):
                self._ser = None
                return False
    def _close_serial(self) -> None:
        with self._ser_lock:
            if self._ser and self._ser.is_open:
                try:
                    self._ser.close()
                except Exception:
                    pass
            self._ser = None
    def _write(self, data: bytes) -> bool:
        """Thread-safe serial write. Returns False if port is not open."""
        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
                return False
@ -134,15 +207,16 @@ class Stm32CmdNode(Node):
                self._ser = None
                return False
-    # ── RX ────────────────────────────────────────────────────────────────
+    # ── RX — read callback ────────────────────────────────────────────────────
    def _read_cb(self) -> None:
        """Read bytes from serial and feed them to the frame parser."""
        raw: bytes | None = None
-        reconnect = False
+        reconnect_needed  = False
        with self._ser_lock:
            if self._ser is None or not self._ser.is_open:
-                reconnect = True
+                reconnect_needed = True
            else:
                try:
                    n = self._ser.in_waiting
@ -151,9 +225,9 @@ class Stm32CmdNode(Node):
                except serial.SerialException as exc:
                    self.get_logger().error(f"Serial read error: {exc}")
                    self._ser = None
-                    reconnect = True
+                    reconnect_needed = True
-        if reconnect:
+        if reconnect_needed:
            self.get_logger().warn(
                "Serial disconnected — will retry",
                throttle_duration_sec=self._reconnect_delay,
@ -166,105 +240,230 @@ class Stm32CmdNode(Node):
            return
        for byte in raw:
-            msg = self._parser.feed(byte)
+            frame = self._parser.feed(byte)
-            if msg is not None:
+            if frame is not None:
-                self._rx_count += 1
+                self._rx_frame_count += 1
-                self._dispatch(msg)
+                self._dispatch_frame(frame)
-    def _dispatch(self, msg) -> None:
+    def _dispatch_frame(self, frame) -> None:
        """Route a decoded frame to the appropriate publisher."""
        now = self.get_clock().now().to_msg()
        ts  = f"{now.sec}.{now.nanosec:09d}"
-        if isinstance(msg, RcChannels):
+        if isinstance(frame, ImuFrame):
-            out = String()
+            self._publish_imu(frame, now)
            out.data = json.dumps({
                "channels": msg.channels,
                "source":   msg.source,
                "ts":       ts,
            })
            self._rc_pub.publish(out)
-        elif isinstance(msg, SensorData):
+        elif isinstance(frame, BatteryFrame):
-            out = String()
+            self._publish_battery(frame, now)
            out.data = json.dumps({
                "pressure_pa":   msg.pressure_pa,
                "temperature_c": msg.temperature_c,
                "tof_mm":        msg.tof_mm,
                "ts":            ts,
            })
            self._sens_pub.publish(out)
-        elif isinstance(msg, tuple):
+        elif isinstance(frame, MotorRpmFrame):
-            type_code, _ = msg
+            self._publish_motor_rpm(frame, now)
            self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
-    # ── TX ────────────────────────────────────────────────────────────────
+        elif isinstance(frame, ArmStateFrame):
            self._publish_arm_state(frame, now)
        elif isinstance(frame, ErrorFrame):
            self._publish_error(frame, now)
        elif isinstance(frame, tuple):
            type_code, payload = frame
            self.get_logger().debug(
                f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}"
            )
    # ── Telemetry publishers ──────────────────────────────────────────────────
    def _publish_imu(self, frame: ImuFrame, stamp) -> None:
        msg = Imu()
        msg.header.stamp    = stamp
        msg.header.frame_id = IMU_FRAME_ID
        # orientation: unknown — signal with -1 in first covariance
        msg.orientation_covariance[0] = -1.0
        msg.angular_velocity.x = math.radians(frame.pitch_deg)
        msg.angular_velocity.y = math.radians(frame.roll_deg)
        msg.angular_velocity.z = math.radians(frame.yaw_deg)
        cov = math.radians(0.3) ** 2    # ±0.3° noise estimate from 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,
            )
    def _heartbeat_cb(self) -> None:
        """Send HEARTBEAT every heartbeat_period (default 200ms)."""
        self._write(encode_heartbeat())
-    def _on_leds(self, msg: String) -> None:
+    def _watchdog_cb(self) -> None:
-        """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
+        """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
-        try:
+        if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
-            d = json.loads(msg.data)
+            if not self._watchdog_sent:
-            frame = encode_led_cmd(
+                self.get_logger().warn(
-                int(d.get("pattern", 0)),
+                    f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
                int(d.get("r", 0)),
                int(d.get("g", 0)),
                int(d.get("b", 0)),
                )
-        except (ValueError, KeyError, json.JSONDecodeError) as exc:
+                self._watchdog_sent = True
-            self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
+            self._last_speed = 0
-            return
+            self._last_steer = 0
-        self._write(frame)
+            self._write(encode_speed_steer(0, 0))
-    def _on_outputs(self, msg: String) -> None:
+    def _on_pid_update(self, msg: String) -> None:
-        """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
+        """Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
        try:
-            d = json.loads(msg.data)
+            data = json.loads(msg.data)
-            frame = encode_output_cmd(
+            kp = float(data["kp"])
-                bool(d.get("horn",     False)),
+            ki = float(data["ki"])
-                bool(d.get("buzzer",   False)),
+            kd = float(data["kd"])
                int(d.get("headlight", 0)),
                int(d.get("fan",       0)),
            )
        except (ValueError, KeyError, json.JSONDecodeError) as exc:
-            self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}")
+            self.get_logger().error(f"Bad PID update JSON: {exc}")
            return
-        self._write(frame)
+        frame = encode_pid_update(kp, ki, kd)
        if self._write(frame):
            self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
        else:
            self.get_logger().warn("PID_UPDATE dropped — serial not open")
-    # ── Diagnostics ───────────────────────────────────────────────────────
+    # ── Services ──────────────────────────────────────────────────────────────
    def _svc_arm(self, request: SetBool.Request, response: SetBool.Response):
        """SetBool(True) = arm, SetBool(False) = disarm."""
        arm = request.data
        frame = encode_arm(arm)
        ok = self._write(frame)
        response.success = ok
        response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open"
        self.get_logger().info(
            f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}"
        )
        return response
    def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response):
        """SetBool: data maps to mode byte (True=1, False=0)."""
        mode = 1 if request.data else 0
        frame = encode_set_mode(mode)
        ok = self._write(frame)
        response.success = ok
        response.message = f"mode={mode}" if ok else "serial not open"
        return response
    # ── Diagnostics ───────────────────────────────────────────────────────────
    def _publish_diagnostics(self) -> None:
        diag = DiagnosticArray()
        diag.header.stamp = self.get_clock().now().to_msg()
        status = DiagnosticStatus()
-        status.name        = "saltybot/esp32_io_bridge"
+        status.name        = "saltybot/stm32_cmd_node"
-        status.hardware_id = "esp32-s3-io"
+        status.hardware_id = "stm32f722"
        port_ok = self._ser is not None and self._ser.is_open
-        status.level   = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
+        if port_ok:
-        status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
+            status.level   = DiagnosticStatus.OK
            status.message = "Serial OK"
        else:
            status.level   = DiagnosticStatus.ERROR
            status.message = f"Serial disconnected: {self._port_name}"
        wd_age = time.monotonic() - self._last_cmd_t
        status.values = [
            KeyValue(key="serial_port",    value=self._port_name),
            KeyValue(key="baud_rate",   value=str(self._baud)),
            KeyValue(key="port_open",      value=str(port_ok)),
-            KeyValue(key="rx_frames",   value=str(self._rx_count)),
+            KeyValue(key="rx_frames",      value=str(self._rx_frame_count)),
            KeyValue(key="rx_errors",      value=str(self._parser.frames_error)),
            KeyValue(key="last_speed",     value=str(self._last_speed)),
            KeyValue(key="last_steer",     value=str(self._last_steer)),
            KeyValue(key="cmd_vel_age_s",  value=f"{wd_age:.2f}"),
            KeyValue(key="battery_mv",     value=str(self._last_battery_mv)),
            KeyValue(key="arm_state",      value=_ARM_LABEL.get(self._last_arm_state, "?")),
        ]
        diag.status.append(status)
        self._diag_pub.publish(diag)
-    # ── Lifecycle ─────────────────────────────────────────────────────────
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
    def destroy_node(self) -> None:
-        self._write(encode_heartbeat(state=0))
+        # Send zero-speed + disarm on shutdown
-        with self._ser_lock:
+        self._write(encode_speed_steer(0, 0))
-            if self._ser and self._ser.is_open:
+        self._write(encode_arm(False))
-                try:
+        self._close_serial()
                    self._ser.close()
                except Exception:
                    pass
            self._ser = None
        super().destroy_node()
--- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py
+++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py
@ -1,260 +1,332 @@
-"""stm32_protocol.py — Inter-board UART frame codec (ESP32 BALANCE ↔ ESP32 IO).
+"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
-File name retained for import compatibility. This module implements the binary
+Issue #119: defines the binary serial protocol between the Jetson Nano and the
-serial protocol that runs between the two ESP32-S3 embedded boards.
+STM32F722 flight controller over USB CDC @ 921600 baud.
-Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 (2026-04-04)
+Frame layout (all multi-byte fields are big-endian):
-Physical: UART @ 460800 baud, 8N1
+  ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
  │  STX │ TYPE │  LEN │     PAYLOAD      │  CRC16    │  ETX │
  │ 0x02 │  1B  │  1B  │    LEN bytes     │   2B BE   │ 0x03 │
  └──────┴──────┴──────┴──────────────────┴───────────┴──────┘
-Frame layout:
+CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
-  ┌────────┬───────┬───────┬──────────────────┬───────┐
+CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
  │ HEADER │  LEN  │  TYPE │    PAYLOAD       │ CRC8  │
  │  0xAA  │  1 B  │  1 B  │   LEN bytes      │  1 B  │
  └────────┴───────┴───────┴──────────────────┴───────┘
-CRC8 covers: LEN + TYPE + PAYLOAD  (polynomial 0x07, init 0x00).
+Command types (Jetson → STM32):
-Max payload: 64 bytes.  No ETX byte.
+  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)
-Note: the Orin communicates with ESP32 BALANCE via CAN (CANable2/slcan0),
+Telemetry types (STM32 → Jetson):
-NOT via this serial protocol.  See mamba_protocol.py for the CAN frame codec.
+  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)
-Message types — IO → BALANCE:
+Usage:
-  0x01  RC_CHANNELS  — raw RC channel values (CRSF or ELRS)
+  # Encoding (Jetson → STM32)
-  0x02  SENSORS      — barometer + ToF data
+  frame = encode_speed_steer(300, -150)
  ser.write(frame)
-Message types — BALANCE → IO:
+  # Decoding (STM32 → Jetson), one byte at a time
-  0x10  LED_CMD      — LED strip pattern  [pattern u8][r u8][g u8][b u8]
+  parser = FrameParser()
-  0x11  OUTPUT_CMD   — horn/buzzer/headlight/fan  [flags u8][headlight u8][fan u8]
+  for byte in incoming_bytes:
-  0x12  MOTOR_CMD    — BTS7960 aux motor  [motor_a i16 BE][motor_b i16 BE]
+      result = parser.feed(byte)
-  0x20  HEARTBEAT    — status keepalive   [state u8][error_flags u8]
+      if result is not None:
-
+          handle_frame(result)
 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, field
+from dataclasses import dataclass
 from enum import IntEnum
 from typing import Optional
 # ── Frame constants ───────────────────────────────────────────────────────────
-FRAME_HEADER    = 0xAA
+STX = 0x02
-MAX_PAYLOAD_LEN = 64
+ETX = 0x03
-BAUD_RATE       = 460800   # inter-board UART
+MAX_PAYLOAD_LEN = 64       # hard limit; any frame larger is corrupt
 # ── Message type codes ────────────────────────────────────────────────────────
 class IOMsg(IntEnum):
    RC_CHANNELS = 0x01
    SENSORS     = 0x02
-class BalMsg(IntEnum):
+# ── Command / telemetry type codes ────────────────────────────────────────────
-    LED_CMD    = 0x10
+
-    OUTPUT_CMD = 0x11
+class CmdType(IntEnum):
-    MOTOR_CMD  = 0x12
+    HEARTBEAT   = 0x01
-    HEARTBEAT  = 0x20
+    SPEED_STEER = 0x02
    ARM         = 0x03
    SET_MODE    = 0x04
    PID_UPDATE  = 0x05
-# ── Parsed message dataclasses ────────────────────────────────────────────────
+class TelType(IntEnum):
    IMU       = 0x10
    BATTERY   = 0x11
    MOTOR_RPM = 0x12
    ARM_STATE = 0x13
    ERROR     = 0x14
 # ── Parsed telemetry objects ──────────────────────────────────────────────────
@dataclass
-class RcChannels:
+class ImuFrame:
-    """RC channel values from TBS Crossfire (primary) or ELRS (failover).
+    pitch_deg: float      # degrees (positive = forward tilt)
-    CRSF range: 172–1811 (centre 992).
+    roll_deg:  float
-    """
+    yaw_deg:   float
-    channels: list = field(default_factory=lambda: [992] * 8)
+    accel_x:   float      # m/s²
-    source: int = 0   # 0 = CRSF, 1 = ELRS failover
+    accel_y:   float
    accel_z:   float
@dataclass
-class SensorData:
+class BatteryFrame:
-    pressure_pa:   float = 0.0   # Pascal (barometer)
+    voltage_mv:  int      # millivolts (e.g. 11100 = 11.1 V)
-    temperature_c: float = 0.0   # °C
+    current_ma:  int      # milliamps (negative = charging)
-    tof_mm:        int   = 0     # Time-of-flight distance mm
+    soc_pct:     int      # state of charge 0–100 (from STM32 fuel gauge or lookup)
@dataclass
-class LedCmd:
+class MotorRpmFrame:
-    pattern: int = 0
+    left_rpm:   int
-    r: int = 0
+    right_rpm:  int
    g: int = 0
    b: int = 0
@dataclass
-class OutputCmd:
+class ArmStateFrame:
-    horn:      bool = False
+    state:       int      # 0=DISARMED 1=ARMED 2=TILT_FAULT
-    buzzer:    bool = False
+    error_flags: int      # bitmask
    headlight: int  = 0    # 0–255 PWM
    fan:       int  = 0    # 0–255 PWM
@dataclass
-class MotorCmd:
+class ErrorFrame:
-    motor_a: int = 0   # -255..+255 (BTS7960 channel A)
+    error_code: int
-    motor_b: int = 0   # -255..+255 (BTS7960 channel B)
+    subcode:    int
-@dataclass
+# Union type for decoded results
-class Heartbeat:
+TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame
    state:       int = 0    # 0=IDLE  1=RUNNING  2=FAULT
    error_flags: int = 0
-InterboardMsg = RcChannels | SensorData | LedCmd | OutputCmd | MotorCmd | Heartbeat
+# ── CRC16 CCITT ───────────────────────────────────────────────────────────────
-
+def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
-# ── CRC-8 (polynomial 0x07, init 0x00) ───────────────────────────────────────
+    """CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR."""
-
+    crc = init
 def _crc8(data: bytes) -> int:
    crc = 0
    for byte in data:
-        crc ^= byte
+        crc ^= byte << 8
        for _ in range(8):
-            crc = ((crc << 1) ^ 0x07) if (crc & 0x80) else (crc << 1)
+            if crc & 0x8000:
-            crc &= 0xFF
+                crc = (crc << 1) ^ 0x1021
            else:
                crc <<= 1
        crc &= 0xFFFF
    return crc
 # ── Frame encoder ─────────────────────────────────────────────────────────────
-def _build_frame(msg_type: int, payload: bytes) -> bytes:
+def _build_frame(cmd_type: int, payload: bytes) -> bytes:
-    assert len(payload) <= MAX_PAYLOAD_LEN
+    """Assemble a complete binary frame with CRC16."""
-    crc_data = bytes([len(payload), msg_type]) + payload
+    assert len(payload) <= MAX_PAYLOAD_LEN, "Payload too large"
-    return bytes([FRAME_HEADER, len(payload), msg_type]) + payload + bytes([_crc8(crc_data)])
+    length = len(payload)
    header = bytes([cmd_type, length])
    crc = _crc16_ccitt(header + payload)
    return bytes([STX, cmd_type, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
-def encode_heartbeat(state: int = 0, error_flags: int = 0) -> bytes:
+def encode_heartbeat() -> bytes:
-    return _build_frame(BalMsg.HEARTBEAT, struct.pack("BB", state & 0xFF, error_flags & 0xFF))
+    """HEARTBEAT frame — no payload."""
    return _build_frame(CmdType.HEARTBEAT, b"")
-def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes:
+def encode_speed_steer(speed: int, steer: int) -> bytes:
-    return _build_frame(BalMsg.LED_CMD, struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF))
+    """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_output_cmd(horn: bool, buzzer: bool, headlight: int, fan: int) -> bytes:
+def encode_arm(arm: bool) -> bytes:
-    flags = (int(horn) & 1) | ((int(buzzer) & 1) << 1)
+    """ARM frame — 0=disarm, 1=arm."""
-    return _build_frame(BalMsg.OUTPUT_CMD, struct.pack("BBB", flags, headlight & 0xFF, fan & 0xFF))
+    return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
-def encode_motor_cmd(motor_a: int, motor_b: int) -> bytes:
+def encode_set_mode(mode: int) -> bytes:
-    a = max(-255, min(255, int(motor_a)))
+    """SET_MODE frame — mode byte."""
-    b = max(-255, min(255, int(motor_b)))
+    return _build_frame(CmdType.SET_MODE, struct.pack("B", mode & 0xFF))
    return _build_frame(BalMsg.MOTOR_CMD, struct.pack(">hh", a, b))
-# ── Streaming frame parser ────────────────────────────────────────────────────
+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))
-class _St(IntEnum):
+
-    HDR  = 0
+# ── Frame decoder (state-machine parser) ─────────────────────────────────────
-    LEN  = 1
+
-    TYPE = 2
+class ParserState(IntEnum):
-    PAY  = 3
+    WAIT_STX  = 0
-    CRC  = 4
+    WAIT_TYPE = 1
    WAIT_LEN  = 2
    PAYLOAD   = 3
    CRC_HI    = 4
    CRC_LO    = 5
    WAIT_ETX  = 6
 class ParseError(Exception):
    pass
 class FrameParser:
-    """Byte-by-byte streaming parser for inter-board frames.
+    """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.
    Usage::
        parser = FrameParser()
        for b in incoming:
-            msg = parser.feed(b)
+            result = parser.feed(b)
-            if msg is not None:
+            if result is not None:
-                handle(msg)
+                process(result)
    """
-    def __init__(self):
+    def __init__(self) -> None:
-        self._s    = _St.HDR
+        self._state    = ParserState.WAIT_STX
        self._len  = 0
        self._type     = 0
-        self._pay  = bytearray()
+        self._length   = 0
        self._payload  = bytearray()
        self._crc_rcvd = 0
        self.frames_ok    = 0
        self.frames_error = 0
-    def reset(self):
+    def reset(self) -> None:
-        self._s   = _St.HDR
+        """Reset parser to initial state (call after error or port reconnect)."""
-        self._pay = bytearray()
+        self._state   = ParserState.WAIT_STX
        self._payload = bytearray()
-    def feed(self, byte: int) -> Optional[InterboardMsg | tuple]:
+    def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
-        s = self._s
+        """Process one byte. Returns decoded frame on success, None otherwise.
-        if s == _St.HDR:
+        On CRC error, increments frames_error and resets. The return value on
-            if byte == FRAME_HEADER:
+        success is a dataclass (ImuFrame, BatteryFrame, etc.) or a
-                self._s = _St.LEN
+        (type_code, raw_payload) tuple for unknown type codes.
        """
        s = self._state
        if s == ParserState.WAIT_STX:
            if byte == STX:
                self._state = ParserState.WAIT_TYPE
            return None
-        if s == _St.LEN:
+        if s == ParserState.WAIT_TYPE:
            if byte > MAX_PAYLOAD_LEN:
                self.frames_error += 1; self.reset(); return None
            self._len = byte
            self._s   = _St.TYPE
            return None
        if s == _St.TYPE:
            self._type  = byte
-            self._pay  = bytearray()
+            self._state = ParserState.WAIT_LEN
            self._s    = _St.PAY if self._len > 0 else _St.CRC
            return None
-        if s == _St.PAY:
+        if s == ParserState.WAIT_LEN:
-            self._pay.append(byte)
+            self._length  = byte
-            if len(self._pay) == self._len:
+            self._payload = bytearray()
-                self._s = _St.CRC
+            if self._length > MAX_PAYLOAD_LEN:
-            return None
+                # Corrupt frame — too big; reset
-
+                self.frames_error += 1
        if s == _St.CRC:
                self.reset()
-            expected = _crc8(bytes([self._len, self._type]) + self._pay)
+                return None
-            if byte != expected:
+            if self._length == 0:
-                self.frames_error += 1; return None
+                self._state = ParserState.CRC_HI
            else:
                self._state = ParserState.PAYLOAD
            return None
        if s == ParserState.PAYLOAD:
            self._payload.append(byte)
            if len(self._payload) == self._length:
                self._state = ParserState.CRC_HI
            return None
        if s == ParserState.CRC_HI:
            self._crc_rcvd = byte << 8
            self._state    = ParserState.CRC_LO
            return None
        if s == ParserState.CRC_LO:
            self._crc_rcvd |= byte
            self._state     = ParserState.WAIT_ETX
            return None
        if s == ParserState.WAIT_ETX:
            self.reset()   # always reset so we look for next STX
            if byte != ETX:
                self.frames_error += 1
                return None
            # Verify CRC
            crc_data = bytes([self._type, self._length]) + self._payload
            expected = _crc16_ccitt(crc_data)
            if expected != self._crc_rcvd:
                self.frames_error += 1
                return None
            # Decode
            self.frames_ok += 1
-            return _decode(self._type, bytes(self._pay))
+            return _decode_telemetry(self._type, bytes(self._payload))
-        self.reset(); return None
+        # Should never reach here
        self.reset()
        return None
-# ── Message decoder ───────────────────────────────────────────────────────────
+# ── Telemetry decoder ─────────────────────────────────────────────────────────
-def _decode(type_code: int, payload: bytes) -> Optional[InterboardMsg | tuple]:
+def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
    """Decode a validated telemetry payload into a typed dataclass."""
    try:
-        if type_code == IOMsg.RC_CHANNELS:
+        if type_code == TelType.IMU:
-            if len(payload) < 17: return None
+            if len(payload) < 12:
-            ch = list(struct.unpack_from("<8H", payload))
+                return None
-            return RcChannels(channels=ch, source=payload[16])
+            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.SENSORS:
+        if type_code == TelType.BATTERY:
-            if len(payload) < 10: return None
+            if len(payload) < 5:
-            pres, temp, tof = struct.unpack_from("<ffH", payload)
+                return None
-            return SensorData(pressure_pa=pres, temperature_c=temp, tof_mm=tof)
+            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 == BalMsg.LED_CMD:
+        if type_code == TelType.MOTOR_RPM:
-            if len(payload) < 4: return None
+            if len(payload) < 4:
-            pat, r, g, b = struct.unpack_from("BBBB", payload)
+                return None
-            return LedCmd(pattern=pat, r=r, g=g, b=b)
+            left, right = struct.unpack_from(">hh", payload)
            return MotorRpmFrame(left_rpm=left, right_rpm=right)
-        if type_code == BalMsg.OUTPUT_CMD:
+        if type_code == TelType.ARM_STATE:
-            if len(payload) < 3: return None
+            if len(payload) < 2:
-            flags, headlight, fan = struct.unpack_from("BBB", payload)
+                return None
            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 Heartbeat(state=state, error_flags=flags)
+            return ArmStateFrame(state=state, error_flags=flags)
        if type_code == TelType.ERROR:
            if len(payload) < 2:
                return None
            code, sub = struct.unpack_from("BB", payload)
            return ErrorFrame(error_code=code, subcode=sub)
    except struct.error:
        return None
    # Unknown telemetry type — return raw
    return (type_code, payload)
--- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
+++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
@ -1,34 +1,27 @@
 #!/usr/bin/env python3
 """
-can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
+can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor
-board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps).
+controller and VESC motor controllers over CAN bus.
-Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
+The node opens the SocketCAN interface (slcan0 by default), spawns a background
 reader thread to process incoming telemetry, and exposes the following interface:
 Subscriptions
 -------------
-  /cmd_vel              geometry_msgs/Twist     → ORIN_CMD_DRIVE (0x300)
+  /cmd_vel          geometry_msgs/Twist          → VESC speed commands (CAN)
-  /estop                std_msgs/Bool           → ORIN_CMD_ESTOP (0x303)
+  /estop            std_msgs/Bool                → Mamba e-stop (CAN)
  /saltybot/arm         std_msgs/Bool           → ORIN_CMD_ARM   (0x301)
 Publications
 ------------
-  /saltybot/attitude      std_msgs/String (JSON)   pitch, speed, yaw_rate, state
+  /can/imu                sensor_msgs/Imu              Mamba IMU telemetry
-  /saltybot/balance_state std_msgs/String (JSON)   alias of /saltybot/attitude
+  /can/battery            sensor_msgs/BatteryState     Mamba battery telemetry
-  /can/battery            sensor_msgs/BatteryState  vbat_mv, fault, rssi
+  /can/vesc/left/state    std_msgs/Float32MultiArray   Left VESC state
-  /can/vesc/left/state    std_msgs/Float32MultiArray  VESC STATUS_1 left
+  /can/vesc/right/state   std_msgs/Float32MultiArray   Right VESC state
  /can/vesc/right/state   std_msgs/Float32MultiArray  VESC STATUS_1 right
  /can/connection_status  std_msgs/String              "connected" | "disconnected"
-Parameters
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
 ----------
  can_interface     str    CAN socket name (default: slcan0)
  speed_scale       float  /cmd_vel linear.x (m/s) → motor units  (default: 1000.0)
  steer_scale       float  /cmd_vel angular.z (rad/s) → motor units (default: -500.0)
  command_timeout_s float  watchdog zero-vel threshold (default: 0.5)
 """
 import json
 import threading
 import time
 from typing import Optional
@ -37,74 +30,87 @@ import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
-from sensor_msgs.msg import BatteryState
+from rcl_interfaces.msg import SetParametersResult
 from sensor_msgs.msg import BatteryState, Imu
 from std_msgs.msg import Bool, Float32MultiArray, String
 from saltybot_can_bridge.mamba_protocol import (
-    ORIN_CMD_DRIVE,
+    MAMBA_CMD_ESTOP,
-    ORIN_CMD_ARM,
+    MAMBA_CMD_MODE,
-    ORIN_CMD_ESTOP,
+    MAMBA_CMD_VELOCITY,
-    ESP32_TELEM_ATTITUDE,
+    MAMBA_TELEM_BATTERY,
-    ESP32_TELEM_BATTERY,
+    MAMBA_TELEM_IMU,
-    VESC_LEFT_ID,
+    VESC_TELEM_STATE,
-    VESC_RIGHT_ID,
+    ORIN_CAN_ID_FC_PID_ACK,
-    VESC_STATUS_1,
+    ORIN_CAN_ID_PID_SET,
    MODE_DRIVE,
    MODE_ESTOP,
    MODE_IDLE,
    encode_drive_cmd,
    encode_arm_cmd,
    encode_estop_cmd,
-    decode_attitude,
+    encode_mode_cmd,
-    decode_battery,
+    encode_velocity_cmd,
-    decode_vesc_status1,
+    encode_pid_set_cmd,
    decode_battery_telem,
    decode_imu_telem,
    decode_pid_ack,
    decode_vesc_state,
 )
 # Reconnect attempt interval when CAN bus is lost
 _RECONNECT_INTERVAL_S: float = 5.0
-# Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent
+# Watchdog timer tick rate (Hz)
 _WATCHDOG_HZ: float = 10.0
 class CanBridgeNode(Node):
-    """CAN bus bridge between Orin ROS2 and ESP32 BALANCE / VESC controllers."""
+    """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers."""
    def __init__(self) -> None:
        super().__init__("can_bridge_node")
        # ── Parameters ────────────────────────────────────────────────────
        self.declare_parameter("can_interface", "slcan0")
-        self.declare_parameter("left_vesc_can_id",  VESC_LEFT_ID)
+        self.declare_parameter("left_vesc_can_id", 56)
-        self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
+        self.declare_parameter("right_vesc_can_id", 68)
-        self.declare_parameter("speed_scale",       1000.0)
+        self.declare_parameter("mamba_can_id", 1)
        self.declare_parameter("steer_scale",       -500.0)
        self.declare_parameter("command_timeout_s", 0.5)
        self.declare_parameter("pid/kp", 0.0)
        self.declare_parameter("pid/ki", 0.0)
        self.declare_parameter("pid/kd", 0.0)
-        self._iface         = self.get_parameter("can_interface").value
+        self._iface: str = self.get_parameter("can_interface").value
-        self._left_vesc_id  = self.get_parameter("left_vesc_can_id").value
+        self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
-        self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
+        self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
-        self._speed_scale   = self.get_parameter("speed_scale").value
+        self._mamba_id: int = self.get_parameter("mamba_can_id").value
-        self._steer_scale   = self.get_parameter("steer_scale").value
+        self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
-        self._cmd_timeout   = self.get_parameter("command_timeout_s").value
+        self._pid_kp: float = self.get_parameter("pid/kp").value
        self._pid_ki: float = self.get_parameter("pid/ki").value
        self._pid_kd: float = self.get_parameter("pid/kd").value
        # ── State ─────────────────────────────────────────────────────────
        self._bus: Optional[can.BusABC] = None
        self._connected: bool = False
        self._last_cmd_time: float = time.monotonic()
-        self._lock = threading.Lock()
+        self._lock = threading.Lock()  # protects _bus / _connected
        # ── Publishers ────────────────────────────────────────────────────
-        self._pub_attitude  = self.create_publisher(String,           "/saltybot/attitude",      10)
+        self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
        self._pub_balance   = self.create_publisher(String,           "/saltybot/balance_state", 10)
        self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
-        self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state",    10)
+        self._pub_vesc_left = self.create_publisher(
-        self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state",   10)
+            Float32MultiArray, "/can/vesc/left/state", 10
-        self._pub_status    = self.create_publisher(String,           "/can/connection_status",  10)
+        )
        self._pub_vesc_right = self.create_publisher(
            Float32MultiArray, "/can/vesc/right/state", 10
        )
        self._pub_status = self.create_publisher(
            String, "/can/connection_status", 10
        )
        # ── Subscriptions ─────────────────────────────────────────────────
        self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
        self.create_subscription(Bool, "/estop", self._estop_cb, 10)
-        self.create_subscription(Bool,  "/saltybot/arm",  self._arm_cb,     10)
+        self.add_on_set_parameters_callback(self._on_set_parameters)
        # ── Timers ────────────────────────────────────────────────────────
        self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
@ -122,17 +128,46 @@ class CanBridgeNode(Node):
        self.get_logger().info(
            f"can_bridge_node ready — iface={self._iface} "
            f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
-            f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
+            f"mamba={self._mamba_id}"
        )
    # -- PID parameter callback (Issue #693) --
    def _on_set_parameters(self, params) -> SetParametersResult:
        """Send new PID gains over CAN when pid/* params change."""
        for p in params:
            if p.name == "pid/kp":
                self._pid_kp = float(p.value)
            elif p.name == "pid/ki":
                self._pid_ki = float(p.value)
            elif p.name == "pid/kd":
                self._pid_kd = float(p.value)
            else:
                continue
            try:
                payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
                self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
                self.get_logger().info(
                    f"PID gains sent: Kp={self._pid_kp:.2f} "
                    f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
                )
            except ValueError as exc:
                return SetParametersResult(successful=False, reason=str(exc))
        return SetParametersResult(successful=True)
    # ── Connection management ──────────────────────────────────────────────
    def _try_connect(self) -> None:
        """Attempt to open the CAN interface; silently skip if already connected."""
        with self._lock:
            if self._connected:
                return
            try:
-                self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
+                bus = can.interface.Bus(
                    channel=self._iface,
                    bustype="socketcan",
                )
                self._bus = bus
                self._connected = True
                self.get_logger().info(f"CAN bus connected: {self._iface}")
                self._publish_status("connected")
@ -145,10 +180,12 @@ class CanBridgeNode(Node):
                self._publish_status("disconnected")
    def _reconnect_cb(self) -> None:
        """Periodic timer: try to reconnect when disconnected."""
        if not self._connected:
            self._try_connect()
    def _handle_can_error(self, exc: Exception, context: str) -> None:
        """Mark bus as disconnected on any CAN error."""
        self.get_logger().warning(f"CAN error in {context}: {exc}")
        with self._lock:
            if self._bus is not None:
@ -163,51 +200,73 @@ class CanBridgeNode(Node):
    # ── ROS callbacks ─────────────────────────────────────────────────────
    def _cmd_vel_cb(self, msg: Twist) -> None:
-        """Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN."""
+        """Convert /cmd_vel Twist to VESC speed commands over CAN."""
        self._last_cmd_time = time.monotonic()
        if not self._connected:
            return
-        speed = int(max(-1000.0, min(1000.0, msg.linear.x  * self._speed_scale)))
+
-        steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
+        # Differential drive decomposition — individual wheel speeds in m/s.
-        self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
+        # The VESC nodes interpret linear velocity directly; angular is handled
        # by the sign difference between left and right.
        linear = msg.linear.x
        angular = msg.angular.z
        # Forward left = forward right for pure translation; for rotation
        # left slows and right speeds up (positive angular = CCW = left turn).
        # The Mamba velocity command carries both wheels independently.
        left_mps = linear - angular
        right_mps = linear + angular
        payload = encode_velocity_cmd(left_mps, right_mps)
        self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
        # Keep Mamba in DRIVE mode while receiving commands
        self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
    def _estop_cb(self, msg: Bool) -> None:
-        """Forward /estop to ESP32 BALANCE over CAN."""
+        """Forward /estop to Mamba over CAN."""
        if not self._connected:
            return
        payload = encode_estop_cmd(msg.data)
        self._send_can(MAMBA_CMD_ESTOP, payload, "estop")
        if msg.data:
-            self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
+            self._send_can(
-            self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
+                MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
-        else:
+            )
-            # Clear estop: send DISARM then re-ARM (let operator decide to re-arm)
+            self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba")
            self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "estop_clear")
    def _arm_cb(self, msg: Bool) -> None:
        """Forward /saltybot/arm to ORIN_CMD_ARM."""
        if not self._connected:
            return
        self._send_can(ORIN_CMD_ARM, encode_arm_cmd(msg.data), "arm")
        self.get_logger().info(f"ARM command: {'ARM' if msg.data else 'DISARM'}")
    # ── Watchdog ──────────────────────────────────────────────────────────
    def _watchdog_cb(self) -> None:
-        """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive)."""
+        """If no /cmd_vel arrives within the timeout, send zero velocity."""
        if not self._connected:
            return
-        if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
+        elapsed = time.monotonic() - self._last_cmd_time
-            self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
+        if elapsed > self._cmd_timeout:
            self._send_can(
                MAMBA_CMD_VELOCITY,
                encode_velocity_cmd(0.0, 0.0),
                "watchdog zero-vel",
            )
            self._send_can(
                MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
            )
    # ── CAN send helper ───────────────────────────────────────────────────
-    def _send_can(self, arb_id: int, data: bytes, context: str,
+    def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
-                  extended: bool = False) -> None:
+        """Send a standard CAN frame; handle errors gracefully."""
        with self._lock:
            if not self._connected or self._bus is None:
                return
            bus = self._bus
-        msg = can.Message(arbitration_id=arb_id, data=data,
+
-                          is_extended_id=extended)
+        msg = can.Message(
            arbitration_id=arb_id,
            data=data,
            is_extended_id=False,
        )
        try:
            bus.send(msg, timeout=0.05)
        except can.CanError as exc:
@ -216,41 +275,55 @@ class CanBridgeNode(Node):
    # ── Background CAN reader ─────────────────────────────────────────────
    def _reader_loop(self) -> None:
        """
        Blocking CAN read loop executed in a daemon thread.
        Dispatches incoming frames to the appropriate handler.
        """
        while rclpy.ok():
            with self._lock:
-                connected, bus = self._connected, self._bus
+                connected = self._connected
                bus = self._bus
            if not connected or bus is None:
                time.sleep(0.1)
                continue
            try:
                frame = bus.recv(timeout=0.5)
            except can.CanError as exc:
                self._handle_can_error(exc, "reader_loop recv")
                continue
            if frame is None:
                # Timeout — no frame within 0.5 s, loop again
                continue
            self._dispatch_frame(frame)
    def _dispatch_frame(self, frame: can.Message) -> None:
        """Route an incoming CAN frame to the correct publisher."""
        arb_id = frame.arbitration_id
        data = bytes(frame.data)
-        vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
+
        vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
        try:
-            if arb_id == ESP32_TELEM_ATTITUDE:
+            if arb_id == MAMBA_TELEM_IMU:
-                self._handle_attitude(data)
+                self._handle_imu(data, frame.timestamp)
-            elif arb_id == ESP32_TELEM_BATTERY:
+
-                self._handle_battery(data)
+            elif arb_id == MAMBA_TELEM_BATTERY:
-            elif arb_id == vesc_l:
+                self._handle_battery(data, frame.timestamp)
-                t = decode_vesc_status1(self._left_vesc_id, data)
+
-                m = Float32MultiArray()
+            elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
-                m.data = [t.erpm, t.duty, 0.0, t.current]
+                self._handle_vesc_state(data, frame.timestamp, side="left")
-                self._pub_vesc_left.publish(m)
+
-            elif arb_id == vesc_r:
+            elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
-                t = decode_vesc_status1(self._right_vesc_id, data)
+                self._handle_vesc_state(data, frame.timestamp, side="right")
-                m = Float32MultiArray()
+
-                m.data = [t.erpm, t.duty, 0.0, t.current]
+            elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
-                self._pub_vesc_right.publish(m)
+                gains = decode_pid_ack(data)
                self.get_logger().debug(
                    f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
                )
        except Exception as exc:
            self.get_logger().warning(
                f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
@ -258,36 +331,52 @@ class CanBridgeNode(Node):
    # ── Frame handlers ────────────────────────────────────────────────────
-    _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
+    def _handle_imu(self, data: bytes, timestamp: float) -> None:
        telem = decode_imu_telem(data)
-    def _handle_attitude(self, data: bytes) -> None:
+        msg = Imu()
-        """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
+        msg.header.stamp = self.get_clock().now().to_msg()
-        t = decode_attitude(data)
+        msg.header.frame_id = "imu_link"
-        now = self.get_clock().now().to_msg()
+
-        payload = {
+        msg.linear_acceleration.x = telem.accel_x
-            "pitch_deg":  round(t.pitch_deg, 2),
+        msg.linear_acceleration.y = telem.accel_y
-            "speed_mps":  round(t.speed, 3),
+        msg.linear_acceleration.z = telem.accel_z
-            "yaw_rate":   round(t.yaw_rate, 3),
+
-            "state":      t.state,
+        msg.angular_velocity.x = telem.gyro_x
-            "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
+        msg.angular_velocity.y = telem.gyro_y
-            "flags":      t.flags,
+        msg.angular_velocity.z = telem.gyro_z
-            "ts":         f"{now.sec}.{now.nanosec:09d}",
+
-        }
+        # Covariance unknown; mark as -1 per REP-145
-        msg = String()
+        msg.orientation_covariance[0] = -1.0
-        msg.data = json.dumps(payload)
+
-        self._pub_attitude.publish(msg)
+        self._pub_imu.publish(msg)
-        self._pub_balance.publish(msg)   # keep /saltybot/balance_state alive
+
    def _handle_battery(self, data: bytes, timestamp: float) -> None:
        telem = decode_battery_telem(data)
    def _handle_battery(self, data: bytes) -> None:
        """BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
        t = decode_battery(data)
        msg = BatteryState()
        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.voltage = t.vbat_mv / 1000.0
+        msg.voltage = telem.voltage
        msg.current = telem.current
        msg.present = True
        msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
        self._pub_battery.publish(msg)
    def _handle_vesc_state(
        self, data: bytes, timestamp: float, side: str
    ) -> None:
        telem = decode_vesc_state(data)
        msg = Float32MultiArray()
        # Layout: [erpm, duty, voltage, current]
        msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
        if side == "left":
            self._pub_vesc_left.publish(msg)
        else:
            self._pub_vesc_right.publish(msg)
    # ── Status helper ─────────────────────────────────────────────────────
    def _publish_status(self, status: str) -> None:
@ -298,10 +387,17 @@ class CanBridgeNode(Node):
    # ── Shutdown ──────────────────────────────────────────────────────────
    def destroy_node(self) -> None:
        """Send zero velocity and shut down the CAN bus cleanly."""
        if self._connected and self._bus is not None:
            try:
-                self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
+                self._send_can(
-                self._send_can(ORIN_CMD_ARM,   encode_arm_cmd(False), "shutdown")
+                    MAMBA_CMD_VELOCITY,
                    encode_velocity_cmd(0.0, 0.0),
                    "shutdown",
                )
                self._send_can(
                    MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
                )
            except Exception:
                pass
            try:
@ -311,6 +407,8 @@ class CanBridgeNode(Node):
        super().destroy_node()
 # ---------------------------------------------------------------------------
 def main(args=None) -> None:
    rclpy.init(args=args)
    node = CanBridgeNode()
--- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py
+++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py
@ -1,217 +1,224 @@
 #!/usr/bin/env python3
-"""mamba_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE.
+"""
 mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller
 and VESC telemetry.
-Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
+CAN message layout
-File name retained for import compatibility.
+------------------
 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
-CAN bus: 500 kbps, standard 11-bit IDs, CANable 2.0 (slcan0 / can0) on Orin.
+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)
-── Orin → ESP32 BALANCE (commands) ───────────────────────────────────────────
+VESC telemetry frame (VESC → Orin):
-  0x300  DRIVE    8 B  [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16]
+  VESC_TELEM_STATE    0x300  16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32)
  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
-── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
+All multi-byte fields are big-endian.
  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]
-speed/steer range: −1000..+1000 (motor units).  f16 = IEEE 754 half-precision.
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
 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 IDs ───────────────────────────────────────────────────────────────────
+# ---------------------------------------------------------------------------
 # CAN message IDs
 # ---------------------------------------------------------------------------
-# Orin → ESP32 BALANCE
+MAMBA_CMD_VELOCITY: int = 0x100
-ORIN_CMD_DRIVE: int  = 0x300
+MAMBA_CMD_MODE: int = 0x101
-ORIN_CMD_ARM:   int  = 0x301
+MAMBA_CMD_ESTOP: int = 0x102
 ORIN_CMD_PID:   int  = 0x302
 ORIN_CMD_ESTOP: int  = 0x303
-# ESP32 BALANCE → Orin
+MAMBA_TELEM_IMU: int = 0x200
-ESP32_TELEM_ATTITUDE: int = 0x400
+MAMBA_TELEM_BATTERY: int = 0x201
 ESP32_TELEM_BATTERY:  int = 0x401
-# Backward-compat aliases used by other nodes
+VESC_TELEM_STATE: int = 0x300
-FC_STATUS: int = ESP32_TELEM_ATTITUDE
+ORIN_CAN_ID_PID_SET: int = 0x305
-FC_VESC:   int = ESP32_TELEM_BATTERY
+ORIN_CAN_ID_FC_PID_ACK: int = 0x405
-# VESC node IDs
+# ---------------------------------------------------------------------------
-VESC_LEFT_ID:  int = 56
+# Mode constants
-VESC_RIGHT_ID: int = 68
+# ---------------------------------------------------------------------------
-# VESC packet types
+MODE_IDLE: int = 0
-VESC_STATUS_1: int = 9
+MODE_DRIVE: int = 1
-VESC_STATUS_4: int = 16
+MODE_ESTOP: int = 2
 VESC_STATUS_5: int = 27
-# ── Mode constants (DRIVE frame mode byte) ─────────────────────────────────────
+# ---------------------------------------------------------------------------
 # Data classes for decoded telemetry
 # ---------------------------------------------------------------------------
 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 AttitudeTelemetry:
+class ImuTelemetry:
-    """Decoded ATTITUDE (0x400) from ESP32 BALANCE."""
+    """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU)."""
-    pitch_deg:  float = 0.0   # degrees, half-float
+
-    speed:      float = 0.0   # m/s, half-float
+    accel_x: float = 0.0  # m/s²
-    yaw_rate:   float = 0.0   # rad/s, half-float
+    accel_y: float = 0.0
-    state:      int   = 0     # 0=IDLE  1=RUNNING  2=FAULT
+    accel_z: float = 0.0
-    flags:      int   = 0     # error bitmask
+    gyro_x: float = 0.0   # rad/s
    gyro_y: float = 0.0
    gyro_z: float = 0.0
@dataclass
 class BatteryTelemetry:
-    """Decoded BATTERY (0x401) from ESP32 BALANCE."""
+    """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY)."""
-    vbat_mv:    int = 0       # millivolts
+
-    fault_code: int = 0       # 0 = OK
+    voltage: float = 0.0   # V
-    rssi:       int = 0       # RC signal dBm (signed)
+    current: float = 0.0   # A
@dataclass
 class VescStateTelemetry:
    """Decoded VESC state telemetry (VESC_TELEM_STATE)."""
    erpm: float = 0.0      # electrical RPM
    duty: float = 0.0      # duty cycle [-1.0, 1.0]
    voltage: float = 0.0   # bus voltage, V
    current: float = 0.0   # phase current, A
@dataclass
 class PidGains:
-    """Balance PID gains."""
+    """Balance PID gains (Issue #693)."""
    kp: float = 0.0
    ki: float = 0.0
    kd: float = 0.0
-@dataclass
+# ---------------------------------------------------------------------------
-class VescStatus1:
+# Encode helpers
-    """Decoded VESC STATUS (cmd=9) — direct from VESC."""
+# ---------------------------------------------------------------------------
-    node_id: int   = 0
+
-    erpm:    float = 0.0
+_FMT_VEL = ">ff"       # 2 × float32, big-endian
-    current: float = 0.0   # A
+_FMT_MODE = ">B"       # 1 × uint8
-    duty:    float = 0.0   # -1.0..+1.0
+_FMT_ESTOP = ">B"      # 1 × uint8
 _FMT_IMU = ">ffffff"   # 6 × float32
 _FMT_BAT = ">ff"       # 2 × float32
 _FMT_VESC = ">ffff"    # 4 × float32
-@dataclass
+def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
 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
    """
-    speed = max(-1000, min(1000, int(speed)))
+    Encode a MAMBA_CMD_VELOCITY payload.
-    steer = max(-1000, min(1000, int(steer)))
+
-    return struct.pack(_FMT_DRIVE, speed, steer, mode & 0xFF, flags & 0xFF, 0)
+    Parameters
    ----------
    left_mps:  target left wheel speed in m/s  (positive = forward)
    right_mps: target right wheel speed in m/s (positive = forward)
    Returns
    -------
    8-byte big-endian payload suitable for a CAN frame.
    """
    return struct.pack(_FMT_VEL, float(left_mps), float(right_mps))
-def encode_arm_cmd(arm: bool) -> bytes:
+def encode_mode_cmd(mode: int) -> bytes:
-    """Encode ORIN_CMD_ARM (0x301) — 1 byte."""
+    """
-    return struct.pack("B", 0x01 if arm else 0x00)
+    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_pid_cmd(kp: float, ki: float, kd: float) -> bytes:
+def encode_estop_cmd(stop: bool = True) -> 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)
+    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_estop_cmd() -> bytes:
+def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes:
-    """Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5."""
+    """Encode ORIN_CAN_ID_PID_SET (6 bytes, uint16 BE x3). Issue #693."""
-    return struct.pack("B", _ESTOP_MAGIC)
+    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))
-# ── BALANCE → Orin decoders ───────────────────────────────────────────────────
+# ---------------------------------------------------------------------------
 # Decode helpers
 # ---------------------------------------------------------------------------
-def decode_attitude(data: bytes) -> AttitudeTelemetry:
+def decode_imu_telem(data: bytes) -> ImuTelemetry:
-    """Decode ATTITUDE (0x400) — 8 bytes."""
+    """
-    if len(data) < 8:
+    Decode a MAMBA_TELEM_IMU payload.
-        raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}")
+
-    pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data)
+    Parameters
-    return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate,
+    ----------
-                             state=state, flags=flags)
+    data: exactly 24 bytes (6 × float32, big-endian).
    Returns
    -------
    ImuTelemetry dataclass instance.
    Raises
    ------
    struct.error if data is the wrong length.
    """
    ax, ay, az, gx, gy, gz = struct.unpack(_FMT_IMU, data)
    return ImuTelemetry(
        accel_x=ax, accel_y=ay, accel_z=az,
        gyro_x=gx, gyro_y=gy, gyro_z=gz,
    )
-def decode_battery(data: bytes) -> BatteryTelemetry:
+def decode_battery_telem(data: bytes) -> BatteryTelemetry:
-    """Decode BATTERY (0x401) — 4 bytes."""
+    """
-    if len(data) < 4:
+    Decode a MAMBA_TELEM_BATTERY payload.
-        raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}")
+
-    vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data)
+    Parameters
-    return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi)
+    ----------
    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)
-# Backward-compat aliases
+def decode_vesc_state(data: bytes) -> VescStateTelemetry:
-def decode_fc_status(data: bytes) -> AttitudeTelemetry:
+    """
-    return decode_attitude(data)
+    Decode a VESC_TELEM_STATE payload.
    Parameters
    ----------
    data: exactly 16 bytes (4 × float32, big-endian).
    Returns
    -------
    VescStateTelemetry dataclass instance.
    """
    erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data)
    return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current)
-def decode_fc_vesc(data: bytes) -> BatteryTelemetry:
+def decode_pid_ack(data: bytes) -> PidGains:
-    return decode_battery(data)
+    """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)
 # ── 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))