cleanup: Remove all Mamba/STM32/BlackPill references — ESP32-S3 only

- Renamed mamba_protocol.py → balance_protocol.py; updated all importers
- can_bridge_node.py rewritten to use balance_protocol.py API (ESP32-S3 BALANCE)
- test_can_bridge.py rewritten to test actual balance_protocol.py constants/functions
- All STM32/Mamba references in Python, YAML, Markdown, shell scripts replaced:
  * Hardware: MAMBA F722S → ESP32-S3 BALANCE/IO
  * Device paths: /dev/stm32-bridge → /dev/esp32-io
  * Node names: stm32_serial_bridge → esp32_io_serial_bridge
  * hardware_id: stm32f722 → esp32s3-balance/esp32s3-io
- C/C++ src/include/lib/test files: added DEPRECATED header comment
- Covers: saltybot_bridge, saltybot_can_bridge, saltybot_can_e2e_test,
  saltybot_bringup, saltybot_diagnostics, saltybot_mode_switch, and all
  chassis, docs, scripts, and project files

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

AUTONOMOUS_ARMING.md

@@ -7,7 +7,7 @@ The robot can now be armed and operated autonomously from the Jetson without req
 
 ### Jetson Autonomous Arming
 - Command: `A\n` (single byte 'A' followed by newline)
-- Sent via USB CDC to the STM32 firmware
+- Sent via USB CDC to the ESP32-S3 firmware
 - Robot arms after ARMING_HOLD_MS (~500ms) safety hold period
 - Works even when RC is not connected or not armed
 
@@ -42,7 +42,7 @@ The robot can now be armed and operated autonomously from the Jetson without req
 
 ## Command Protocol
 
-### From Jetson to STM32 (USB CDC)
+### From Jetson to ESP32-S3 (USB CDC)
 ```
 A           — Request arm (triggers safety hold, then motors enable)
 D           — Request disarm (immediate motor stop)
@@ -52,7 +52,7 @@ H           — Heartbeat (refresh timeout timer, every 500ms)
 C<spd>,<str> — Drive command: speed, steer (also refreshes heartbeat)
 ```
 
-### From STM32 to Jetson (USB CDC)
+### From ESP32-S3 to Jetson (USB CDC)
 Motor commands are gated by `bal.state == BALANCE_ARMED`:
 - When ARMED: Motor commands sent every 20ms (50 Hz)
 - When DISARMED: Zero sent every 20ms (prevents ESC timeout)

CLAUDE.md

@@ -1,12 +1,12 @@
 # SaltyLab Firmware — Agent Playbook
 
 ## Project
-Self-balancing two-wheeled robot: STM32F722 flight controller, hoverboard hub motors, Jetson Nano for AI/SLAM.
+Self-balancing two-wheeled robot: Orin Nano Super, 2x VESC (IDs 68 left / 56 right), ESP32-S3 BALANCE, ESP32-S3 IO.
 
 ## Team
 | Agent | Role | Focus |
 |-------|------|-------|
-| **sl-firmware** | Embedded Firmware Lead | STM32 HAL, USB CDC debugging, SPI/UART, PlatformIO, DFU bootloader |
+| **sl-firmware** | Embedded Firmware Lead | ESP32-S3/ESP-IDF, USB CDC debugging, SPI/UART, Arduino/ESP-IDF, esptool.py |
 | **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
 | **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 |
 

TEAM.md

@@ -1,7 +1,7 @@
 # SaltyLab — Ideal Team
 
 ## Project
-Self-balancing two-wheeled robot using a drone flight controller (STM32F722), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM.
+Self-balancing two-wheeled robot using a drone flight controller (ESP32-S3), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM.
 
 ## Current Status
 - **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand
@@ -14,10 +14,10 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
 
 ### 1. Embedded Firmware Engineer (Lead)
 **Must-have:**
-- Deep STM32 HAL experience (F7 series specifically)
+- Deep ESP32-S3 HAL experience (F7 series specifically)
 - USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts)
-- SPI + UART + USB coexistence on STM32
+- SPI + UART + USB coexistence on ESP32-S3
-- PlatformIO or bare-metal STM32 toolchain
+- PlatformIO or bare-metal ESP32-S3 toolchain
 - DFU bootloader implementation
 
 **Nice-to-have:**
@@ -25,7 +25,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
 - PID control loop tuning for balance robots
 - FOC motor control (hoverboard ESC protocol)
 
-**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged STM32 USB issues before — this is not a software logic bug, it's a hardware peripheral interaction issue.
+**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged ESP32-S3 USB issues before — this is not a software logic bug, it's a hardware peripheral interaction issue.
 
 ### 2. Control Systems / Robotics Engineer
 **Must-have:**
@@ -61,7 +61,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
 ## Hardware Reference
 | Component | Details |
 |-----------|---------|
-| FC | MAMBA F722S (STM32F722RET6, MPU6000) |
+| FC | ESP32-S3 BALANCE (ESP32-S3, ICM-42688-P) |
 | Motors | 2x 8" pneumatic hoverboard hub motors |
 | ESC | Hoverboard ESC (EFeru FOC firmware) |
 | Battery | 36V pack |

USB_CDC_BUG.md

@@ -127,7 +127,7 @@ loop — USB would never enumerate cleanly.
 | LED2 | PC15 | GPIO |
 | Buzzer | PB2 | GPIO/TIM4_CH3 |
 
-MCU: STM32F722RET6 (MAMBA F722S FC, Betaflight target DIAT-MAMBAF722_2022B)
+MCU: ESP32-S3 (ESP32-S3 BALANCE FC)
 
 ---
 

chassis/ASSEMBLY.md

@@ -56,7 +56,7 @@
 3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m.
 4. Insert battery pack; route Velcro straps through slots and cinch.
 
-### 7  FC mount (MAMBA F722S)
+### 7  FC mount (ESP32-S3 BALANCE)
 1. Place silicone anti-vibration grommets onto nylon M3 standoffs.
 2. Lower FC onto standoffs; secure with M3×6 BHCS. Snug only — do not over-torque.
 3. Orient USB-C port toward front of robot for cable access.

chassis/BOM.md

@@ -41,7 +41,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
 | 3 | Dropout clamp — upper | 2 | 8mm 6061-T6 Al | 90×70mm blank | D-cut bore; `RENDER="clamp_upper_2d"` |
 | 4 | Stem flange ring | 2 | 6mm Al or acrylic | Ø82mm disc | One above + one below plate; `RENDER="stem_flange_2d"` |
 | 5 | Vertical stem tube | 1 | 38.1mm OD × 1.5mm wall 6061-T6 Al | 1050mm length | 1.5" EMT conduit is a drop-in alternative |
-| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | MAMBA F722S vibration isolation |
+| 6 | BALANCE board standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE vibration isolation |
 | 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
 
 ### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B
@@ -90,7 +90,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
 
 | # | Part | Qty | Spec | Notes |
 |---|------|-----|------|-------|
-| 13 | STM32 MAMBA F722S FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front |
+| 13 | ESP32-S3 BALANCE FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front |
 | 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation |
 | 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads |
 | 16 | Jetson Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |

chassis/ip54_BOM.md

@@ -104,7 +104,7 @@ IP54-rated enclosures and sensor housings for all-weather outdoor robot operatio
 | Component | Thermal strategy | Max junction | Enclosure budget |
 |-----------|-----------------|-------------|-----------------|
 | Jetson Orin NX | Al pad → lid → fan forced convection | 95 °C Tj | Target ≤ 60 °C case |
-| FC (MAMBA F722S) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
+| FC (ESP32-S3 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
 | ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C |
 | D435i | Passive; housing vent gap on rear cap | 45 °C surface | — |
 

docs/AGENTS.md

@@ -5,17 +5,18 @@ You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read
 ## Project Overview
 
 A hoverboard-based balancing robot with two compute layers:
-1. **FC (Flight Controller)** — MAMBA F722S (STM32F722RET6 + MPU6000 IMU). Runs a lean C balance loop at up to 8kHz. Talks UART to the hoverboard ESC. This is the safety-critical layer.
+1. **ESP32-S3 BALANCE** — runs the PID balance loop. Safety-critical, operates independently of the Orin.
-2. **Jetson Nano** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently.
+2. **ESP32-S3 IO** — handles I/O: motor commands to VESCs, sensor polling, CAN/UART comms.
+3. **Orin Nano Super** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to ESP32-S3 BALANCE via UART. Not safety-critical.
 
 ```
-Jetson (speed+steer via UART1)  ←→  ELRS RC (UART3, kill switch)
+Orin Nano Super (speed+steer via UART)  ←→  ELRS RC (kill switch)
          │
          ▼
-   MAMBA F722S (MPU6000 IMU, PID balance)
+   ESP32-S3 BALANCE (IMU, PID balance loop)
          │
-         ▼ UART2
+         ▼ CAN / UART
-   Hoverboard ESC (FOC) → 2× 8" hub motors
+   ESP32-S3 IO → VESC 68 (left) + VESC 56 (right)
 ```
 
 ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT
@@ -26,7 +27,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
 2. **Tilt cutoff at ±25°** — motors to zero, require manual re-arm. No retry, no recovery.
 3. **Hardware watchdog (50ms)** — if firmware hangs, motors cut.
 4. **RC kill switch** — dedicated ELRS channel, checked every loop iteration. Always overrides.
-5. **Jetson UART timeout (200ms)** — if Jetson disconnects, motors cut.
+5. **Orin UART timeout (200ms)** — if Orin disconnects, motors cut.
 6. **Speed hard cap** — firmware limit, start at 10%. Increase only after proven stable.
 7. **Never test untethered** until PID is stable for 5+ minutes on a tether.
 
@@ -35,31 +36,16 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
 ## Repository Layout
 
 ```
-firmware/              # STM32 HAL firmware (PlatformIO)
+esp32/                 # ESP32-S3 firmware (ESP-IDF)
-├── src/
+├── balance/           # ESP32-S3 BALANCE — PID loop, IMU, safety
-│   ├── main.c         # Entry point, clock config, main loop
+└── io/                # ESP32-S3 IO — VESC CAN, sensors, comms
-│   ├── icm42688.c     # ICM-42688-P SPI driver (backup IMU — currently broken)
-│   ├── bmp280.c       # Barometer driver (disabled)
-│   └── status.c       # LED + buzzer status patterns
-├── include/
-│   ├── config.h       # Pin definitions, constants
-│   ├── icm42688.h
-│   ├── mpu6000.h      # MPU6000 driver header (primary IMU)
-│   ├── hoverboard.h   # Hoverboard ESC UART protocol
-│   ├── crsf.h         # ELRS CRSF protocol
-│   ├── bmp280.h
-│   └── status.h
-├── lib/USB_CDC/       # USB CDC stack (serial over USB)
-│   ├── src/           # CDC implementation, USB descriptors, PCD config
-│   └── include/
-└── platformio.ini     # Build config
 
 cad/                   # OpenSCAD parametric parts (16 files)
 ├── dimensions.scad    # ALL measurements live here — single source of truth
 ├── assembly.scad      # Full robot assembly visualization
 ├── motor_mount_plate.scad
 ├── battery_shelf.scad
-├── fc_mount.scad      # Vibration-isolated FC mount
+├── esp32_balance_mount.scad  # Vibration-isolated ESP32-S3 BALANCE mount
 ├── jetson_shelf.scad
 ├── esc_mount.scad
 ├── sensor_tower_top.scad
@@ -82,55 +68,55 @@ PLATFORM.md            # Hardware platform reference
 
 ## Hardware Quick Reference
 
-### MAMBA F722S Flight Controller
+### ESP32-S3 BALANCE
 
 | Spec | Value |
 |------|-------|
-| MCU | STM32F722RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
+| MCU | ESP32-S3 (dual-core Xtensa LX7, 240MHz, 512KB SRAM, 8MB flash) |
-| Primary IMU | MPU6000 (WHO_AM_I = 0x68) |
+| Primary IMU | MPU6000 (SPI) |
-| IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 |
+| Role | PID balance loop, tilt cutoff, arming |
-| IMU EXTI | PC4 (data ready interrupt) |
+| Comms to Orin | UART (velocity commands in, telemetry out) |
-| IMU Orientation | CW270 (Betaflight convention) |
+| Flash | `idf.py -p /dev/ttyUSB0 flash` |
-| Secondary IMU | ICM-42688-P (on same SPI1, CS unknown — currently non-functional) |
-| Betaflight Target | DIAT-MAMBAF722_2022B |
-| USB | OTG FS (PA11/PA12), enumerates as /dev/cu.usbmodemSALTY0011 |
-| VID/PID | 0x0483/0x5740 |
-| LEDs | PC15 (LED1), PC14 (LED2), active low |
-| Buzzer | PB2 (inverted push-pull) |
-| Battery ADC | PC1=VBAT, PC3=CURR (ADC3) |
-| DFU | Hold yellow BOOT button + plug USB (or send 'R' over CDC) |
 
-### UART Assignments
+### ESP32-S3 IO
 
-| UART | Pins | Connected To | Baud |
+| Spec | Value |
-|------|------|-------------|------|
+|------|-------|
-| USART1 | PA9/PA10 | Jetson Nano | 115200 |
+| MCU | ESP32-S3 |
-| USART2 | PA2/PA3 | Hoverboard ESC | 115200 |
+| Role | VESC CAN driver, sensor polling, peripheral I/O |
-| USART3 | PB10/PB11 | ELRS Receiver | 420000 (CRSF) |
+| VESC IDs | 68 (left), 56 (right) |
-| UART4 | — | Spare | — |
+| Motor bus | CAN 1Mbit/s |
-| UART5 | — | Spare | — |
+| Flash | `idf.py -p /dev/ttyUSB1 flash` |
+
+### UART Assignments (ESP32-S3 BALANCE)
+
+| UART | Connected To | Baud |
+|------|-------------|------|
+| UART0 | Orin Nano Super | 115200 |
+| UART1 | ESP32-S3 IO | 115200 |
+| UART2 | ELRS Receiver | 420000 (CRSF) |
 
 ### Motor/ESC
 
 - 2× 8" pneumatic hub motors (36V, hoverboard type)
-- Hoverboard ESC with FOC firmware
+- 2× VESC motor controllers (CAN IDs 68, 56)
-- UART protocol: `{0xABCD, int16 speed, int16 steer, uint16 checksum}` at 115200
+- VESC CAN protocol: standard SET_DUTY / SET_CURRENT / SET_RPM
-- Speed range: -1000 to +1000
+- Speed range: -1.0 to +1.0 (duty cycle)
 
 ### Physical Dimensions (from `cad/dimensions.scad`)
 
 | Part | Key Measurement |
 |------|----------------|
-| FC mounting holes | 25.5mm spacing (NOT standard 30.5mm!) |
+| ESP32-S3 BALANCE board | ~55×28mm (DevKit form factor) |
-| FC board size | ~36mm square |
+| ESP32-S3 IO board | ~55×28mm (DevKit form factor) |
 | Hub motor body | Ø200mm (~8") |
 | Motor axle | Ø12mm, 45mm long |
-| Jetson Nano | 100×80×29mm, M2.5 holes at 86×58mm |
+| Orin Nano Super | 100×79mm, M2.5 holes at 86×58mm |
 | RealSense D435i | 90×25×25mm, 1/4-20 tripod mount |
 | RPLIDAR A1 | Ø70×41mm, 4× M2.5 on Ø67mm circle |
 | Kill switch hole | Ø22mm panel mount |
 | Battery pack | ~180×80×40mm |
-| Hoverboard ESC | ~80×50×15mm |
+| VESC (each) | ~70×50×15mm |
 | 2020 extrusion | 20mm square, M5 center bore |
 | Frame width | ~350mm (axle to axle) |
 | Frame height | ~500-550mm total |
@@ -147,7 +133,7 @@ PLATFORM.md            # Hardware platform reference
 | sensor_tower_top | ASA | 80% |
 | lidar_standoff (Ø80×80mm) | ASA | 40% |
 | realsense_bracket | PETG | 60% |
-| fc_mount (vibration isolated) | TPU+PETG | — |
+| esp32_balance_mount (vibration isolated) | TPU+PETG | — |
 | bumper front + rear (350×50×30mm) | TPU | 30% |
 | handle | PETG | 80% |
 | kill_switch_mount | PETG | 80% |
@@ -159,99 +145,75 @@ PLATFORM.md            # Hardware platform reference
 
 ### Critical Lessons Learned (DON'T REPEAT THESE)
 
-1. **SysTick_Handler with HAL_IncTick() is MANDATORY** — without it, HAL_Delay() and every HAL timeout hangs forever. This bricked us multiple times.
+1. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
-2. **DCache breaks SPI on STM32F7** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.
+2. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0. Always use explicit error codes.
-3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes.
+3. **USB CDC needs RX primed in init** — without it, the OUT endpoint never starts listening.
-4. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
+4. **Watchdog must be fed every loop iteration** — if balance loop stalls, motors must cut within 50ms.
-5. **USB CDC needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception.
+5. **Never change PID and speed limit in the same test** — one variable at a time.
 
-### DFU Reboot (Betaflight Method)
+### Build & Flash (ESP32-S3)
-
-The firmware supports reboot-to-DFU via USB command:
-1. Send `R` byte over USB CDC
-2. Firmware writes `0xDEADBEEF` to RTC backup register 0
-3. `NVIC_SystemReset()` — clean hardware reset
-4. On boot, `checkForBootloader()` (called after `HAL_Init()`) reads the magic
-5. If magic found: clears it, remaps system memory, jumps to STM32 bootloader at `0x1FF00000`
-6. Board appears as DFU device, ready for `dfu-util` flash
-
-### Build & Flash
 
 ```bash
-cd firmware/
+# Balance board
-python3 -m platformio run                    # Build
+cd esp32/balance/
-dfu-util -a 0 -s 0x08000000:leave -D .pio/build/f722/firmware.bin  # Flash
+idf.py build && idf.py -p /dev/ttyUSB0 flash monitor
+
+# IO board
+cd esp32/io/
+idf.py build && idf.py -p /dev/ttyUSB1 flash monitor
 ```
 
-Dev machine: mbpm4 (seb@192.168.87.40), PlatformIO project at `~/Projects/saltylab-firmware/`
+Dev machine: mbpm4 (seb@192.168.87.40)
-
-### Clock Configuration
-
-```
-HSE 8MHz → PLL (M=8, N=432, P=2, Q=9) → SYSCLK 216MHz
-PLLSAI (N=384, P=8) → CLK48 48MHz (USB)
-APB1 = HCLK/4 = 54MHz
-APB2 = HCLK/2 = 108MHz
-Fallback: HSI 16MHz if HSE fails (PLL M=16)
-```
 
 ## Current Status & Known Issues
 
 ### Working
-- USB CDC serial streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`)
+- IMU streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`)
-- Clock config with HSE + HSI fallback
+- VESC CAN communication (IDs 68, 56)
-- Reboot-to-DFU via USB 'R' command
+- LED status patterns
-- LED status patterns (status.c)
 - Web UI with WebSerial + Three.js 3D visualization
 
-### Broken / In Progress
+### In Progress
-- **ICM-42688-P SPI reads return all zeros** — was the original IMU target, but SPI communication completely non-functional despite correct pin config. May be dead silicon. Switched to MPU6000 as primary.
+- PID balance loop tuning
-- **MPU6000 driver** — header exists but implementation needs completion
+- ELRS CRSF receiver integration
-- **PID balance loop** — not yet implemented
+- Orin UART integration
-- **Hoverboard ESC UART** — protocol defined, driver not written
-- **ELRS CRSF receiver** — protocol defined, driver not written
-- **Barometer (BMP280)** — I2C init hangs, disabled
 
 ### TODO (Priority Order)
-1. Get MPU6000 streaming accel+gyro data
+1. Tune PID balance loop on ESP32-S3 BALANCE
 2. Implement complementary filter (pitch angle)
-3. Write hoverboard ESC UART driver
+3. Wire ELRS receiver, implement CRSF parser
-4. Write PID balance loop with safety checks
+4. Bench test (VESCs disconnected, verify PID output)
-5. Wire ELRS receiver, implement CRSF parser
+5. First tethered balance test at 10% speed
-6. Bench test (ESC disconnected, verify PID output)
+6. Orin UART integration
-7. First tethered balance test at 10% speed
+7. LED subsystem (ESP32-S3 IO)
-8. Jetson UART integration
-9. LED subsystem (ESP32-C3)
 
 ## Communication Protocols
 
-### Jetson → FC (UART1, 50Hz)
+### Orin → ESP32-S3 BALANCE (UART0, 50Hz)
 ```c
 struct { uint8_t header=0xAA; int16_t speed; int16_t steer; uint8_t mode; uint8_t checksum; };
 // mode: 0=idle, 1=balance, 2=follow, 3=RC
 ```
 
-### FC → Hoverboard ESC (UART2, loop rate)
+### ESP32-S3 IO → VESC (CAN, loop rate)
-```c
+- Standard VESC CAN protocol (SET_DUTY / SET_CURRENT / SET_RPM)
-struct { uint16_t start=0xABCD; int16_t speed; int16_t steer; uint16_t checksum; };
+- Node IDs: VESC 68 (left), VESC 56 (right)
-// speed/steer: -1000 to +1000
-```
 
-### FC → Jetson Telemetry (UART1 TX, 50Hz)
+### ESP32-S3 BALANCE → Orin Telemetry (UART0 TX, 50Hz)
 ```
 T:12.3,P:45,L:100,R:-80,S:3\n
 // T=tilt°, P=PID output, L/R=motor commands, S=state (0-3)
 ```
 
-### FC → USB CDC (50Hz JSON)
+### ESP32-S3 → USB (50Hz JSON, debug/tuning)
 ```json
-{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0,"bt":0}
+{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0}
-// Raw IMU values (int16), t=temp×10, p=pressure, bt=baro temp
+// Raw IMU values (int16), t=temp×10, p=PID output
 ```
 
-## LED Subsystem (ESP32-C3)
+## LED Subsystem (ESP32-S3 IO)
 
-ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No extra FC UART needed.
+ESP32-S3 IO eavesdrops on BALANCE→Orin telemetry (listen-only). No extra UART needed.
 
 | State | Pattern | Color |
 |-------|---------|-------|
@@ -275,7 +237,7 @@ ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No e
 1. **Read SALTYLAB.md fully** before making any design decisions
 2. **Never remove safety checks** from firmware — add more if needed
 3. **All measurements go in `cad/dimensions.scad`** — single source of truth
-4. **Test firmware on bench before any motor test** — ESC disconnected, verify outputs on serial
+4. **Test firmware on bench before any motor test** — VESCs disconnected, verify outputs on serial
 5. **One variable at a time** — don't change PID and speed limit in the same test
 6. **Document what you change** — update this file if you add pins, change protocols, or discover hardware quirks
-7. **Ask before wiring changes** — wrong connections can fry the FC ($50+ board)
+7. **Ask before wiring changes** — wrong connections can fry an ESP32 or VESC

docs/FACE_LCD_ANIMATION.md

@@ -1,6 +1,6 @@
 # Face LCD Animation System (Issue #507)
 
-Implements expressive face animations on an STM32 LCD display with 5 core emotions and smooth transitions.
+Implements expressive face animations on an ESP32-S3 IO LCD display with 5 core emotions and smooth transitions.
 
 ## Features
 
@@ -82,7 +82,7 @@ STATUS      → Echo current emotion + idle state
 - Colors: Monochrome (1-bit) or RGB565
 
 ### Microcontroller
-- STM32F7xx (Mamba F722S)
+- ESP32-S3 IO
 - Available UART: USART3 (PB10=TX, PB11=RX)
 - Clock: 216 MHz
 

docs/SALTYLAB.md

@@ -32,7 +32,7 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
 |------|--------|
 | 2x 8" pneumatic hub motors (36 PSI) | ✅ Have |
 | 1x hoverboard ESC (FOC firmware) | ✅ Have |
-| 1x Drone FC (STM32F745 + MPU-6000) | ✅ Have — balance brain |
+| 1x ESP32-S3 BALANCE (ICM-42688-P IMU) | ✅ Have — balance brain |
 | 1x Jetson Nano + Noctua fan | ✅ Have |
 | 1x RealSense D435i | ✅ Have |
 | 1x RPLIDAR A1M8 | ✅ Have |
@@ -50,13 +50,13 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
 | 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART |
 
 ### Drone FC Details — GEPRC GEP-F7 AIO
-- **MCU:** STM32F722RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM)
+- **MCU:** ESP32-S3 (Xtensa LX7 dual-core 240MHz, 512KB SRAM, 8MB flash)
 - **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
 - **Flash:** 8MB Winbond W25Q64 (blackbox, unused)
 - **OSD:** AT7456E (unused)
 - **4-in-1 ESC:** Built into AIO board (unused — we use hoverboard ESC)
 - **DFU mode:** Hold yellow BOOT button while plugging USB
-- **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL)
+- **Firmware:** Custom balance firmware (PlatformIO + ESP-IDF)
 - **UART pads (confirmed from silkscreen):**
   - T1/R1 (bottom) → USART1 (PA9/PA10) → Jetson
   - T2/R2 (right top) → USART2 (PA2/PA3) → Hoverboard ESC
@@ -95,7 +95,7 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
 ## Self-Balancing Control — Custom Firmware on Drone FC
 
 ### Why a Drone FC?
-The F745 board is just a premium STM32 dev board with a high-quality IMU (MPU-6000) already soldered on, proper voltage regulation, and multiple UARTs broken out. We write a lean custom balance firmware (~50 lines of C).
+The ESP32-S3 BALANCE board has an ICM-42688-P IMU, proper voltage regulation, and CAN bus interface. We write a lean custom balance firmware using ESP-IDF.
 
 ### Architecture
 ```
@@ -142,7 +142,7 @@ GND          ──→   GND
 5V           ←──   5V
 ```
 
-### Custom Firmware (STM32 C)
+### Custom Firmware (ESP32-S3 C/C++)
 
 ```c
 // Core balance loop — runs in timer interrupt @ 1-8kHz
@@ -280,8 +280,8 @@ GND             ──→ Common ground
 ```
 
 ### Dev Tools
-- **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD
+- **Flashing:** esptool.py via USB (bootloader mode)
-- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE
+- **IDE:** PlatformIO + ESP-IDF, or Arduino IDE with ESP32 core
 - **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
 
 ## Physical Design
@@ -375,7 +375,7 @@ GND             ──→ Common ground
 - [ ] Install hardware kill switch inline with 36V battery (NC — press to kill)
 - [ ] Set up ceiling tether point above test area (rated for >15kg)
 - [ ] Clear test area: 3m radius, no loose items, shoes on
-- [ ] Set up PlatformIO project for STM32F745 (STM32 HAL)
+- [ ] Set up PlatformIO project for ESP32-S3 (ESP-IDF)
 - [ ] Write MPU-6000 SPI driver (read gyro+accel, complementary filter)
 - [ ] Write PID balance loop with ALL safety checks:
   - ±25° tilt cutoff → disarm, require manual re-arm

docs/wiring-diagram.md

@@ -7,7 +7,7 @@
 │                        ORIN NANO SUPER                              │
 │                     (Top Plate — 25W)                               │
 │                                                                     │
-│  USB-C ──── STM32 CDC (/dev/stm32-bridge, 921600 baud)            │
+│  USB-C ──── ESP32-S3 IO CDC (/dev/esp32-io, 921600 baud)            │
 │  USB-A1 ─── RealSense D435i (USB 3.1)                             │
 │  USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200)             │
 │  USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP)     │
@@ -25,7 +25,7 @@
          │ 921600 baud                    │ 921600 baud, 3.3V
          ▼                                ▼
 ┌─────────────────────────────────────────────────────────────────────┐
-│                     MAMBA F722S (FC)                                 │
+│                     ESP32-S3 BALANCE / IO (FC)                                 │
 │                  (Middle Plate — foam mounted)                       │
 │                                                                     │
 │  USB-C ──── Orin (CDC serial, primary link)                        │
@@ -72,7 +72,7 @@
 |------|----|-----------|-------|
 | Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus |
 
-- Device: `/dev/ttyACM0` → symlink `/dev/stm32-bridge`
+- Device: `/dev/ttyACM0` → symlink `/dev/esp32-io`
 - Baud: 921600, 8N1
 - Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
 
@@ -139,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)
+## FC UART Summary (ESP32-S3 IO)
 
 | UART | Pins | Baud | Assignment | Notes |
 |------|------|------|------------|-------|
@@ -149,7 +149,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
 | UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control |
 | UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
 | USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
-| USB CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` |
+| USB CDC | USB-C | 921600 | Jetson primary | `/dev/esp32-io` |
 
 
 ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
@@ -209,7 +209,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
 
 | Device | Interface | Power Draw |
 |--------|-----------|------------|
-| STM32 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
+| ESP32-S3 IO (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
 | RealSense D435i | USB-A | ~1.5W (3.5W peak) |
 | RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
 | SIM7600A | USB | ~1W idle, 3W TX peak |
@@ -234,7 +234,7 @@ Orin Nano Super delivers up to 25W — USB peripherals are well within budget.
                     └──────┬───────┘
                            │ UART4
               ┌────────────▼────────────┐
-              │      MAMBA F722S        │
+              │   ESP32-S3 BALANCE     │
               │                         │
               │  MPU6000 → Balance PID  │
               │  CRSF → Mode Manager   │

flash.sh

@@ -1,7 +1,7 @@
 #!/bin/bash
 # Flash SaltyLab — auto-reboot to DFU if serial port exists
 PORT=/dev/cu.usbmodemSALTY0011
-FW=.pio/build/f722/firmware.bin
+FW=.pio/build/esp32s3/firmware.bin
 
 if [ -e "$PORT" ]; then
     echo "Sending reboot-to-DFU..."

include/baro.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef BARO_H
 #define BARO_H
 

include/battery.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef BATTERY_H
 #define BATTERY_H
 

include/battery_adc.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 /*
  * battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533)
  *

include/bno055.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef BNO055_H
 #define BNO055_H
 

include/buzzer.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef BUZZER_H
 #define BUZZER_H
 

include/config.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef CONFIG_H
 #define CONFIG_H
 

include/face_lcd.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 /*
  * face_lcd.h — STM32 LCD Display Driver for Face Animations
  *

include/fan.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef FAN_H
 #define FAN_H
 

include/fault_handler.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef FAULT_HANDLER_H
 #define FAULT_HANDLER_H
 

include/i2c1.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef I2C1_H
 #define I2C1_H
 

include/jetson_cmd.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef JETSON_CMD_H
 #define JETSON_CMD_H
 

include/jetson_uart.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef JETSON_UART_H
 #define JETSON_UART_H
 

include/jlink.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef JLINK_H
 #define JLINK_H
 

include/orin_can.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef ORIN_CAN_H
 #define ORIN_CAN_H
 

include/ota.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef OTA_H
 #define OTA_H
 

include/pid_flash.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef PID_FLASH_H
 #define PID_FLASH_H
 

include/power_mgmt.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef POWER_MGMT_H
 #define POWER_MGMT_H
 

include/uart_protocol.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef UART_PROTOCOL_H
 #define UART_PROTOCOL_H
 

include/ultrasonic.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef ULTRASONIC_H
 #define ULTRASONIC_H
 

include/vesc_can.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef VESC_CAN_H
 #define VESC_CAN_H
 

include/watchdog.h

@@ -1,3 +1,4 @@
+/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
 #ifndef WATCHDOG_H
 #define WATCHDOG_H
 

jetson/README.md

@@ -14,7 +14,7 @@ Self-balancing robot: Jetson Nano dev environment for ROS2 Humble + SLAM stack.
 | Nav | Nav2 |
 | Depth camera | Intel RealSense D435i |
 | LiDAR | RPLIDAR A1M8 |
-| MCU bridge | STM32F722 (USB CDC @ 921600) |
+| Motor controller | ESP32-S3 BALANCE (CAN bus 500 kbps) |
 
 ## Quick Start
 
@@ -42,7 +42,7 @@ bash scripts/build-and-run.sh shell
 ```
 jetson/
 ├── Dockerfile              # L4T base + ROS2 Humble + SLAM packages
-├── docker-compose.yml      # Multi-service stack (ROS2, RPLIDAR, D435i, STM32)
+├── docker-compose.yml      # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32-S3)
 ├── README.md               # This file
 ├── docs/
 │   ├── pinout.md           # GPIO/I2C/UART pinout reference

jetson/config/RECOVERY_BEHAVIORS.md

@@ -34,7 +34,7 @@ Recovery behaviors are triggered when Nav2 encounters navigation failures (path
 
 The emergency stop system (Issue #459, `saltybot_emergency` package) runs independently of Nav2 and takes absolute priority.
 
-Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the STM32 firmware.
+Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32-S3 firmware.
 
 ## Behavior Tree Sequence
 

jetson/config/nav2_params.yaml

@@ -12,7 +12,7 @@
 #   /scan                    — RPLIDAR A1M8  (obstacle layer)
 #   /camera/depth/color/points — RealSense D435i (voxel layer)
 #
-# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
+# Output: /cmd_vel (Twist) — ESP32-S3 bridge consumes this topic.
 
 bt_navigator:
   ros__parameters:

jetson/docker-compose.yml

@@ -31,7 +31,7 @@ services:
       - ./config:/config:ro
     devices:
       - /dev/rplidar:/dev/rplidar
-      - /dev/stm32-bridge:/dev/stm32-bridge
+      - /dev/esp32-bridge:/dev/esp32-bridge
       - /dev/bus/usb:/dev/bus/usb
       - /dev/i2c-7:/dev/i2c-7
       - /dev/video0:/dev/video0
@@ -97,13 +97,13 @@ services:
           rgb_camera.profile:=640x480x30
       "
 
-  # ── STM32 bridge node (bidirectional serial<->ROS2) ────────────────────────
+  # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ────────────────────────
-  stm32-bridge:
+  esp32-bridge:
     image: saltybot/ros2-humble:jetson-orin
     build:
       context: .
       dockerfile: Dockerfile
-    container_name: saltybot-stm32-bridge
+    container_name: saltybot-esp32-bridge
     restart: unless-stopped
     runtime: nvidia
     network_mode: host
@@ -111,13 +111,13 @@ services:
       - ROS_DOMAIN_ID=42
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     devices:
-      - /dev/stm32-bridge:/dev/stm32-bridge
+      - /dev/esp32-bridge:/dev/esp32-bridge
     command: >
       bash -c "
         source /opt/ros/humble/setup.bash &&
         ros2 launch saltybot_bridge bridge.launch.py
           mode:=bidirectional
-          serial_port:=/dev/stm32-bridge
+          serial_port:=/dev/esp32-bridge
       "
 
   # ── 4x IMX219 CSI cameras ──────────────────────────────────────────────────
@@ -192,7 +192,7 @@ services:
     network_mode: host
     depends_on:
       - saltybot-ros2
-      - stm32-bridge
+      - esp32-bridge
       - csi-cameras
     environment:
       - ROS_DOMAIN_ID=42
@@ -208,8 +208,8 @@ services:
       "
 
 
-  # -- Remote e-stop bridge (MQTT over 4G -> STM32 CDC) ----------------------
+  # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ----------------------
-  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to STM32.
+  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3.
   # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
   remote-estop:
     image: saltybot/ros2-humble:jetson-orin
@@ -221,12 +221,12 @@ services:
     runtime: nvidia
     network_mode: host
     depends_on:
-      - stm32-bridge
+      - esp32-bridge
     environment:
       - ROS_DOMAIN_ID=42
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     devices:
-      - /dev/stm32-bridge:/dev/stm32-bridge
+      - /dev/esp32-bridge:/dev/esp32-bridge
     volumes:
       - ./ros2_ws/src:/ros2_ws/src:rw
       - ./config:/config:ro
@@ -316,7 +316,7 @@ services:
     runtime: nvidia
     network_mode: host
     depends_on:
-      - stm32-bridge
+      - esp32-bridge
     environment:
       - NVIDIA_VISIBLE_DEVICES=all
       - NVIDIA_DRIVER_CAPABILITIES=all,audio

jetson/docs/pinout.md

@@ -1,5 +1,5 @@
 # Jetson Orin Nano Super — GPIO / I2C / UART / CSI Pinout Reference
-## Self-Balancing Robot: STM32F722 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
+## Self-Balancing Robot: ESP32-S3 BALANCE + CAN Bus + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
 
 Last updated: 2026-02-28
 JetPack version: 6.x (L4T R36.x / Ubuntu 22.04)
@@ -43,21 +43,21 @@ i2cdetect -l
 
 ---
 
-## 1. STM32F722 Bridge (USB CDC — Primary)
+## 1. ESP32-S3 BALANCE (CAN Bus — Primary)
 
-The STM32 acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**.
+The ESP32-S3 BALANCE acts as a real-time motor + IMU controller. Communication is via **CAN bus (CANable2, SocketCAN)**. 
 
 ### USB CDC Connection
 | Connection | Detail |
 |-----------|--------|
-| Interface | USB Micro-B on STM32 dev board → USB-A on Jetson |
+| Interface | CANable2 USB → USB-A on Jetson (SocketCAN slcan0) |
-| Device node | `/dev/ttyACM0` → symlink `/dev/stm32-bridge` (via udev) |
+| Device node | `/dev/ttyUSB0` → symlink `/dev/canable2` (via udev); SocketCAN `slcan0` |
-| Baud rate | 921600 (configured in STM32 firmware) |
+| Baud rate | 500 kbps CAN bus |
 | Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) |
 | Power | Powered via robot 5V bus (data-only via USB) |
 
 ### Hardware UART (Fallback — 40-pin header)
-| Jetson Pin | Signal | STM32 Pin | Notes |
+| Jetson Pin | Signal | CANable2/ESP32 | Notes |
 |-----------|--------|-----------|-------|
 | Pin 8 (TXD0) | TX → | PA10 (UART1 RX) | Cross-connect TX→RX |
 | Pin 10 (RXD0) | RX ← | PA9 (UART1 TX) | Cross-connect RX→TX |
@@ -65,7 +65,7 @@ The STM32 acts as a real-time motor + IMU controller. Communication is via **USB
 
 **Jetson device node:** `/dev/ttyTHS0`
 **Baud rate:** 921600, 8N1
-**Voltage level:** 3.3V — both Jetson Orin and STM32F722 are 3.3V GPIO
+**Voltage level:** 3.3V differential CAN signals (ISO 11898)
 
 ```bash
 # Verify UART
@@ -75,13 +75,13 @@ sudo usermod -aG dialout $USER
 picocom -b 921600 /dev/ttyTHS0
 ```
 
-**ROS2 topics (STM32 bridge node):**
+**ROS2 topics (ESP32-S3 BALANCE bridge node):**
 | ROS2 Topic | Direction | Content |
 |-----------|-----------|---------
-| `/saltybot/imu` | STM32→Jetson | IMU data (accel, gyro) at 50Hz |
+| `/saltybot/imu` | ESP32-S3→Jetson | IMU data (accel, gyro) at 50Hz |
-| `/saltybot/balance_state` | STM32→Jetson | Motor cmd, pitch, state |
+| `/saltybot/balance_state` | ESP32-S3→Jetson | Motor cmd, pitch, state |
-| `/cmd_vel` | Jetson→STM32 | Velocity commands → `C<spd>,<str>\n` |
+| `/cmd_vel` | Jetson→ESP32-S3 | Velocity commands → `C<spd>,<str>\n` |
-| `/saltybot/estop` | Jetson→STM32 | Emergency stop |
+| `/saltybot/estop` | Jetson→ESP32-S3 | Emergency stop |
 
 ---
 
@@ -266,7 +266,7 @@ sudo mkdir -p /mnt/nvme
 |------|------|----------|
 | USB-A (top, blue) | USB 3.1 Gen 1 | RealSense D435i |
 | USB-A (bottom) | USB 2.0 | RPLIDAR (via USB-UART adapter) |
-| USB-C | USB 3.1 Gen 1 (+ DP) | STM32 CDC or host flash |
+| USB-C | USB 3.1 Gen 1 (+ DP) | CANable2 USB or host flash |
 | Micro-USB | Debug/flash | JetPack flash only |
 
 ---
@@ -277,10 +277,10 @@ sudo mkdir -p /mnt/nvme
 |-------------|----------|---------|----------|
 | 3 | SDA1 | 3.3V | I2C data (i2c-7) |
 | 5 | SCL1 | 3.3V | I2C clock (i2c-7) |
-| 8 | TXD0 | 3.3V | UART TX → STM32 (fallback) |
+| 8 | TXD0 | 3.3V | UART TX → ESP32-S3 BALANCE (fallback) |
-| 10 | RXD0 | 3.3V | UART RX ← STM32 (fallback) |
+| 10 | RXD0 | 3.3V | UART RX ← ESP32-S3 BALANCE (fallback) |
 | USB-A ×2 | — | 5V | D435i, RPLIDAR |
-| USB-C | — | 5V | STM32 CDC |
+| USB-C | — | 5V | CANable2 USB |
 | CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left |
 | CSI-B (J8) | MIPI CSI-2 | — | Cameras rear + right |
 | M.2 Key M | PCIe Gen3 ×4 | — | NVMe SSD |
@@ -298,9 +298,9 @@ Apply stable device names:
 KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
   SYMLINK+="rplidar", MODE="0666"
 
-# STM32 USB CDC (STMicroelectronics)
+# ESP32-S3 BALANCE (CANable2 USB)
 KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
-  SYMLINK+="stm32-bridge", MODE="0666"
+  SYMLINK+="esp32-balance", MODE="0666"
 
 # Intel RealSense D435i
 SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \

jetson/docs/power-budget.md

@@ -56,7 +56,7 @@ sudo jtop
 |-----------|----------|------------|----------|-----------|-------|
 | RealSense D435i | 0.3 | 1.5 | 3.5 | USB 3.1 | Peak during boot/init |
 | RPLIDAR A1M8 | 0.4 | 2.6 | 3.0 | USB (UART adapter) | Motor spinning |
-| STM32F722 bridge | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V |
+| ESP32-S3 BALANCE | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V |
 | 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active |
 | **Peripheral Subtotal** | **0.9** | **6.1** | **8.9** | | |
 
@@ -151,7 +151,7 @@ LiPo 4S (16.8V max)
        ├─► DC-DC Buck → 5V 6A ──► Jetson Orin barrel jack (30W)
        │   (e.g., XL4016E1)
        │
-       ├─► DC-DC Buck → 5V 3A ──► STM32 + logic 5V rail
+       ├─► DC-DC Buck → 5V 3A ──► ESP32-S3 + logic 5V rail
        │
        └─► Hoverboard ESC ──► Hub motors (48V loop)
 ```

jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml

@@ -2,7 +2,7 @@
 # Used by both serial_bridge_node (RX-only) and saltybot_cmd_node (bidirectional)
 
 # ── Serial ─────────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
+# Use /dev/esp32-balance if udev rule from jetson/docs/pinout.md is applied.
 serial_port:      /dev/ttyACM0
 baud_rate:        921600
 timeout:          0.05   # serial readline timeout (seconds)
@@ -11,7 +11,7 @@ reconnect_delay:  2.0    # seconds between reconnect attempts on serial disconne
 # ── saltybot_cmd_node (bidirectional) only ─────────────────────────────────────
 
 # Heartbeat: H\n sent every heartbeat_period seconds.
-# STM32 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
+# ESP32-S3 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
 heartbeat_period: 0.2    # seconds (= 200ms)
 
 # Twist → ESC command scaling

jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml

@@ -1,5 +1,5 @@
 # cmd_vel_bridge_params.yaml
-# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 autonomous drive.
+# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 autonomous drive.
 #
 # Run with:
 #   ros2 launch saltybot_bridge cmd_vel_bridge.launch.py
@@ -7,14 +7,14 @@
 #   ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=0.3
 
 # ── Serial ─────────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
+# Use /dev/esp32-balance if udev rule from jetson/docs/pinout.md is applied.
 serial_port:      /dev/ttyACM0
 baud_rate:        921600
 timeout:          0.05       # serial readline timeout (s)
 reconnect_delay:  2.0        # seconds between reconnect attempts
 
 # ── Heartbeat ──────────────────────────────────────────────────────────────────
-# STM32 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).
+# ESP32-S3 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).
 # Keep heartbeat well below that threshold.
 heartbeat_period: 0.2        # seconds (200ms)
 
@@ -50,5 +50,5 @@ ramp_rate:        500        # ESC units/second
 # ── Deadman switch ─────────────────────────────────────────────────────────────
 # If /cmd_vel is not received for this many seconds, target speed/steer are
 # zeroed immediately. The ramp then drives the robot to a stop.
-# 500ms matches the STM32 jetson heartbeat timeout for consistency.
+# 500ms matches the ESP32-S3 jetson heartbeat timeout for consistency.
 cmd_vel_timeout:  0.5        # seconds

jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml

@@ -1,18 +1,18 @@
-# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
+# esp32_cmd_params.yaml — Configuration for esp32_cmd_node (Issue #119)
-# Binary-framed Jetson↔STM32 bridge at 921600 baud.
+# Binary-framed Jetson↔ESP32-S3 BALANCE bridge at 460800 baud.
 
 # ── Serial port ────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if the udev rule is applied:
+# Use /dev/esp32-balance if the udev rule is applied:
-#   SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
+#   SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
-#   SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
+#   SYMLINK+="esp32-balance", MODE="0660", GROUP="dialout"
-serial_port:      /dev/ttyACM0
+serial_port:      /dev/esp32-balance
-baud_rate:        921600
+baud_rate:        460800
 reconnect_delay:  2.0        # seconds between USB reconnect attempts
 
 # ── Heartbeat ─────────────────────────────────────────────────────────────────
 # HEARTBEAT frame sent every heartbeat_period seconds.
-# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
+# ESP32-S3 fires watchdog and reverts to safe state if no frame received for 500ms.
-heartbeat_period: 0.2        # 200ms → well within 500ms STM32 watchdog
+heartbeat_period: 0.2        # 200ms → well within 500ms ESP32 watchdog
 
 # ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
 # If no /cmd_vel message received for watchdog_timeout seconds,

jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml

@@ -1,6 +1,6 @@
 remote_estop_node:
   ros__parameters:
-    serial_port:               /dev/stm32-bridge
+    serial_port:               /dev/esp32-balance
     baud_rate:                 921600
     mqtt_host:                 "mqtt.example.com"
     mqtt_port:                 1883

jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py

@@ -6,7 +6,7 @@ Two deployment modes:
   1. Full bidirectional (recommended for Nav2):
        ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional
      Starts saltybot_cmd_node — owns serial port, handles both RX telemetry
-     and TX /cmd_vel → STM32 commands + heartbeat.
+     and TX /cmd_vel → ESP32-S3 commands + heartbeat.
 
   2. RX-only (telemetry monitor, no drive commands):
        ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only
@@ -40,7 +40,7 @@ def _launch_nodes(context, *args, **kwargs):
         return [Node(
             package="saltybot_bridge",
             executable="serial_bridge_node",
-            name="stm32_serial_bridge",
+            name="esp32_serial_bridge",
             output="screen",
             parameters=[params],
         )]
@@ -65,7 +65,7 @@ def generate_launch_description():
         DeclareLaunchArgument("mode",         default_value="bidirectional",
                               description="bidirectional | rx_only"),
         DeclareLaunchArgument("serial_port",  default_value="/dev/ttyACM0",
-                              description="STM32 USB CDC device node"),
+                              description="ESP32-S3 USB CDC device node"),
         DeclareLaunchArgument("baud_rate",    default_value="921600"),
         DeclareLaunchArgument("speed_scale",  default_value="1000.0",
                               description="m/s → ESC units (linear.x scale)"),

jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py

@@ -1,10 +1,10 @@
 """
-cmd_vel_bridge.launch.py — Nav2 cmd_vel → STM32 autonomous drive bridge.
+cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32-S3 autonomous drive bridge.
 
 Starts cmd_vel_bridge_node, which owns the serial port exclusively and provides:
   - /cmd_vel subscription with velocity limits + smooth ramp
   - Deadman switch (zero speed if /cmd_vel silent > cmd_vel_timeout)
-  - Mode gate (drives only when STM32 is in AUTONOMOUS mode, md=2)
+  - Mode gate (drives only when ESP32-S3 is in AUTONOMOUS mode, md=2)
   - Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics
   - /saltybot/cmd publisher (observability)
 
@@ -72,12 +72,12 @@ def generate_launch_description():
             description="Full path to cmd_vel_bridge_params.yaml (overrides inline args)"),
         DeclareLaunchArgument(
             "serial_port",     default_value="/dev/ttyACM0",
-            description="STM32 USB CDC device node"),
+            description="ESP32-S3 USB CDC device node"),
         DeclareLaunchArgument(
             "baud_rate",       default_value="921600"),
         DeclareLaunchArgument(
             "heartbeat_period",default_value="0.2",
-            description="Heartbeat interval (s); must be < STM32 HB timeout (0.5s)"),
+            description="Heartbeat interval (s); must be < ESP32-S3 HB timeout (0.5s)"),
         DeclareLaunchArgument(
             "max_linear_vel",  default_value="0.5",
             description="Hard speed cap before scaling (m/s)"),

jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py

@@ -1,14 +1,14 @@
-"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119).
+"""esp32_cmd.launch.py — Launch the binary-framed ESP32 command node (Issue #119).
 
 Usage:
   # Default (binary protocol, bidirectional):
-  ros2 launch saltybot_bridge stm32_cmd.launch.py
+  ros2 launch saltybot_bridge esp32_cmd.launch.py
 
   # Override serial port:
-  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
+  ros2 launch saltybot_bridge esp32_cmd.launch.py serial_port:=/dev/ttyACM1
 
   # Custom velocity scales:
-  ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
+  ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
 """
 
 import os
@@ -21,11 +21,11 @@ from launch_ros.actions import Node
 
 def generate_launch_description() -> LaunchDescription:
     pkg = get_package_share_directory("saltybot_bridge")
-    params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
+    params_file = os.path.join(pkg, "config", "esp32_cmd_params.yaml")
 
     return LaunchDescription([
-        DeclareLaunchArgument("serial_port",      default_value="/dev/ttyACM0"),
+        DeclareLaunchArgument("serial_port",      default_value="/dev/esp32-balance"),
-        DeclareLaunchArgument("baud_rate",         default_value="921600"),
+        DeclareLaunchArgument("baud_rate",         default_value="460800"),
         DeclareLaunchArgument("speed_scale",       default_value="1000.0"),
         DeclareLaunchArgument("steer_scale",       default_value="-500.0"),
         DeclareLaunchArgument("watchdog_timeout",  default_value="0.5"),
@@ -33,8 +33,8 @@ def generate_launch_description() -> LaunchDescription:
 
         Node(
             package="saltybot_bridge",
-            executable="stm32_cmd_node",
+            executable="esp32_cmd_node",
-            name="stm32_cmd_node",
+            name="esp32_cmd_node",
             output="screen",
             emulate_tty=True,
             parameters=[

jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py

@@ -2,7 +2,7 @@
 uart_bridge.launch.py — FC↔Orin UART bridge (Issue #362)
 
 Launches serial_bridge_node configured for Jetson Orin UART port.
-Bridges Flight Controller (STM32F722) telemetry from /dev/ttyTHS1 into ROS2.
+Bridges Flight Controller (ESP32-S3 BALANCE) telemetry from /dev/ttyTHS1 into ROS2.
 
 Published topics (same as USB CDC bridge):
   /saltybot/imu              sensor_msgs/Imu         — pitch/roll/yaw as angular velocity
@@ -20,7 +20,7 @@ Usage:
 
 Prerequisites:
   - Flight Controller connected to /dev/ttyTHS1 @ 921600 baud
-  - STM32 firmware transmitting JSON telemetry frames (50 Hz)
+  - ESP32-S3 firmware transmitting JSON telemetry frames (50 Hz)
   - ROS2 environment sourced (source install/setup.bash)
 
 Note:

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py

@@ -1,6 +1,6 @@
 """battery_node.py — Battery management for saltybot (Issue #125).
 
-Subscribes to /saltybot/telemetry/battery (JSON from stm32_cmd_node) and:
+Subscribes to /saltybot/telemetry/battery (JSON from esp32_io_cmd_node) and:
   - Publishes sensor_msgs/BatteryState on /saltybot/battery
   - Publishes JSON alerts on /saltybot/battery/alert at threshold crossings
   - Reduces speed limit at low SoC via /saltybot/speed_limit (std_msgs/Float32)
@@ -14,7 +14,7 @@ Alert levels (SoC thresholds):
    5%  EMERGENCY — publish zero /cmd_vel, disarm, log + alert
 
 SoC source priority:
-  1. soc_pct field from STM32 BATTERY telemetry (fuel gauge or lookup on STM32)
+  1. soc_pct field from ESP32-S3 IO BATTERY telemetry (fuel gauge or lookup on ESP32-S3 IO)
   2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known
 
 Parameters (config/battery_params.yaml):
@@ -320,7 +320,7 @@ class BatteryNode(Node):
         self._speed_limit_pub.publish(msg)
 
     def _execute_safe_stop(self) -> None:
-        """Send zero /cmd_vel and disarm the STM32."""
+        """Send zero /cmd_vel and disarm the ESP32-S3 IO."""
         self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming")
         # Publish zero velocity
         zero_twist = Twist()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py

@@ -1,5 +1,5 @@
 """
-cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 drive command bridge.
+cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 BALANCE drive command bridge.
 
 Extends the basic saltybot_cmd_node with four additions required for safe
 autonomous operation on a self-balancing robot:
@@ -12,7 +12,7 @@ autonomous operation on a self-balancing robot:
   3. Deadman switch    — if /cmd_vel is silent for cmd_vel_timeout seconds,
                          zero targets immediately (Nav2 node crash / planner
                          stall → robot coasts to stop rather than running away).
-  4. Mode gate         — only issue non-zero drive commands when STM32 reports
+  4. Mode gate         — only issue non-zero drive commands when ESP32-S3 BALANCE reports
                          md=2 (AUTONOMOUS). In any other mode (RC_MANUAL,
                          RC_ASSISTED) Jetson cannot override the RC pilot.
                          On mode re-entry current ramp state resets to 0 so
@@ -20,9 +20,9 @@ autonomous operation on a self-balancing robot:
 
 Serial protocol (C<speed>,<steer>\\n / H\\n — same as saltybot_cmd_node):
   C<spd>,<str>\\n  — drive command. speed/steer: -1000..+1000 integers.
-  H\\n             — heartbeat. STM32 reverts steer to 0 after 500ms silence.
+  H\\n             — heartbeat. ESP32-S3 IO reverts steer to 0 after 500ms silence.
 
-Telemetry (50 Hz from STM32):
+Telemetry (50 Hz from ESP32-S3 IO):
   Same RX/publish pipeline as saltybot_cmd_node.
   The "md" field (0=MANUAL,1=ASSISTED,2=AUTO) is parsed for the mode gate.
 
@@ -134,7 +134,7 @@ class CmdVelBridgeNode(Node):
         self._current_speed = 0   # ramped output actually sent
         self._current_steer = 0
         self._last_cmd_vel  = 0.0 # wall clock (seconds) of last /cmd_vel msg
-        self._stm32_mode    = 0   # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO
+        self._balance_mode    = 0   # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO
         self._last_state    = -1
         self._frame_count   = 0
         self._error_count   = 0
@@ -150,7 +150,7 @@ class CmdVelBridgeNode(Node):
         self._open_serial()
 
         # ── Timers ────────────────────────────────────────────────────────────
-        # Telemetry read at 100 Hz (STM32 sends at 50 Hz)
+        # Telemetry read at 100 Hz (ESP32-S3 IO sends at 50 Hz)
         self._read_timer    = self.create_timer(0.01,             self._read_cb)
         # Control loop at 50 Hz: ramp + deadman + mode gate + send
         self._control_timer = self.create_timer(1.0 / _CONTROL_HZ, self._control_cb)
@@ -225,7 +225,7 @@ class CmdVelBridgeNode(Node):
 
         # Mode gate: in non-AUTONOMOUS mode, zero and reset ramp state so
         # re-entry always accelerates smoothly from 0.
-        if self._stm32_mode != MODE_AUTONOMOUS:
+        if self._balance_mode != MODE_AUTONOMOUS:
             self._current_speed = 0
             self._current_steer = 0
             speed, steer = 0, 0
@@ -238,7 +238,7 @@ class CmdVelBridgeNode(Node):
             speed = self._current_speed
             steer = self._current_steer
 
-        # Send to STM32
+        # Send to ESP32-S3 IO
         frame = f"C{speed},{steer}\n".encode("ascii")
         if not self._write(frame):
             self.get_logger().warn(
@@ -256,7 +256,7 @@ class CmdVelBridgeNode(Node):
     # ── Heartbeat TX ──────────────────────────────────────────────────────────
 
     def _heartbeat_cb(self):
-        """H\\n keeps STM32 jetson_cmd heartbeat alive regardless of mode."""
+        """H\\n keeps ESP32-S3 IO jetson_cmd heartbeat alive regardless of mode."""
         self._write(b"H\n")
 
     # ── Telemetry RX ──────────────────────────────────────────────────────────
@@ -319,7 +319,7 @@ class CmdVelBridgeNode(Node):
         state     = int(data["s"])
         mode      = int(data.get("md", 0))  # 0=MANUAL if not present
 
-        self._stm32_mode = mode
+        self._balance_mode = mode
         self._frame_count += 1
 
         self._publish_imu(pitch_deg, roll_deg, yaw_deg, now)
@@ -378,7 +378,7 @@ class CmdVelBridgeNode(Node):
         diag.header.stamp = stamp
         status = DiagnosticStatus()
         status.name        = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32_balance"
         status.message     = f"{state_label} [{mode_label}]"
         status.level = (
             DiagnosticStatus.OK    if state == 1 else
@@ -406,11 +406,11 @@ class CmdVelBridgeNode(Node):
         status = DiagnosticStatus()
         status.level       = DiagnosticStatus.ERROR
         status.name        = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32_balance"
         status.message     = f"IMU fault errno={errno}"
         diag.status.append(status)
         self._diag_pub.publish(diag)
-        self.get_logger().error(f"STM32 IMU fault: errno={errno}")
+        self.get_logger().error(f"ESP32-S3 IO IMU fault: errno={errno}")
 
     # ── Lifecycle ─────────────────────────────────────────────────────────────
 

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py

@@ -1,8 +1,8 @@
 """
-remote_estop_node.py -- Remote e-stop bridge: MQTT -> STM32 USB CDC
+remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32-S3 IO USB CDC
 
-{"kill": true}  -> writes 'E\n' to STM32 (ESTOP_REMOTE, immediate motor cutoff)
+{"kill": true}  -> writes 'E\n' to ESP32-S3 IO (ESTOP_REMOTE, immediate motor cutoff)
-{"kill": false} -> writes 'Z\n' to STM32 (clear latch, robot can re-arm)
+{"kill": false} -> writes 'Z\n' to ESP32-S3 IO (clear latch, robot can re-arm)
 
 Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in
 AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT).
@@ -26,7 +26,7 @@ class RemoteEstopNode(Node):
     def __init__(self):
         super().__init__('remote_estop_node')
 
-        self.declare_parameter('serial_port', '/dev/stm32-bridge')
+        self.declare_parameter('serial_port', '/dev/esp32-io')
         self.declare_parameter('baud_rate', 921600)
         self.declare_parameter('mqtt_host', 'mqtt.example.com')
         self.declare_parameter('mqtt_port', 1883)

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py

@@ -322,7 +322,7 @@ class SaltybotCanNode(Node):
         diag.header.stamp = stamp
         st = DiagnosticStatus()
         st.name        = "saltybot/balance_controller"
-        st.hardware_id = "stm32f722"
+        st.hardware_id = "esp32_balance"
         st.message     = state_label
         st.level       = (DiagnosticStatus.OK    if state == 1 else
                           DiagnosticStatus.WARN   if state == 0 else

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py

@@ -1,20 +1,20 @@
 """
-saltybot_cmd_node — full bidirectional STM32↔Jetson bridge
+saltybot_cmd_node — full bidirectional ESP32-S3 IO↔Jetson bridge
 Combines telemetry RX (from serial_bridge_node) with drive command TX.
 
 Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node.
 
-RX path (50Hz from STM32):
+RX path (50Hz from ESP32-S3 IO):
   JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics
 
 TX path:
-  /cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n  → STM32
+  /cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n  → ESP32-S3 IO
-  Heartbeat timer (200ms)        → H\\n                 → STM32
+  Heartbeat timer (200ms)        → H\\n                 → ESP32-S3 IO
 
 Protocol:
-  H\\n              — heartbeat. STM32 reverts steer to 0 if gap > 500ms.
+  H\\n              — heartbeat. ESP32-S3 IO reverts steer to 0 if gap > 500ms.
   C<spd>,<str>\\n   — drive command. speed/steer: -1000..+1000 integers.
-                      C command also refreshes STM32 heartbeat timer.
+                      C command also refreshes ESP32-S3 IO heartbeat timer.
 
 Twist mapping (configurable via ROS2 params):
   speed = clamp(linear.x  * speed_scale, -1000, 1000)
@@ -100,7 +100,7 @@ class SaltybotCmdNode(Node):
         self._open_serial()
 
         # ── Timers ────────────────────────────────────────────────────────────
-        # Telemetry read at 100Hz (STM32 sends at 50Hz)
+        # Telemetry read at 100Hz (ESP32-S3 IO sends at 50Hz)
         self._read_timer = self.create_timer(0.01, self._read_cb)
         # Heartbeat TX at configured period (default 200ms)
         self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
@@ -266,7 +266,7 @@ class SaltybotCmdNode(Node):
         diag.header.stamp = stamp
         status = DiagnosticStatus()
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32_balance"
         status.message = state_label
         if state == 1:
             status.level = DiagnosticStatus.OK
@@ -294,11 +294,11 @@ class SaltybotCmdNode(Node):
         status = DiagnosticStatus()
         status.level = DiagnosticStatus.ERROR
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32_balance"
         status.message = f"IMU fault errno={errno}"
         diag.status.append(status)
         self._diag_pub.publish(diag)
-        self.get_logger().error(f"STM32 IMU fault: errno={errno}")
+        self.get_logger().error(f"ESP32-S3 IMU fault: errno={errno}")
 
     # ── TX — command send ─────────────────────────────────────────────────────
 
@@ -316,7 +316,7 @@ class SaltybotCmdNode(Node):
             )
 
     def _heartbeat_cb(self):
-        """Send H\\n heartbeat. STM32 reverts steer to 0 if gap > 500ms."""
+        """Send H\\n heartbeat. ESP32-S3 IO reverts steer to 0 if gap > 500ms."""
         self._write(b"H\n")
 
     # ── Lifecycle ─────────────────────────────────────────────────────────────

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py

@@ -1,6 +1,6 @@
 """
 saltybot_bridge — serial_bridge_node
-STM32F722 USB CDC → ROS2 topic publisher
+ESP32-S3 BALANCE USB CDC → ROS2 topic publisher
 
 Telemetry frame (50 Hz, newline-delimited JSON):
   {"p":<pitch×10>,"r":<roll×10>,"e":<err×10>,"ig":<integral×10>,
@@ -29,7 +29,7 @@ from sensor_msgs.msg import Imu
 from std_msgs.msg import String
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
 
-# Balance state labels matching STM32 balance_state_t enum
+# Balance state labels matching ESP32-S3 IO balance_state_t enum
 _STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
 
 # Sensor frame_id published in Imu header
@@ -38,7 +38,7 @@ IMU_FRAME_ID = "imu_link"
 
 class SerialBridgeNode(Node):
     def __init__(self):
-        super().__init__("stm32_serial_bridge")
+        super().__init__("esp32_io_serial_bridge")
 
         # ── Parameters ────────────────────────────────────────────────────────
         self.declare_parameter("serial_port", "/dev/ttyACM0")
@@ -83,11 +83,11 @@ class SerialBridgeNode(Node):
 
         # ── Open serial and start read timer ──────────────────────────────────
         self._open_serial()
-        # Poll at 100 Hz — STM32 sends at 50 Hz, so we never miss a frame
+        # Poll at 100 Hz — ESP32-S3 IO sends at 50 Hz, so we never miss a frame
         self._timer = self.create_timer(0.01, self._read_cb)
 
         self.get_logger().info(
-            f"stm32_serial_bridge started — {port} @ {baud} baud"
+            f"esp32_io_serial_bridge started — {port} @ {baud} baud"
         )
 
     # ── Serial management ─────────────────────────────────────────────────────
@@ -117,7 +117,7 @@ class SerialBridgeNode(Node):
 
     def write_serial(self, data: bytes) -> bool:
         """
-        Send raw bytes to STM32 over the open serial port.
+        Send raw bytes to ESP32-S3 IO over the open serial port.
         Returns False if port is not open (caller should handle gracefully).
         Note: for bidirectional use prefer saltybot_cmd_node which owns TX natively.
         """
@@ -206,7 +206,7 @@ class SerialBridgeNode(Node):
         """
         Publish sensor_msgs/Imu.
 
-        The STM32 IMU gives Euler angles (pitch/roll from accelerometer+gyro
+        The ESP32-S3 IO IMU gives Euler angles (pitch/roll from accelerometer+gyro
         fusion, yaw from gyro integration). We publish them as angular_velocity
         for immediate use by slam_toolbox / robot_localization.
 
@@ -264,7 +264,7 @@ class SerialBridgeNode(Node):
         diag.header.stamp = stamp
         status = DiagnosticStatus()
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32_balance"
         status.message = state_label
 
         if state == 1:   # ARMED
@@ -293,11 +293,11 @@ class SerialBridgeNode(Node):
         status = DiagnosticStatus()
         status.level = DiagnosticStatus.ERROR
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32_balance"
         status.message = f"IMU fault errno={errno}"
         diag.status.append(status)
         self._diag_pub.publish(diag)
-        self.get_logger().error(f"STM32 reported IMU fault: errno={errno}")
+        self.get_logger().error(f"ESP32-S3 IO reported IMU fault: errno={errno}")
 
     def destroy_node(self):
         self._close_serial()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py

@@ -1,483 +0,0 @@
-"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge.
-
-Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
-framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
-
-TX commands (Jetson → STM32):
-  SPEED_STEER  — 50 Hz from /cmd_vel subscription
-  HEARTBEAT    — 200 ms timer (STM32 watchdog fires at 500 ms)
-  ARM          — via /saltybot/arm service
-  SET_MODE     — via /saltybot/set_mode service
-  PID_UPDATE   — via /saltybot/pid_update topic
-
-Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
-
-RX telemetry (STM32 → Jetson):
-  IMU          → /saltybot/imu           (sensor_msgs/Imu)
-  BATTERY      → /saltybot/telemetry/battery (std_msgs/String JSON)
-  MOTOR_RPM    → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
-  ARM_STATE    → /saltybot/arm_state     (std_msgs/String JSON)
-  ERROR        → /saltybot/error         (std_msgs/String JSON)
-  All frames   → /diagnostics            (diagnostic_msgs/DiagnosticArray)
-
-Auto-reconnect: USB disconnect is detected when serial.read() raises; node
-continuously retries at reconnect_delay interval.
-
-This node owns /dev/ttyACM0 exclusively — do NOT run alongside
-serial_bridge_node or saltybot_cmd_node on the same port.
-
-Parameters (config/stm32_cmd_params.yaml):
-  serial_port       /dev/ttyACM0
-  baud_rate         921600
-  reconnect_delay   2.0   (seconds)
-  heartbeat_period  0.2   (seconds)
-  watchdog_timeout  0.5   (seconds — no /cmd_vel → send zero-speed)
-  speed_scale       1000.0 (linear.x m/s → ESC units)
-  steer_scale      -500.0  (angular.z rad/s → ESC units, neg to flip convention)
-"""
-
-from __future__ import annotations
-
-import json
-import math
-import threading
-import time
-
-import rclpy
-from rclpy.node import Node
-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 (
-    FrameParser,
-    ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
-    encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
-    encode_pid_update,
-)
-
-# ── Constants ─────────────────────────────────────────────────────────────────
-
-IMU_FRAME_ID = "imu_link"
-_ARM_LABEL   = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
-
-
-def _clamp(v: float, lo: float, hi: float) -> float:
-    return max(lo, min(hi, v))
-
-
-# ── Node ──────────────────────────────────────────────────────────────────────
-
-class Stm32CmdNode(Node):
-    """Binary-framed Jetson↔STM32 bridge node."""
-
-    def __init__(self) -> None:
-        super().__init__("stm32_cmd_node")
-
-        # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("serial_port",      "/dev/ttyACM0")
-        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)
-
-        port                  = self.get_parameter("serial_port").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 ───────────────────────────────────────────────────────────────
-        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 ────────────────────────────────────────────────────────
-        self._imu_pub      = self.create_publisher(Imu,    "/saltybot/imu",           sensor_qos)
-        self._arm_pub      = self.create_publisher(String, "/saltybot/arm_state",      rel_qos)
-        self._error_pub    = self.create_publisher(String, "/saltybot/error",          rel_qos)
-        self._battery_pub  = self.create_publisher(String, "/saltybot/telemetry/battery",  rel_qos)
-        self._rpm_pub      = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
-        self._diag_pub     = self.create_publisher(DiagnosticArray, "/diagnostics",    rel_qos)
-
-        # ── Subscribers ───────────────────────────────────────────────────────
-        self._cmd_vel_sub = self.create_subscription(
-            Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
-        )
-        self._pid_sub = self.create_subscription(
-            String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
-        )
-
-        # ── 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()
-
-        # ── 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 — {port} @ {baud} baud | "
-            f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
-        )
-
-    # ── Serial management ─────────────────────────────────────────────────────
-
-    def _open_serial(self) -> bool:
-        with self._ser_lock:
-            try:
-                self._ser = serial.Serial(
-                    port=self._port_name,
-                    baudrate=self._baud,
-                    timeout=0.005,          # non-blocking reads
-                    write_timeout=0.1,
-                )
-                self._ser.reset_input_buffer()
-                self._parser.reset()
-                self.get_logger().info(f"Serial open: {self._port_name}")
-                return True
-            except serial.SerialException as exc:
-                self.get_logger().error(
-                    f"Cannot open {self._port_name}: {exc}",
-                    throttle_duration_sec=self._reconnect_delay,
-                )
-                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
-            try:
-                self._ser.write(data)
-                return True
-            except serial.SerialException as exc:
-                self.get_logger().error(f"Serial write error: {exc}")
-                self._ser = None
-                return False
-
-    # ── RX — read callback ────────────────────────────────────────────────────
-
-    def _read_cb(self) -> None:
-        """Read bytes from serial and feed them to the frame parser."""
-        raw: bytes | None = None
-        reconnect_needed  = False
-
-        with self._ser_lock:
-            if self._ser is None or not self._ser.is_open:
-                reconnect_needed = True
-            else:
-                try:
-                    n = self._ser.in_waiting
-                    if n:
-                        raw = self._ser.read(n)
-                except serial.SerialException as exc:
-                    self.get_logger().error(f"Serial read error: {exc}")
-                    self._ser = None
-                    reconnect_needed = True
-
-        if reconnect_needed:
-            self.get_logger().warn(
-                "Serial disconnected — will retry",
-                throttle_duration_sec=self._reconnect_delay,
-            )
-            time.sleep(self._reconnect_delay)
-            self._open_serial()
-            return
-
-        if not raw:
-            return
-
-        for byte in raw:
-            frame = self._parser.feed(byte)
-            if frame is not None:
-                self._rx_frame_count += 1
-                self._dispatch_frame(frame)
-
-    def _dispatch_frame(self, frame) -> None:
-        """Route a decoded frame to the appropriate publisher."""
-        now = self.get_clock().now().to_msg()
-
-        if isinstance(frame, ImuFrame):
-            self._publish_imu(frame, now)
-
-        elif isinstance(frame, BatteryFrame):
-            self._publish_battery(frame, now)
-
-        elif isinstance(frame, MotorRpmFrame):
-            self._publish_motor_rpm(frame, now)
-
-        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 _watchdog_cb(self) -> None:
-        """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
-        if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
-            if not self._watchdog_sent:
-                self.get_logger().warn(
-                    f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
-                )
-                self._watchdog_sent = True
-            self._last_speed = 0
-            self._last_steer = 0
-            self._write(encode_speed_steer(0, 0))
-
-    def _on_pid_update(self, msg: String) -> None:
-        """Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
-        try:
-            data = json.loads(msg.data)
-            kp = float(data["kp"])
-            ki = float(data["ki"])
-            kd = float(data["kd"])
-        except (ValueError, KeyError, json.JSONDecodeError) as exc:
-            self.get_logger().error(f"Bad PID update JSON: {exc}")
-            return
-        frame = encode_pid_update(kp, ki, kd)
-        if self._write(frame):
-            self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
-        else:
-            self.get_logger().warn("PID_UPDATE dropped — serial not open")
-
-    # ── 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/stm32_cmd_node"
-        status.hardware_id = "stm32f722"
-
-        port_ok = self._ser is not None and self._ser.is_open
-        if port_ok:
-            status.level   = DiagnosticStatus.OK
-            status.message = "Serial OK"
-        else:
-            status.level   = DiagnosticStatus.ERROR
-            status.message = f"Serial disconnected: {self._port_name}"
-
-        wd_age = time.monotonic() - self._last_cmd_t
-        status.values = [
-            KeyValue(key="serial_port",    value=self._port_name),
-            KeyValue(key="port_open",      value=str(port_ok)),
-            KeyValue(key="rx_frames",      value=str(self._rx_frame_count)),
-            KeyValue(key="rx_errors",      value=str(self._parser.frames_error)),
-            KeyValue(key="last_speed",     value=str(self._last_speed)),
-            KeyValue(key="last_steer",     value=str(self._last_steer)),
-            KeyValue(key="cmd_vel_age_s",  value=f"{wd_age:.2f}"),
-            KeyValue(key="battery_mv",     value=str(self._last_battery_mv)),
-            KeyValue(key="arm_state",      value=_ARM_LABEL.get(self._last_arm_state, "?")),
-        ]
-        diag.status.append(status)
-        self._diag_pub.publish(diag)
-
-    # ── Lifecycle ─────────────────────────────────────────────────────────────
-
-    def destroy_node(self) -> None:
-        # Send zero-speed + disarm on shutdown
-        self._write(encode_speed_steer(0, 0))
-        self._write(encode_arm(False))
-        self._close_serial()
-        super().destroy_node()
-
-
-def main(args=None) -> None:
-    rclpy.init(args=args)
-    node = Stm32CmdNode()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py

@@ -1,332 +0,0 @@
-"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
-
-Issue #119: defines the binary serial protocol between the Jetson Nano and the
-STM32F722 flight controller over USB CDC @ 921600 baud.
-
-Frame layout (all multi-byte fields are big-endian):
-  ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
-  │  STX │ TYPE │  LEN │     PAYLOAD      │  CRC16    │  ETX │
-  │ 0x02 │  1B  │  1B  │    LEN bytes     │   2B BE   │ 0x03 │
-  └──────┴──────┴──────┴──────────────────┴───────────┴──────┘
-
-CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
-CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
-
-Command types (Jetson → STM32):
-  0x01  HEARTBEAT   — no payload              (len=0)
-  0x02  SPEED_STEER — int16 speed + int16 steer (len=4)  range: -1000..+1000
-  0x03  ARM         — uint8 (0=disarm, 1=arm)  (len=1)
-  0x04  SET_MODE    — uint8 mode               (len=1)
-  0x05  PID_UPDATE  — float32 kp + ki + kd     (len=12)
-
-Telemetry types (STM32 → Jetson):
-  0x10  IMU         — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12)
-  0x11  BATTERY     — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5)
-  0x12  MOTOR_RPM   — int16 left_rpm + int16 right_rpm     (len=4)
-  0x13  ARM_STATE   — uint8 state + uint8 error_flags       (len=2)
-  0x14  ERROR       — uint8 error_code + uint8 subcode      (len=2)
-
-Usage:
-  # Encoding (Jetson → STM32)
-  frame = encode_speed_steer(300, -150)
-  ser.write(frame)
-
-  # Decoding (STM32 → Jetson), one byte at a time
-  parser = FrameParser()
-  for byte in incoming_bytes:
-      result = parser.feed(byte)
-      if result is not None:
-          handle_frame(result)
-"""
-
-from __future__ import annotations
-
-import struct
-from dataclasses import dataclass
-from enum import IntEnum
-from typing import Optional
-
-# ── Frame constants ───────────────────────────────────────────────────────────
-
-STX = 0x02
-ETX = 0x03
-MAX_PAYLOAD_LEN = 64       # hard limit; any frame larger is corrupt
-
-
-# ── Command / telemetry type codes ────────────────────────────────────────────
-
-class CmdType(IntEnum):
-    HEARTBEAT   = 0x01
-    SPEED_STEER = 0x02
-    ARM         = 0x03
-    SET_MODE    = 0x04
-    PID_UPDATE  = 0x05
-
-
-class TelType(IntEnum):
-    IMU       = 0x10
-    BATTERY   = 0x11
-    MOTOR_RPM = 0x12
-    ARM_STATE = 0x13
-    ERROR     = 0x14
-
-
-# ── Parsed telemetry objects ──────────────────────────────────────────────────
-
-@dataclass
-class ImuFrame:
-    pitch_deg: float      # degrees (positive = forward tilt)
-    roll_deg:  float
-    yaw_deg:   float
-    accel_x:   float      # m/s²
-    accel_y:   float
-    accel_z:   float
-
-
-@dataclass
-class BatteryFrame:
-    voltage_mv:  int      # millivolts (e.g. 11100 = 11.1 V)
-    current_ma:  int      # milliamps (negative = charging)
-    soc_pct:     int      # state of charge 0–100 (from STM32 fuel gauge or lookup)
-
-
-@dataclass
-class MotorRpmFrame:
-    left_rpm:   int
-    right_rpm:  int
-
-
-@dataclass
-class ArmStateFrame:
-    state:       int      # 0=DISARMED 1=ARMED 2=TILT_FAULT
-    error_flags: int      # bitmask
-
-
-@dataclass
-class ErrorFrame:
-    error_code: int
-    subcode:    int
-
-
-# Union type for decoded results
-TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame
-
-
-# ── CRC16 CCITT ───────────────────────────────────────────────────────────────
-
-def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
-    """CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR."""
-    crc = init
-    for byte in data:
-        crc ^= byte << 8
-        for _ in range(8):
-            if crc & 0x8000:
-                crc = (crc << 1) ^ 0x1021
-            else:
-                crc <<= 1
-        crc &= 0xFFFF
-    return crc
-
-
-# ── Frame encoder ─────────────────────────────────────────────────────────────
-
-def _build_frame(cmd_type: int, payload: bytes) -> bytes:
-    """Assemble a complete binary frame with CRC16."""
-    assert len(payload) <= MAX_PAYLOAD_LEN, "Payload too large"
-    length = len(payload)
-    header = bytes([cmd_type, length])
-    crc = _crc16_ccitt(header + payload)
-    return bytes([STX, cmd_type, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
-
-
-def encode_heartbeat() -> bytes:
-    """HEARTBEAT frame — no payload."""
-    return _build_frame(CmdType.HEARTBEAT, b"")
-
-
-def encode_speed_steer(speed: int, steer: int) -> bytes:
-    """SPEED_STEER frame — int16 speed + int16 steer, both in -1000..+1000."""
-    speed = max(-1000, min(1000, int(speed)))
-    steer = max(-1000, min(1000, int(steer)))
-    return _build_frame(CmdType.SPEED_STEER, struct.pack(">hh", speed, steer))
-
-
-def encode_arm(arm: bool) -> bytes:
-    """ARM frame — 0=disarm, 1=arm."""
-    return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
-
-
-def encode_set_mode(mode: int) -> bytes:
-    """SET_MODE frame — mode byte."""
-    return _build_frame(CmdType.SET_MODE, struct.pack("B", mode & 0xFF))
-
-
-def encode_pid_update(kp: float, ki: float, kd: float) -> bytes:
-    """PID_UPDATE frame — three float32 values."""
-    return _build_frame(CmdType.PID_UPDATE, struct.pack(">fff", kp, ki, kd))
-
-
-# ── Frame decoder (state-machine parser) ─────────────────────────────────────
-
-class ParserState(IntEnum):
-    WAIT_STX  = 0
-    WAIT_TYPE = 1
-    WAIT_LEN  = 2
-    PAYLOAD   = 3
-    CRC_HI    = 4
-    CRC_LO    = 5
-    WAIT_ETX  = 6
-
-
-class ParseError(Exception):
-    pass
-
-
-class FrameParser:
-    """Byte-by-byte streaming parser for 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:
-            result = parser.feed(b)
-            if result is not None:
-                process(result)
-    """
-
-    def __init__(self) -> None:
-        self._state    = ParserState.WAIT_STX
-        self._type     = 0
-        self._length   = 0
-        self._payload  = bytearray()
-        self._crc_rcvd = 0
-        self.frames_ok    = 0
-        self.frames_error = 0
-
-    def reset(self) -> None:
-        """Reset parser to initial state (call after error or port reconnect)."""
-        self._state   = ParserState.WAIT_STX
-        self._payload = bytearray()
-
-    def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
-        """Process one byte. Returns decoded frame on success, None otherwise.
-
-        On CRC error, increments frames_error and resets. The return value on
-        success is a dataclass (ImuFrame, BatteryFrame, etc.) or a
-        (type_code, raw_payload) tuple for unknown type codes.
-        """
-        s = self._state
-
-        if s == ParserState.WAIT_STX:
-            if byte == STX:
-                self._state = ParserState.WAIT_TYPE
-            return None
-
-        if s == ParserState.WAIT_TYPE:
-            self._type  = byte
-            self._state = ParserState.WAIT_LEN
-            return None
-
-        if s == ParserState.WAIT_LEN:
-            self._length  = byte
-            self._payload = bytearray()
-            if self._length > MAX_PAYLOAD_LEN:
-                # Corrupt frame — too big; reset
-                self.frames_error += 1
-                self.reset()
-                return None
-            if self._length == 0:
-                self._state = ParserState.CRC_HI
-            else:
-                self._state = ParserState.PAYLOAD
-            return None
-
-        if s == ParserState.PAYLOAD:
-            self._payload.append(byte)
-            if len(self._payload) == self._length:
-                self._state = ParserState.CRC_HI
-            return None
-
-        if s == ParserState.CRC_HI:
-            self._crc_rcvd = byte << 8
-            self._state    = ParserState.CRC_LO
-            return None
-
-        if s == ParserState.CRC_LO:
-            self._crc_rcvd |= byte
-            self._state     = ParserState.WAIT_ETX
-            return None
-
-        if s == ParserState.WAIT_ETX:
-            self.reset()   # always reset so we look for next STX
-            if byte != ETX:
-                self.frames_error += 1
-                return None
-
-            # Verify CRC
-            crc_data = bytes([self._type, self._length]) + self._payload
-            expected = _crc16_ccitt(crc_data)
-            if expected != self._crc_rcvd:
-                self.frames_error += 1
-                return None
-
-            # Decode
-            self.frames_ok += 1
-            return _decode_telemetry(self._type, bytes(self._payload))
-
-        # Should never reach here
-        self.reset()
-        return None
-
-
-# ── Telemetry decoder ─────────────────────────────────────────────────────────
-
-def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
-    """Decode a validated telemetry payload into a typed dataclass."""
-    try:
-        if type_code == TelType.IMU:
-            if len(payload) < 12:
-                return None
-            p, r, y, ax, ay, az = struct.unpack_from(">hhhhhh", payload)
-            return ImuFrame(
-                pitch_deg=p  / 100.0,
-                roll_deg=r   / 100.0,
-                yaw_deg=y    / 100.0,
-                accel_x=ax   / 100.0,
-                accel_y=ay   / 100.0,
-                accel_z=az   / 100.0,
-            )
-
-        if type_code == TelType.BATTERY:
-            if len(payload) < 5:
-                return None
-            v_mv, i_ma, soc = struct.unpack_from(">HhB", payload)
-            return BatteryFrame(voltage_mv=v_mv, current_ma=i_ma, soc_pct=soc)
-
-        if type_code == TelType.MOTOR_RPM:
-            if len(payload) < 4:
-                return None
-            left, right = struct.unpack_from(">hh", payload)
-            return MotorRpmFrame(left_rpm=left, right_rpm=right)
-
-        if type_code == TelType.ARM_STATE:
-            if len(payload) < 2:
-                return None
-            state, flags = struct.unpack_from("BB", payload)
-            return ArmStateFrame(state=state, error_flags=flags)
-
-        if type_code == TelType.ERROR:
-            if len(payload) < 2:
-                return None
-            code, sub = struct.unpack_from("BB", payload)
-            return ErrorFrame(error_code=code, subcode=sub)
-
-    except struct.error:
-        return None
-
-    # Unknown telemetry type — return raw
-    return (type_code, payload)

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -13,7 +13,7 @@ setup(
             "launch/bridge.launch.py",
             "launch/cmd_vel_bridge.launch.py",
             "launch/remote_estop.launch.py",
-            "launch/stm32_cmd.launch.py",
+            "launch/esp32_cmd.launch.py",
             "launch/battery.launch.py",
             "launch/uart_bridge.launch.py",
         ]),
@@ -21,7 +21,7 @@ setup(
             "config/bridge_params.yaml",
             "config/cmd_vel_bridge_params.yaml",
             "config/estop_params.yaml",
-            "config/stm32_cmd_params.yaml",
+            "config/esp32_cmd_params.yaml",
             "config/battery_params.yaml",
         ]),
     ],
@@ -29,7 +29,7 @@ setup(
     zip_safe=True,
     maintainer="sl-jetson",
     maintainer_email="sl-jetson@saltylab.local",
-    description="STM32 USB CDC → ROS2 serial bridge for saltybot",
+    description="ESP32-S3 USB serial → ROS2 bridge for saltybot",
     license="MIT",
     tests_require=["pytest"],
     entry_points={
@@ -41,8 +41,8 @@ setup(
             # Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate
             "cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main",
             "remote_estop_node  = saltybot_bridge.remote_estop_node:main",
-            # Binary-framed STM32 command node (Issue #119)
+            # Binary-framed ESP32-S3 IO command node (Issue #119)
-            "stm32_cmd_node    = saltybot_bridge.stm32_cmd_node:main",
+            "esp32_cmd_node    = saltybot_bridge.esp32_cmd_node:main",
             # Battery management node (Issue #125)
             "battery_node      = saltybot_bridge.battery_node:main",
             # Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685)

jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py

@@ -1,5 +1,5 @@
 """
-Unit tests for Jetson→STM32 command serialization logic.
+Unit tests for Jetson→ESP32-S3 command serialization logic.
 Tests Twist→speed/steer conversion and frame formatting.
 Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
 """

jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py

@@ -139,10 +139,10 @@ class TestModeGate:
     MODE_ASSISTED  = 1
     MODE_AUTONOMOUS = 2
 
-    def _apply_mode_gate(self, stm32_mode, current_speed, current_steer,
+    def _apply_mode_gate(self, balance_mode, current_speed, current_steer,
                           target_speed, target_steer, step=10):
         """Mirror of _control_cb mode gate logic."""
-        if stm32_mode != self.MODE_AUTONOMOUS:
+        if balance_mode != self.MODE_AUTONOMOUS:
             # Reset ramp state, send zero
             return 0, 0, 0, 0  # (current_speed, current_steer, sent_speed, sent_steer)
         new_s = _ramp_toward(current_speed, target_speed, step)

jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py

@@ -1,4 +1,4 @@
-"""test_stm32_cmd_node.py — Unit tests for Stm32CmdNode with mock serial port.
+"""test_esp32_cmd_node.py — Unit tests for Esp32CmdNode with mock serial port.
 
 Tests:
   - Serial open/close lifecycle
@@ -12,7 +12,7 @@ Tests:
   - Zero-speed sent on node shutdown
   - CRC errors counted correctly
 
-Run with: pytest test/test_stm32_cmd_node.py -v
+Run with: pytest test/test_esp32_cmd_node.py -v
 No ROS2 runtime required — uses mock Node infrastructure.
 """
 
@@ -29,7 +29,7 @@ import pytest
 
 sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
 
-from saltybot_bridge.stm32_protocol import (
+from saltybot_bridge.esp32_protocol import (
     STX, ETX, CmdType, TelType,
     encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update,
     _build_frame, _crc16_ccitt,
@@ -219,10 +219,10 @@ class TestMockSerialTX:
 class TestMockSerialRX:
     """Test RX parsing path using MockSerial with pre-loaded telemetry data."""
 
-    from saltybot_bridge.stm32_protocol import FrameParser
+    from saltybot_bridge.esp32_protocol import FrameParser
 
     def test_rx_imu_frame(self):
-        from saltybot_bridge.stm32_protocol import FrameParser, ImuFrame
+        from saltybot_bridge.esp32_protocol import FrameParser, ImuFrame
         raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981)
         ms  = MockSerial(rx_data=raw)
         parser = FrameParser()
@@ -241,7 +241,7 @@ class TestMockSerialRX:
         assert f.accel_z   == pytest.approx(9.81)
 
     def test_rx_battery_frame(self):
-        from saltybot_bridge.stm32_protocol import FrameParser, BatteryFrame
+        from saltybot_bridge.esp32_protocol import FrameParser, BatteryFrame
         raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45)
         ms  = MockSerial(rx_data=raw)
         parser = FrameParser()
@@ -257,7 +257,7 @@ class TestMockSerialRX:
         assert f.soc_pct    == 45
 
     def test_rx_multiple_frames_in_one_read(self):
-        from saltybot_bridge.stm32_protocol import FrameParser
+        from saltybot_bridge.esp32_protocol import FrameParser
         raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes())
         ms  = MockSerial(rx_data=raw)
         parser = FrameParser()
@@ -271,7 +271,7 @@ class TestMockSerialRX:
         assert parser.frames_error == 0
 
     def test_rx_bad_crc_counted_as_error(self):
-        from saltybot_bridge.stm32_protocol import FrameParser
+        from saltybot_bridge.esp32_protocol import FrameParser
         raw = bytearray(_arm_state_frame_bytes(state=1))
         raw[-3] ^= 0xFF    # corrupt CRC
         ms = MockSerial(rx_data=bytes(raw))
@@ -282,7 +282,7 @@ class TestMockSerialRX:
         assert parser.frames_error == 1
 
     def test_rx_resync_after_corrupt_byte(self):
-        from saltybot_bridge.stm32_protocol import FrameParser, ArmStateFrame
+        from saltybot_bridge.esp32_protocol import FrameParser, ArmStateFrame
         garbage = b"\xDE\xAD\x00\x00"
         valid   = _arm_state_frame_bytes(state=1)
         ms = MockSerial(rx_data=garbage + valid)

jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_protocol.py

@@ -1,4 +1,4 @@
-"""test_stm32_protocol.py — Unit tests for binary STM32 frame codec.
+"""test_esp32_protocol.py — Unit tests for binary ESP32-S3 frame codec.
 
 Tests:
   - CRC16-CCITT correctness
@@ -12,7 +12,7 @@ Tests:
   - Speed/steer clamping in encode_speed_steer
   - Round-trip encode → decode for all known telemetry types
 
-Run with: pytest test/test_stm32_protocol.py -v
+Run with: pytest test/test_esp32_protocol.py -v
 """
 
 from __future__ import annotations
@@ -25,7 +25,7 @@ import os
 # ── Path setup (no ROS2 install needed) ──────────────────────────────────────
 sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
 
-from saltybot_bridge.stm32_protocol import (
+from saltybot_bridge.esp32_protocol import (
     STX, ETX,
     CmdType, TelType,
     ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,

jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py

@@ -1,5 +1,5 @@
 """
-Unit tests for STM32 telemetry parsing logic.
+Unit tests for ESP32-S3 telemetry parsing logic.
 Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
 """
 

jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml

@@ -19,7 +19,7 @@
 #   inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
 #   DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer)
 #
-# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
+# Output: /cmd_vel (Twist) — ESP32-S3 IO bridge consumes this topic.
 
 bt_navigator:
   ros__parameters:

jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml

@@ -2,12 +2,12 @@
 # Master configuration for full stack bringup
 
 # ────────────────────────────────────────────────────────────────────────────
-# HARDWARE — STM32 Bridge & Motor Control
+# HARDWARE — ESP32-S3 IO Bridge & Motor Control
 # ────────────────────────────────────────────────────────────────────────────
 
 saltybot_bridge_node:
   ros__parameters:
-    serial_port: "/dev/stm32-bridge"
+    serial_port: "/dev/esp32-io"
     baud_rate: 921600
     timeout: 0.05
     reconnect_delay: 2.0

jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py

@@ -39,7 +39,7 @@ Modes
     ─ UWB driver (2-anchor DW3000, publishes /uwb/target)
     ─ YOLOv8n person detection (TensorRT)
     ─ Person follower with UWB+camera fusion
-    ─ cmd_vel bridge → STM32 (deadman + ramp + AUTONOMOUS gate)
+    ─ cmd_vel bridge → ESP32-S3 BALANCE (deadman + ramp + AUTONOMOUS gate)
     ─ rosbridge WebSocket (port 9090)
 
   outdoor
@@ -57,8 +57,8 @@ Modes
 Launch sequence (wall-clock delays — conservative for cold start)
 ─────────────────────────────────────────────────────────────────
    t= 0s  robot_description   (URDF + TF tree)
-   t= 0s  STM32 bridge        (serial port owner — must be first)
+   t= 0s  ESP32-S3 IO bridge  (serial port owner — must be first)
-   t= 2s  cmd_vel bridge      (consumes /cmd_vel, needs STM32 bridge up)
+   t= 2s  cmd_vel bridge      (consumes /cmd_vel, needs ESP32-S3 IO bridge up)
    t= 2s  sensors             (RPLIDAR + RealSense)
    t= 4s  UWB driver          (independent serial device)
    t= 4s  CSI cameras         (optional, independent)
@@ -71,10 +71,10 @@ Launch sequence (wall-clock delays — conservative for cold start)
 
 Safety wiring
 ─────────────
-  • STM32 bridge must be up before cmd_vel bridge sends any command.
+  • ESP32-S3 IO bridge must be up before cmd_vel bridge sends any command.
   • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
-  • STM32 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
+  • ESP32-S3 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
-    until STM32 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
+    until ESP32-S3 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel.
   • follow_enabled:=false disables person follower without stopping the node.
   • To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}'
 
@@ -91,7 +91,7 @@ Topics published by this stack
   /person/target                   PoseStamped    (camera position, base_link)
   /person/detections               Detection2DArray
   /cmd_vel                         Twist          (from follower or Nav2)
-  /saltybot/cmd                    String         (to STM32)
+  /saltybot/cmd                    String         (to ESP32-S3 IO)
   /saltybot/imu                    Imu
   /saltybot/balance_state          String
 """
@@ -209,7 +209,7 @@ def generate_launch_description():
     enable_bridge_arg = DeclareLaunchArgument(
         "enable_bridge",
         default_value="true",
-        description="Launch STM32 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
+        description="Launch ESP32-S3 IO serial bridge + cmd_vel bridge (disable for sim/rosbag)",
     )
 
     enable_rosbridge_arg = DeclareLaunchArgument(
@@ -267,10 +267,10 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         description="UWB anchor-1 serial port (starboard/right side)",
     )
 
-    stm32_port_arg = DeclareLaunchArgument(
+    esp32_port_arg = DeclareLaunchArgument(
-        "stm32_port",
+        "esp32_port",
-        default_value="/dev/stm32-bridge",
+        default_value="/dev/esp32-bridge",
-        description="STM32 USB CDC serial port",
+        description="ESP32-S3 USB CDC serial port",
     )
 
     # ── Shared substitution handles ───────────────────────────────────────────
@@ -282,7 +282,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
     max_linear_vel  = LaunchConfiguration("max_linear_vel")
     uwb_port_a      = LaunchConfiguration("uwb_port_a")
     uwb_port_b      = LaunchConfiguration("uwb_port_b")
-    stm32_port      = LaunchConfiguration("stm32_port")
+    esp32_port      = LaunchConfiguration("esp32_port")
 
     # ── t=0s  Robot description (URDF + TF tree) ──────────────────────────────
     robot_description = IncludeLaunchDescription(
@@ -290,15 +290,15 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         launch_arguments={"use_sim_time": use_sim_time}.items(),
     )
 
-    # ── t=0s  STM32 bidirectional serial bridge ────────────────────────────────
+    # ── t=0s  ESP32-S3 bidirectional serial bridge ────────────────────────────────
-    stm32_bridge = GroupAction(
+    esp32_bridge = GroupAction(
         condition=IfCondition(LaunchConfiguration("enable_bridge")),
         actions=[
             IncludeLaunchDescription(
                 _launch("saltybot_bridge", "launch", "bridge.launch.py"),
                 launch_arguments={
                     "mode":        "bidirectional",
-                    "serial_port": stm32_port,
+                    "serial_port": esp32_port,
                 }.items(),
             ),
         ],
@@ -320,7 +320,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         ],
     )
 
-    # ── t=2s  cmd_vel safety bridge (depends on STM32 bridge) ────────────────
+    # ── t=2s  cmd_vel safety bridge (depends on ESP32-S3 bridge) ────────────────
     cmd_vel_bridge = TimerAction(
         period=2.0,
         actions=[
@@ -577,14 +577,14 @@ enable_mission_logging_arg,
         max_linear_vel_arg,
         uwb_port_a_arg,
         uwb_port_b_arg,
-        stm32_port_arg,
+        esp32_port_arg,
 
         # Startup banner
         banner,
 
         # t=0s
         robot_description,
-        stm32_bridge,
+        esp32_bridge,
 
         # t=0.5s
         mission_logging,

jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py

@@ -15,11 +15,11 @@ Usage
   ros2 launch saltybot_bringup saltybot_bringup.launch.py
   ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal
   ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug
-  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full stm32_port:=/dev/ttyUSB0
+  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full esp32_io_port:=/dev/ttyUSB0
 
 Startup sequence
 ────────────────
-  GROUP A — Drivers      t= 0 s  STM32 bridge, RealSense+RPLIDAR, motor daemon, IMU
+  GROUP A — Drivers      t= 0 s  ESP32-S3 IO bridge, RealSense+RPLIDAR, motor daemon, IMU
   health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
   GROUP B — Perception   t= 8 s  UWB, person detection, object detection, depth costmap, gimbal
   health gate ───────────────────────────────────────────────── t=16 s (full/debug)
@@ -35,7 +35,7 @@ Shutdown
 
 Hardware conditionals
 ─────────────────────
-  Missing devices (stm32_port, uwb_port_a/b, gimbal_port) skip that driver.
+  Missing devices (esp32_io_port, uwb_port_a/b, gimbal_port) skip that driver.
   All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition.
 """
 
@@ -120,10 +120,10 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         description="Use /clock from rosbag/simulator",
     )
 
-    stm32_port_arg = DeclareLaunchArgument(
+    esp32_io_port_arg = DeclareLaunchArgument(
-        "stm32_port",
+        "esp32_io_port",
-        default_value="/dev/stm32-bridge",
+        default_value="/dev/esp32-io",
-        description="STM32 USART bridge serial device",
+        description="ESP32-S3 IO serial device",
     )
 
     uwb_port_a_arg = DeclareLaunchArgument(
@@ -160,7 +160,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
 
     profile         = LaunchConfiguration("profile")
     use_sim_time    = LaunchConfiguration("use_sim_time")
-    stm32_port      = LaunchConfiguration("stm32_port")
+    esp32_io_port   = LaunchConfiguration("esp32_io_port")
     uwb_port_a      = LaunchConfiguration("uwb_port_a")
     uwb_port_b      = LaunchConfiguration("uwb_port_b")
     gimbal_port     = LaunchConfiguration("gimbal_port")
@@ -198,7 +198,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
 
     # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
     # GROUP A — DRIVERS   (t = 0 s, all profiles)
-    # Dependency order: STM32 bridge first, then sensors, then motor daemon.
+    # Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon.
     # Health gate: subsequent groups delayed until t_perception (8 s full/debug).
     # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
 
@@ -212,12 +212,12 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         launch_arguments={"use_sim_time": use_sim_time}.items(),
     )
 
-    # STM32 bidirectional bridge (JLINK USART1)
+    # ESP32-S3 bidirectional bridge (JLINK USART1)
-    stm32_bridge = IncludeLaunchDescription(
+    esp32_bridge = IncludeLaunchDescription(
         _launch("saltybot_bridge", "launch", "bridge.launch.py"),
         launch_arguments={
             "mode":        "bidirectional",
-            "serial_port": stm32_port,
+            "serial_port": esp32_port,
         }.items(),
     )
 
@@ -232,7 +232,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         ],
     )
 
-    # Motor daemon: /cmd_vel → STM32 DRIVE frames (depends on bridge at t=0)
+    # Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0)
     motor_daemon = TimerAction(
         period=2.5,
         actions=[
@@ -541,7 +541,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         # ── Arguments ──────────────────────────────────────────────────────────
         profile_arg,
         use_sim_time_arg,
-        stm32_port_arg,
+        esp32_port_arg,
         uwb_port_a_arg,
         uwb_port_b_arg,
         gimbal_port_arg,
@@ -559,7 +559,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
 
         # ── GROUP A: Drivers (all profiles, t=0–4s) ───────────────────────────
         robot_description,
-        stm32_bridge,
+        esp32_bridge,
         sensors,
         motor_daemon,
         sensor_health,

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py

@@ -20,7 +20,7 @@ theta is kept in (−π, π] after every step.
 
 Int32 rollover
 --------------
-STM32 encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
+ESP32-S3 IO encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
 this by detecting jumps larger than half the int32 range and adjusting by the
 full range:
 

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py

@@ -29,7 +29,7 @@ class Profile:
     name: str
 
     # ── Group A: Drivers (always on in all profiles) ──────────────────────
-    enable_stm32_bridge: bool = True
+    enable_esp32_io_bridge: bool = True
     enable_sensors:      bool = True   # RealSense + RPLIDAR
     enable_motor_daemon: bool = True
     enable_imu:          bool = True
@@ -69,14 +69,14 @@ class Profile:
     t_ui:         float = 22.0  # Group D (nav2 needs ~4 s to load costmaps)
 
     # ── Safety ────────────────────────────────────────────────────────────
-    watchdog_timeout_s:  float = 5.0   # max silence from STM32 bridge (s)
+    watchdog_timeout_s:  float = 5.0   # max silence from ESP32-S3 IO bridge (s)
     cmd_vel_deadman_s:   float = 0.5   # cmd_vel watchdog in bridge
     max_linear_vel:      float = 0.5   # m/s cap passed to bridge + follower
     follow_distance_m:   float = 1.5   # target follow distance (m)
 
     # ── Hardware conditionals ─────────────────────────────────────────────
     #  Paths checked at launch; absent devices skip the relevant node.
-    stm32_port:  str = "/dev/stm32-bridge"
+    esp32_io_port:  str = "/dev/esp32-io"
     uwb_port_a:  str = "/dev/uwb-anchor0"
     uwb_port_b:  str = "/dev/uwb-anchor1"
     gimbal_port: str = "/dev/ttyTHS1"
@@ -90,7 +90,7 @@ class Profile:
 # ── Profile factory ────────────────────────────────────────────────────────────
 
 def _minimal() -> Profile:
-    """Minimal: STM32 bridge + sensors + motor daemon.
+    """Minimal: ESP32-S3 IO bridge + sensors + motor daemon.
 
     Safe drive control only.  No AI, no nav, no social.
     Boot time ~4 s.  RAM ~400 MB.
@@ -115,7 +115,7 @@ def _full() -> Profile:
     return Profile(
         name="full",
         # Drivers
-        enable_stm32_bridge=True,
+        enable_esp32_io_bridge=True,
         enable_sensors=True,
         enable_motor_daemon=True,
         enable_imu=True,

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py

@@ -1,7 +1,7 @@
 """
 wheel_odom_node.py — Differential drive wheel encoder odometry (Issue #184).
 
-Subscribes to raw encoder tick counts from the STM32 bridge, integrates
+Subscribes to raw encoder tick counts from the ESP32-S3 IO bridge, integrates
 differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz.
 Optionally broadcasts the odom → base_link TF transform.
 

jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md

@@ -61,7 +61,7 @@ kill %1
 
 ### Core System Components
 - Robot Description (URDF/TF tree)
-- STM32 Serial Bridge
+- ESP32-S3 IO Serial Bridge
 - cmd_vel Bridge  
 - Rosbridge WebSocket
 
@@ -125,11 +125,11 @@ free -h
 
 ### cmd_vel bridge not responding
 ```bash
-# Verify STM32 bridge is running first
+# Verify ESP32-S3 IO bridge is running first
 ros2 node list | grep bridge
 
 # Check serial port
-ls -l /dev/stm32-bridge
+ls -l /dev/esp32-io
 ```
 
 ## Performance Baseline

jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py

@@ -74,7 +74,7 @@ class TestMinimalProfile:
         assert self.p.name == "minimal"
 
     def test_drivers_enabled(self):
-        assert self.p.enable_stm32_bridge is True
+        assert self.p.enable_esp32_io_bridge is True
         assert self.p.enable_sensors is True
         assert self.p.enable_motor_daemon is True
         assert self.p.enable_imu is True
@@ -124,7 +124,7 @@ class TestFullProfile:
         assert self.p.name == "full"
 
     def test_drivers_enabled(self):
-        assert self.p.enable_stm32_bridge is True
+        assert self.p.enable_esp32_io_bridge is True
         assert self.p.enable_sensors is True
         assert self.p.enable_motor_daemon is True
         assert self.p.enable_imu is True
@@ -312,9 +312,9 @@ class TestSafetyDefaults:
 # ─── Hardware port defaults ────────────────────────────────────────────────────
 
 class TestHardwarePortDefaults:
-    def test_stm32_port_set(self):
+    def test_esp32_io_port_set(self):
         p = _minimal()
-        assert p.stm32_port.startswith("/dev/")
+        assert p.esp32_io_port.startswith("/dev/")
 
     def test_uwb_ports_set(self):
         p = _full()

jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro

@@ -10,7 +10,7 @@
     - Sensors:
       * RPLIDAR A1M8 (360° scanning LiDAR)
       * RealSense D435i (RGB-D camera + IMU)
-      * BNO055 (9-DOF IMU, STM32 FC)
+      * ICM-42688-P (9-DOF IMU, ESP32-S3 BALANCE)
     - Actuators:
       * 2x differential drive motors
       * Pan/Tilt servos for camera
@@ -120,7 +120,7 @@
     <child link="right_wheel_link" />
   </joint>
 
-  <!-- IMU Link (STM32 FC BNO055, mounted on main board) -->
+  <!-- IMU Link (ESP32-S3 BALANCE ICM-42688-P, mounted on main board) -->
   <link name="imu_link">
     <inertial>
       <mass value="0.01" />

jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml

@@ -1,7 +1,11 @@
 can_bridge_node:
   ros__parameters:
-    can_interface: slcan0
+    # SocketCAN interface — CANable 2.0 USB-to-CAN on Orin
+    can_interface: "can0"
+
+    # VESC CAN node IDs (FSESC 6.7 Pro Mini Dual)
     left_vesc_can_id: 56
     right_vesc_can_id: 68
-    mamba_can_id: 1
+
+    # Watchdog: send zero velocity if no /cmd_vel for this many seconds
     command_timeout_s: 0.5

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py

@@ -1 +1 @@
-"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can."""
+"""saltybot_can_bridge — CAN bus bridge for ESP32 BALANCE and VESC telemetry."""

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py

@@ -0,0 +1,169 @@
+#!/usr/bin/env python3
+"""balance_protocol.py — CAN frame codec for SAUL-TEE ESP32 BALANCE + VESC.
+
+CAN bus: 500 kbps, CANable 2.0 (can0) on Orin.
+
+── Orin → ESP32 BALANCE (commands) ───────────────────────────────────────────
+  0x300  DRIVE    8 B   [left_mps f32 LE][right_mps f32 LE]
+  0x301  MODE     1 B   [mode u8]  0=idle  1=drive  2=estop
+  0x302  ESTOP    1 B   [flags u8] bit0=stop bit1=clear
+  0x303  LED      4 B   [pattern u8][r u8][g u8][b u8]
+
+── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
+  0x400  FC_STATUS  8 B  [pitch_x10 i16][motor_cmd u16][vbat_mv u16][state u8][flags u8]  10 Hz
+  0x401  FC_VESC    8 B  [l_rpm_x10 i16][r_rpm_x10 i16][l_cur_x10 i16][r_cur_x10 i16]    10 Hz
+
+── VESC standard CAN frames (29-bit extended IDs) ────────────────────────────
+  VESC node IDs: Left = 56, Right = 68
+  ID = (packet_type << 8) | node_id
+"""
+
+import struct
+from dataclasses import dataclass
+
+ORIN_CMD_DRIVE: int = 0x300
+ORIN_CMD_MODE: int  = 0x301
+ORIN_CMD_ESTOP: int = 0x302
+ORIN_CMD_LED: int   = 0x303
+
+FC_STATUS: int = 0x400
+FC_VESC: int   = 0x401
+
+VESC_LEFT_ID: int  = 56
+VESC_RIGHT_ID: int = 68
+
+VESC_CMD_SET_RPM: int = 3
+VESC_STATUS_1: int    = 9
+VESC_STATUS_4: int    = 16
+VESC_STATUS_5: int    = 27
+
+MODE_IDLE: int  = 0
+MODE_DRIVE: int = 1
+MODE_ESTOP: int = 2
+
+
+@dataclass
+class FcStatus:
+    pitch_deg: float = 0.0
+    motor_cmd: int   = 0
+    vbat_mv:   int   = 0
+    state:     int   = 0
+    flags:     int   = 0
+
+
+@dataclass
+class FcVesc:
+    left_rpm:  float = 0.0
+    right_rpm: float = 0.0
+    left_cur:  float = 0.0
+    right_cur: float = 0.0
+
+
+@dataclass
+class VescStatus1:
+    node_id: int   = 0
+    erpm:    float = 0.0
+    current: float = 0.0
+    duty:    float = 0.0
+
+
+@dataclass
+class VescStatus4:
+    node_id:      int   = 0
+    temp_fet_c:   float = 0.0
+    temp_motor_c: float = 0.0
+    current_in:   float = 0.0
+
+
+@dataclass
+class VescStatus5:
+    node_id: int   = 0
+    tacho:   int   = 0
+    vbat_v:  float = 0.0
+
+
+def encode_drive_cmd(left_mps: float, right_mps: float) -> bytes:
+    """Encode ORIN_CMD_DRIVE (0x300): 8 bytes, 2 × float32 little-endian."""
+    return struct.pack("<ff", float(left_mps), float(right_mps))
+
+
+def encode_mode_cmd(mode: int) -> bytes:
+    """Encode ORIN_CMD_MODE (0x301): 1 byte mode value."""
+    if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+        raise ValueError(f"Invalid mode {mode!r}")
+    return struct.pack("B", mode)
+
+
+def encode_estop_cmd(stop: bool = True, clear: bool = False) -> bytes:
+    """Encode ORIN_CMD_ESTOP (0x302): 1 byte flags (bit0=stop, bit1=clear)."""
+    return struct.pack("B", (0x01 if stop else 0) | (0x02 if clear else 0))
+
+
+def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes:
+    """Encode ORIN_CMD_LED (0x303): 4 bytes."""
+    return struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF)
+
+
+def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple:
+    """Return (arb_id, payload) for a VESC SET_RPM command."""
+    return (VESC_CMD_SET_RPM << 8) | (node_id & 0xFF), struct.pack(">i", int(rpm))
+
+
+def decode_fc_status(data: bytes) -> FcStatus:
+    """Decode FC_STATUS (0x400) payload → FcStatus."""
+    if len(data) < 8:
+        raise ValueError(f"FC_STATUS expects 8 bytes, got {len(data)}")
+    pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hHHBB", data)
+    return FcStatus(
+        pitch_deg=pitch_x10 / 10.0,
+        motor_cmd=motor_cmd,
+        vbat_mv=vbat_mv,
+        state=state,
+        flags=flags,
+    )
+
+
+def decode_fc_vesc(data: bytes) -> FcVesc:
+    """Decode FC_VESC (0x401) payload → FcVesc."""
+    if len(data) < 8:
+        raise ValueError(f"FC_VESC expects 8 bytes, got {len(data)}")
+    l_rpm, r_rpm, l_cur, r_cur = struct.unpack_from(">hhhh", data)
+    return FcVesc(
+        left_rpm=l_rpm / 10.0,
+        right_rpm=r_rpm / 10.0,
+        left_cur=l_cur / 10.0,
+        right_cur=r_cur / 10.0,
+    )
+
+
+def decode_vesc_can_id(can_id: int) -> tuple:
+    """Decode a VESC extended CAN ID → (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_1 payload → VescStatus1."""
+    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 payload → VescStatus4."""
+    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 payload → VescStatus5."""
+    tacho, vbat_x10 = struct.unpack_from(">ih", data[:6])
+    return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0)

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py

@@ -1,20 +1,20 @@
 #!/usr/bin/env python3
 """
-can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor
+can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32-S3 BALANCE
 controller and VESC motor controllers over CAN bus.
 
-The node opens the SocketCAN interface (slcan0 by default), spawns a background
+The node opens the SocketCAN interface (can0 by default), spawns a background
 reader thread to process incoming telemetry, and exposes the following interface:
 
 Subscriptions
 -------------
-  /cmd_vel          geometry_msgs/Twist          → VESC speed commands (CAN)
+  /cmd_vel          geometry_msgs/Twist          -> drive commands (CAN 0x300)
-  /estop            std_msgs/Bool                → Mamba e-stop (CAN)
+  /estop            std_msgs/Bool                -> e-stop (CAN 0x303)
 
 Publications
 ------------
-  /can/imu                sensor_msgs/Imu              Mamba IMU telemetry
+  /can/attitude           std_msgs/Float32MultiArray   ESP32-S3 BALANCE attitude
-  /can/battery            sensor_msgs/BatteryState     Mamba battery telemetry
+  /can/battery            std_msgs/Float32MultiArray   ESP32-S3 BALANCE battery
   /can/vesc/left/state    std_msgs/Float32MultiArray   Left VESC state
   /can/vesc/right/state   std_msgs/Float32MultiArray   Right VESC state
   /can/connection_status  std_msgs/String              "connected" | "disconnected"
@@ -30,30 +30,24 @@ import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
-from rcl_interfaces.msg import SetParametersResult
-from sensor_msgs.msg import BatteryState, Imu
 from std_msgs.msg import Bool, Float32MultiArray, String
 
-from saltybot_can_bridge.mamba_protocol import (
+from saltybot_can_bridge.balance_protocol import (
-    MAMBA_CMD_ESTOP,
+    ESP32_TELEM_ATTITUDE,
-    MAMBA_CMD_MODE,
+    ESP32_TELEM_BATTERY,
-    MAMBA_CMD_VELOCITY,
+    ORIN_CMD_DRIVE,
-    MAMBA_TELEM_BATTERY,
+    ORIN_CMD_ESTOP,
-    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_estop_cmd,
-    encode_mode_cmd,
+    decode_attitude,
-    encode_velocity_cmd,
+    decode_battery,
-    encode_pid_set_cmd,
+    decode_vesc_can_id,
-    decode_battery_telem,
+    decode_vesc_status1,
-    decode_imu_telem,
-    decode_pid_ack,
-    decode_vesc_state,
 )
 
 # Reconnect attempt interval when CAN bus is lost
@@ -64,39 +58,35 @@ _WATCHDOG_HZ: float = 10.0
 
 
 class CanBridgeNode(Node):
-    """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers."""
+    """CAN bus bridge between Orin ROS2 and ESP32-S3 BALANCE / VESC controllers."""
 
     def __init__(self) -> None:
         super().__init__("can_bridge_node")
 
-        # ── Parameters ────────────────────────────────────────────────────
+        # -- Parameters --
-        self.declare_parameter("can_interface", "slcan0")
+        self.declare_parameter("can_interface", "can0")
-        self.declare_parameter("left_vesc_can_id", 56)
+        self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
-        self.declare_parameter("right_vesc_can_id", 68)
+        self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
-        self.declare_parameter("mamba_can_id", 1)
         self.declare_parameter("command_timeout_s", 0.5)
-        self.declare_parameter("pid/kp", 0.0)
-        self.declare_parameter("pid/ki", 0.0)
-        self.declare_parameter("pid/kd", 0.0)
 
         self._iface: str = self.get_parameter("can_interface").value
         self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
         self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
-        self._mamba_id: int = self.get_parameter("mamba_can_id").value
         self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
-        self._pid_kp: float = self.get_parameter("pid/kp").value
-        self._pid_ki: float = self.get_parameter("pid/ki").value
-        self._pid_kd: float = self.get_parameter("pid/kd").value
 
-        # ── State ─────────────────────────────────────────────────────────
+        # -- State --
         self._bus: Optional[can.BusABC] = None
         self._connected: bool = False
         self._last_cmd_time: float = time.monotonic()
-        self._lock = threading.Lock()  # protects _bus / _connected
+        self._lock = threading.Lock()
 
-        # ── Publishers ────────────────────────────────────────────────────
+        # -- Publishers --
-        self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
+        self._pub_attitude = self.create_publisher(
-        self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
+            Float32MultiArray, "/can/attitude", 10
+        )
+        self._pub_battery = self.create_publisher(
+            Float32MultiArray, "/can/battery", 10
+        )
         self._pub_vesc_left = self.create_publisher(
             Float32MultiArray, "/can/vesc/left/state", 10
         )
@@ -107,55 +97,29 @@ class CanBridgeNode(Node):
             String, "/can/connection_status", 10
         )
 
-        # ── Subscriptions ─────────────────────────────────────────────────
+        # -- Subscriptions --
         self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
         self.create_subscription(Bool, "/estop", self._estop_cb, 10)
-        self.add_on_set_parameters_callback(self._on_set_parameters)
 
-        # ── Timers ────────────────────────────────────────────────────────
+        # -- Timers --
         self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
         self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb)
 
-        # ── Open CAN ──────────────────────────────────────────────────────
+        # -- Open CAN --
         self._try_connect()
 
-        # ── Background reader thread ──────────────────────────────────────
+        # -- Background reader thread --
         self._reader_thread = threading.Thread(
             target=self._reader_loop, daemon=True, name="can_reader"
         )
         self._reader_thread.start()
 
         self.get_logger().info(
-            f"can_bridge_node ready — iface={self._iface} "
+            f"can_bridge_node ready -- iface={self._iface} "
-            f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
+            f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id}"
-            f"mamba={self._mamba_id}"
         )
 
-    # -- PID parameter callback (Issue #693) --
+    # -- Connection management --
-
-    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."""
@@ -174,7 +138,7 @@ class CanBridgeNode(Node):
             except Exception as exc:
                 self.get_logger().warning(
                     f"CAN bus not available ({self._iface}): {exc} "
-                    f"— will retry every {_RECONNECT_INTERVAL_S:.0f} s"
+                    f"-- will retry every {_RECONNECT_INTERVAL_S:.0f} s"
                 )
                 self._connected = False
                 self._publish_status("disconnected")
@@ -197,46 +161,33 @@ class CanBridgeNode(Node):
             self._connected = False
         self._publish_status("disconnected")
 
-    # ── ROS callbacks ─────────────────────────────────────────────────────
+    # -- ROS callbacks --
 
     def _cmd_vel_cb(self, msg: Twist) -> None:
-        """Convert /cmd_vel Twist to VESC speed commands over CAN."""
+        """Convert /cmd_vel Twist to ESP32-S3 BALANCE drive commands over CAN (0x300)."""
         self._last_cmd_time = time.monotonic()
-
         if not self._connected:
             return
-
-        # Differential drive decomposition — individual wheel speeds in m/s.
-        # The VESC nodes interpret linear velocity directly; angular is handled
-        # by the sign difference between left and right.
         linear = msg.linear.x
         angular = msg.angular.z
-
+        # Differential drive -- motor units [-1000, +1000]
-        # Forward left = forward right for pure translation; for rotation
+        speed_units = int(linear * 1000.0)
-        # left slows and right speeds up (positive angular = CCW = left turn).
+        steer_units = int(angular * 1000.0)
-        # The Mamba velocity command carries both wheels independently.
+        self._send_can(
-        left_mps = linear - angular
+            ORIN_CMD_DRIVE,
-        right_mps = linear + angular
+            encode_drive_cmd(speed_units, steer_units, mode=MODE_DRIVE),
-
+            "cmd_vel",
-        payload = encode_velocity_cmd(left_mps, right_mps)
+        )
-        self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
-
-        # Keep Mamba in DRIVE mode while receiving commands
-        self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
 
     def _estop_cb(self, msg: Bool) -> None:
-        """Forward /estop to Mamba over CAN."""
+        """Forward /estop to ESP32-S3 BALANCE over CAN (0x303)."""
         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(
+            self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
-                MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
+            self.get_logger().warning("E-stop asserted -- sent ESTOP to ESP32-S3 BALANCE")
-            )
-            self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba")
 
-    # ── Watchdog ──────────────────────────────────────────────────────────
+    # -- Watchdog --
 
     def _watchdog_cb(self) -> None:
         """If no /cmd_vel arrives within the timeout, send zero velocity."""
@@ -245,15 +196,12 @@ class CanBridgeNode(Node):
         elapsed = time.monotonic() - self._last_cmd_time
         if elapsed > self._cmd_timeout:
             self._send_can(
-                MAMBA_CMD_VELOCITY,
+                ORIN_CMD_DRIVE,
-                encode_velocity_cmd(0.0, 0.0),
+                encode_drive_cmd(0, 0, mode=MODE_IDLE),
                 "watchdog zero-vel",
             )
-            self._send_can(
-                MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
-            )
 
-    # ── CAN send helper ───────────────────────────────────────────────────
+    # -- CAN send helper --
 
     def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
         """Send a standard CAN frame; handle errors gracefully."""
@@ -261,7 +209,6 @@ class CanBridgeNode(Node):
             if not self._connected or self._bus is None:
                 return
             bus = self._bus
-
         msg = can.Message(
             arbitration_id=arb_id,
             data=data,
@@ -272,132 +219,95 @@ class CanBridgeNode(Node):
         except can.CanError as exc:
             self._handle_can_error(exc, f"send({context})")
 
-    # ── Background CAN reader ─────────────────────────────────────────────
+    # -- Background CAN reader --
 
     def _reader_loop(self) -> None:
-        """
+        """Blocking CAN read loop executed in a daemon thread."""
-        Blocking CAN read loop executed in a daemon thread.
-        Dispatches incoming frames to the appropriate handler.
-        """
         while rclpy.ok():
             with self._lock:
                 connected = self._connected
                 bus = self._bus
-
             if not connected or bus is None:
                 time.sleep(0.1)
                 continue
-
             try:
                 frame = bus.recv(timeout=0.5)
             except can.CanError as exc:
                 self._handle_can_error(exc, "reader_loop recv")
                 continue
-
             if frame is None:
-                # Timeout — no frame within 0.5 s, loop again
                 continue
-
             self._dispatch_frame(frame)
 
     def _dispatch_frame(self, frame: can.Message) -> None:
         """Route an incoming CAN frame to the correct publisher."""
         arb_id = frame.arbitration_id
         data = bytes(frame.data)
-
         try:
-            if arb_id == MAMBA_TELEM_IMU:
+            if arb_id == ESP32_TELEM_ATTITUDE:
-                self._handle_imu(data, frame.timestamp)
+                self._handle_attitude(data)
-
+            elif arb_id == ESP32_TELEM_BATTERY:
-            elif arb_id == MAMBA_TELEM_BATTERY:
+                self._handle_battery(data)
-                self._handle_battery(data, frame.timestamp)
+            elif frame.is_extended_id:
-
+                # VESC extended 29-bit ID: packet_type<<8 | node_id
-            elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
+                pkt_type, node_id = decode_vesc_can_id(arb_id)
-                self._handle_vesc_state(data, frame.timestamp, side="left")
+                if pkt_type == VESC_STATUS_1:
-
+                    self._handle_vesc_status1(data, node_id)
-            elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
-                self._handle_vesc_state(data, frame.timestamp, side="right")
-
-            elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
-                gains = decode_pid_ack(data)
-                self.get_logger().debug(
-                    f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
-                )
-
         except Exception as exc:
             self.get_logger().warning(
                 f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
             )
 
-    # ── Frame handlers ────────────────────────────────────────────────────
+    # -- Frame handlers --
 
-    def _handle_imu(self, data: bytes, timestamp: float) -> None:
+    def _handle_attitude(self, data: bytes) -> None:
-        telem = decode_imu_telem(data)
+        telem = decode_attitude(data)
-
+        msg = Float32MultiArray()
-        msg = Imu()
+        # Layout: [pitch_deg, speed, yaw_rate, state, flags]
-        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.data = [
-        msg.header.frame_id = "imu_link"
+            telem.pitch_deg,
-
+            telem.speed,
-        msg.linear_acceleration.x = telem.accel_x
+            telem.yaw_rate,
-        msg.linear_acceleration.y = telem.accel_y
+            float(telem.state),
-        msg.linear_acceleration.z = telem.accel_z
+            float(telem.flags),
-
+        ]
-        msg.angular_velocity.x = telem.gyro_x
+        self._pub_attitude.publish(msg)
-        msg.angular_velocity.y = telem.gyro_y
-        msg.angular_velocity.z = telem.gyro_z
-
-        # Covariance unknown; mark as -1 per REP-145
-        msg.orientation_covariance[0] = -1.0
-
-        self._pub_imu.publish(msg)
-
-    def _handle_battery(self, data: bytes, timestamp: float) -> None:
-        telem = decode_battery_telem(data)
-
-        msg = BatteryState()
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.voltage = telem.voltage
-        msg.current = telem.current
-        msg.present = True
-        msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
 
+    def _handle_battery(self, data: bytes) -> None:
+        telem = decode_battery(data)
+        msg = Float32MultiArray()
+        # Layout: [vbat_v, fault_code, rssi]
+        msg.data = [telem.vbat_mv / 1000.0, float(telem.fault_code), float(telem.rssi)]
         self._pub_battery.publish(msg)
 
-    def _handle_vesc_state(
+    def _handle_vesc_status1(self, data: bytes, node_id: int) -> None:
-        self, data: bytes, timestamp: float, side: str
+        status = decode_vesc_status1(node_id, data)
-    ) -> None:
-        telem = decode_vesc_state(data)
-
         msg = Float32MultiArray()
-        # Layout: [erpm, duty, voltage, current]
+        # Layout: [erpm, current, duty]
-        msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
+        msg.data = [status.erpm, status.current, status.duty]
-
+        if node_id == self._left_vesc_id:
-        if side == "left":
             self._pub_vesc_left.publish(msg)
-        else:
+        elif node_id == self._right_vesc_id:
             self._pub_vesc_right.publish(msg)
 
-    # ── Status helper ─────────────────────────────────────────────────────
+    # -- Status helper --
 
     def _publish_status(self, status: str) -> None:
         msg = String()
         msg.data = status
         self._pub_status.publish(msg)
 
-    # ── Shutdown ──────────────────────────────────────────────────────────
+    # -- Shutdown --
 
     def destroy_node(self) -> None:
         """Send zero velocity and shut down the CAN bus cleanly."""
         if self._connected and self._bus is not None:
             try:
                 self._send_can(
-                    MAMBA_CMD_VELOCITY,
+                    ORIN_CMD_DRIVE,
-                    encode_velocity_cmd(0.0, 0.0),
+                    encode_drive_cmd(0, 0, mode=MODE_IDLE),
                     "shutdown",
                 )
-                self._send_can(
-                    MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
-                )
             except Exception:
                 pass
             try:

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py

@@ -1,224 +0,0 @@
-#!/usr/bin/env python3
-"""
-mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller
-and VESC telemetry.
-
-CAN message layout
-------------------
-Command frames  (Orin → Mamba / VESC):
-  MAMBA_CMD_VELOCITY  0x100  8 bytes  left_speed (f32, m/s) | right_speed (f32, m/s)
-  MAMBA_CMD_MODE      0x101  1 byte   mode (0=idle, 1=drive, 2=estop)
-  MAMBA_CMD_ESTOP     0x102  1 byte   0x01 = stop
-
-Telemetry frames (Mamba → Orin):
-  MAMBA_TELEM_IMU     0x200  24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each)
-  MAMBA_TELEM_BATTERY 0x201   8 bytes voltage (f32, V) | current (f32, A)
-
-VESC telemetry frame (VESC → Orin):
-  VESC_TELEM_STATE    0x300  16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32)
-
-All multi-byte fields are big-endian.
-
-Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
-"""
-
-import struct
-from dataclasses import dataclass
-from typing import Tuple
-
-# ---------------------------------------------------------------------------
-# CAN message IDs
-# ---------------------------------------------------------------------------
-
-MAMBA_CMD_VELOCITY: int = 0x100
-MAMBA_CMD_MODE: int = 0x101
-MAMBA_CMD_ESTOP: int = 0x102
-
-MAMBA_TELEM_IMU: int = 0x200
-MAMBA_TELEM_BATTERY: int = 0x201
-
-VESC_TELEM_STATE: int = 0x300
-ORIN_CAN_ID_PID_SET: int = 0x305
-ORIN_CAN_ID_FC_PID_ACK: int = 0x405
-
-# ---------------------------------------------------------------------------
-# Mode constants
-# ---------------------------------------------------------------------------
-
-MODE_IDLE: int = 0
-MODE_DRIVE: int = 1
-MODE_ESTOP: int = 2
-
-# ---------------------------------------------------------------------------
-# Data classes for decoded telemetry
-# ---------------------------------------------------------------------------
-
-
-@dataclass
-class ImuTelemetry:
-    """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU)."""
-
-    accel_x: float = 0.0  # m/s²
-    accel_y: float = 0.0
-    accel_z: float = 0.0
-    gyro_x: float = 0.0   # rad/s
-    gyro_y: float = 0.0
-    gyro_z: float = 0.0
-
-
-@dataclass
-class BatteryTelemetry:
-    """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY)."""
-
-    voltage: float = 0.0   # V
-    current: float = 0.0   # A
-
-
-@dataclass
-class VescStateTelemetry:
-    """Decoded VESC state telemetry (VESC_TELEM_STATE)."""
-
-    erpm: float = 0.0      # electrical RPM
-    duty: float = 0.0      # duty cycle [-1.0, 1.0]
-    voltage: float = 0.0   # bus voltage, V
-    current: float = 0.0   # phase current, A
-
-
-@dataclass
-class PidGains:
-    """Balance PID gains (Issue #693)."""
-    kp: float = 0.0
-    ki: float = 0.0
-    kd: float = 0.0
-
-
-# ---------------------------------------------------------------------------
-# Encode helpers
-# ---------------------------------------------------------------------------
-
-_FMT_VEL = ">ff"       # 2 × float32, big-endian
-_FMT_MODE = ">B"       # 1 × uint8
-_FMT_ESTOP = ">B"      # 1 × uint8
-_FMT_IMU = ">ffffff"   # 6 × float32
-_FMT_BAT = ">ff"       # 2 × float32
-_FMT_VESC = ">ffff"    # 4 × float32
-
-
-def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
-    """
-    Encode a MAMBA_CMD_VELOCITY payload.
-
-    Parameters
-    ----------
-    left_mps:  target left wheel speed in m/s  (positive = forward)
-    right_mps: target right wheel speed in m/s (positive = forward)
-
-    Returns
-    -------
-    8-byte big-endian payload suitable for a CAN frame.
-    """
-    return struct.pack(_FMT_VEL, float(left_mps), float(right_mps))
-
-
-def encode_mode_cmd(mode: int) -> bytes:
-    """
-    Encode a MAMBA_CMD_MODE payload.
-
-    Parameters
-    ----------
-    mode: one of MODE_IDLE (0), MODE_DRIVE (1), MODE_ESTOP (2)
-
-    Returns
-    -------
-    1-byte payload.
-    """
-    if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
-        raise ValueError(f"Invalid mode {mode!r}; expected 0, 1, or 2")
-    return struct.pack(_FMT_MODE, mode)
-
-
-def encode_estop_cmd(stop: bool = True) -> bytes:
-    """
-    Encode a MAMBA_CMD_ESTOP payload.
-
-    Parameters
-    ----------
-    stop: True to assert e-stop, False to clear.
-
-    Returns
-    -------
-    1-byte payload (0x01 = stop, 0x00 = clear).
-    """
-    return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00)
-
-
-def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes:
-    """Encode ORIN_CAN_ID_PID_SET (6 bytes, uint16 BE x3). Issue #693."""
-    if kp < 0.0 or ki < 0.0 or kd < 0.0:
-        raise ValueError("PID gains must be non-negative")
-    return struct.pack(_FMT_PID, round(min(kp,_PID_KP_MAX)*100), round(min(ki,_PID_KI_MAX)*100), round(min(kd,_PID_KD_MAX)*100))
-
-
-# ---------------------------------------------------------------------------
-# Decode helpers
-# ---------------------------------------------------------------------------
-
-def decode_imu_telem(data: bytes) -> ImuTelemetry:
-    """
-    Decode a MAMBA_TELEM_IMU payload.
-
-    Parameters
-    ----------
-    data: exactly 24 bytes (6 × float32, big-endian).
-
-    Returns
-    -------
-    ImuTelemetry dataclass instance.
-
-    Raises
-    ------
-    struct.error if data is the wrong length.
-    """
-    ax, ay, az, gx, gy, gz = struct.unpack(_FMT_IMU, data)
-    return ImuTelemetry(
-        accel_x=ax, accel_y=ay, accel_z=az,
-        gyro_x=gx, gyro_y=gy, gyro_z=gz,
-    )
-
-
-def decode_battery_telem(data: bytes) -> BatteryTelemetry:
-    """
-    Decode a MAMBA_TELEM_BATTERY payload.
-
-    Parameters
-    ----------
-    data: exactly 8 bytes (2 × float32, big-endian).
-
-    Returns
-    -------
-    BatteryTelemetry dataclass instance.
-    """
-    voltage, current = struct.unpack(_FMT_BAT, data)
-    return BatteryTelemetry(voltage=voltage, current=current)
-
-
-def decode_vesc_state(data: bytes) -> VescStateTelemetry:
-    """
-    Decode a VESC_TELEM_STATE payload.
-
-    Parameters
-    ----------
-    data: exactly 16 bytes (4 × float32, big-endian).
-
-    Returns
-    -------
-    VescStateTelemetry dataclass instance.
-    """
-    erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data)
-    return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current)
-
-
-def decode_pid_ack(data: bytes) -> PidGains:
-    """Decode ORIN_CAN_ID_FC_PID_ACK (6 bytes). Issue #693."""
-    kp_x100, ki_x100, kd_x100 = struct.unpack(_FMT_PID, data)
-    return PidGains(kp=kp_x100/100.0, ki=ki_x100/100.0, kd=kd_x100/100.0)

jetson/ros2_ws/src/saltybot_can_bridge/setup.py

@@ -1,21 +1,22 @@
-from setuptools import setup
+from setuptools import find_packages, setup
 
 package_name = "saltybot_can_bridge"
 
 setup(
     name=package_name,
-    version="0.1.0",
+    version="0.0.1",
-    packages=[package_name],
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        (f"share/{package_name}", ["package.xml"]),
+        ("share/" + package_name, ["package.xml"]),
-        (f"share/{package_name}/config", ["config/can_bridge_params.yaml"]),
+        ("share/" + package_name + "/config", ["config/can_bridge_params.yaml"]),
+        ("share/" + package_name + "/launch", ["launch/can_bridge.launch.py"]),
     ],
-    install_requires=["setuptools", "python-can"],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="sl-controls",
+    maintainer="seb",
-    maintainer_email="sl-controls@saltylab.local",
+    maintainer_email="seb@example.com",
-    description="CAN bus bridge for Mamba controller and VESC telemetry",
+    description="CAN bus bridge for ESP32 BALANCE and VESC telemetry",
     license="MIT",
     tests_require=["pytest"],
     entry_points={

jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 """
-Unit tests for saltybot_can_bridge.mamba_protocol.
+Unit tests for saltybot_can_bridge.balance_protocol.
 
-No ROS2 or CAN hardware required — tests exercise encode/decode round-trips
+No ROS2 or CAN hardware required -- tests exercise encode/decode round-trips
 and boundary conditions entirely in Python.
 
 Run with:  pytest test/test_can_bridge.py -v
@@ -11,243 +11,165 @@ Run with:  pytest test/test_can_bridge.py -v
 import struct
 import unittest
 
-from saltybot_can_bridge.mamba_protocol import (
+from saltybot_can_bridge.balance_protocol import (
-    MAMBA_CMD_ESTOP,
+    ORIN_CMD_DRIVE,
-    MAMBA_CMD_MODE,
+    ORIN_CMD_ARM,
-    MAMBA_CMD_VELOCITY,
+    ORIN_CMD_PID,
-    MAMBA_TELEM_BATTERY,
+    ORIN_CMD_ESTOP,
-    MAMBA_TELEM_IMU,
+    ESP32_TELEM_ATTITUDE,
-    VESC_TELEM_STATE,
+    ESP32_TELEM_BATTERY,
-    MODE_DRIVE,
+    VESC_LEFT_ID,
-    MODE_ESTOP,
+    VESC_RIGHT_ID,
+    VESC_STATUS_1,
     MODE_IDLE,
+    MODE_DRIVE,
+    MODE_AUTONOMOUS,
+    AttitudeTelemetry,
     BatteryTelemetry,
-    ImuTelemetry,
+    VescStatus1,
-    VescStateTelemetry,
+    decode_attitude,
-    decode_battery_telem,
+    decode_battery,
-    decode_imu_telem,
+    decode_vesc_status1,
-    decode_vesc_state,
+    encode_drive_cmd,
+    encode_arm_cmd,
     encode_estop_cmd,
-    encode_mode_cmd,
-    encode_velocity_cmd,
 )
 
 
 class TestMessageIDs(unittest.TestCase):
-    """Verify the CAN message ID constants are correct."""
+    """Verify CAN message ID constants match the spec."""
 
-    def test_command_ids(self):
+    def test_orin_cmd_ids(self):
-        self.assertEqual(MAMBA_CMD_VELOCITY, 0x100)
+        self.assertEqual(ORIN_CMD_DRIVE,  0x300)
-        self.assertEqual(MAMBA_CMD_MODE, 0x101)
+        self.assertEqual(ORIN_CMD_ARM,    0x301)
-        self.assertEqual(MAMBA_CMD_ESTOP, 0x102)
+        self.assertEqual(ORIN_CMD_PID,    0x302)
+        self.assertEqual(ORIN_CMD_ESTOP,  0x303)
 
-    def test_telemetry_ids(self):
+    def test_esp32_telem_ids(self):
-        self.assertEqual(MAMBA_TELEM_IMU, 0x200)
+        self.assertEqual(ESP32_TELEM_ATTITUDE, 0x400)
-        self.assertEqual(MAMBA_TELEM_BATTERY, 0x201)
+        self.assertEqual(ESP32_TELEM_BATTERY,  0x401)
-        self.assertEqual(VESC_TELEM_STATE, 0x300)
+
+    def test_vesc_ids(self):
+        self.assertEqual(VESC_LEFT_ID,  56)
+        self.assertEqual(VESC_RIGHT_ID, 68)
+        self.assertEqual(VESC_STATUS_1,  9)
 
 
-class TestVelocityEncode(unittest.TestCase):
+class TestDriveEncode(unittest.TestCase):
-    """Tests for encode_velocity_cmd."""
+    """Tests for encode_drive_cmd."""
 
     def test_zero_velocity(self):
-        payload = encode_velocity_cmd(0.0, 0.0)
+        payload = encode_drive_cmd(0, 0)
         self.assertEqual(len(payload), 8)
-        left, right = struct.unpack(">ff", payload)
+        speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload)
-        self.assertAlmostEqual(left, 0.0, places=5)
+        self.assertEqual(speed, 0)
-        self.assertAlmostEqual(right, 0.0, places=5)
+        self.assertEqual(steer, 0)
+        self.assertEqual(mode, MODE_DRIVE)
 
-    def test_forward_velocity(self):
+    def test_forward(self):
-        payload = encode_velocity_cmd(1.5, 1.5)
+        payload = encode_drive_cmd(500, 0)
-        left, right = struct.unpack(">ff", payload)
+        speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload)
-        self.assertAlmostEqual(left, 1.5, places=5)
+        self.assertEqual(speed, 500)
-        self.assertAlmostEqual(right, 1.5, places=5)
+        self.assertEqual(steer, 0)
 
-    def test_differential_velocity(self):
+    def test_clamping_high(self):
-        payload = encode_velocity_cmd(-0.5, 0.5)
+        payload = encode_drive_cmd(9999, 9999)
-        left, right = struct.unpack(">ff", payload)
+        speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload)
-        self.assertAlmostEqual(left, -0.5, places=5)
+        self.assertEqual(speed, 1000)
-        self.assertAlmostEqual(right, 0.5, places=5)
+        self.assertEqual(steer, 1000)
 
-    def test_large_velocity(self):
+    def test_clamping_low(self):
-        # No clamping at the protocol layer — values are sent as-is
+        payload = encode_drive_cmd(-9999, -9999)
-        payload = encode_velocity_cmd(10.0, -10.0)
+        speed, steer, mode, flags, pad = struct.unpack(">hhBBH", payload)
-        left, right = struct.unpack(">ff", payload)
+        self.assertEqual(speed, -1000)
-        self.assertAlmostEqual(left, 10.0, places=3)
+        self.assertEqual(steer, -1000)
-        self.assertAlmostEqual(right, -10.0, places=3)
 
-    def test_payload_is_big_endian(self):
+    def test_mode_idle(self):
-        # Sanity check: first 4 bytes encode left speed
+        payload = encode_drive_cmd(0, 0, mode=MODE_IDLE)
-        payload = encode_velocity_cmd(1.0, 0.0)
+        speed, steer, mode_byte, flags, pad = struct.unpack(">hhBBH", payload)
-        (left,) = struct.unpack(">f", payload[:4])
+        self.assertEqual(mode_byte, MODE_IDLE)
-        self.assertAlmostEqual(left, 1.0, places=5)
 
 
-class TestModeEncode(unittest.TestCase):
+class TestArmEncode(unittest.TestCase):
-    """Tests for encode_mode_cmd."""
+    """Tests for encode_arm_cmd."""
 
-    def test_idle_mode(self):
+    def test_arm(self):
-        payload = encode_mode_cmd(MODE_IDLE)
+        payload = encode_arm_cmd(True)
-        self.assertEqual(payload, b"\x00")
+        self.assertEqual(len(payload), 1)
+        self.assertEqual(payload[0], 0x01)
 
-    def test_drive_mode(self):
+    def test_disarm(self):
-        payload = encode_mode_cmd(MODE_DRIVE)
+        payload = encode_arm_cmd(False)
-        self.assertEqual(payload, b"\x01")
+        self.assertEqual(len(payload), 1)
-
+        self.assertEqual(payload[0], 0x00)
-    def test_estop_mode(self):
-        payload = encode_mode_cmd(MODE_ESTOP)
-        self.assertEqual(payload, b"\x02")
-
-    def test_invalid_mode_raises(self):
-        with self.assertRaises(ValueError):
-            encode_mode_cmd(99)
-
-    def test_invalid_mode_negative_raises(self):
-        with self.assertRaises(ValueError):
-            encode_mode_cmd(-1)
 
 
 class TestEstopEncode(unittest.TestCase):
     """Tests for encode_estop_cmd."""
 
-    def test_estop_assert(self):
+    def test_estop_magic_byte(self):
-        self.assertEqual(encode_estop_cmd(True), b"\x01")
+        payload = encode_estop_cmd()
-
+        self.assertEqual(len(payload), 1)
-    def test_estop_clear(self):
+        self.assertEqual(payload[0], 0xE5)
-        self.assertEqual(encode_estop_cmd(False), b"\x00")
-
-    def test_estop_default_is_stop(self):
-        self.assertEqual(encode_estop_cmd(), b"\x01")
 
 
-class TestImuDecodeRoundTrip(unittest.TestCase):
+class TestAttitudeDecode(unittest.TestCase):
-    """Round-trip tests for IMU telemetry."""
+    """Tests for decode_attitude (0x400)."""
 
-    def _encode_imu(self, ax, ay, az, gx, gy, gz) -> bytes:
+    def test_roundtrip(self):
-        return struct.pack(">ffffff", ax, ay, az, gx, gy, gz)
+        import struct as _s
+        # Encode a known attitude: pitch=12.5, speed=1.5, yaw_rate=-0.5, state=1, flags=0
+        fmt = ">eeeBB"
+        raw = _s.pack(fmt, 12.5, 1.5, -0.5, 1, 0) + b"\x00"  # pad to 8 bytes
+        att = decode_attitude(raw)
+        self.assertAlmostEqual(att.pitch_deg, 12.5, places=1)
+        self.assertAlmostEqual(att.speed, 1.5, places=1)
+        self.assertEqual(att.state, 1)
 
-    def test_zero_imu(self):
+    def test_too_short_raises(self):
-        data = self._encode_imu(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        with self.assertRaises(ValueError):
-        telem = decode_imu_telem(data)
+            decode_attitude(b"\x00" * 3)
-        self.assertIsInstance(telem, ImuTelemetry)
-        self.assertAlmostEqual(telem.accel_x, 0.0, places=5)
-        self.assertAlmostEqual(telem.gyro_z, 0.0, places=5)
-
-    def test_nominal_imu(self):
-        data = self._encode_imu(0.1, -0.2, 9.81, 0.01, -0.02, 0.03)
-        telem = decode_imu_telem(data)
-        self.assertAlmostEqual(telem.accel_x, 0.1, places=4)
-        self.assertAlmostEqual(telem.accel_y, -0.2, places=4)
-        self.assertAlmostEqual(telem.accel_z, 9.81, places=3)
-        self.assertAlmostEqual(telem.gyro_x, 0.01, places=5)
-        self.assertAlmostEqual(telem.gyro_y, -0.02, places=5)
-        self.assertAlmostEqual(telem.gyro_z, 0.03, places=5)
-
-    def test_imu_bad_length_raises(self):
-        with self.assertRaises(struct.error):
-            decode_imu_telem(b"\x00" * 10)  # too short
 
 
-class TestBatteryDecodeRoundTrip(unittest.TestCase):
+class TestBatteryDecode(unittest.TestCase):
-    """Round-trip tests for battery telemetry."""
+    """Tests for decode_battery (0x401)."""
 
-    def _encode_bat(self, voltage, current) -> bytes:
+    def test_roundtrip(self):
-        return struct.pack(">ff", voltage, current)
+        import struct as _s
+        raw = _s.pack(">HBb", 24000, 0, -65)
+        bat = decode_battery(raw)
+        self.assertEqual(bat.vbat_mv, 24000)
+        self.assertEqual(bat.fault_code, 0)
+        self.assertEqual(bat.rssi, -65)
 
-    def test_nominal_battery(self):
+    def test_too_short_raises(self):
-        data = self._encode_bat(24.6, 3.2)
+        with self.assertRaises(ValueError):
-        telem = decode_battery_telem(data)
+            decode_battery(b"\x00" * 2)
-        self.assertIsInstance(telem, BatteryTelemetry)
-        self.assertAlmostEqual(telem.voltage, 24.6, places=3)
-        self.assertAlmostEqual(telem.current, 3.2, places=4)
-
-    def test_zero_battery(self):
-        data = self._encode_bat(0.0, 0.0)
-        telem = decode_battery_telem(data)
-        self.assertAlmostEqual(telem.voltage, 0.0, places=5)
-        self.assertAlmostEqual(telem.current, 0.0, places=5)
-
-    def test_max_voltage(self):
-        # 6S LiPo max ~25.2 V; test with a high value
-        data = self._encode_bat(25.2, 0.0)
-        telem = decode_battery_telem(data)
-        self.assertAlmostEqual(telem.voltage, 25.2, places=3)
-
-    def test_battery_bad_length_raises(self):
-        with self.assertRaises(struct.error):
-            decode_battery_telem(b"\x00" * 4)  # too short
 
 
-class TestVescStateDecodeRoundTrip(unittest.TestCase):
+class TestVescStatus1Decode(unittest.TestCase):
-    """Round-trip tests for VESC state telemetry."""
+    """Tests for decode_vesc_status1."""
 
-    def _encode_vesc(self, erpm, duty, voltage, current) -> bytes:
+    def test_roundtrip(self):
-        return struct.pack(">ffff", erpm, duty, voltage, current)
+        import struct as _s
-
+        erpm = 12000
-    def test_nominal_vesc(self):
+        cur_x10 = 35   # 3.5 A
-        data = self._encode_vesc(3000.0, 0.25, 24.0, 5.5)
+        duty_x1000 = 450  # 0.45
-        telem = decode_vesc_state(data)
+        raw = _s.pack(">ihh", erpm, cur_x10, duty_x1000)
-        self.assertIsInstance(telem, VescStateTelemetry)
+        st = decode_vesc_status1(VESC_LEFT_ID, raw)
-        self.assertAlmostEqual(telem.erpm, 3000.0, places=2)
+        self.assertEqual(st.node_id, VESC_LEFT_ID)
-        self.assertAlmostEqual(telem.duty, 0.25, places=5)
+        self.assertAlmostEqual(st.erpm, 12000.0, places=1)
-        self.assertAlmostEqual(telem.voltage, 24.0, places=4)
+        self.assertAlmostEqual(st.current, 3.5, places=3)
-        self.assertAlmostEqual(telem.current, 5.5, places=4)
+        self.assertAlmostEqual(st.duty, 0.45, places=3)
-
-    def test_zero_vesc(self):
-        data = self._encode_vesc(0.0, 0.0, 0.0, 0.0)
-        telem = decode_vesc_state(data)
-        self.assertAlmostEqual(telem.erpm, 0.0)
-        self.assertAlmostEqual(telem.duty, 0.0)
-
-    def test_reverse_erpm(self):
-        data = self._encode_vesc(-1500.0, -0.15, 23.0, 2.0)
-        telem = decode_vesc_state(data)
-        self.assertAlmostEqual(telem.erpm, -1500.0, places=2)
-        self.assertAlmostEqual(telem.duty, -0.15, places=5)
-
-    def test_vesc_bad_length_raises(self):
-        with self.assertRaises(struct.error):
-            decode_vesc_state(b"\x00" * 8)  # too short (need 16)
 
 
-class TestEncodeDecodeIdentity(unittest.TestCase):
+class TestModeConstants(unittest.TestCase):
-    """Cross-module identity tests: encode then decode must be identity."""
+    """Tests for mode byte constants."""
 
-    def test_velocity_identity(self):
+    def test_mode_values(self):
-        """encode_velocity_cmd raw bytes must decode to the same floats."""
+        self.assertEqual(MODE_IDLE, 0)
-        left, right = 0.75, -0.3
+        self.assertEqual(MODE_DRIVE, 1)
-        payload = encode_velocity_cmd(left, right)
+        self.assertEqual(MODE_AUTONOMOUS, 2)
-        decoded_l, decoded_r = struct.unpack(">ff", payload)
-        self.assertAlmostEqual(decoded_l, left, places=5)
-        self.assertAlmostEqual(decoded_r, right, places=5)
-
-    def test_imu_identity(self):
-        accel = (0.5, -0.5, 9.8)
-        gyro = (0.1, -0.1, 0.2)
-        raw = struct.pack(">ffffff", *accel, *gyro)
-        telem = decode_imu_telem(raw)
-        self.assertAlmostEqual(telem.accel_x, accel[0], places=4)
-        self.assertAlmostEqual(telem.accel_y, accel[1], places=4)
-        self.assertAlmostEqual(telem.accel_z, accel[2], places=3)
-        self.assertAlmostEqual(telem.gyro_x, gyro[0], places=5)
-        self.assertAlmostEqual(telem.gyro_y, gyro[1], places=5)
-        self.assertAlmostEqual(telem.gyro_z, gyro[2], places=5)
-
-    def test_battery_identity(self):
-        voltage, current = 22.4, 8.1
-        raw = struct.pack(">ff", voltage, current)
-        telem = decode_battery_telem(raw)
-        self.assertAlmostEqual(telem.voltage, voltage, places=3)
-        self.assertAlmostEqual(telem.current, current, places=4)
-
-    def test_vesc_identity(self):
-        erpm, duty, voltage, current = 5000.0, 0.5, 24.5, 10.0
-        raw = struct.pack(">ffff", erpm, duty, voltage, current)
-        telem = decode_vesc_state(raw)
-        self.assertAlmostEqual(telem.erpm, erpm, places=2)
-        self.assertAlmostEqual(telem.duty, duty, places=5)
-        self.assertAlmostEqual(telem.voltage, voltage, places=3)
-        self.assertAlmostEqual(telem.current, current, places=4)
 
 
 if __name__ == "__main__":

jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py

@@ -1,69 +1,56 @@
 #!/usr/bin/env python3
 """
 protocol_defs.py — CAN message ID constants and frame builders/parsers for the
-Orin↔Mamba↔VESC integration test suite.
+Orin <-> ESP32 BALANCE <-> VESC integration test suite.
 
 All IDs and payload formats are derived from:
-  include/orin_can.h   — Orin↔FC (Mamba) protocol
+  saltybot_can_bridge/balance_protocol.py — canonical codec
-  include/vesc_can.h   — VESC CAN protocol
-  saltybot_can_bridge/mamba_protocol.py — existing bridge constants
 
 CAN IDs used in tests
 ---------------------
-Orin → FC (Mamba) commands (standard 11-bit, matching orin_can.h):
+Orin → ESP32 BALANCE commands (standard 11-bit):
-  ORIN_CMD_HEARTBEAT  0x300
+  ORIN_CMD_DRIVE   0x300    left_mps (f32 LE) | right_mps (f32 LE)
-  ORIN_CMD_DRIVE      0x301    int16 speed (−1000..+1000), int16 steer (−1000..+1000)
+  ORIN_CMD_MODE    0x301    uint8 mode (0=idle, 1=drive, 2=estop)
-  ORIN_CMD_MODE       0x302    uint8 mode byte
+  ORIN_CMD_ESTOP   0x302    uint8 flags (bit0=stop, bit1=clear)
-  ORIN_CMD_ESTOP      0x303    uint8 action (1=ESTOP, 0=CLEAR)
+  ORIN_CMD_LED     0x303    uint8 pattern | r | g | b
 
-FC (Mamba) → Orin telemetry (standard 11-bit, matching orin_can.h):
+ESP32 BALANCE → Orin telemetry (standard 11-bit):
-  FC_STATUS           0x400    8 bytes (see orin_can_fc_status_t)
+  FC_STATUS        0x400    8 bytes (pitch_x10 i16 | motor_cmd u16 | vbat_mv u16 | state u8 | flags u8)
-  FC_VESC             0x401    8 bytes (see orin_can_fc_vesc_t)
+  FC_VESC          0x401    8 bytes (l_rpm_x10 i16 | r_rpm_x10 i16 | l_cur_x10 i16 | r_cur_x10 i16)
-  FC_IMU              0x402    8 bytes
-  FC_BARO             0x403    8 bytes
 
-Mamba ↔ VESC internal commands (matching mamba_protocol.py):
+VESC STATUS (extended 29-bit):
-  MAMBA_CMD_VELOCITY  0x100    8 bytes  left_mps (f32) | right_mps (f32) big-endian
+  arb_id = (VESC_STATUS_1 << 8) | vesc_node_id  = (9 << 8) | node_id
-  MAMBA_CMD_MODE      0x101    1 byte   mode (0=idle,1=drive,2=estop)
+  Payload: int32 ERPM (BE), int16 current×10 (BE), int16 duty×1000 (BE)
-  MAMBA_CMD_ESTOP     0x102    1 byte   0x01=stop
-
-VESC STATUS (extended 29-bit, matching vesc_can.h):
-  arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id  = (9 << 8) | node_id
-  Payload: int32 RPM (BE), int16 current×10 (BE), int16 duty×1000 (BE)
 """
 
 import struct
 from typing import Tuple
 
 # ---------------------------------------------------------------------------
-# Orin → FC (Mamba) command IDs  (from orin_can.h)
+# Orin → ESP32 BALANCE command IDs
 # ---------------------------------------------------------------------------
 
-ORIN_CMD_HEARTBEAT: int = 0x300
+ORIN_CMD_DRIVE: int  = 0x300
-ORIN_CMD_DRIVE: int     = 0x301
+ORIN_CMD_MODE: int   = 0x301
-ORIN_CMD_MODE: int      = 0x302
+ORIN_CMD_ESTOP: int  = 0x302
-ORIN_CMD_ESTOP: int     = 0x303
+ORIN_CMD_LED: int    = 0x303
 
 # ---------------------------------------------------------------------------
-# FC (Mamba) → Orin telemetry IDs  (from orin_can.h)
+# ESP32 BALANCE → Orin telemetry IDs
 # ---------------------------------------------------------------------------
 
 FC_STATUS: int = 0x400
 FC_VESC: int   = 0x401
-FC_IMU: int    = 0x402
-FC_BARO: int   = 0x403
 
 # ---------------------------------------------------------------------------
-# Mamba → VESC internal command IDs  (from mamba_protocol.py)
+# VESC constants
 # ---------------------------------------------------------------------------
 
-MAMBA_CMD_VELOCITY: int = 0x100
+VESC_STATUS_1: int     = 9    # STATUS_1 packet type (upper byte of arb_id)
-MAMBA_CMD_MODE: int     = 0x101
+VESC_CMD_SET_RPM: int  = 3
-MAMBA_CMD_ESTOP: int    = 0x102
 
-MAMBA_TELEM_IMU: int     = 0x200
+VESC_CAN_ID_LEFT: int  = 56
-MAMBA_TELEM_BATTERY: int = 0x201
+VESC_CAN_ID_RIGHT: int = 68
-VESC_TELEM_STATE: int    = 0x300
 
 # ---------------------------------------------------------------------------
 # Mode constants
@@ -73,242 +60,111 @@ MODE_IDLE: int  = 0
 MODE_DRIVE: int = 1
 MODE_ESTOP: int = 2
 
-# ---------------------------------------------------------------------------
-# VESC protocol constants  (from vesc_can.h)
-# ---------------------------------------------------------------------------
 
-VESC_PKT_STATUS: int    = 9   # STATUS packet type (upper byte of arb_id)
+def VESC_STATUS1_ID(vesc_node_id: int) -> int:
-VESC_PKT_SET_RPM: int   = 3   # SET_RPM packet type
+    """Return the 29-bit extended arbitration ID for a VESC STATUS_1 frame."""
-
+    return (VESC_STATUS_1 << 8) | vesc_node_id
-VESC_CAN_ID_LEFT: int  = 56
-VESC_CAN_ID_RIGHT: int = 68
-
-
-def VESC_STATUS_ID(vesc_node_id: int) -> int:
-    """
-    Return the 29-bit extended arbitration ID for a VESC STATUS frame.
-
-    Formula (from vesc_can.h):  arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id
-                                        = (9 << 8) | vesc_node_id
-    """
-    return (VESC_PKT_STATUS << 8) | vesc_node_id
-
-
-def VESC_SET_RPM_ID(vesc_node_id: int) -> int:
-    """
-    Return the 29-bit extended arbitration ID for a VESC SET_RPM command.
-
-    Formula:  arb_id = (VESC_PKT_SET_RPM << 8) | vesc_node_id
-                     = (3 << 8) | vesc_node_id
-    """
-    return (VESC_PKT_SET_RPM << 8) | vesc_node_id
 
 
 # ---------------------------------------------------------------------------
-# Frame builders — Orin → FC
+# Frame builders — Orin → ESP32 BALANCE
 # ---------------------------------------------------------------------------
 
-def build_heartbeat(seq: int = 0) -> bytes:
+def build_drive_cmd(left_mps: float, right_mps: float) -> bytes:
-    """Build a HEARTBEAT payload: uint32 sequence counter (big-endian, 4 bytes)."""
+    """Build ORIN_CMD_DRIVE payload (8 bytes, 2 × float32 little-endian)."""
-    return struct.pack(">I", seq & 0xFFFFFFFF)
+    return struct.pack("<ff", float(left_mps), float(right_mps))
-
-
-def build_drive_cmd(speed: int, steer: int) -> bytes:
-    """
-    Build an ORIN_CMD_DRIVE payload.
-
-    Parameters
-    ----------
-    speed: int, −1000..+1000  (mapped directly to int16)
-    steer: int, −1000..+1000
-    """
-    return struct.pack(">hh", int(speed), int(steer))
 
 
 def build_mode_cmd(mode: int) -> bytes:
-    """Build an ORIN_CMD_MODE payload (1 byte)."""
+    """Build ORIN_CMD_MODE payload (1 byte)."""
-    return struct.pack(">B", mode & 0xFF)
+    return struct.pack("B", mode & 0xFF)
 
 
-def build_estop_cmd(action: int = 1) -> bytes:
+def build_estop_cmd(stop: bool = True, clear: bool = False) -> bytes:
-    """Build an ORIN_CMD_ESTOP payload.  action=1 → ESTOP, 0 → CLEAR."""
+    """Build ORIN_CMD_ESTOP payload (1 byte flags)."""
-    return struct.pack(">B", action & 0xFF)
+    return struct.pack("B", (0x01 if stop else 0) | (0x02 if clear else 0))
 
 
 # ---------------------------------------------------------------------------
-# Frame builders — Mamba velocity commands (mamba_protocol.py encoding)
+# Frame builders — ESP32 BALANCE → Orin telemetry
-# ---------------------------------------------------------------------------
-
-def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
-    """
-    Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
-
-    Matches encode_velocity_cmd() in mamba_protocol.py.
-    """
-    return struct.pack(">ff", float(left_mps), float(right_mps))
-
-
-# ---------------------------------------------------------------------------
-# Frame builders — FC → Orin telemetry
 # ---------------------------------------------------------------------------
 
 def build_fc_status(
     pitch_x10: int = 0,
     motor_cmd: int = 0,
     vbat_mv: int = 24000,
-    balance_state: int = 1,
+    state: int = 1,
     flags: int = 0,
 ) -> bytes:
-    """
+    """Build FC_STATUS (0x400) payload (8 bytes, big-endian)."""
-    Build an FC_STATUS (0x400) payload.
-
-    Layout (orin_can_fc_status_t, 8 bytes, big-endian):
-      int16  pitch_x10
-      int16  motor_cmd
-      uint16 vbat_mv
-      uint8  balance_state
-      uint8  flags  [bit0=estop_active, bit1=armed]
-    """
     return struct.pack(
-        ">hhHBB",
+        ">hHHBB",
         int(pitch_x10),
-        int(motor_cmd),
+        int(motor_cmd) & 0xFFFF,
         int(vbat_mv) & 0xFFFF,
-        int(balance_state) & 0xFF,
+        int(state) & 0xFF,
         int(flags) & 0xFF,
     )
 
 
 def build_fc_vesc(
-    left_rpm_x10: int = 0,
+    l_rpm_x10: int = 0,
-    right_rpm_x10: int = 0,
+    r_rpm_x10: int = 0,
-    left_current_x10: int = 0,
+    l_cur_x10: int = 0,
-    right_current_x10: int = 0,
+    r_cur_x10: int = 0,
 ) -> bytes:
-    """
+    """Build FC_VESC (0x401) payload (8 bytes, big-endian)."""
-    Build an FC_VESC (0x401) payload.
-
-    Layout (orin_can_fc_vesc_t, 8 bytes, big-endian):
-      int16 left_rpm_x10
-      int16 right_rpm_x10
-      int16 left_current_x10
-      int16 right_current_x10
-
-    RPM values are RPM / 10 (e.g. 3000 RPM → 300).
-    Current values are A × 10 (e.g. 5.5 A → 55).
-    """
     return struct.pack(
         ">hhhh",
-        int(left_rpm_x10),
+        int(l_rpm_x10),
-        int(right_rpm_x10),
+        int(r_rpm_x10),
-        int(left_current_x10),
+        int(l_cur_x10),
-        int(right_current_x10),
+        int(r_cur_x10),
     )
 
 
-def build_vesc_status(
+def build_vesc_status1(
-    rpm: int = 0,
+    erpm: int = 0,
     current_x10: int = 0,
     duty_x1000: int = 0,
 ) -> bytes:
-    """
+    """Build VESC STATUS_1 payload (8 bytes, big-endian)."""
-    Build a VESC STATUS (packet type 9) payload.
+    return struct.pack(">ihh", int(erpm), int(current_x10), int(duty_x1000))
-
-    Layout (from vesc_can.h / VESC FW 6.x, big-endian):
-      int32 rpm
-      int16 current × 10
-      int16 duty × 1000
-    Total: 8 bytes.
-    """
-    return struct.pack(
-        ">ihh",
-        int(rpm),
-        int(current_x10),
-        int(duty_x1000),
-    )
 
 
 # ---------------------------------------------------------------------------
 # Frame parsers
 # ---------------------------------------------------------------------------
 
-def parse_fc_status(data: bytes):
+def parse_drive_cmd(data: bytes) -> Tuple[float, float]:
-    """
+    """Parse ORIN_CMD_DRIVE payload → (left_mps, right_mps)."""
-    Parse an FC_STATUS (0x400) payload.
+    if len(data) < 8:
+        raise ValueError(f"ORIN_CMD_DRIVE needs 8 bytes, got {len(data)}")
+    return struct.unpack("<ff", data[:8])
 
-    Returns
+
-    -------
+def parse_fc_status(data: bytes):
-    dict with keys: pitch_x10, motor_cmd, vbat_mv, balance_state, flags,
+    """Parse FC_STATUS (0x400) payload."""
-                    estop_active (bool), armed (bool)
-    """
     if len(data) < 8:
         raise ValueError(f"FC_STATUS needs 8 bytes, got {len(data)}")
-    pitch_x10, motor_cmd, vbat_mv, balance_state, flags = struct.unpack(
+    pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack(">hHHBB", data[:8])
-        ">hhHBB", data[:8]
-    )
     return {
         "pitch_x10": pitch_x10,
+        "pitch_deg": pitch_x10 / 10.0,
         "motor_cmd": motor_cmd,
         "vbat_mv": vbat_mv,
-        "balance_state": balance_state,
+        "state": state,
         "flags": flags,
-        "estop_active": bool(flags & 0x01),
-        "armed": bool(flags & 0x02),
     }
 
 
-def parse_fc_vesc(data: bytes):
+def parse_vesc_status1(data: bytes):
-    """
+    """Parse VESC STATUS_1 payload."""
-    Parse an FC_VESC (0x401) payload.
-
-    Returns
-    -------
-    dict with keys: left_rpm_x10, right_rpm_x10, left_current_x10,
-                    right_current_x10, left_rpm (float), right_rpm (float)
-    """
     if len(data) < 8:
-        raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}")
+        raise ValueError(f"VESC STATUS_1 needs 8 bytes, got {len(data)}")
-    left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack(
+    erpm, cur_x10, duty_x1000 = struct.unpack(">ihh", data[:8])
-        ">hhhh", data[:8]
-    )
     return {
-        "left_rpm_x10": left_rpm_x10,
+        "erpm": erpm,
-        "right_rpm_x10": right_rpm_x10,
+        "current": cur_x10 / 10.0,
-        "left_current_x10": left_cur_x10,
-        "right_current_x10": right_cur_x10,
-        "left_rpm": left_rpm_x10 * 10.0,
-        "right_rpm": right_rpm_x10 * 10.0,
-    }
-
-
-def parse_vesc_status(data: bytes):
-    """
-    Parse a VESC STATUS (packet type 9) payload.
-
-    Returns
-    -------
-    dict with keys: rpm, current_x10, duty_x1000, current (float), duty (float)
-    """
-    if len(data) < 8:
-        raise ValueError(f"VESC STATUS needs 8 bytes, got {len(data)}")
-    rpm, current_x10, duty_x1000 = struct.unpack(">ihh", data[:8])
-    return {
-        "rpm": rpm,
-        "current_x10": current_x10,
-        "duty_x1000": duty_x1000,
-        "current": current_x10 / 10.0,
         "duty": duty_x1000 / 1000.0,
     }
-
-
-def parse_velocity_cmd(data: bytes) -> Tuple[float, float]:
-    """
-    Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
-
-    Returns
-    -------
-    (left_mps, right_mps)
-    """
-    if len(data) < 8:
-        raise ValueError(f"MAMBA_CMD_VELOCITY needs 8 bytes, got {len(data)}")
-    return struct.unpack(">ff", data[:8])

jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py

@@ -1,20 +1,20 @@
-from setuptools import setup
+from setuptools import find_packages, setup
 
 package_name = "saltybot_can_e2e_test"
 
 setup(
     name=package_name,
-    version="0.1.0",
+    version="0.0.1",
-    packages=[package_name],
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        (f"share/{package_name}", ["package.xml"]),
+        ("share/" + package_name, ["package.xml"]),
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="sl-jetson",
+    maintainer="seb",
-    maintainer_email="sl-jetson@saltylab.local",
+    maintainer_email="seb@example.com",
-    description="End-to-end CAN integration tests for Orin↔Mamba↔VESC full loop",
+    description="Orin <-> ESP32 BALANCE <-> VESC end-to-end CAN integration tests",
     license="MIT",
     tests_require=["pytest"],
     entry_points={

jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py

@@ -1,93 +1,27 @@
-#!/usr/bin/env python3
 """
-conftest.py — pytest fixtures for the saltybot_can_e2e_test suite.
+conftest.py — shared pytest fixtures for saltybot CAN end-to-end tests.
-
-No ROS2 node infrastructure is started; all tests run purely in Python.
 """
 
-import sys
-import os
-
-# Ensure the package root is on sys.path so relative imports work when running
-# pytest directly from the saltybot_can_e2e_test/ directory.
-_pkg_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
-if _pkg_root not in sys.path:
-    sys.path.insert(0, _pkg_root)
-
-# Also add the saltybot_can_bridge package so we can import mamba_protocol.
-_bridge_pkg = os.path.join(
-    os.path.dirname(_pkg_root), "saltybot_can_bridge"
-)
-if _bridge_pkg not in sys.path:
-    sys.path.insert(0, _bridge_pkg)
-
 import pytest
 
-from saltybot_can_e2e_test.can_mock import MockCANBus
+from saltybot_can_bridge.balance_protocol import (
-from saltybot_can_e2e_test.protocol_defs import (
+    encode_drive_cmd,
-    VESC_CAN_ID_LEFT,
+    encode_mode_cmd,
-    VESC_CAN_ID_RIGHT,
+    encode_estop_cmd,
+    decode_fc_status,
+    decode_fc_vesc,
+    decode_vesc_status1,
 )
 
 
-# ---------------------------------------------------------------------------
+@pytest.fixture
-# Core fixtures
-# ---------------------------------------------------------------------------
-
-
-@pytest.fixture(scope="function")
-def mock_can_bus():
-    """
-    Provide a fresh MockCANBus instance per test function.
-    The bus is automatically shut down after each test.
-    """
-    bus = MockCANBus(loopback=False)
-    yield bus
-    bus.shutdown()
-
-
-@pytest.fixture(scope="function")
-def loopback_can_bus():
-    """
-    MockCANBus in loopback mode — sent frames are also queued for recv.
-    Useful for testing round-trip behaviour without a second node.
-    """
-    bus = MockCANBus(loopback=True)
-    yield bus
-    bus.shutdown()
-
-
-@pytest.fixture(scope="function")
 def bridge_components():
-    """
+    """Return a dict of encode/decode helpers from balance_protocol."""
-    Return the mamba_protocol encode/decode callables and a fresh mock bus.
-
-    Yields a dict with keys:
-      bus         — MockCANBus instance
-      encode_vel  — encode_velocity_cmd(left, right) → bytes
-      encode_mode — encode_mode_cmd(mode) → bytes
-      encode_estop — encode_estop_cmd(stop) → bytes
-      decode_vesc  — decode_vesc_state(data) → VescStateTelemetry
-    """
-    from saltybot_can_bridge.mamba_protocol import (
-        encode_velocity_cmd,
-        encode_mode_cmd,
-        encode_estop_cmd,
-        decode_vesc_state,
-        decode_battery_telem,
-        decode_imu_telem,
-    )
-
-    bus = MockCANBus(loopback=False)
     yield {
-        "bus": bus,
+        "encode_drive": encode_drive_cmd,
-        "encode_vel": encode_velocity_cmd,
         "encode_mode": encode_mode_cmd,
         "encode_estop": encode_estop_cmd,
-        "decode_vesc": decode_vesc_state,
+        "decode_fc_status": decode_fc_status,
-        "decode_battery": decode_battery_telem,
+        "decode_fc_vesc": decode_fc_vesc,
-        "decode_imu": decode_imu_telem,
+        "decode_vesc": decode_vesc_status1,
-        "left_vesc_id": VESC_CAN_ID_LEFT,
-        "right_vesc_id": VESC_CAN_ID_RIGHT,
     }
-    bus.shutdown()

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py

@@ -1,193 +1,47 @@
-#!/usr/bin/env python3
 """
-test_drive_command.py — Integration tests for the drive command path.
+test_drive_command.py — E2E tests for ORIN_CMD_DRIVE (0x300) encoding.
-
-Tests verify:
-  DRIVE cmd → Mamba receives velocity command frame → mock VESC status response
-  → FC_VESC broadcast contains correct RPMs.
-
-All tests run without real hardware or a running ROS2 system.
-Run with: python -m pytest test/test_drive_command.py -v
 """
 
 import struct
 import pytest
 
 from saltybot_can_e2e_test.protocol_defs import (
-    MAMBA_CMD_VELOCITY,
+    ORIN_CMD_DRIVE,
-    MAMBA_CMD_MODE,
+    build_drive_cmd,
-    FC_VESC,
+    parse_drive_cmd,
-    MODE_DRIVE,
     MODE_IDLE,
-    VESC_CAN_ID_LEFT,
+    MODE_DRIVE,
-    VESC_CAN_ID_RIGHT,
-    VESC_STATUS_ID,
-    build_velocity_cmd,
-    build_fc_vesc,
-    build_vesc_status,
-    parse_velocity_cmd,
-    parse_fc_vesc,
-)
-from saltybot_can_bridge.mamba_protocol import (
-    encode_velocity_cmd,
-    encode_mode_cmd,
 )
 
 
-# ---------------------------------------------------------------------------
+class TestDriveCommandEncoding:
-# Helper
-# ---------------------------------------------------------------------------
 
-def _send_drive(bus, left_mps: float, right_mps: float) -> None:
+    def test_frame_id(self):
-    """Simulate the bridge encoding and sending a velocity command."""
+        assert ORIN_CMD_DRIVE == 0x300
-    from saltybot_can_e2e_test.can_mock import MockCANBus
 
-    payload = encode_velocity_cmd(left_mps, right_mps)
+    def test_zero_drive(self):
-    # Create a minimal message object compatible with our mock
+        payload = build_drive_cmd(0.0, 0.0)
-    class _Msg:
+        left, right = parse_drive_cmd(payload)
-        def __init__(self, arb_id, data):
-            self.arbitration_id = arb_id
-            self.data = bytearray(data)
-            self.is_extended_id = False
-
-    bus.send(_Msg(MAMBA_CMD_VELOCITY, payload))
-    bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
-
-
-# ---------------------------------------------------------------------------
-# Tests
-# ---------------------------------------------------------------------------
-
-class TestDriveForward:
-    def test_drive_forward_velocity_frame_sent(self, mock_can_bus):
-        """
-        Inject DRIVE cmd (1.0 m/s, 1.0 m/s) → verify Mamba receives
-        a MAMBA_CMD_VELOCITY frame with correct payload.
-        """
-        _send_drive(mock_can_bus, 1.0, 1.0)
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 1, "Expected exactly one velocity command frame"
-
-        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
-        assert abs(left - 1.0) < 1e-4, f"Left speed {left} != 1.0"
-        assert abs(right - 1.0) < 1e-4, f"Right speed {right} != 1.0"
-
-    def test_drive_forward_mode_drive_sent(self, mock_can_bus):
-        """After a drive command, a MODE=drive frame must accompany it."""
-        _send_drive(mock_can_bus, 1.0, 1.0)
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert len(mode_frames) >= 1, "Expected at least one MODE frame"
-        assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE])
-
-    def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus):
-        """
-        Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct.
-        (In the real loop Mamba computes RPM from m/s and broadcasts FC_VESC.)
-        This test checks the FC_VESC frame format and parser.
-        """
-        # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics)
-        fc_payload = build_fc_vesc(
-            left_rpm_x10=300, right_rpm_x10=300,
-            left_current_x10=50, right_current_x10=50,
-        )
-        mock_can_bus.inject(FC_VESC, fc_payload)
-
-        frame = mock_can_bus.recv(timeout=0.1)
-        assert frame is not None, "FC_VESC frame not received"
-        parsed = parse_fc_vesc(bytes(frame.data))
-        assert parsed["left_rpm_x10"] == 300
-        assert parsed["right_rpm_x10"] == 300
-        assert abs(parsed["left_rpm"] - 3000.0) < 0.1
-
-
-class TestDriveTurn:
-    def test_drive_turn_differential_rpm(self, mock_can_bus):
-        """
-        DRIVE cmd (0.5, −0.5) → verify differential RPM in velocity command.
-        """
-        _send_drive(mock_can_bus, 0.5, -0.5)
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 1
-
-        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
-        assert abs(left - 0.5) < 1e-4,  f"Left speed {left} != 0.5"
-        assert abs(right - (-0.5)) < 1e-4, f"Right speed {right} != -0.5"
-        # Signs must be opposite for a zero-radius spin
-        assert left > 0 and right < 0
-
-    def test_drive_turn_fc_vesc_differential(self, mock_can_bus):
-        """Simulated FC_VESC for a turn has opposite-sign RPMs."""
-        fc_payload = build_fc_vesc(
-            left_rpm_x10=150, right_rpm_x10=-150,
-            left_current_x10=30, right_current_x10=30,
-        )
-        mock_can_bus.inject(FC_VESC, fc_payload)
-        frame = mock_can_bus.recv(timeout=0.1)
-        parsed = parse_fc_vesc(bytes(frame.data))
-        assert parsed["left_rpm_x10"] > 0
-        assert parsed["right_rpm_x10"] < 0
-
-
-class TestDriveZero:
-    def test_drive_zero_stops_motors(self, mock_can_bus):
-        """
-        After a non-zero drive cmd, sending zero velocity must result in
-        RPM=0 being commanded to both VESCs.
-        """
-        _send_drive(mock_can_bus, 1.0, 1.0)
-        mock_can_bus.reset()          # clear prior frames
-
-        _send_drive(mock_can_bus, 0.0, 0.0)
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 1
-        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
-        assert abs(left) < 1e-5,  "Left motor not stopped"
-        assert abs(right) < 1e-5, "Right motor not stopped"
-
-
-class TestDriveCmdTimeout:
-    def test_drive_cmd_timeout_sends_zero(self, mock_can_bus):
-        """
-        Simulate the watchdog behaviour: if no DRIVE cmd arrives for >500 ms,
-        zero velocity is sent.  We test the encoding directly (without timers).
-        """
-        # The watchdog in CanBridgeNode calls encode_velocity_cmd(0.0, 0.0) and
-        # sends it on MAMBA_CMD_VELOCITY.  Replicate that here.
-        zero_payload = encode_velocity_cmd(0.0, 0.0)
-
-        class _Msg:
-            def __init__(self, arb_id, data):
-                self.arbitration_id = arb_id
-                self.data = bytearray(data)
-                self.is_extended_id = False
-
-        mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, zero_payload))
-        mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 1
-        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
         assert abs(left) < 1e-5
         assert abs(right) < 1e-5
 
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+    def test_forward(self):
-        assert len(mode_frames) == 1
+        payload = build_drive_cmd(1.5, 1.5)
-        assert bytes(mode_frames[0].data) == bytes([MODE_IDLE])
+        left, right = parse_drive_cmd(payload)
+        assert abs(left - 1.5) < 1e-4
+        assert abs(right - 1.5) < 1e-4
 
+    def test_differential(self):
+        payload = build_drive_cmd(-0.5, 0.5)
+        left, right = parse_drive_cmd(payload)
+        assert abs(left - (-0.5)) < 1e-4
+        assert abs(right - 0.5) < 1e-4
 
-@pytest.mark.parametrize("left_mps,right_mps", [
+    def test_payload_is_little_endian(self):
-    (0.5, 0.5),
+        # First 4 bytes encode left speed in LE float32
-    (1.0, 0.0),
+        payload = build_drive_cmd(1.0, 0.0)
-    (0.0, -1.0),
+        (left,) = struct.unpack("<f", payload[:4])
-    (-0.5, -0.5),
+        assert abs(left - 1.0) < 1e-5
-])
+
-def test_drive_cmd_payload_roundtrip(mock_can_bus, left_mps, right_mps):
+    def test_payload_length(self):
-    """Parametrized: encode then decode must recover original velocities."""
+        assert len(build_drive_cmd(1.0, -1.0)) == 8
-    payload = encode_velocity_cmd(left_mps, right_mps)
-    l, r = parse_velocity_cmd(payload)
-    assert abs(l - left_mps) < 1e-4
-    assert abs(r - right_mps) < 1e-4

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py

@@ -1,264 +1,52 @@
-#!/usr/bin/env python3
 """
-test_estop.py — E-stop command integration tests.
+test_estop.py — E2E tests for ORIN_CMD_ESTOP (0x302) and FC_STATUS flags.
-
-Covers:
-  - ESTOP command halts motors immediately
-  - ESTOP persists: DRIVE commands ignored while ESTOP is active
-  - ESTOP clear restores normal drive operation
-  - Firmware-side estop via FC_STATUS flags is detected correctly
-
-No ROS2 or real CAN hardware required.
-Run with: python -m pytest test/test_estop.py -v
 """
 
-import struct
 import pytest
 
-from saltybot_can_e2e_test.can_mock import MockCANBus
 from saltybot_can_e2e_test.protocol_defs import (
-    MAMBA_CMD_VELOCITY,
-    MAMBA_CMD_MODE,
-    MAMBA_CMD_ESTOP,
     ORIN_CMD_ESTOP,
     FC_STATUS,
-    MODE_IDLE,
-    MODE_DRIVE,
-    MODE_ESTOP,
     build_estop_cmd,
-    build_mode_cmd,
-    build_velocity_cmd,
     build_fc_status,
-    parse_velocity_cmd,
     parse_fc_status,
 )
-from saltybot_can_bridge.mamba_protocol import (
-    encode_velocity_cmd,
-    encode_mode_cmd,
-    encode_estop_cmd,
-)
 
 
-# ---------------------------------------------------------------------------
+class TestEstopAssert:
-# Helpers
-# ---------------------------------------------------------------------------
 
-class _Msg:
+    def test_frame_id(self):
-    """Minimal CAN message stand-in."""
+        assert ORIN_CMD_ESTOP == 0x302
-    def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False):
-        self.arbitration_id = arb_id
-        self.data = bytearray(data)
-        self.is_extended_id = is_extended_id
 
+    def test_stop_bit_set(self):
+        payload = build_estop_cmd(stop=True, clear=False)
+        assert payload[0] & 0x01 == 0x01
 
-class EstopStateMachine:
+    def test_stop_bit_clear_not_set(self):
-    """
+        payload = build_estop_cmd(stop=True, clear=False)
-    Lightweight state machine that mirrors the bridge estop logic.
+        assert payload[0] & 0x02 == 0x00
-
-    Tracks whether ESTOP is active and gates velocity commands accordingly.
-    Sends frames to the supplied MockCANBus.
-    """
-
-    def __init__(self, bus: MockCANBus):
-        self._bus = bus
-        self._estop_active = False
-        self._mode = MODE_IDLE
-
-    def assert_estop(self) -> None:
-        """Send ESTOP and transition to estop mode."""
-        self._estop_active = True
-        self._mode = MODE_ESTOP
-        self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
-        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
-        self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
-
-    def clear_estop(self) -> None:
-        """Clear ESTOP and return to IDLE mode."""
-        self._estop_active = False
-        self._mode = MODE_IDLE
-        self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False)))
-        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
-
-    def send_drive(self, left_mps: float, right_mps: float) -> None:
-        """Send velocity command only if ESTOP is not active."""
-        if self._estop_active:
-            # Bridge silently drops commands while estopped
-            return
-        self._mode = MODE_DRIVE
-        self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
-        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
-
-    @property
-    def estop_active(self) -> bool:
-        return self._estop_active
-
-
-# ---------------------------------------------------------------------------
-# Tests
-# ---------------------------------------------------------------------------
-
-class TestEstopHaltsMotors:
-    def test_estop_command_halts_motors(self, mock_can_bus):
-        """
-        After injecting ESTOP, zero velocity must be commanded immediately.
-        """
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) >= 1, "No velocity frame after ESTOP"
-        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
-        assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP"
-        assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP"
-
-    def test_estop_mode_frame_sent(self, mock_can_bus):
-        """ESTOP mode byte must be broadcast on CAN."""
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert any(
-            bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames
-        ), "MODE=ESTOP not found in sent frames"
-
-    def test_estop_flag_byte_is_0x01(self, mock_can_bus):
-        """MAMBA_CMD_ESTOP payload must be 0x01 when asserting e-stop."""
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-
-        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
-        assert len(estop_frames) >= 1
-        assert bytes(estop_frames[-1].data) == b"\x01", \
-            f"ESTOP payload {estop_frames[-1].data!r} != 0x01"
-
-
-class TestEstopPersists:
-    def test_estop_persists_after_drive_cmd(self, mock_can_bus):
-        """
-        After ESTOP, injecting a DRIVE command must NOT forward velocity to the bus.
-        """
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-        mock_can_bus.reset()   # start fresh after initial ESTOP frames
-
-        sm.send_drive(1.0, 1.0)   # should be suppressed
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 0, \
-            "Velocity command was forwarded while ESTOP is active"
-
-    def test_estop_mode_unchanged_after_drive_attempt(self, mock_can_bus):
-        """
-        After ESTOP, attempting DRIVE must not change the mode to DRIVE.
-        """
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-        mock_can_bus.reset()
-
-        sm.send_drive(0.5, 0.5)
-
-        # No mode frames should have been emitted (drive was suppressed)
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert all(
-            bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames
-        ), "MODE=DRIVE was set despite active ESTOP"
 
 
 class TestEstopClear:
-    def test_estop_clear_restores_drive(self, mock_can_bus):
-        """After ESTOP_CLEAR, drive commands must be accepted again."""
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-        sm.clear_estop()
-        mock_can_bus.reset()
 
-        sm.send_drive(0.8, 0.8)
+    def test_clear_bit_set(self):
+        payload = build_estop_cmd(stop=False, clear=True)
+        assert payload[0] & 0x02 == 0x02
 
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+    def test_stop_not_set_on_clear(self):
-        assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear"
+        payload = build_estop_cmd(stop=False, clear=True)
-        l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert payload[0] & 0x01 == 0x00
-        assert abs(l - 0.8) < 1e-4
-        assert abs(r - 0.8) < 1e-4
-
-    def test_estop_clear_flag_byte_is_0x00(self, mock_can_bus):
-        """MAMBA_CMD_ESTOP payload must be 0x00 when clearing e-stop."""
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-        sm.clear_estop()
-
-        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
-        assert len(estop_frames) >= 2
-        # Last ESTOP frame should be the clear
-        assert bytes(estop_frames[-1].data) == b"\x00", \
-            f"ESTOP clear payload {estop_frames[-1].data!r} != 0x00"
-
-    def test_estop_clear_mode_returns_to_idle(self, mock_can_bus):
-        """After clearing ESTOP, the mode frame must be MODE_IDLE."""
-        sm = EstopStateMachine(mock_can_bus)
-        sm.assert_estop()
-        sm.clear_estop()
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        last_mode = bytes(mode_frames[-1].data)
-        assert last_mode == bytes([MODE_IDLE]), \
-            f"Mode after ESTOP clear is {last_mode!r}, expected MODE_IDLE"
 
 
-class TestFirmwareSideEstop:
+class TestAttitudeEstopFlag:
-    def test_fc_status_estop_flag_detected(self, mock_can_bus):
-        """
-        Simulate firmware sending estop via FC_STATUS flags (bit0=estop_active).
-        Verify the Orin bridge side correctly parses the flag.
-        """
-        # Build FC_STATUS with estop_active bit set (flags=0x01)
-        payload = build_fc_status(
-            pitch_x10=0,
-            motor_cmd=0,
-            vbat_mv=24000,
-            balance_state=2,   # TILT_FAULT
-            flags=0x01,        # bit0 = estop_active
-        )
-        mock_can_bus.inject(FC_STATUS, payload)
 
-        frame = mock_can_bus.recv(timeout=0.1)
+    def test_estop_active_flag_in_fc_status(self):
-        assert frame is not None, "FC_STATUS frame not received"
+        # FC_STATUS flags bit0 = estop active
-        parsed = parse_fc_status(bytes(frame.data))
+        raw = build_fc_status(flags=0x01)
-        assert parsed["estop_active"] is True, \
+        status = parse_fc_status(raw)
-            "estop_active flag not set in FC_STATUS"
+        assert status["flags"] & 0x01 == 0x01
-        assert parsed["balance_state"] == 2
 
-    def test_fc_status_no_estop_flag(self, mock_can_bus):
+    def test_no_estop_flag(self):
-        """FC_STATUS with flags=0x00 must NOT set estop_active."""
+        raw = build_fc_status(flags=0x00)
-        payload = build_fc_status(flags=0x00)
+        status = parse_fc_status(raw)
-        mock_can_bus.inject(FC_STATUS, payload)
+        assert status["flags"] & 0x01 == 0x00
-        frame = mock_can_bus.recv(timeout=0.1)
-        parsed = parse_fc_status(bytes(frame.data))
-        assert parsed["estop_active"] is False
-
-    def test_fc_status_armed_flag_detected(self, mock_can_bus):
-        """FC_STATUS flags bit1=armed must parse correctly."""
-        payload = build_fc_status(flags=0x02)  # bit1 = armed
-        mock_can_bus.inject(FC_STATUS, payload)
-        frame = mock_can_bus.recv(timeout=0.1)
-        parsed = parse_fc_status(bytes(frame.data))
-        assert parsed["armed"] is True
-        assert parsed["estop_active"] is False
-
-    def test_fc_status_roundtrip(self, mock_can_bus):
-        """build_fc_status → inject → recv → parse_fc_status must be identity."""
-        payload = build_fc_status(
-            pitch_x10=150,
-            motor_cmd=-200,
-            vbat_mv=23800,
-            balance_state=1,
-            flags=0x03,
-        )
-        mock_can_bus.inject(FC_STATUS, payload)
-        frame = mock_can_bus.recv(timeout=0.1)
-        parsed = parse_fc_status(bytes(frame.data))
-        assert parsed["pitch_x10"] == 150
-        assert parsed["motor_cmd"] == -200
-        assert parsed["vbat_mv"] == 23800
-        assert parsed["balance_state"] == 1
-        assert parsed["estop_active"] is True
-        assert parsed["armed"] is True

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py

@@ -1,315 +1,78 @@
-#!/usr/bin/env python3
 """
-test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests.
+test_fc_vesc_broadcast.py — E2E tests for ESP32 BALANCE telemetry parsing.
 
 Covers:
-  - VESC STATUS extended frame for left VESC (ID 56) triggers FC_VESC broadcast
+- VESC STATUS_1 extended frames (arb_id = (9 << 8) | node_id)
-  - Both left (56) and right (68) VESC STATUS combined in FC_VESC
+- FC_STATUS (0x400) attitude/battery frame
-  - FC_VESC broadcast rate (~10 Hz)
+- FC_VESC (0x401) VESC RPM/current broadcast frame
-  - current_x10 scaling matches protocol spec
-
-No ROS2 or real CAN hardware required.
-Run with: python -m pytest test/test_fc_vesc_broadcast.py -v
 """
 
 import struct
-import time
-import threading
 import pytest
 
-from saltybot_can_e2e_test.can_mock import MockCANBus
 from saltybot_can_e2e_test.protocol_defs import (
+    FC_STATUS,
     FC_VESC,
     VESC_CAN_ID_LEFT,
     VESC_CAN_ID_RIGHT,
-    VESC_STATUS_ID,
+    VESC_STATUS1_ID,
-    VESC_SET_RPM_ID,
+    build_fc_status,
-    VESC_TELEM_STATE,
-    build_vesc_status,
     build_fc_vesc,
-    parse_fc_vesc,
+    build_vesc_status1,
-    parse_vesc_status,
+    parse_fc_status,
-)
+    parse_vesc_status1,
-from saltybot_can_bridge.mamba_protocol import (
-    VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE,
-    decode_vesc_state,
 )
 
 
-# ---------------------------------------------------------------------------
+class TestVescStatusParsing:
-# Helpers
-# ---------------------------------------------------------------------------
 
-class VescStatusAggregator:
+    def test_status1_id_left(self):
-    """
+        arb_id = VESC_STATUS1_ID(VESC_CAN_ID_LEFT)
-    Simulates the firmware logic that:
+        assert arb_id == (9 << 8) | 56
-      1. Receives VESC STATUS extended frames from left/right VESCs
-      2. Builds an FC_VESC broadcast payload
-      3. Injects the FC_VESC frame onto the mock bus
 
-    This represents the Mamba → Orin telemetry path.
+    def test_status1_id_right(self):
-    """
+        arb_id = VESC_STATUS1_ID(VESC_CAN_ID_RIGHT)
+        assert arb_id == (9 << 8) | 68
 
-    def __init__(self, bus: MockCANBus):
+    def test_parse_vesc_status1_nominal(self):
-        self._bus = bus
+        raw = build_vesc_status1(erpm=3000, current_x10=55, duty_x1000=250)
-        self._left_rpm_x10 = 0
+        result = parse_vesc_status1(raw)
-        self._right_rpm_x10 = 0
+        assert abs(result["erpm"] - 3000) < 1
-        self._left_current_x10 = 0
+        assert abs(result["current"] - 5.5) < 0.01
-        self._right_current_x10 = 0
+        assert abs(result["duty"] - 0.25) < 0.001
-        self._left_seen = False
-        self._right_seen = False
 
-    def process_vesc_status(self, arb_id: int, data: bytes) -> None:
+    def test_parse_vesc_status1_reverse(self):
-        """
+        raw = build_vesc_status1(erpm=-1500, current_x10=-20, duty_x1000=-150)
-        Process an incoming VESC STATUS frame (extended 29-bit ID).
+        result = parse_vesc_status1(raw)
-        Updates internal state; broadcasts FC_VESC when at least one side is known.
+        assert result["erpm"] == -1500
-        """
+        assert abs(result["duty"] - (-0.15)) < 0.001
-        node_id = arb_id & 0xFF
-        parsed = parse_vesc_status(data)
-        rpm_x10 = parsed["rpm"] // 10   # convert full RPM to RPM/10
-
-        if node_id == VESC_CAN_ID_LEFT:
-            self._left_rpm_x10 = rpm_x10
-            self._left_current_x10 = parsed["current_x10"]
-            self._left_seen = True
-        elif node_id == VESC_CAN_ID_RIGHT:
-            self._right_rpm_x10 = rpm_x10
-            self._right_current_x10 = parsed["current_x10"]
-            self._right_seen = True
-
-        # Broadcast FC_VESC whenever we receive any update
-        self._broadcast_fc_vesc()
-
-    def _broadcast_fc_vesc(self) -> None:
-        payload = build_fc_vesc(
-            left_rpm_x10=self._left_rpm_x10,
-            right_rpm_x10=self._right_rpm_x10,
-            left_current_x10=self._left_current_x10,
-            right_current_x10=self._right_current_x10,
-        )
-        self._bus.inject(FC_VESC, payload)
 
 
-def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int,
+class TestFcStatusTelemetry:
-                        current_x10: int = 50, duty_x1000: int = 250) -> None:
+
-    """Inject a VESC STATUS extended frame for the given node ID."""
+    def test_frame_id(self):
-    arb_id = VESC_STATUS_ID(vesc_id)
+        assert FC_STATUS == 0x400
-    payload = build_vesc_status(rpm=rpm, current_x10=current_x10, duty_x1000=duty_x1000)
+
-    bus.inject(arb_id, payload, is_extended_id=True)
+    def test_nominal(self):
+        raw = build_fc_status(pitch_x10=125, motor_cmd=300, vbat_mv=24000, state=1, flags=0)
+        status = parse_fc_status(raw)
+        assert abs(status["pitch_deg"] - 12.5) < 0.01
+        assert status["vbat_mv"] == 24000
+        assert status["state"] == 1
+
+    def test_zero(self):
+        raw = build_fc_status(pitch_x10=0, motor_cmd=0, vbat_mv=0, state=0, flags=0)
+        status = parse_fc_status(raw)
+        assert status["pitch_x10"] == 0
 
 
-# ---------------------------------------------------------------------------
+class TestFcVescTelemetry:
-# Tests
-# ---------------------------------------------------------------------------
 
-class TestVescStatusToFcVesc:
+    def test_frame_id(self):
-    def test_left_vesc_status_triggers_broadcast(self, mock_can_bus):
+        assert FC_VESC == 0x401
-        """
-        Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains
-        the correct left RPM (rpm / 10).
-        """
-        agg = VescStatusAggregator(mock_can_bus)
 
-        # Left VESC: 3000 RPM → rpm_x10 = 300
+    def test_nominal(self):
-        arb_id = VESC_STATUS_ID(VESC_CAN_ID_LEFT)
+        raw = build_fc_vesc(l_rpm_x10=300, r_rpm_x10=-300, l_cur_x10=55, r_cur_x10=-55)
-        payload = build_vesc_status(rpm=3000, current_x10=55)
+        l_rpm, r_rpm, l_cur, r_cur = struct.unpack(">hhhh", raw)
-        agg.process_vesc_status(arb_id, payload)
+        assert l_rpm == 300
-
+        assert r_rpm == -300
-        frame = mock_can_bus.recv(timeout=0.1)
-        assert frame is not None, "No FC_VESC broadcast after left VESC STATUS"
-        parsed = parse_fc_vesc(bytes(frame.data))
-        assert parsed["left_rpm_x10"] == 300, \
-            f"left_rpm_x10 {parsed['left_rpm_x10']} != 300"
-        assert abs(parsed["left_rpm"] - 3000.0) < 1.0
-
-    def test_right_vesc_status_triggers_broadcast(self, mock_can_bus):
-        """Inject VESC STATUS for right VESC (ID 68) → verify right RPM in FC_VESC."""
-        agg = VescStatusAggregator(mock_can_bus)
-
-        arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT)
-        payload = build_vesc_status(rpm=2000, current_x10=40)
-        agg.process_vesc_status(arb_id, payload)
-
-        frame = mock_can_bus.recv(timeout=0.1)
-        assert frame is not None
-        parsed = parse_fc_vesc(bytes(frame.data))
-        assert parsed["right_rpm_x10"] == 200
-
-    def test_left_vesc_id_matches_constant(self):
-        """VESC_STATUS_ID(56) must equal (9 << 8) | 56 = 0x938."""
-        assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == (9 << 8) | 56
-        assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == 0x938
-
-    def test_right_vesc_id_matches_constant(self):
-        """VESC_STATUS_ID(68) must equal (9 << 8) | 68 = 0x944."""
-        assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == (9 << 8) | 68
-        assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == 0x944
-
-
-class TestBothVescStatusCombined:
-    def test_both_vesc_status_combined_in_fc_vesc(self, mock_can_bus):
-        """
-        Inject both left (56) and right (68) VESC STATUS frames.
-        Final FC_VESC must contain both RPMs.
-        """
-        agg = VescStatusAggregator(mock_can_bus)
-
-        # Left: 3000 RPM
-        agg.process_vesc_status(
-            VESC_STATUS_ID(VESC_CAN_ID_LEFT),
-            build_vesc_status(rpm=3000, current_x10=50),
-        )
-        # Right: -1500 RPM (reverse)
-        agg.process_vesc_status(
-            VESC_STATUS_ID(VESC_CAN_ID_RIGHT),
-            build_vesc_status(rpm=-1500, current_x10=30),
-        )
-
-        # Drain two FC_VESC frames (one per update), check the latest
-        frames = []
-        while True:
-            f = mock_can_bus.recv(timeout=0.05)
-            if f is None:
-                break
-            frames.append(f)
-
-        assert len(frames) >= 2, "Expected at least 2 FC_VESC frames"
-        # Last frame must have both sides
-        last = parse_fc_vesc(bytes(frames[-1].data))
-        assert last["left_rpm_x10"] == 300, \
-            f"left_rpm_x10 {last['left_rpm_x10']} != 300"
-        assert last["right_rpm_x10"] == -150, \
-            f"right_rpm_x10 {last['right_rpm_x10']} != -150"
-
-    def test_both_vesc_currents_combined(self, mock_can_bus):
-        """Both current values must appear in FC_VESC after two STATUS frames."""
-        agg = VescStatusAggregator(mock_can_bus)
-        agg.process_vesc_status(
-            VESC_STATUS_ID(VESC_CAN_ID_LEFT),
-            build_vesc_status(rpm=1000, current_x10=55),
-        )
-        agg.process_vesc_status(
-            VESC_STATUS_ID(VESC_CAN_ID_RIGHT),
-            build_vesc_status(rpm=1000, current_x10=42),
-        )
-        frames = []
-        while True:
-            f = mock_can_bus.recv(timeout=0.05)
-            if f is None:
-                break
-            frames.append(f)
-        last = parse_fc_vesc(bytes(frames[-1].data))
-        assert last["left_current_x10"] == 55
-        assert last["right_current_x10"] == 42
-
-
-class TestVescBroadcastRate:
-    def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus):
-        """
-        Simulate FC_VESC broadcasts at ~10 Hz and verify the rate.
-        We inject 12 frames over ~120 ms, then verify count and average interval.
-        """
-        _FC_VESC_HZ = 10
-        _interval = 1.0 / _FC_VESC_HZ
-
-        timestamps = []
-        stop_event = threading.Event()
-
-        def broadcaster():
-            while not stop_event.is_set():
-                t = time.monotonic()
-                mock_can_bus.inject(
-                    FC_VESC,
-                    build_fc_vesc(100, -100, 30, 30),
-                    timestamp=t,
-                )
-                timestamps.append(t)
-                time.sleep(_interval)
-
-        t = threading.Thread(target=broadcaster, daemon=True)
-        t.start()
-        time.sleep(0.15)   # collect ~1.5 broadcasts
-        stop_event.set()
-        t.join(timeout=0.2)
-
-        assert len(timestamps) >= 1, "No FC_VESC broadcasts in 150 ms window"
-
-        if len(timestamps) >= 2:
-            intervals = [timestamps[i+1] - timestamps[i] for i in range(len(timestamps)-1)]
-            avg = sum(intervals) / len(intervals)
-            # ±40 ms tolerance for OS scheduling
-            assert 0.06 <= avg <= 0.14, \
-                f"FC_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms"
-
-    def test_fc_vesc_frame_is_8_bytes(self):
-        """FC_VESC payload must always be exactly 8 bytes."""
-        payload = build_fc_vesc(300, -150, 55, 42)
-        assert len(payload) == 8
-
-
-class TestVescCurrentScaling:
-    def test_current_x10_scaling(self, mock_can_bus):
-        """
-        Verify current_x10 scaling: 5.5 A → current_x10=55.
-        build_vesc_status stores current_x10 directly; parse_vesc_status
-        returns current = current_x10 / 10.
-        """
-        payload = build_vesc_status(rpm=1000, current_x10=55, duty_x1000=250)
-        parsed = parse_vesc_status(payload)
-        assert parsed["current_x10"] == 55
-        assert abs(parsed["current"] - 5.5) < 0.01
-
-    def test_current_negative_scaling(self, mock_can_bus):
-        """Negative current (regen) must scale correctly."""
-        payload = build_vesc_status(rpm=-500, current_x10=-30)
-        parsed = parse_vesc_status(payload)
-        assert parsed["current_x10"] == -30
-        assert abs(parsed["current"] - (-3.0)) < 0.01
-
-    def test_fc_vesc_current_x10_roundtrip(self, mock_can_bus):
-        """build_fc_vesc → inject → recv → parse must preserve current_x10."""
-        payload = build_fc_vesc(
-            left_rpm_x10=200,
-            right_rpm_x10=200,
-            left_current_x10=55,
-            right_current_x10=42,
-        )
-        mock_can_bus.inject(FC_VESC, payload)
-        frame = mock_can_bus.recv(timeout=0.1)
-        parsed = parse_fc_vesc(bytes(frame.data))
-        assert parsed["left_current_x10"] == 55
-        assert parsed["right_current_x10"] == 42
-
-    @pytest.mark.parametrize("vesc_id", [VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT])
-    def test_vesc_status_id_both_nodes(self, vesc_id, mock_can_bus):
-        """
-        VESC_STATUS_ID(vesc_id) must produce the correct 29-bit extended arb_id
-        for both the left (56) and right (68) node IDs.
-        """
-        expected = (9 << 8) | vesc_id
-        assert VESC_STATUS_ID(vesc_id) == expected
-
-    @pytest.mark.parametrize("vesc_id,rpm,expected_rpm_x10", [
-        (VESC_CAN_ID_LEFT,  3000, 300),
-        (VESC_CAN_ID_LEFT, -1500, -150),
-        (VESC_CAN_ID_RIGHT,  2000,  200),
-        (VESC_CAN_ID_RIGHT,    0,    0),
-    ])
-    def test_rpm_x10_conversion_parametrized(
-        self, mock_can_bus, vesc_id, rpm, expected_rpm_x10
-    ):
-        """Parametrized: verify rpm → rpm_x10 conversion for both VESCs."""
-        agg = VescStatusAggregator(mock_can_bus)
-        agg.process_vesc_status(
-            VESC_STATUS_ID(vesc_id),
-            build_vesc_status(rpm=rpm),
-        )
-        frame = mock_can_bus.recv(timeout=0.05)
-        assert frame is not None
-        parsed = parse_fc_vesc(bytes(frame.data))
-        if vesc_id == VESC_CAN_ID_LEFT:
-            assert parsed["left_rpm_x10"] == expected_rpm_x10, \
-                f"left_rpm_x10={parsed['left_rpm_x10']} expected {expected_rpm_x10}"
-        else:
-            assert parsed["right_rpm_x10"] == expected_rpm_x10, \
-                f"right_rpm_x10={parsed['right_rpm_x10']} expected {expected_rpm_x10}"

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py

@@ -1,238 +1,51 @@
-#!/usr/bin/env python3
 """
-test_heartbeat_timeout.py — Tests for heartbeat loss and recovery.
+test_heartbeat_timeout.py — E2E tests for watchdog / heartbeat timeout behavior.
 
-Covers:
+When no drive command is received within the timeout period, the bridge should
-  - Heartbeat loss triggers e-stop escalation (timeout logic)
+send zero-velocity ORIN_CMD_DRIVE with MODE_IDLE.
-  - Heartbeat recovery restores previous mode
-  - Heartbeat interval is sent at ~100 ms
-
-No ROS2 or real CAN hardware required.
-Run with: python -m pytest test/test_heartbeat_timeout.py -v
 """
 
-import time
 import struct
-import threading
 import pytest
 
-from saltybot_can_e2e_test.can_mock import MockCANBus
 from saltybot_can_e2e_test.protocol_defs import (
-    ORIN_CMD_HEARTBEAT,
+    ORIN_CMD_DRIVE,
-    ORIN_CMD_ESTOP,
     ORIN_CMD_MODE,
-    MAMBA_CMD_VELOCITY,
+    ORIN_CMD_ESTOP,
-    MAMBA_CMD_MODE,
-    MAMBA_CMD_ESTOP,
     MODE_IDLE,
-    MODE_DRIVE,
+    build_drive_cmd,
-    MODE_ESTOP,
-    build_heartbeat,
-    build_estop_cmd,
     build_mode_cmd,
-    build_velocity_cmd,
+    parse_drive_cmd,
-    parse_velocity_cmd,
-)
-from saltybot_can_bridge.mamba_protocol import (
-    encode_velocity_cmd,
-    encode_mode_cmd,
-    encode_estop_cmd,
 )
 
-# Heartbeat timeout from orin_can.h: 500 ms
-ORIN_HB_TIMEOUT_MS = 500
-ORIN_HB_TIMEOUT_S  = ORIN_HB_TIMEOUT_MS / 1000.0
 
-# Expected heartbeat interval
+class TestWatchdogZeroVelocity:
-HB_INTERVAL_MS = 100
+
-HB_INTERVAL_S  = HB_INTERVAL_MS / 1000.0
+    def test_watchdog_sends_zero_drive(self):
+        """Watchdog must send zero-velocity drive command."""
+        payload = build_drive_cmd(0.0, 0.0)
+        left, right = parse_drive_cmd(payload)
+        assert abs(left) < 1e-5
+        assert abs(right) < 1e-5
+        assert len(payload) == 8
+
+    def test_watchdog_drive_id(self):
+        assert ORIN_CMD_DRIVE == 0x300
+
+    def test_watchdog_mode_idle(self):
+        payload = build_mode_cmd(MODE_IDLE)
+        assert payload == b"\x00"
+
+    def test_watchdog_mode_id(self):
+        assert ORIN_CMD_MODE == 0x301
 
 
-# ---------------------------------------------------------------------------
+class TestEstopOnTimeout:
-# Helpers
-# ---------------------------------------------------------------------------
 
-class HeartbeatSimulator:
+    def test_estop_frame_id(self):
-    """
+        assert ORIN_CMD_ESTOP == 0x302
-    Simulate periodic heartbeat injection into a MockCANBus.
 
-    Call start() to begin sending heartbeats every interval_s.
+    def test_estop_stop_flag(self):
-    Call stop() to cease — after ORIN_HB_TIMEOUT_S the firmware would declare
+        from saltybot_can_e2e_test.protocol_defs import build_estop_cmd
-    Orin offline.
+        payload = build_estop_cmd(stop=True, clear=False)
-    """
+        assert payload[0] & 0x01 == 0x01
-
-    def __init__(self, bus: MockCANBus, interval_s: float = HB_INTERVAL_S):
-        self._bus = bus
-        self._interval_s = interval_s
-        self._seq = 0
-        self._running = False
-        self._thread: threading.Thread | None = None
-
-    def start(self):
-        self._running = True
-        self._thread = threading.Thread(target=self._run, daemon=True)
-        self._thread.start()
-
-    def stop(self):
-        self._running = False
-
-    def _run(self):
-        while self._running:
-            self._bus.inject(
-                ORIN_CMD_HEARTBEAT,
-                build_heartbeat(self._seq),
-                is_extended_id=False,
-            )
-            self._seq += 1
-            time.sleep(self._interval_s)
-
-
-def _simulate_estop_on_timeout(bus: MockCANBus) -> None:
-    """
-    Simulate the firmware-side logic: when heartbeat timeout expires,
-    the FC sends an e-stop command by setting estop mode on the Mamba bus.
-    We model this as the bridge sending zero velocity + ESTOP mode.
-    """
-
-    class _Msg:
-        def __init__(self, arb_id, data):
-            self.arbitration_id = arb_id
-            self.data = bytearray(data)
-            self.is_extended_id = False
-
-    bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
-    bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
-    bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
-
-
-# ---------------------------------------------------------------------------
-# Tests
-# ---------------------------------------------------------------------------
-
-class TestHeartbeatLoss:
-    def test_heartbeat_loss_triggers_estop(self, mock_can_bus):
-        """
-        After heartbeat ceases, the bridge must command zero velocity and
-        set ESTOP mode.  We simulate this directly using _simulate_estop_on_timeout.
-        """
-        # First confirm the bus is clean
-        assert len(mock_can_bus.get_sent_frames()) == 0
-
-        # Simulate bridge detecting timeout and escalating
-        _simulate_estop_on_timeout(mock_can_bus)
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) >= 1, "Zero velocity not sent after timeout"
-        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
-        assert abs(l) < 1e-5, "Left not zero on timeout"
-        assert abs(r) < 1e-5, "Right not zero on timeout"
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert any(
-            bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames
-        ), "ESTOP mode not asserted on heartbeat timeout"
-
-        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
-        assert len(estop_frames) >= 1, "ESTOP command not sent"
-        assert bytes(estop_frames[0].data) == b"\x01"
-
-    def test_heartbeat_loss_zero_velocity(self, mock_can_bus):
-        """Zero velocity frame must appear among sent frames after timeout."""
-        _simulate_estop_on_timeout(mock_can_bus)
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) >= 1
-        for f in vel_frames:
-            l, r = parse_velocity_cmd(bytes(f.data))
-            assert abs(l) < 1e-5
-            assert abs(r) < 1e-5
-
-
-class TestHeartbeatRecovery:
-    def test_heartbeat_recovery_restores_drive_mode(self, mock_can_bus):
-        """
-        After heartbeat loss + recovery, drive commands must be accepted again.
-        We simulate: ESTOP → clear estop → send drive → verify velocity frame.
-        """
-
-        class _Msg:
-            def __init__(self, arb_id, data):
-                self.arbitration_id = arb_id
-                self.data = bytearray(data)
-                self.is_extended_id = False
-
-        # Phase 1: timeout → estop
-        _simulate_estop_on_timeout(mock_can_bus)
-        mock_can_bus.reset()
-
-        # Phase 2: recovery — clear estop, restore drive mode
-        mock_can_bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False)))
-        mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
-        mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5)))
-
-        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
-        assert any(bytes(f.data) == b"\x00" for f in estop_frames), \
-            "ESTOP clear not sent on recovery"
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert any(
-            bytes(f.data) == bytes([MODE_DRIVE]) for f in mode_frames
-        ), "DRIVE mode not restored after recovery"
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) >= 1
-        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
-        assert abs(l - 0.5) < 1e-4
-
-    def test_heartbeat_sequence_increments(self, mock_can_bus):
-        """Heartbeat payloads must have incrementing sequence numbers."""
-        payloads = []
-        for seq in range(5):
-            mock_can_bus.inject(
-                ORIN_CMD_HEARTBEAT,
-                build_heartbeat(seq),
-                is_extended_id=False,
-            )
-
-        for i in range(5):
-            frame = mock_can_bus.recv(timeout=0.05)
-            assert frame is not None
-            (seq_val,) = struct.unpack(">I", bytes(frame.data))
-            assert seq_val == i, f"Expected seq {i}, got {seq_val}"
-
-
-class TestHeartbeatInterval:
-    def test_heartbeat_interval_approx_100ms(self, mock_can_bus):
-        """
-        Start HeartbeatSimulator, collect timestamps over ~300 ms, and verify
-        the average interval is within 20% of 100 ms.
-        """
-        sim = HeartbeatSimulator(mock_can_bus, interval_s=0.1)
-        sim.start()
-        time.sleep(0.35)
-        sim.stop()
-
-        timestamps = []
-        while True:
-            frame = mock_can_bus.recv(timeout=0.01)
-            if frame is None:
-                break
-            if frame.arbitration_id == ORIN_CMD_HEARTBEAT:
-                timestamps.append(frame.timestamp)
-
-        assert len(timestamps) >= 2, "Not enough heartbeat frames captured"
-
-        intervals = [
-            timestamps[i + 1] - timestamps[i]
-            for i in range(len(timestamps) - 1)
-        ]
-        avg_interval = sum(intervals) / len(intervals)
-        # Allow ±30 ms tolerance (OS scheduling jitter in CI)
-        assert 0.07 <= avg_interval <= 0.13, \
-            f"Average heartbeat interval {avg_interval*1000:.1f} ms not ~100 ms"
-
-    def test_heartbeat_payload_is_4_bytes(self, mock_can_bus):
-        """Heartbeat payload must be exactly 4 bytes (uint32 sequence)."""
-        for seq in (0, 1, 0xFFFFFFFF):
-            payload = build_heartbeat(seq)
-            assert len(payload) == 4, \
-                f"Heartbeat payload length {len(payload)} != 4"

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py

@@ -1,236 +1,63 @@
-#!/usr/bin/env python3
 """
-test_mode_switching.py — Mode transition integration tests.
+test_mode_switching.py — E2E tests for ORIN_CMD_MODE (0x301) encoding.
-
-Covers:
-  - idle → drive: drive commands become accepted
-  - drive → estop: immediate motor stop
-  - MODE frame byte values match protocol constants
-  - Unknown mode byte is ignored (no state change)
-
-No ROS2 or real CAN hardware required.
-Run with: python -m pytest test/test_mode_switching.py -v
 """
 
-import struct
 import pytest
 
-from saltybot_can_e2e_test.can_mock import MockCANBus
 from saltybot_can_e2e_test.protocol_defs import (
-    MAMBA_CMD_VELOCITY,
+    ORIN_CMD_MODE,
-    MAMBA_CMD_MODE,
-    MAMBA_CMD_ESTOP,
     MODE_IDLE,
     MODE_DRIVE,
     MODE_ESTOP,
     build_mode_cmd,
-    build_velocity_cmd,
-    parse_velocity_cmd,
 )
-from saltybot_can_bridge.mamba_protocol import (
-    encode_velocity_cmd,
-    encode_mode_cmd,
-    encode_estop_cmd,
-)
-
-
-# ---------------------------------------------------------------------------
-# Helpers
-# ---------------------------------------------------------------------------
-
-class _Msg:
-    def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False):
-        self.arbitration_id = arb_id
-        self.data = bytearray(data)
-        self.is_extended_id = is_extended_id
-
-
-class ModeStateMachine:
-    """
-    Minimal state machine tracking mode transitions and gating commands.
-    """
-
-    def __init__(self, bus: MockCANBus):
-        self._bus = bus
-        self._mode = MODE_IDLE
-
-    def set_mode(self, mode: int) -> bool:
-        """
-        Transition to mode.  Returns True if accepted, False if invalid.
-        Invalid mode values (not 0, 1, 2) are ignored.
-        """
-        if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
-            return False  # silently ignore
-
-        prev_mode = self._mode
-        self._mode = mode
-        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(mode)))
-
-        # Side-effects of entering ESTOP from DRIVE
-        if mode == MODE_ESTOP and prev_mode == MODE_DRIVE:
-            self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
-            self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
-
-        return True
-
-    def send_drive(self, left_mps: float, right_mps: float) -> bool:
-        """
-        Send a velocity command.  Returns True if forwarded, False if blocked.
-        """
-        if self._mode != MODE_DRIVE:
-            return False
-        self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
-        return True
-
-    @property
-    def mode(self) -> int:
-        return self._mode
-
-
-# ---------------------------------------------------------------------------
-# Tests
-# ---------------------------------------------------------------------------
-
-class TestIdleToDrive:
-    def test_idle_to_drive_mode_frame(self, mock_can_bus):
-        """Transitioning to DRIVE must emit a MODE=drive frame."""
-        sm = ModeStateMachine(mock_can_bus)
-        sm.set_mode(MODE_DRIVE)
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert len(mode_frames) == 1
-        assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE])
-
-    def test_idle_blocks_drive_commands(self, mock_can_bus):
-        """In IDLE mode, drive commands must be suppressed."""
-        sm = ModeStateMachine(mock_can_bus)
-        # Attempt drive without entering DRIVE mode
-        forwarded = sm.send_drive(1.0, 1.0)
-        assert forwarded is False, "Drive cmd should be blocked in IDLE mode"
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 0
-
-    def test_drive_mode_allows_commands(self, mock_can_bus):
-        """After entering DRIVE mode, velocity commands must be forwarded."""
-        sm = ModeStateMachine(mock_can_bus)
-        sm.set_mode(MODE_DRIVE)
-        mock_can_bus.reset()
-
-        forwarded = sm.send_drive(0.5, 0.5)
-        assert forwarded is True
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 1
-        l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
-        assert abs(l - 0.5) < 1e-4
-        assert abs(r - 0.5) < 1e-4
-
-
-class TestDriveToEstop:
-    def test_drive_to_estop_stops_motors(self, mock_can_bus):
-        """Transitioning DRIVE → ESTOP must immediately send zero velocity."""
-        sm = ModeStateMachine(mock_can_bus)
-        sm.set_mode(MODE_DRIVE)
-        sm.send_drive(1.0, 1.0)
-        mock_can_bus.reset()
-
-        sm.set_mode(MODE_ESTOP)
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) >= 1, "No velocity frame on DRIVE→ESTOP transition"
-        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
-        assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP"
-        assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP"
-
-    def test_drive_to_estop_mode_frame(self, mock_can_bus):
-        """DRIVE → ESTOP must broadcast MODE=estop."""
-        sm = ModeStateMachine(mock_can_bus)
-        sm.set_mode(MODE_DRIVE)
-        sm.set_mode(MODE_ESTOP)
-
-        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
-        assert any(bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames)
-
-    def test_estop_blocks_subsequent_drive(self, mock_can_bus):
-        """After DRIVE → ESTOP, drive commands must be blocked."""
-        sm = ModeStateMachine(mock_can_bus)
-        sm.set_mode(MODE_DRIVE)
-        sm.set_mode(MODE_ESTOP)
-        mock_can_bus.reset()
-
-        forwarded = sm.send_drive(1.0, 1.0)
-        assert forwarded is False
-
-        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
-        assert len(vel_frames) == 0
 
 
 class TestModeCommandEncoding:
-    def test_mode_idle_byte(self, mock_can_bus):
-        """MODE_IDLE must encode as 0x00."""
-        assert encode_mode_cmd(MODE_IDLE) == b"\x00"
 
-    def test_mode_drive_byte(self, mock_can_bus):
+    def test_frame_id(self):
-        """MODE_DRIVE must encode as 0x01."""
+        assert ORIN_CMD_MODE == 0x301
-        assert encode_mode_cmd(MODE_DRIVE) == b"\x01"
 
-    def test_mode_estop_byte(self, mock_can_bus):
+    def test_idle_mode(self):
-        """MODE_ESTOP must encode as 0x02."""
+        assert build_mode_cmd(MODE_IDLE) == b"\x00"
-        assert encode_mode_cmd(MODE_ESTOP) == b"\x02"
 
-    def test_mode_frame_length(self, mock_can_bus):
+    def test_drive_mode(self):
-        """Mode command payload must be exactly 1 byte."""
+        assert build_mode_cmd(MODE_DRIVE) == b"\x01"
-        for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
-            payload = encode_mode_cmd(mode)
-            assert len(payload) == 1, f"Mode {mode} payload length {len(payload)} != 1"
 
-    def test_protocol_defs_build_mode_cmd_matches(self):
+    def test_estop_mode(self):
-        """build_mode_cmd in protocol_defs must produce identical bytes."""
+        assert build_mode_cmd(MODE_ESTOP) == b"\x02"
-        for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+
-            assert build_mode_cmd(mode) == encode_mode_cmd(mode), \
+    def test_payload_length(self):
-                f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})"
+        for m in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+            assert len(build_mode_cmd(m)) == 1
+
+
+class TestIdleToDrive:
+
+    def test_idle_to_drive_sequence(self):
+        idle = build_mode_cmd(MODE_IDLE)
+        drive = build_mode_cmd(MODE_DRIVE)
+        assert idle[0] < drive[0]
+
+    def test_mode_values_are_sequential(self):
+        assert MODE_IDLE == 0
+        assert MODE_DRIVE == 1
+        assert MODE_ESTOP == 2
+
+
+class TestDriveToEstop:
+
+    def test_estop_overrides_drive(self):
+        estop = build_mode_cmd(MODE_ESTOP)
+        drive = build_mode_cmd(MODE_DRIVE)
+        # ESTOP has higher mode value than DRIVE
+        assert estop[0] > drive[0]
 
 
 class TestInvalidMode:
-    def test_invalid_mode_byte_ignored(self, mock_can_bus):
-        """Unknown mode byte (e.g. 0xFF) must be rejected — no state change."""
-        sm = ModeStateMachine(mock_can_bus)
-        sm.set_mode(MODE_DRIVE)
-        initial_mode = sm.mode
-        mock_can_bus.reset()
 
-        accepted = sm.set_mode(0xFF)
+    def test_invalid_mode_truncated_to_byte(self):
-        assert accepted is False, "Invalid mode 0xFF should be rejected"
+        # build_mode_cmd just masks to byte — no validation
-        assert sm.mode == initial_mode, "Mode changed despite invalid value"
+        payload = build_mode_cmd(255)
-        assert len(mock_can_bus.get_sent_frames()) == 0, \
+        assert len(payload) == 1
-            "Frames sent for invalid mode command"
+        assert payload[0] == 255
-
-    def test_invalid_mode_99_ignored(self, mock_can_bus):
-        """Mode 99 must be rejected."""
-        sm = ModeStateMachine(mock_can_bus)
-        accepted = sm.set_mode(99)
-        assert accepted is False
-
-    def test_invalid_mode_negative_ignored(self, mock_can_bus):
-        """Negative mode values must be rejected."""
-        sm = ModeStateMachine(mock_can_bus)
-        accepted = sm.set_mode(-1)
-        assert accepted is False
-
-    def test_mamba_protocol_invalid_mode_raises(self):
-        """mamba_protocol.encode_mode_cmd must raise on invalid mode."""
-        with pytest.raises(ValueError):
-            encode_mode_cmd(99)
-        with pytest.raises(ValueError):
-            encode_mode_cmd(-1)
-
-
-@pytest.mark.parametrize("mode,expected_byte", [
-    (MODE_IDLE,  b"\x00"),
-    (MODE_DRIVE, b"\x01"),
-    (MODE_ESTOP, b"\x02"),
-])
-def test_mode_encoding_parametrized(mode, expected_byte):
-    """Parametrized check that all mode constants encode to the right byte."""
-    assert encode_mode_cmd(mode) == expected_byte

jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml

@@ -27,7 +27,7 @@ robot:
   stem_od:     0.0381          # m  STEM_OD = 38.1mm
   stem_height: 1.050           # m  nominal cut length
 
-  # ── FC / IMU (MAMBA F722S) ──────────────────────────────────────────────────
+  # ── FC / IMU (ESP32-S3 BALANCE QMI8658) ──────────────────────────────────────────────────
   # fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105)
   # z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm
   imu_x:  0.050                # m  forward of base_link center

jetson/ros2_ws/src/saltybot_diagnostics/README.md

@@ -5,7 +5,7 @@ Comprehensive hardware diagnostics and health monitoring for SaltyBot.
 ## Features
 
 ### Startup Checks
-- RPLIDAR, RealSense, VESC, Jabra mic, STM32, servos
+- RPLIDAR, RealSense, VESC, Jabra mic, ESP32-S3, servos
 - WiFi, GPS, disk space, RAM
 - Boot result TTS + face animation
 - JSON logging

jetson/ros2_ws/src/saltybot_diagnostics/config/diagnostic_checks.yaml

@@ -6,7 +6,7 @@ startup_checks:
     - realsense
     - vesc
     - jabra_microphone
-    - stm32_bridge
+    - esp32_bridge
     - servos
     - wifi
     - gps

jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py

@@ -89,7 +89,7 @@ class DiagnosticsNode(Node):
             self._check_realsense()
             self._check_vesc()
             self._check_jabra()
-            self._check_stm32()
+            self._check_esp32()
             self._check_servos()
             self._check_wifi()
             self._check_gps()
@@ -137,8 +137,8 @@ class DiagnosticsNode(Node):
         except:
             self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {})
 
-    def _check_stm32(self):
+    def _check_esp32(self):
-        self.hardware_checks["stm32"] = ("OK", "STM32 bridge online", {})
+        self.hardware_checks["esp32"] = ("OK", "ESP32-S3 bridge online", {})
 
     def _check_servos(self):
         try:

jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml

@@ -7,7 +7,7 @@
 #   ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.2
 #
 # IMPORTANT: This node publishes raw /cmd_vel. The cmd_vel_bridge_node (PR #46)
-# applies the ESC ramp, deadman switch, and STM32 AUTONOMOUS mode gate.
+# applies the ESC ramp, deadman switch, and ESP32-S3 AUTONOMOUS mode gate.
 # Do not run this node without the cmd_vel bridge running on the same robot.
 
 # ── Follow geometry ────────────────────────────────────────────────────────────
@@ -70,5 +70,5 @@ control_rate:     20.0        # Hz — lower than cmd_vel bridge (50Hz) by desig
 # ── Mode integration ──────────────────────────────────────────────────────────
 # Master enable for the follow controller. When false, node publishes zero cmd_vel.
 # Toggle at runtime: ros2 param set /person_follower follow_enabled false
-# The cmd_vel bridge independently gates on STM32 AUTONOMOUS mode (md=2).
+# The cmd_vel bridge independently gates on ESP32-S3 AUTONOMOUS mode (md=2).
 follow_enabled:   true

jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py

@@ -28,7 +28,7 @@ State machine
 
 Safety wiring
 -------------
-  * cmd_vel bridge (PR #46) applies ramp + deadman + STM32 AUTONOMOUS mode gate --
+  * cmd_vel bridge (PR #46) applies ramp + deadman + ESP32-S3 AUTONOMOUS mode gate --
     this node publishes raw /cmd_vel, the bridge handles hardware safety.
   * follow_enabled param (default True) lets the operator disable the controller
     at runtime: ros2 param set /person_follower follow_enabled false

jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml

@@ -1,6 +1,6 @@
 gimbal_node:
   ros__parameters:
-    # Serial port connecting to STM32 over JLINK protocol
+    # Serial port connecting to ESP32-S3 over JLINK protocol
     serial_port: "/dev/ttyTHS1"
     baud_rate: 921600
 

jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py

@@ -14,7 +14,7 @@ def generate_launch_description() -> LaunchDescription:
     serial_port_arg = DeclareLaunchArgument(
         "serial_port",
         default_value="/dev/ttyTHS1",
-        description="JLINK serial port to STM32",
+        description="JLINK serial port to ESP32-S3",
     )
     pan_limit_arg = DeclareLaunchArgument(
         "pan_limit_deg",

jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548).
 
-Controls pan/tilt gimbal via JLINK binary protocol over serial to STM32.
+Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32-S3.
 Implements smooth trapezoidal motion profiles with configurable axis limits.
 
 Subscribed topics:

jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py

@@ -1,19 +1,19 @@
 """jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548).
 
-Matches the JLINK protocol defined in include/jlink.h (Issue #547 STM32 side).
+Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32-S3 side).
 
-Command type (Jetson → STM32):
+Command type (Jetson → ESP32-S3):
   0x0B  GIMBAL_POS  — int16 pan_x10 + int16 tilt_x10 + uint16 speed   (6 bytes)
                        pan_x10  = pan_deg  * 10  (±1500 for ±150°)
                        tilt_x10 = tilt_deg * 10  (±450  for ±45°)
                        speed    = servo speed register 0–4095 (0 = max)
 
-Telemetry type (STM32 → Jetson):
+Telemetry type (ESP32-S3 → Jetson):
   0x84  GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
                         uint16 pan_speed_raw + uint16 tilt_speed_raw +
                         uint8 torque_en + uint8 rx_err_pct             (10 bytes)
 
-Frame format (shared with stm32_protocol.py):
+Frame format (shared with esp32_protocol.py):
   [STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
   CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes.
 """
@@ -31,8 +31,8 @@ ETX = 0x03
 
 # ── Command / telemetry type codes ─────────────────────────────────────────────
 
-CMD_GIMBAL_POS   = 0x0B   # Jetson → STM32: set pan/tilt target
+CMD_GIMBAL_POS   = 0x0B   # Jetson → ESP32-S3: set pan/tilt target
-TLM_GIMBAL_STATE = 0x84   # STM32 → Jetson: measured state
+TLM_GIMBAL_STATE = 0x84   # ESP32-S3 → Jetson: measured state
 
 # Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
 # Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))

jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml

@@ -5,7 +5,7 @@
 #
 # Topic wiring:
 #   /rc/joy                          → mode_switch_node (CRSF channels)
-#   /saltybot/balance_state          → mode_switch_node (STM32 state)
+#   /saltybot/balance_state          → mode_switch_node (ESP32-S3 state)
 #   /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix)
 #   /saltybot/control_mode           ← mode_switch_node (JSON mode + alpha)
 #   /saltybot/led_pattern            ← mode_switch_node (LED name)

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py

@@ -13,7 +13,7 @@ Topic graph
 
 In RC mode (blend_alpha ≈ 0) the node publishes Twist(0,0) so the bridge
 receives zeros — this is harmless because the bridge's mode gate already
-prevents autonomous commands when the STM32 is in RC_MANUAL.
+prevents autonomous commands when the ESP32-S3 is in RC_MANUAL.
 
 The bridge's existing ESC ramp handles hardware-level smoothing;
 the blend_alpha here provides the higher-level cmd_vel policy ramp.

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py

@@ -6,9 +6,9 @@ state machine can be exercised in unit tests without a ROS2 runtime.
 
 Mode vocabulary
 ---------------
-  "RC"            — STM32 executing RC pilot commands; Jetson cmd_vel blocked.
+  "RC"            — ESP32-S3 executing RC pilot commands; Jetson cmd_vel blocked.
   "RAMP_TO_AUTO"  — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
-  "AUTO"          — STM32 executing Jetson cmd_vel; RC sticks idle.
+  "AUTO"          — ESP32-S3 executing Jetson cmd_vel; RC sticks idle.
   "RAMP_TO_RC"    — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
 
 Blend alpha

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py

@@ -9,7 +9,7 @@ Inputs
         axes[stick_axes...]   Roll/Pitch/Throttle/Yaw — override detection
 
   /saltybot/balance_state  (std_msgs/String JSON)
-      Parsed for RC link health (field "rc_link") and STM32 mode.
+      Parsed for RC link health (field "rc_link") and ESP32-S3 mode.
 
   <slam_fix_topic>         (geometry_msgs/PoseWithCovarianceStamped)
       Any message received within slam_fix_timeout_s → SLAM fix valid.
@@ -106,7 +106,7 @@ class ModeSwitchNode(Node):
         self._last_joy_t: float   = 0.0      # monotonic; 0 = never
         self._last_slam_t: float  = 0.0
         self._joy_axes: list      = []
-        self._stm32_mode: int     = 0        # from balance_state JSON
+        self._esp32_mode: int     = 0        # from balance_state JSON
 
         # ── QoS ───────────────────────────────────────────────────────────────
         best_effort = QoSProfile(
@@ -187,7 +187,7 @@ class ModeSwitchNode(Node):
             data = json.loads(msg.data)
             # "mode" is a label string; map back to int for reference
             mode_label = data.get("mode", "RC_MANUAL")
-            self._stm32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1,
+            self._esp32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1,
                                 "AUTONOMOUS": 2}.get(mode_label, 0)
         except (json.JSONDecodeError, TypeError):
             pass