seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: remove all Mamba/STM32 refs, replace with balance_protocol #717

Closed
sl-perception wants to merge 3 commits from sl-perception/cleanup-legacy-hw into main
pull from: sl-perception/cleanup-legacy-hw
merge into: seb:main
Conversation 0 Commits 3 Files Changed 177 +5627 -7786
177 changed files with 5627 additions and 7786 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
AUTONOMOUS_ARMING.md
View File

@ -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)

4
CLAUDE.md
View File

@ -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 |

12
TEAM.md
View File

@ -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
- PlatformIO or bare-metal STM32 toolchain
- SPI + UART + USB coexistence on ESP32-S3
- 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 |

2
USB_CDC_BUG.md
View File

@ -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)
---

2
chassis/ASSEMBLY.md
View File

@ -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.

4
chassis/BOM.md
View File

@ -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 |

2
chassis/ip54_BOM.md
View File

@ -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 | — |

200
docs/AGENTS.md
View File

@ -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.
2. **Jetson Nano** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently.
1. **ESP32-S3 BALANCE** — runs the PID balance loop. Safety-critical, operates independently of the Orin.
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
Hoverboard ESC (FOC) → 2× 8" hub motors
▼ CAN / UART
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)
├── src/
│ ├── main.c # Entry point, clock config, main loop
│ ├── 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
esp32/ # ESP32-S3 firmware (ESP-IDF)
├── balance/ # ESP32-S3 BALANCE — PID loop, IMU, safety
└── io/ # ESP32-S3 IO — VESC CAN, sensors, comms
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) |
| Primary IMU | MPU6000 (WHO_AM_I = 0x68) |
| IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 |
| IMU EXTI | PC4 (data ready interrupt) |
| IMU Orientation | CW270 (Betaflight convention) |
| 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) |
| MCU | ESP32-S3 (dual-core Xtensa LX7, 240MHz, 512KB SRAM, 8MB flash) |
| Primary IMU | MPU6000 (SPI) |
| Role | PID balance loop, tilt cutoff, arming |
| Comms to Orin | UART (velocity commands in, telemetry out) |
| Flash | `idf.py -p /dev/ttyUSB0 flash` |
### UART Assignments
### ESP32-S3 IO
| UART | Pins | Connected To | Baud |
|------|------|-------------|------|
| USART1 | PA9/PA10 | Jetson Nano | 115200 |
| USART2 | PA2/PA3 | Hoverboard ESC | 115200 |
| USART3 | PB10/PB11 | ELRS Receiver | 420000 (CRSF) |
| UART4 | — | Spare | — |
| UART5 | — | Spare | — |
| Spec | Value |
|------|-------|
| MCU | ESP32-S3 |
| Role | VESC CAN driver, sensor polling, peripheral I/O |
| VESC IDs | 68 (left), 56 (right) |
| Motor bus | CAN 1Mbit/s |
| 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
- UART protocol: `{0xABCD, int16 speed, int16 steer, uint16 checksum}` at 115200
- Speed range: -1000 to +1000
- 2× VESC motor controllers (CAN IDs 68, 56)
- VESC CAN protocol: standard SET_DUTY / SET_CURRENT / SET_RPM
- 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!) |
| FC board size | ~36mm square |
| ESP32-S3 BALANCE board | ~55×28mm (DevKit form factor) |
| 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.
2. **DCache breaks SPI on STM32F7** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.
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.
4. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
5. **USB CDC needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception.
1. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
2. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0. Always use explicit error codes.
3. **USB CDC needs RX primed in init** — without it, the OUT endpoint never starts listening.
4. **Watchdog must be fed every loop iteration** — if balance loop stalls, motors must cut within 50ms.
5. **Never change PID and speed limit in the same test** — one variable at a time.
### DFU Reboot (Betaflight Method)
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
### Build & Flash (ESP32-S3)
```bash
cd firmware/
python3 -m platformio run # Build
dfu-util -a 0 -s 0x08000000:leave -D .pio/build/f722/firmware.bin # Flash
# Balance board
cd esp32/balance/
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/`
### 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)
```
Dev machine: mbpm4 (seb@192.168.87.40)
## Current Status & Known Issues
### Working
- USB CDC serial streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`)
- Clock config with HSE + HSI fallback
- Reboot-to-DFU via USB 'R' command
- LED status patterns (status.c)
- IMU streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`)
- VESC CAN communication (IDs 68, 56)
- LED status patterns
- Web UI with WebSerial + Three.js 3D visualization
### Broken / 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.
- **MPU6000 driver** — header exists but implementation needs completion
- **PID balance loop** — not yet implemented
- **Hoverboard ESC UART** — protocol defined, driver not written
- **ELRS CRSF receiver** — protocol defined, driver not written
- **Barometer (BMP280)** — I2C init hangs, disabled
### In Progress
- PID balance loop tuning
- ELRS CRSF receiver integration
- Orin UART integration
### 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
4. Write PID balance loop with safety checks
5. Wire ELRS receiver, implement CRSF parser
6. Bench test (ESC disconnected, verify PID output)
7. First tethered balance test at 10% speed
8. Jetson UART integration
9. LED subsystem (ESP32-C3)
3. Wire ELRS receiver, implement CRSF parser
4. Bench test (VESCs disconnected, verify PID output)
5. First tethered balance test at 10% speed
6. Orin UART integration
7. LED subsystem (ESP32-S3 IO)
## 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)
```c
struct { uint16_t start=0xABCD; int16_t speed; int16_t steer; uint16_t checksum; };
// speed/steer: -1000 to +1000
```
### ESP32-S3 IO → VESC (CAN, loop rate)
- Standard VESC CAN protocol (SET_DUTY / SET_CURRENT / SET_RPM)
- Node IDs: VESC 68 (left), VESC 56 (right)
### 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}
// Raw IMU values (int16), t=temp×10, p=pressure, bt=baro temp
{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0}
// 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

4
docs/FACE_LCD_ANIMATION.md
View File

@ -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

16
docs/SALTYLAB.md
View File

@ -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
- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE
- **Flashing:** esptool.py via USB (bootloader mode)
- **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

14
docs/wiring-diagram.md
View File

@ -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 │

2
flash.sh
View File

@ -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..."

1
include/baro.h
View File

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

1
include/battery.h
View File

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

1
include/battery_adc.h
View File

@ -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)
*

1
include/bno055.h
View File

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

1
include/buzzer.h
View File

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

1
include/config.h
View File

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

1
include/face_lcd.h
View File

@ -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
*

1
include/fan.h
View File

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

1
include/fault_handler.h
View File

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

1
include/i2c1.h
View File

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

1
include/jetson_cmd.h
View File

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

1
include/jetson_uart.h
View File

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

1
include/jlink.h
View File

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

1
include/orin_can.h
View File

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

1
include/ota.h
View File

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

1
include/pid_flash.h
View File

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

1
include/power_mgmt.h
View File

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

1
include/uart_protocol.h
View File

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

1
include/ultrasonic.h
View File

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

1
include/vesc_can.h
View File

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

1
include/watchdog.h
View File

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

4
jetson/README.md
View File

@ -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

2
jetson/config/RECOVERY_BEHAVIORS.md
View File

@ -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

2
jetson/config/nav2_params.yaml
View File

@ -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:

24
jetson/docker-compose.yml
View File

@ -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) ────────────────────────
stm32-bridge:
# ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ────────────────────────
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) ----------------------
# Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to STM32.
# -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ----------------------
# 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

38
jetson/docs/pinout.md
View File

@ -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 |
| Device node | `/dev/ttyACM0` → symlink `/dev/stm32-bridge` (via udev) |
| Baud rate | 921600 (configured in STM32 firmware) |
| Interface | CANable2 USB → USB-A on Jetson (SocketCAN slcan0) |
| Device node | `/dev/ttyUSB0` → symlink `/dev/canable2` (via udev); SocketCAN `slcan0` |
| 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/balance_state` | STM32→Jetson | Motor cmd, pitch, state |
| `/cmd_vel` | Jetson→STM32 | Velocity commands → `C<spd>,<str>\n` |
| `/saltybot/estop` | Jetson→STM32 | Emergency stop |
| `/saltybot/imu` | ESP32-S3→Jetson | IMU data (accel, gyro) at 50Hz |
| `/saltybot/balance_state` | ESP32-S3→Jetson | Motor cmd, pitch, state |
| `/cmd_vel` | Jetson→ESP32-S3 | Velocity commands → `C<spd>,<str>\n` |
| `/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) |
| 10 | RXD0 | 3.3V | UART RX ← STM32 (fallback) |
| 8 | TXD0 | 3.3V | UART TX → ESP32-S3 BALANCE (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", \

4
jetson/docs/power-budget.md
View File

@ -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)
```

4
jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml
View File

@ -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

8
jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml
View File

@ -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

18
jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml → jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml
View File

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

2
jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml
View File

@ -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

6
jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py
View File

@ -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)"),

8
jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py
View File

@ -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)"),

18
jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py → jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py
View File

@ -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("baud_rate", default_value="921600"),
DeclareLaunchArgument("serial_port", default_value="/dev/esp32-balance"),
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",
name="stm32_cmd_node",
executable="esp32_cmd_node",
name="esp32_cmd_node",
output="screen",
emulate_tty=True,
parameters=[

4
jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py
View File

@ -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:

6
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py
View File

@ -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()

26
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py
View File

@ -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 ─────────────────────────────────────────────────────────────

8
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py
View File

@ -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": false} -> writes 'Z\n' to STM32 (clear latch, robot can re-arm)
{"kill": true} -> writes 'E\n' to ESP32-S3 IO (ESTOP_REMOTE, immediate motor cutoff)
{"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)

2
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py
View File

@ -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

22
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py
View File

@ -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
Heartbeat timer (200ms) → H\\n → STM32
/cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n → ESP32-S3 IO
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 ─────────────────────────────────────────────────────────────

20
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py
View File

@ -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()

483
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
View File

@ -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()

332
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py
View File

@ -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)

10
jetson/ros2_ws/src/saltybot_bridge/setup.py
View File

@ -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)
"stm32_cmd_node = saltybot_bridge.stm32_cmd_node:main",
# Binary-framed ESP32-S3 IO command node (Issue #119)
"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)

2
jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
View File

@ -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
"""

4
jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py
View File

@ -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)

18
jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_cmd_node.py → jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_cmd_node.py
View File

@ -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)

6
jetson/ros2_ws/src/saltybot_bridge/test/test_stm32_protocol.py → jetson/ros2_ws/src/saltybot_bridge/test/test_esp32_protocol.py
View File

@ -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,

2
jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
View File

@ -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
"""

2
jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
View File

@ -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:

4
jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml
View File

@ -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

38
jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py
View File

@ -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= 2s cmd_vel bridge (consumes /cmd_vel, needs STM32 bridge up)
t= 0s ESP32-S3 IO bridge (serial port owner — must be first)
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
until STM32 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
• ESP32-S3 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
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(
"stm32_port",
default_value="/dev/stm32-bridge",
description="STM32 USB CDC serial port",
esp32_port_arg = DeclareLaunchArgument(
"esp32_port",
default_value="/dev/esp32-bridge",
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 ────────────────────────────────
stm32_bridge = GroupAction(
# ── t=0s ESP32-S3 bidirectional serial bridge ────────────────────────────────
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,

30
jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py
View File

@ -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(
"stm32_port",
default_value="/dev/stm32-bridge",
description="STM32 USART bridge serial device",
esp32_io_port_arg = DeclareLaunchArgument(
"esp32_io_port",
default_value="/dev/esp32-io",
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)
stm32_bridge = IncludeLaunchDescription(
# ESP32-S3 bidirectional bridge (JLINK USART1)
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,

2
jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py
View File

@ -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:

10
jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py
View File

@ -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,

2
jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py
View File

@ -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.

6
jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md
View File

@ -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

8
jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py
View File

@ -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()

4
jetson/ros2_ws/src/saltybot_bringup/urdf/saltybot.urdf.xacro
View File

@ -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" />

8
jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml
View File

@ -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

2
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py
View File

@ -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."""

169
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/balance_protocol.py Normal file
View File

@ -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)

284
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
View File

@ -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)
/estop std_msgs/Bool → Mamba e-stop (CAN)
/cmd_vel geometry_msgs/Twist -> drive commands (CAN 0x300)
/estop std_msgs/Bool -> e-stop (CAN 0x303)
Publications
------------
/can/imu sensor_msgs/Imu Mamba IMU telemetry
/can/battery sensor_msgs/BatteryState Mamba battery telemetry
/can/attitude std_msgs/Float32MultiArray ESP32-S3 BALANCE attitude
/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 (
MAMBA_CMD_ESTOP,
MAMBA_CMD_MODE,
MAMBA_CMD_VELOCITY,
MAMBA_TELEM_BATTERY,
MAMBA_TELEM_IMU,
VESC_TELEM_STATE,
ORIN_CAN_ID_FC_PID_ACK,
ORIN_CAN_ID_PID_SET,
from saltybot_can_bridge.balance_protocol import (
ESP32_TELEM_ATTITUDE,
ESP32_TELEM_BATTERY,
ORIN_CMD_DRIVE,
ORIN_CMD_ESTOP,
VESC_LEFT_ID,
VESC_RIGHT_ID,
VESC_STATUS_1,
MODE_DRIVE,
MODE_ESTOP,
MODE_IDLE,
encode_drive_cmd,
encode_estop_cmd,
encode_mode_cmd,
encode_velocity_cmd,
encode_pid_set_cmd,
decode_battery_telem,
decode_imu_telem,
decode_pid_ack,
decode_vesc_state,
decode_attitude,
decode_battery,
decode_vesc_can_id,
decode_vesc_status1,
)
# 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 ────────────────────────────────────────────────────
self.declare_parameter("can_interface", "slcan0")
self.declare_parameter("left_vesc_can_id", 56)
self.declare_parameter("right_vesc_can_id", 68)
self.declare_parameter("mamba_can_id", 1)
# -- Parameters --
self.declare_parameter("can_interface", "can0")
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
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 ────────────────────────────────────────────────────
self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
# -- Publishers --
self._pub_attitude = self.create_publisher(
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"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
f"mamba={self._mamba_id}"
f"can_bridge_node ready -- iface={self._iface} "
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id}"
)
# -- PID parameter callback (Issue #693) --
def _on_set_parameters(self, params) -> SetParametersResult:
"""Send new PID gains over CAN when pid/* params change."""
for p in params:
if p.name == "pid/kp":
self._pid_kp = float(p.value)
elif p.name == "pid/ki":
self._pid_ki = float(p.value)
elif p.name == "pid/kd":
self._pid_kd = float(p.value)
else:
continue
try:
payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
self.get_logger().info(
f"PID gains sent: Kp={self._pid_kp:.2f} "
f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
)
except ValueError as exc:
return SetParametersResult(successful=False, reason=str(exc))
return SetParametersResult(successful=True)
# ── Connection management ──────────────────────────────────────────────
# -- 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
# Forward left = forward right for pure translation; for rotation
# left slows and right speeds up (positive angular = CCW = left turn).
# The Mamba velocity command carries both wheels independently.
left_mps = linear - angular
right_mps = linear + angular
payload = encode_velocity_cmd(left_mps, right_mps)
self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
# Keep Mamba in DRIVE mode while receiving commands
self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
# Differential drive -- motor units [-1000, +1000]
speed_units = int(linear * 1000.0)
steer_units = int(angular * 1000.0)
self._send_can(
ORIN_CMD_DRIVE,
encode_drive_cmd(speed_units, steer_units, mode=MODE_DRIVE),
"cmd_vel",
)
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(
MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
)
self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba")
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
self.get_logger().warning("E-stop asserted -- sent ESTOP to ESP32-S3 BALANCE")
# ── 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,
encode_velocity_cmd(0.0, 0.0),
ORIN_CMD_DRIVE,
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.
Dispatches incoming frames to the appropriate handler.
"""
"""Blocking CAN read loop executed in a daemon thread."""
while rclpy.ok():
with self._lock:
connected = self._connected
bus = self._bus
if not connected or bus is None:
time.sleep(0.1)
continue
try:
frame = bus.recv(timeout=0.5)
except can.CanError as exc:
self._handle_can_error(exc, "reader_loop recv")
continue
if frame is None:
# Timeout — no frame within 0.5 s, loop again
continue
self._dispatch_frame(frame)
def _dispatch_frame(self, frame: can.Message) -> None:
"""Route an incoming CAN frame to the correct publisher."""
arb_id = frame.arbitration_id
data = bytes(frame.data)
try:
if arb_id == MAMBA_TELEM_IMU:
self._handle_imu(data, frame.timestamp)
elif arb_id == MAMBA_TELEM_BATTERY:
self._handle_battery(data, frame.timestamp)
elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
self._handle_vesc_state(data, frame.timestamp, side="left")
elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
self._handle_vesc_state(data, frame.timestamp, side="right")
elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
gains = decode_pid_ack(data)
self.get_logger().debug(
f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
)
if arb_id == ESP32_TELEM_ATTITUDE:
self._handle_attitude(data)
elif arb_id == ESP32_TELEM_BATTERY:
self._handle_battery(data)
elif frame.is_extended_id:
# VESC extended 29-bit ID: packet_type<<8 | node_id
pkt_type, node_id = decode_vesc_can_id(arb_id)
if pkt_type == VESC_STATUS_1:
self._handle_vesc_status1(data, node_id)
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:
telem = decode_imu_telem(data)
msg = Imu()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "imu_link"
msg.linear_acceleration.x = telem.accel_x
msg.linear_acceleration.y = telem.accel_y
msg.linear_acceleration.z = telem.accel_z
msg.angular_velocity.x = telem.gyro_x
msg.angular_velocity.y = telem.gyro_y
msg.angular_velocity.z = telem.gyro_z
# Covariance unknown; mark as -1 per REP-145
msg.orientation_covariance[0] = -1.0
self._pub_imu.publish(msg)
def _handle_battery(self, data: bytes, timestamp: float) -> None:
telem = decode_battery_telem(data)
msg = BatteryState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.voltage = telem.voltage
msg.current = telem.current
msg.present = True
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
def _handle_attitude(self, data: bytes) -> None:
telem = decode_attitude(data)
msg = Float32MultiArray()
# Layout: [pitch_deg, speed, yaw_rate, state, flags]
msg.data = [
telem.pitch_deg,
telem.speed,
telem.yaw_rate,
float(telem.state),
float(telem.flags),
]
self._pub_attitude.publish(msg)
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(
self, data: bytes, timestamp: float, side: str
) -> None:
telem = decode_vesc_state(data)
def _handle_vesc_status1(self, data: bytes, node_id: int) -> None:
status = decode_vesc_status1(node_id, data)
msg = Float32MultiArray()
# Layout: [erpm, duty, voltage, current]
msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
if side == "left":
# Layout: [erpm, current, duty]
msg.data = [status.erpm, status.current, status.duty]
if node_id == self._left_vesc_id:
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,
encode_velocity_cmd(0.0, 0.0),
ORIN_CMD_DRIVE,
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:

224
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py
View File

@ -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)

21
jetson/ros2_ws/src/saltybot_can_bridge/setup.py
View File

@ -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",
packages=[package_name],
version="0.0.1",
packages=find_packages(exclude=["test"]),
data_files=[
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
(f"share/{package_name}", ["package.xml"]),
(f"share/{package_name}/config", ["config/can_bridge_params.yaml"]),
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
("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_email="sl-controls@saltylab.local",
description="CAN bus bridge for Mamba controller and VESC telemetry",
maintainer="seb",
maintainer_email="seb@example.com",
description="CAN bus bridge for ESP32 BALANCE and VESC telemetry",
license="MIT",
tests_require=["pytest"],
entry_points={

318
jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py
View File

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

288
jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py
View File

@ -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
include/vesc_can.h — VESC CAN protocol
saltybot_can_bridge/mamba_protocol.py — existing bridge constants
saltybot_can_bridge/balance_protocol.py — canonical codec
CAN IDs used in tests
---------------------
Orin → FC (Mamba) commands (standard 11-bit, matching orin_can.h):
ORIN_CMD_HEARTBEAT 0x300
ORIN_CMD_DRIVE 0x301 int16 speed (−1000..+1000), int16 steer (−1000..+1000)
ORIN_CMD_MODE 0x302 uint8 mode byte
ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR)
Orin → ESP32 BALANCE commands (standard 11-bit):
ORIN_CMD_DRIVE 0x300 left_mps (f32 LE) | right_mps (f32 LE)
ORIN_CMD_MODE 0x301 uint8 mode (0=idle, 1=drive, 2=estop)
ORIN_CMD_ESTOP 0x302 uint8 flags (bit0=stop, bit1=clear)
ORIN_CMD_LED 0x303 uint8 pattern | r | g | b
FC (Mamba) → Orin telemetry (standard 11-bit, matching orin_can.h):
FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t)
FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t)
FC_IMU 0x402 8 bytes
FC_BARO 0x403 8 bytes
ESP32 BALANCE → Orin telemetry (standard 11-bit):
FC_STATUS 0x400 8 bytes (pitch_x10 i16 | motor_cmd u16 | vbat_mv u16 | state u8 | flags u8)
FC_VESC 0x401 8 bytes (l_rpm_x10 i16 | r_rpm_x10 i16 | l_cur_x10 i16 | r_cur_x10 i16)
Mamba ↔ VESC internal commands (matching mamba_protocol.py):
MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian
MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop)
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)
VESC STATUS (extended 29-bit):
arb_id = (VESC_STATUS_1 << 8) | vesc_node_id = (9 << 8) | node_id
Payload: int32 ERPM (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 = 0x301
ORIN_CMD_MODE: int = 0x302
ORIN_CMD_ESTOP: int = 0x303
ORIN_CMD_DRIVE: int = 0x300
ORIN_CMD_MODE: int = 0x301
ORIN_CMD_ESTOP: int = 0x302
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
MAMBA_CMD_MODE: int = 0x101
MAMBA_CMD_ESTOP: int = 0x102
VESC_STATUS_1: int = 9 # STATUS_1 packet type (upper byte of arb_id)
VESC_CMD_SET_RPM: int = 3
MAMBA_TELEM_IMU: int = 0x200
MAMBA_TELEM_BATTERY: int = 0x201
VESC_TELEM_STATE: int = 0x300
VESC_CAN_ID_LEFT: int = 56
VESC_CAN_ID_RIGHT: int = 68
# ---------------------------------------------------------------------------
# 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)
VESC_PKT_SET_RPM: int = 3 # SET_RPM packet type
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
def VESC_STATUS1_ID(vesc_node_id: int) -> int:
"""Return the 29-bit extended arbitration ID for a VESC STATUS_1 frame."""
return (VESC_STATUS_1 << 8) | vesc_node_id
# ---------------------------------------------------------------------------
# Frame builders — Orin → FC
# Frame builders — Orin → ESP32 BALANCE
# ---------------------------------------------------------------------------
def build_heartbeat(seq: int = 0) -> bytes:
"""Build a HEARTBEAT payload: uint32 sequence counter (big-endian, 4 bytes)."""
return struct.pack(">I", seq & 0xFFFFFFFF)
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_drive_cmd(left_mps: float, right_mps: float) -> bytes:
"""Build ORIN_CMD_DRIVE payload (8 bytes, 2 × float32 little-endian)."""
return struct.pack("<ff", float(left_mps), float(right_mps))
def build_mode_cmd(mode: int) -> bytes:
"""Build an ORIN_CMD_MODE payload (1 byte)."""
return struct.pack(">B", mode & 0xFF)
"""Build ORIN_CMD_MODE payload (1 byte)."""
return struct.pack("B", mode & 0xFF)
def build_estop_cmd(action: int = 1) -> bytes:
"""Build an ORIN_CMD_ESTOP payload. action=1 → ESTOP, 0 → CLEAR."""
return struct.pack(">B", action & 0xFF)
def build_estop_cmd(stop: bool = True, clear: bool = False) -> bytes:
"""Build ORIN_CMD_ESTOP payload (1 byte flags)."""
return struct.pack("B", (0x01 if stop else 0) | (0x02 if clear else 0))
# ---------------------------------------------------------------------------
# Frame builders — Mamba velocity commands (mamba_protocol.py encoding)
# ---------------------------------------------------------------------------
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
# Frame builders — ESP32 BALANCE → 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 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]
"""
"""Build FC_STATUS (0x400) payload (8 bytes, big-endian)."""
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,
right_rpm_x10: int = 0,
left_current_x10: int = 0,
right_current_x10: int = 0,
l_rpm_x10: int = 0,
r_rpm_x10: int = 0,
l_cur_x10: int = 0,
r_cur_x10: int = 0,
) -> bytes:
"""
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).
"""
"""Build FC_VESC (0x401) payload (8 bytes, big-endian)."""
return struct.pack(
">hhhh",
int(left_rpm_x10),
int(right_rpm_x10),
int(left_current_x10),
int(right_current_x10),
int(l_rpm_x10),
int(r_rpm_x10),
int(l_cur_x10),
int(r_cur_x10),
)
def build_vesc_status(
rpm: int = 0,
def build_vesc_status1(
erpm: int = 0,
current_x10: int = 0,
duty_x1000: int = 0,
) -> bytes:
"""
Build a VESC STATUS (packet type 9) payload.
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),
)
"""Build VESC STATUS_1 payload (8 bytes, big-endian)."""
return struct.pack(">ihh", int(erpm), int(current_x10), int(duty_x1000))
# ---------------------------------------------------------------------------
# Frame parsers
# ---------------------------------------------------------------------------
def parse_fc_status(data: bytes):
"""
Parse an FC_STATUS (0x400) payload.
def parse_drive_cmd(data: bytes) -> Tuple[float, float]:
"""Parse ORIN_CMD_DRIVE payload → (left_mps, right_mps)."""
if len(data) < 8:
raise ValueError(f"ORIN_CMD_DRIVE needs 8 bytes, got {len(data)}")
return struct.unpack("<ff", data[:8])
Returns
-------
dict with keys: pitch_x10, motor_cmd, vbat_mv, balance_state, flags,
estop_active (bool), armed (bool)
"""
def parse_fc_status(data: bytes):
"""Parse FC_STATUS (0x400) payload."""
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(
">hhHBB", data[:8]
)
pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack(">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):
"""
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)
"""
def parse_vesc_status1(data: bytes):
"""Parse VESC STATUS_1 payload."""
if len(data) < 8:
raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}")
left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack(
">hhhh", data[:8]
)
raise ValueError(f"VESC STATUS_1 needs 8 bytes, got {len(data)}")
erpm, cur_x10, duty_x1000 = struct.unpack(">ihh", data[:8])
return {
"left_rpm_x10": left_rpm_x10,
"right_rpm_x10": right_rpm_x10,
"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,
"erpm": erpm,
"current": cur_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])

16
jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py
View File

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

94
jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py
View File

@ -1,93 +1,27 @@
#!/usr/bin/env python3
"""
conftest.py — pytest fixtures for the saltybot_can_e2e_test suite.
No ROS2 node infrastructure is started; all tests run purely in Python.
conftest.py — shared pytest fixtures for saltybot CAN end-to-end tests.
"""
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_e2e_test.protocol_defs import (
VESC_CAN_ID_LEFT,
VESC_CAN_ID_RIGHT,
from saltybot_can_bridge.balance_protocol import (
encode_drive_cmd,
encode_mode_cmd,
encode_estop_cmd,
decode_fc_status,
decode_fc_vesc,
decode_vesc_status1,
)
# ---------------------------------------------------------------------------
# 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")
@pytest.fixture
def bridge_components():
"""
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)
"""Return a dict of encode/decode helpers from balance_protocol."""
yield {
"bus": bus,
"encode_vel": encode_velocity_cmd,
"encode_drive": encode_drive_cmd,
"encode_mode": encode_mode_cmd,
"encode_estop": encode_estop_cmd,
"decode_vesc": decode_vesc_state,
"decode_battery": decode_battery_telem,
"decode_imu": decode_imu_telem,
"left_vesc_id": VESC_CAN_ID_LEFT,
"right_vesc_id": VESC_CAN_ID_RIGHT,
"decode_fc_status": decode_fc_status,
"decode_fc_vesc": decode_fc_vesc,
"decode_vesc": decode_vesc_status1,
}
bus.shutdown()

204
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py
View File

@ -1,193 +1,47 @@
#!/usr/bin/env python3
"""
test_drive_command.py — Integration tests for the drive command path.
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
test_drive_command.py — E2E tests for ORIN_CMD_DRIVE (0x300) encoding.
"""
import struct
import pytest
from saltybot_can_e2e_test.protocol_defs import (
MAMBA_CMD_VELOCITY,
MAMBA_CMD_MODE,
FC_VESC,
MODE_DRIVE,
ORIN_CMD_DRIVE,
build_drive_cmd,
parse_drive_cmd,
MODE_IDLE,
VESC_CAN_ID_LEFT,
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,
MODE_DRIVE,
)
# ---------------------------------------------------------------------------
# Helper
# ---------------------------------------------------------------------------
class TestDriveCommandEncoding:
def _send_drive(bus, left_mps: float, right_mps: float) -> None:
"""Simulate the bridge encoding and sending a velocity command."""
from saltybot_can_e2e_test.can_mock import MockCANBus
def test_frame_id(self):
assert ORIN_CMD_DRIVE == 0x300
payload = encode_velocity_cmd(left_mps, right_mps)
# Create a minimal message object compatible with our mock
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, 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))
def test_zero_drive(self):
payload = build_drive_cmd(0.0, 0.0)
left, right = parse_drive_cmd(payload)
assert abs(left) < 1e-5
assert abs(right) < 1e-5
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_IDLE])
def test_forward(self):
payload = build_drive_cmd(1.5, 1.5)
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", [
(0.5, 0.5),
(1.0, 0.0),
(0.0, -1.0),
(-0.5, -0.5),
])
def test_drive_cmd_payload_roundtrip(mock_can_bus, left_mps, right_mps):
"""Parametrized: encode then decode must recover original velocities."""
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
def test_payload_is_little_endian(self):
# First 4 bytes encode left speed in LE float32
payload = build_drive_cmd(1.0, 0.0)
(left,) = struct.unpack("<f", payload[:4])
assert abs(left - 1.0) < 1e-5
def test_payload_length(self):
assert len(build_drive_cmd(1.0, -1.0)) == 8

264
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py
View File

@ -1,264 +1,52 @@
#!/usr/bin/env python3
"""
test_estop.py — E-stop command integration tests.
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
test_estop.py — E2E tests for ORIN_CMD_ESTOP (0x302) and FC_STATUS flags.
"""
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,
)
# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------
class TestEstopAssert:
class _Msg:
"""Minimal CAN message stand-in."""
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_frame_id(self):
assert ORIN_CMD_ESTOP == 0x302
def test_stop_bit_set(self):
payload = build_estop_cmd(stop=True, clear=False)
assert payload[0] & 0x01 == 0x01
class EstopStateMachine:
"""
Lightweight state machine that mirrors the bridge estop logic.
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"
def test_stop_bit_clear_not_set(self):
payload = build_estop_cmd(stop=True, clear=False)
assert payload[0] & 0x02 == 0x00
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)
assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear"
l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
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"
def test_stop_not_set_on_clear(self):
payload = build_estop_cmd(stop=False, clear=True)
assert payload[0] & 0x01 == 0x00
class TestFirmwareSideEstop:
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)
class TestAttitudeEstopFlag:
frame = mock_can_bus.recv(timeout=0.1)
assert frame is not None, "FC_STATUS frame not received"
parsed = parse_fc_status(bytes(frame.data))
assert parsed["estop_active"] is True, \
"estop_active flag not set in FC_STATUS"
assert parsed["balance_state"] == 2
def test_estop_active_flag_in_fc_status(self):
# FC_STATUS flags bit0 = estop active
raw = build_fc_status(flags=0x01)
status = parse_fc_status(raw)
assert status["flags"] & 0x01 == 0x01
def test_fc_status_no_estop_flag(self, mock_can_bus):
"""FC_STATUS with flags=0x00 must NOT set estop_active."""
payload = build_fc_status(flags=0x00)
mock_can_bus.inject(FC_STATUS, payload)
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
def test_no_estop_flag(self):
raw = build_fc_status(flags=0x00)
status = parse_fc_status(raw)
assert status["flags"] & 0x01 == 0x00

341
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py
View File

@ -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
- Both left (56) and right (68) VESC STATUS combined in FC_VESC
- FC_VESC broadcast rate (~10 Hz)
- 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
- VESC STATUS_1 extended frames (arb_id = (9 << 8) | node_id)
- FC_STATUS (0x400) attitude/battery frame
- FC_VESC (0x401) VESC RPM/current broadcast frame
"""
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_SET_RPM_ID,
VESC_TELEM_STATE,
build_vesc_status,
VESC_STATUS1_ID,
build_fc_status,
build_fc_vesc,
parse_fc_vesc,
parse_vesc_status,
)
from saltybot_can_bridge.mamba_protocol import (
VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE,
decode_vesc_state,
build_vesc_status1,
parse_fc_status,
parse_vesc_status1,
)
# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------
class TestVescStatusParsing:
class VescStatusAggregator:
"""
Simulates the firmware logic that:
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
def test_status1_id_left(self):
arb_id = VESC_STATUS1_ID(VESC_CAN_ID_LEFT)
assert arb_id == (9 << 8) | 56
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):
self._bus = bus
self._left_rpm_x10 = 0
self._right_rpm_x10 = 0
self._left_current_x10 = 0
self._right_current_x10 = 0
self._left_seen = False
self._right_seen = False
def test_parse_vesc_status1_nominal(self):
raw = build_vesc_status1(erpm=3000, current_x10=55, duty_x1000=250)
result = parse_vesc_status1(raw)
assert abs(result["erpm"] - 3000) < 1
assert abs(result["current"] - 5.5) < 0.01
assert abs(result["duty"] - 0.25) < 0.001
def process_vesc_status(self, arb_id: int, data: bytes) -> None:
"""
Process an incoming VESC STATUS frame (extended 29-bit ID).
Updates internal state; broadcasts FC_VESC when at least one side is known.
"""
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 test_parse_vesc_status1_reverse(self):
raw = build_vesc_status1(erpm=-1500, current_x10=-20, duty_x1000=-150)
result = parse_vesc_status1(raw)
assert result["erpm"] == -1500
assert abs(result["duty"] - (-0.15)) < 0.001
def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int,
current_x10: int = 50, duty_x1000: int = 250) -> None:
"""Inject a VESC STATUS extended frame for the given node ID."""
arb_id = VESC_STATUS_ID(vesc_id)
payload = build_vesc_status(rpm=rpm, current_x10=current_x10, duty_x1000=duty_x1000)
bus.inject(arb_id, payload, is_extended_id=True)
class TestFcStatusTelemetry:
def test_frame_id(self):
assert FC_STATUS == 0x400
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
# ---------------------------------------------------------------------------
# Tests
# ---------------------------------------------------------------------------
class TestFcVescTelemetry:
class TestVescStatusToFcVesc:
def test_left_vesc_status_triggers_broadcast(self, mock_can_bus):
"""
Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains
the correct left RPM (rpm / 10).
"""
agg = VescStatusAggregator(mock_can_bus)
def test_frame_id(self):
assert FC_VESC == 0x401
# Left VESC: 3000 RPM → rpm_x10 = 300
arb_id = VESC_STATUS_ID(VESC_CAN_ID_LEFT)
payload = build_vesc_status(rpm=3000, current_x10=55)
agg.process_vesc_status(arb_id, payload)
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}"
def test_nominal(self):
raw = build_fc_vesc(l_rpm_x10=300, r_rpm_x10=-300, l_cur_x10=55, r_cur_x10=-55)
l_rpm, r_rpm, l_cur, r_cur = struct.unpack(">hhhh", raw)
assert l_rpm == 300
assert r_rpm == -300

253
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py
View File

@ -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:
- Heartbeat loss triggers e-stop escalation (timeout logic)
- 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
When no drive command is received within the timeout period, the bridge should
send zero-velocity ORIN_CMD_DRIVE with MODE_IDLE.
"""
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_ESTOP,
ORIN_CMD_DRIVE,
ORIN_CMD_MODE,
MAMBA_CMD_VELOCITY,
MAMBA_CMD_MODE,
MAMBA_CMD_ESTOP,
ORIN_CMD_ESTOP,
MODE_IDLE,
MODE_DRIVE,
MODE_ESTOP,
build_heartbeat,
build_estop_cmd,
build_drive_cmd,
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,
parse_drive_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
HB_INTERVAL_MS = 100
HB_INTERVAL_S = HB_INTERVAL_MS / 1000.0
class TestWatchdogZeroVelocity:
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
# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------
class TestEstopOnTimeout:
class HeartbeatSimulator:
"""
Simulate periodic heartbeat injection into a MockCANBus.
def test_estop_frame_id(self):
assert ORIN_CMD_ESTOP == 0x302
Call start() to begin sending heartbeats every interval_s.
Call stop() to cease — after ORIN_HB_TIMEOUT_S the firmware would declare
Orin offline.
"""
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"
def test_estop_stop_flag(self):
from saltybot_can_e2e_test.protocol_defs import build_estop_cmd
payload = build_estop_cmd(stop=True, clear=False)
assert payload[0] & 0x01 == 0x01

255
jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py
View File

@ -1,236 +1,63 @@
#!/usr/bin/env python3
"""
test_mode_switching.py — Mode transition integration tests.
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
test_mode_switching.py — E2E tests for ORIN_CMD_MODE (0x301) encoding.
"""
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_MODE,
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):
"""MODE_DRIVE must encode as 0x01."""
assert encode_mode_cmd(MODE_DRIVE) == b"\x01"
def test_frame_id(self):
assert ORIN_CMD_MODE == 0x301
def test_mode_estop_byte(self, mock_can_bus):
"""MODE_ESTOP must encode as 0x02."""
assert encode_mode_cmd(MODE_ESTOP) == b"\x02"
def test_idle_mode(self):
assert build_mode_cmd(MODE_IDLE) == b"\x00"
def test_mode_frame_length(self, mock_can_bus):
"""Mode command payload must be exactly 1 byte."""
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_drive_mode(self):
assert build_mode_cmd(MODE_DRIVE) == b"\x01"
def test_protocol_defs_build_mode_cmd_matches(self):
"""build_mode_cmd in protocol_defs must produce identical bytes."""
for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
assert build_mode_cmd(mode) == encode_mode_cmd(mode), \
f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})"
def test_estop_mode(self):
assert build_mode_cmd(MODE_ESTOP) == b"\x02"
def test_payload_length(self):
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)
assert accepted is False, "Invalid mode 0xFF should be rejected"
assert sm.mode == initial_mode, "Mode changed despite invalid value"
assert len(mock_can_bus.get_sent_frames()) == 0, \
"Frames sent for invalid mode command"
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
def test_invalid_mode_truncated_to_byte(self):
# build_mode_cmd just masks to byte — no validation
payload = build_mode_cmd(255)
assert len(payload) == 1
assert payload[0] == 255

2
jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml
View File

@ -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

2
jetson/ros2_ws/src/saltybot_diagnostics/README.md
View File

@ -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

2
jetson/ros2_ws/src/saltybot_diagnostics/config/diagnostic_checks.yaml
View File

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

6
jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py
View File

@ -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):
self.hardware_checks["stm32"] = ("OK", "STM32 bridge online", {})
def _check_esp32(self):
self.hardware_checks["esp32"] = ("OK", "ESP32-S3 bridge online", {})
def _check_servos(self):
try:

4
jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml
View File

@ -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

2
jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py
View File

@ -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

2
jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml
View File

@ -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

2
jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py
View File

@ -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",

2
jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py
View File

@ -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:

12
jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py
View File

@ -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
TLM_GIMBAL_STATE = 0x84 # STM32 → Jetson: measured state
CMD_GIMBAL_POS = 0x0B # Jetson → ESP32-S3: set pan/tilt target
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))

2
jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml
View File

@ -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)

2
jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py
View File

@ -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.

4
jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py
View File

@ -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

6
jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py
View File

@ -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

Some files were not shown because too many files have changed in this diff Show More