Compare commits

..

8 Commits

Author SHA1 Message Date
fda6ab99ff feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec
Update CAN and serial bridge code to match authoritative protocol spec
from docs/SAUL-TEE-SYSTEM-REFERENCE.md §5-6 (hal, 2026-04-04).

mamba_protocol.py (CAN, Orin ↔ ESP32 BALANCE):
- 0x300 DRIVE: [speed:i16][steer:i16][mode:u8][flags:u8][_:u16] — combined frame
- 0x301 ARM:   [arm:u8]
- 0x302 PID:   [kp:f16][ki:f16][kd:f16][_:u16]  — half-float gains
- 0x303 ESTOP: [0xE5] — magic byte cut
- 0x400 ATTITUDE: [pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8]
- 0x401 BATTERY:  [vbat_mv:u16][fault_code:u8][rssi:i8]
- Add VESC STATUS1/4/5 decode helpers; VESC IDs 56 (left) / 68 (right)

can_bridge_node.py:
- /cmd_vel → encode_drive_cmd (speed/steer int16, MODE_DRIVE)
- /estop → encode_estop_cmd (magic 0xE5); clear → DISARM
- /saltybot/arm → encode_arm_cmd (new subscription)
- Watchdog sends DRIVE(0,0,MODE_IDLE) when /cmd_vel silent
- ATTITUDE (0x400) → /saltybot/attitude + /saltybot/balance_state JSON
- BATTERY  (0x401) → /can/battery BatteryState
- VESC STATUS1 frames → /can/vesc/left|right/state

stm32_cmd_node.py — rewritten for inter-board protocol API:
- Imports from updated stm32_protocol (BAUD_RATE=460800, new frame types)
- RX: RcChannels → /saltybot/rc_channels, SensorData → /saltybot/sensors
- TX: encode_led_cmd, encode_output_cmd from /saltybot/leds + /saltybot/outputs
- HEARTBEAT (0x20) timer replaces old SPEED_STEER/ARM logic

stm32_cmd_params.yaml: serial_port=/dev/esp32-io, baud=460800
stm32_cmd.launch.py: updated defaults and description

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:59:45 -04:00
308be74330 feat(arch): implement SAUL-TEE ESP32 protocol specs from hal reference doc
Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md (hal, 2026-04-04)

stm32_protocol.py — rewritten for inter-board UART protocol (ESP32 BALANCE ↔ IO):
- Frame: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud (was STX/ETX/CRC16)
- CRC-8 poly 0x07 over LEN+TYPE+PAYLOAD
- New message types: RC_CHANNELS(0x01), SENSORS(0x02), LED_CMD(0x10),
  OUTPUT_CMD(0x11), MOTOR_CMD(0x12), HEARTBEAT(0x20)

mamba_protocol.py — updated CAN IDs and frame formats:
- Orin→BALANCE: DRIVE(0x300) f32×2 LE, MODE(0x301), ESTOP(0x302), LED(0x303)
- BALANCE→Orin: FC_STATUS(0x400) pitch/vbat/state, FC_VESC(0x401) rpm/current
- VESC node IDs: Left=56, Right=68 (authoritative per §8)
- VESC extended frames: STATUS1(cmd=9), STATUS4(cmd=16), STATUS5(cmd=27)
- Replaced old MAMBA_CMD_*/MAMBA_TELEM_* constants

can_bridge_node.py — updated to use new IDs:
- ORIN_CMD_DRIVE/MODE/ESTOP replace MAMBA_CMD_VELOCITY/MODE/ESTOP
- FC_STATUS handler: publishes pitch→/can/imu, vbat_mv→/can/battery
- FC_VESC handler: publishes rpm/cur→/can/vesc/left|right/state
- VESC STATUS1 extended frames decoded per node ID (56/68)
- Removed PID CAN command (not in new spec)

CLAUDE.md — updated with ESP32-S3 BALANCE/IO hardware summary + key protocols

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:59:28 -04:00
19be6bbe11 Merge pull request 'docs: SAUL-TEE full ESP32-S3 system reference (arch migration)' (#712) from sl-firmware/arch-esp32-migration into main 2026-04-04 08:57:11 -04:00
5ef1f7e365 docs: full SAUL-TEE ESP32-S3 spec — pins, CAN, UART, RC mapping
Complete hardware reference from hal@Orin spec (2026-04-04):
- docs/SAUL-TEE-SYSTEM-REFERENCE.md: authoritative pin/protocol/CAN reference
  ESP32-S3 BALANCE: QMI8658 SPI(IO38-42), GC9A01 LCD, SN65HVD230 CAN(IO43/44),
  inter-board UART(IO17/18)
  ESP32-S3 IO: Crossfire UART0(IO43/44), ELRS UART2(IO16/17), BTS7960(IO1-8),
  I2C(IO11/12), WS2812(IO13), buzzer/headlight/fan, arming btn, kill-sw, UART(IO18/21)
- Inter-board binary protocol: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud
- CAN: VESC L=68, R=56; Orin cmds 0x300-0x303; telemetry 0x400-0x401 @ 10Hz
- RC: CH5=ARM, CH6=ESTOP, CH7=speed-limit; CRSF loss >100ms = motors cut
- CLAUDE.md, TEAM.md, docs/AGENTS.md, docs/SALTYLAB.md updated with full spec

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:25:24 -04:00
bfca6d1d92 docs: Add SAUL-TEE system reference + update wiring diagram
- docs/SAUL-TEE-SYSTEM-REFERENCE.md: authoritative architecture doc for
  the new 4-wheel wagon. Covers ESP32-S3 BALANCE (Waveshare LCD 1.28,
  QMI8658, SN65HVD230 CAN), ESP32-S3 IO (TBS Crossfire, ELRS, BTS7960,
  NFC/baro/ToF, WS2812), inter-board UART protocol (460800 baud,
  [0xAA][len][type][payload][crc8]), CAN IDs (VESCs 68/56, Orin
  0x300-0x303 cmd / 0x400-0x401 telemetry), RC channel map, power
  architecture, safety systems, and firmware layout.

- docs/wiring-diagram.md: banner pointing to new reference doc;
  old Mamba F722S UART summary marked OBSOLETE.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:25:24 -04:00
f71dad5344 feat(arch): migrate all STM32/Mamba/BlackPill refs to ESP32 BALANCE/IO + fix roslib@1.4.0
Architecture change (2026-04-03): Mamba F722S (STM32F722) and BlackPill
replaced by ESP32 BALANCE (PID loop) and ESP32 IO (motors/sensors/comms).

- Update CLAUDE.md, docs, chassis BOM/ASSEMBLY, pinout, power-budget,
  wiring-diagram, TEAM.md, AUTONOMOUS_ARMING.md, docker-compose
- Update all ROS2 package comments, config labels, launch args
  (stm32_port→esp32_port, /dev/stm32-bridge→/dev/esp32-bridge)
- Update WebUI: stm32Mode→esp32Mode, stm32Version→esp32Version,
  "STM32 State/Mode" labels → "ESP32 State/Mode" (ControlMode, SettingsPanel)
- Add TODO(esp32-migration) markers on stm32_protocol.py and mamba_protocol.py
  binary frame layouts — pending ESP32 protocol spec from max
- Fix roslib CDN 1.3.0→1.4.0 in all 11 HTML panels (fixes ROS2 Humble
  rosbridge "Received a message without an op" incompatibility)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:25:24 -04:00
5e97676703 docs: Update chassis docs for ESP32 architecture (retire Mamba F722S)
Replace Mamba F722S / STM32F722 references in BOM.md and ASSEMBLY.md
with ESP32 BALANCE + ESP32 IO. Board dimensions marked TBD pending
spec from max.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:25:24 -04:00
30b0f245e1 docs: retire Mamba F722S/BlackPill, adopt ESP32 BALANCE + ESP32 IO architecture
Effective 2026-04-03: STM32F722 flight controller no longer used.
New architecture:
- ESP32 BALANCE: PID balance loop
- ESP32 IO: motors, sensors, comms

Updated: CLAUDE.md, TEAM.md, docs/AGENTS.md, docs/SALTYLAB.md
Legacy src/ STM32 firmware is archived — not extended.
Source code migration pending ESP32 hardware spec from max.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-04 08:25:24 -04:00
103 changed files with 661 additions and 1329 deletions

View File

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

View File

@ -1,31 +1,26 @@
# SaltyLab Firmware — Agent Playbook # SaltyLab Firmware — Agent Playbook
## Project ## Project
**SAUL-TEE** — 4-wheel wagon robot (870×510×550 mm, 23 kg). **SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg).
Two ESP32-S3 boards + Jetson Orin for AI/ROS2. Two ESP32-S3 boards + Jetson Orin via CAN. Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
> **Full hardware spec:** `docs/SAUL-TEE-SYSTEM-REFERENCE.md` | Board | Role |
|-------|------|
| **ESP32-S3 BALANCE** | QMI8658 IMU, PID balance, CAN→VESC (L:68 / R:56), GC9A01 LCD (Waveshare Touch LCD 1.28) |
| **ESP32-S3 IO** | TBS Crossfire RC, ELRS failover, BTS7960 motors, NFC/baro/ToF, WS2812 |
| **Jetson Orin** | AI/SLAM, CANable2 USB→CAN, cmds 0x3000x303, telemetry 0x4000x401 |
### Embedded boards > **Legacy:** `src/` and `include/` = archived STM32 HAL — do not extend. New firmware in `esp32/`.
| Board | Hardware | Role |
|-------|----------|------|
| **ESP32-S3 BALANCE** | Waveshare Touch LCD 1.28 (CH343 USB) | QMI8658 IMU, PID balance loop, CAN→VESCs (SN65HVD230, 500 kbps) |
| **ESP32-S3 IO** | Bare DevKit (JTAG USB) | BTS7960 motors, TBS Crossfire (UART0) + ELRS failover (UART2), NFC/baro/ToF (I2C), WS2812 LEDs, horn/headlight/fan/buzzer |
### Key protocols
- **Orin ↔ BALANCE:** CAN 500 kbps via CANable2 (slcan0). Cmds 0x3000x303, telemetry 0x4000x401
- **BALANCE ↔ IO:** UART 460800 baud, frame `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
- **VESC IDs:** Left = 56, Right = 68
## Team ## Team
| Agent | Role | Focus | | Agent | Role | Focus |
|-------|------|-------| |-------|------|-------|
| **sl-firmware** | Embedded Firmware Lead | ESP32-S3 firmware (PlatformIO), QMI8658 IMU, PID, VESC CAN, inter-board UART | | **sl-firmware** | Embedded Firmware Lead | ESP32-S3, ESP-IDF, QMI8658, CAN/UART protocol, BTS7960 |
| **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems | | **sl-controls** | Control Systems Engineer | PID tuning, IMU fusion, balance loop, safety |
| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 | | **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 |
## Status ## Status
Architecture migrated from Mamba F722S/BlackPill → ESP32-S3 BALANCE + IO (PR #712, 2026-04-04). USB CDC TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).
## Repo Structure ## Repo Structure
- `projects/saltybot/SALTYLAB.md` — Design doc - `projects/saltybot/SALTYLAB.md` — Design doc

19
TEAM.md
View File

@ -1,12 +1,13 @@
# SaltyLab — Ideal Team # SaltyLab — Ideal Team
## Project ## Project
Self-balancing two-wheeled robot using a drone flight controller (STM32F722), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM. **SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg).
Two ESP32-S3 boards (BALANCE + IO) + Jetson Orin. See `docs/SAUL-TEE-SYSTEM-REFERENCE.md`.
## Current Status ## Current Status
- **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand - **Hardware:** ESP32-S3 BALANCE (Waveshare Touch LCD 1.28, CH343 USB) + ESP32-S3 IO (bare devkit, JTAG USB)
- **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB CDC bug - **Firmware:** ESP-IDF/PlatformIO target; legacy `src/` STM32 HAL archived
- **Blocker:** USB CDC TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB OTG FS — see `USB_CDC_BUG.md` - **Comms:** UART 460800 baud inter-board; CANable2 USB→CAN for Orin; CAN 500 kbps to VESCs (L:68 / R:56)
--- ---
@ -14,10 +15,10 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
### 1. Embedded Firmware Engineer (Lead) ### 1. Embedded Firmware Engineer (Lead)
**Must-have:** **Must-have:**
- Deep STM32 HAL experience (F7 series specifically) - Deep ESP32 (Arduino/ESP-IDF) or STM32 HAL experience
- USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts) - USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts)
- SPI + UART + USB coexistence on STM32 - SPI + UART + USB coexistence on ESP32
- PlatformIO or bare-metal STM32 toolchain - PlatformIO or bare-metal ESP32 toolchain
- DFU bootloader implementation - DFU bootloader implementation
**Nice-to-have:** **Nice-to-have:**
@ -25,7 +26,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
- PID control loop tuning for balance robots - PID control loop tuning for balance robots
- FOC motor control (hoverboard ESC protocol) - 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 STM32 USB issues before — ESP32 firmware for the balance loop and I/O needs to be written from scratch.
### 2. Control Systems / Robotics Engineer ### 2. Control Systems / Robotics Engineer
**Must-have:** **Must-have:**
@ -61,7 +62,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
## Hardware Reference ## Hardware Reference
| Component | Details | | Component | Details |
|-----------|---------| |-----------|---------|
| FC | MAMBA F722S (STM32F722RET6, MPU6000) | | FC | ESP32 BALANCE (ESP32RET6, MPU6000) |
| Motors | 2x 8" pneumatic hoverboard hub motors | | Motors | 2x 8" pneumatic hoverboard hub motors |
| ESC | Hoverboard ESC (EFeru FOC firmware) | | ESC | Hoverboard ESC (EFeru FOC firmware) |
| Battery | 36V pack | | Battery | 36V pack |

View File

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

View File

@ -56,10 +56,15 @@
3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m. 3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m.
4. Insert battery pack; route Velcro straps through slots and cinch. 4. Insert battery pack; route Velcro straps through slots and cinch.
### 7 FC mount (MAMBA F722S) ### 7 MCU mount (ESP32 BALANCE + ESP32 IO)
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE retired. Two ESP32 boards replace it.
> Board dimensions and hole patterns TBD — await spec from max before machining mount plate.
1. Place silicone anti-vibration grommets onto nylon M3 standoffs. 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. 2. Lower ESP32 BALANCE board onto standoffs; secure with M3×6 BHCS. Snug only.
3. Orient USB-C port toward front of robot for cable access. 3. Mount ESP32 IO board adjacent — exact placement TBD pending board dimensions.
4. Orient USB connectors toward front of robot for cable access.
### 8 Jetson Nano mount plate ### 8 Jetson Nano mount plate
1. Press or thread M3 nylon standoffs (8mm) into plate holes. 1. Press or thread M3 nylon standoffs (8mm) into plate holes.
@ -86,7 +91,8 @@
| Wheelbase (axle C/L to C/L) | 600 mm | ±1 mm | | Wheelbase (axle C/L to C/L) | 600 mm | ±1 mm |
| Motor fork slot width | 24 mm | +0.5 / 0 | | Motor fork slot width | 24 mm | +0.5 / 0 |
| Motor fork dropout depth | 60 mm | ±0.5 mm | | Motor fork dropout depth | 60 mm | ±0.5 mm |
| FC hole pattern | 30.5 × 30.5 mm | ±0.2 mm | | ESP32 BALANCE hole pattern | TBD — await spec from max | ±0.2 mm |
| ESP32 IO hole pattern | TBD — await spec from max | ±0.2 mm |
| Jetson hole pattern | 58 × 58 mm | ±0.2 mm | | Jetson hole pattern | 58 × 58 mm | ±0.2 mm |
| Battery tray inner | 185 × 72 × 52 mm | +2 / 0 mm | | Battery tray inner | 185 × 72 × 52 mm | +2 / 0 mm |

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"` | | 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"` | | 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 | | 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 | MCU standoff M3×6mm nylon | 4 | Nylon | — | ESP32 BALANCE / IO board isolation (dimensions TBD) |
| 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment | | 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B ### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B
@ -88,12 +88,16 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
## Electronics Mounts ## Electronics Mounts
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE (ESP32) is retired.
> Replaced by **ESP32 BALANCE** + **ESP32 IO**. Board dimensions and hole patterns TBD — await spec from max.
| # | Part | Qty | Spec | Notes | | # | 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 BALANCE board | 1 | TBD — mount pattern TBD | PID balance loop; replaces ESP32 BALANCE |
| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation | | 13b | ESP32 IO board | 1 | TBD — mount pattern TBD | Motor/sensor/comms I/O |
| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads | | 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | ESP32 board isolation |
| 16 | Jetson Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern | | 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under ESP32 mount pads |
| 16 | Jetson Orin module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
| 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs | | 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs |
--- ---
@ -144,8 +148,8 @@ Slide entire carousel up/down the stem with M6 collar bolts loosened. Tighten at
| 26 | M6×60 SHCS | 4 | ISO 4762, SS | Collar clamping bolts | | 26 | M6×60 SHCS | 4 | ISO 4762, SS | Collar clamping bolts |
| 27 | M6 hex nut | 4 | ISO 4032, SS | Captured in collar pockets | | 27 | M6 hex nut | 4 | ISO 4032, SS | Captured in collar pockets |
| 28 | M6×12 set screw | 2 | ISO 4026, SS cup-point | Stem height lock (1 per collar half) | | 28 | M6×12 set screw | 2 | ISO 4026, SS cup-point | Stem height lock (1 per collar half) |
| 29 | M3×10 SHCS | 12 | ISO 4762, SS | FC mount + miscellaneous | | 29 | M3×10 SHCS | 12 | ISO 4762, SS | ESP32 mount + miscellaneous |
| 30 | M3×6 BHCS | 4 | ISO 4762, SS | FC board bolts | | 30 | M3×6 BHCS | 4 | ISO 4762, SS | ESP32 board bolts (qty TBD pending board spec) |
| 31 | Axle lock nut (match axle tip thread) | 4 | Flanged, confirm thread | 2 per motor | | 31 | Axle lock nut (match axle tip thread) | 4 | Flanged, confirm thread | 2 per motor |
| 32 | Flat washer M5 | 32 | SS | | | 32 | Flat washer M5 | 32 | SS | |
| 33 | Flat washer M4 | 32 | SS | | | 33 | Flat washer M4 | 32 | SS | |

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 | | Component | Thermal strategy | Max junction | Enclosure budget |
|-----------|-----------------|-------------|-----------------| |-----------|-----------------|-------------|-----------------|
| Jetson Orin NX | Al pad → lid → fan forced convection | 95 °C Tj | Target ≤ 60 °C case | | 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 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
| ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C | | ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C |
| D435i | Passive; housing vent gap on rear cap | 45 °C surface | — | | D435i | Passive; housing vent gap on rear cap | 45 °C surface | — |

View File

@ -2,22 +2,29 @@
You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read this entire file before touching anything. You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read this entire file before touching anything.
## Project Overview ## ⚠️ ARCHITECTURE — SAUL-TEE (finalised 2026-04-04)
A hoverboard-based balancing robot with two compute layers: Full hardware spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` — **read it before writing firmware.**
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. | Board | Role |
|-------|------|
| **ESP32-S3 BALANCE** | Waveshare Touch LCD 1.28 (CH343 USB). QMI8658 IMU, PID loop, CAN→VESC L(68)/R(56), GC9A01 LCD |
| **ESP32-S3 IO** | Bare devkit (JTAG USB). TBS Crossfire RC (UART0), ELRS failover (UART2), BTS7960 motors, NFC/baro/ToF, WS2812, buzzer/horn/headlight/fan |
| **Jetson Orin** | CANable2 USB→CAN. Cmds on 0x3000x303, telemetry on 0x4000x401 |
``` ```
Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch) Jetson Orin ──CANable2──► CAN 500kbps ◄───────────────────────┐
ESP32-S3 BALANCE ←─UART 460800─► ESP32-S3 IO
MAMBA F722S (MPU6000 IMU, PID balance) (QMI8658, PID loop) (BTS7960, RC, sensors)
CAN 500kbps
▼ UART2 ┌─────────┴──────────┐
Hoverboard ESC (FOC) → 2× 8" hub motors VESC Left (ID 68) VESC Right (ID 56)
``` ```
Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
Legacy `src/` STM32 HAL code is **archived — do not extend.**
## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT
This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, and launch the frame. Every firmware change must preserve these invariants: This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, and launch the frame. Every firmware change must preserve these invariants:
@ -35,7 +42,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
## Repository Layout ## Repository Layout
``` ```
firmware/ # STM32 HAL firmware (PlatformIO) firmware/ # Legacy ESP32/STM32 HAL firmware (PlatformIO, archived)
├── src/ ├── src/
│ ├── main.c # Entry point, clock config, main loop │ ├── main.c # Entry point, clock config, main loop
│ ├── icm42688.c # ICM-42688-P SPI driver (backup IMU — currently broken) │ ├── icm42688.c # ICM-42688-P SPI driver (backup IMU — currently broken)
@ -82,11 +89,11 @@ PLATFORM.md # Hardware platform reference
## Hardware Quick Reference ## Hardware Quick Reference
### MAMBA F722S Flight Controller ### ESP32 BALANCE Flight Controller
| Spec | Value | | Spec | Value |
|------|-------| |------|-------|
| MCU | STM32F722RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) | | MCU | ESP32RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
| Primary IMU | MPU6000 (WHO_AM_I = 0x68) | | Primary IMU | MPU6000 (WHO_AM_I = 0x68) |
| IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 | | IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 |
| IMU EXTI | PC4 (data ready interrupt) | | IMU EXTI | PC4 (data ready interrupt) |
@ -160,7 +167,7 @@ PLATFORM.md # Hardware platform reference
### Critical Lessons Learned (DON'T REPEAT THESE) ### Critical Lessons Learned (DON'T REPEAT THESE)
1. **SysTick_Handler with HAL_IncTick() is MANDATORY** — without it, HAL_Delay() and every HAL timeout hangs forever. This bricked us multiple times. 1. **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. 2. **DCache breaks SPI on ESP32** — 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. 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. 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. 5. **USB CDC needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception.
@ -172,7 +179,7 @@ The firmware supports reboot-to-DFU via USB command:
2. Firmware writes `0xDEADBEEF` to RTC backup register 0 2. Firmware writes `0xDEADBEEF` to RTC backup register 0
3. `NVIC_SystemReset()` — clean hardware reset 3. `NVIC_SystemReset()` — clean hardware reset
4. On boot, `checkForBootloader()` (called after `HAL_Init()`) reads the magic 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` 5. If magic found: clears it, remaps system memory, jumps to ESP32 BALANCE bootloader at `0x1FF00000`
6. Board appears as DFU device, ready for `dfu-util` flash 6. Board appears as DFU device, ready for `dfu-util` flash
### Build & Flash ### Build & Flash

View File

@ -1,6 +1,6 @@
# Face LCD Animation System (Issue #507) # 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 LCD display with 5 core emotions and smooth transitions.
## Features ## Features
@ -82,7 +82,7 @@ STATUS → Echo current emotion + idle state
- Colors: Monochrome (1-bit) or RGB565 - Colors: Monochrome (1-bit) or RGB565
### Microcontroller ### Microcontroller
- STM32F7xx (Mamba F722S) - ESP32xx (ESP32 BALANCE)
- Available UART: USART3 (PB10=TX, PB11=RX) - Available UART: USART3 (PB10=TX, PB11=RX)
- Clock: 216 MHz - Clock: 216 MHz

View File

@ -1,6 +1,6 @@
# SaltyLab — Self-Balancing Indoor Bot 🔬 # SAUL-TEE — Self-Balancing Wagon Robot 🔬
Two-wheeled, self-balancing robot for indoor AI/SLAM experiments. Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
## ⚠️ SAFETY — TOP PRIORITY ## ⚠️ SAFETY — TOP PRIORITY
@ -32,8 +32,10 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
|------|--------| |------|--------|
| 2x 8" pneumatic hub motors (36 PSI) | ✅ Have | | 2x 8" pneumatic hub motors (36 PSI) | ✅ Have |
| 1x hoverboard ESC (FOC firmware) | ✅ Have | | 1x hoverboard ESC (FOC firmware) | ✅ Have |
| 1x Drone FC (STM32F745 + MPU-6000) | ✅ Have — balance brain | | ~~1x Drone FC (ESP3245 + MPU-6000)~~ | ❌ RETIRED — replaced by ESP32 BALANCE |
| 1x Jetson Nano + Noctua fan | ✅ Have | | 1x ESP32 BALANCE (PID loop) | ⬜ TBD — spec from max |
| 1x ESP32 IO (motors/sensors/comms) | ⬜ TBD — spec from max |
| 1x Jetson Orin + Noctua fan | ✅ Have |
| 1x RealSense D435i | ✅ Have | | 1x RealSense D435i | ✅ Have |
| 1x RPLIDAR A1M8 | ✅ Have | | 1x RPLIDAR A1M8 | ✅ Have |
| 1x battery pack (36V) | ✅ Have | | 1x battery pack (36V) | ✅ Have |
@ -50,13 +52,13 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
| 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART | | 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART |
### Drone FC Details — GEPRC GEP-F7 AIO ### Drone FC Details — GEPRC GEP-F7 AIO
- **MCU:** STM32F722RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM) - **MCU:** ESP32RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM)
- **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one! - **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
- **Flash:** 8MB Winbond W25Q64 (blackbox, unused) - **Flash:** 8MB Winbond W25Q64 (blackbox, unused)
- **OSD:** AT7456E (unused) - **OSD:** AT7456E (unused)
- **4-in-1 ESC:** Built into AIO board (unused — we use hoverboard ESC) - **4-in-1 ESC:** Built into AIO board (unused — we use hoverboard ESC)
- **DFU mode:** Hold yellow BOOT button while plugging USB - **DFU mode:** Hold yellow BOOT button while plugging USB
- **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL) - **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL) — LEGACY, see ESP32 BALANCE
- **UART pads (confirmed from silkscreen):** - **UART pads (confirmed from silkscreen):**
- T1/R1 (bottom) → USART1 (PA9/PA10) → Jetson - T1/R1 (bottom) → USART1 (PA9/PA10) → Jetson
- T2/R2 (right top) → USART2 (PA2/PA3) → Hoverboard ESC - T2/R2 (right top) → USART2 (PA2/PA3) → Hoverboard ESC
@ -95,7 +97,7 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
## Self-Balancing Control — Custom Firmware on Drone FC ## Self-Balancing Control — Custom Firmware on Drone FC
### Why a 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 F745 board was a premium STM32 dev board (legacy; now replaced by ESP32 BALANCE) 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).
### Architecture ### Architecture
``` ```
@ -142,7 +144,7 @@ GND ──→ GND
5V ←── 5V 5V ←── 5V
``` ```
### Custom Firmware (STM32 C) ### Custom Firmware (Legacy STM32 C — archived)
```c ```c
// Core balance loop — runs in timer interrupt @ 1-8kHz // Core balance loop — runs in timer interrupt @ 1-8kHz
@ -280,8 +282,8 @@ GND ──→ Common ground
``` ```
### Dev Tools ### Dev Tools
- **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD - **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD (legacy)
- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE - **IDE:** PlatformIO + ESP-IDF (new) or STM32 HAL/STM32CubeIDE (legacy)
- **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug) - **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
## Physical Design ## Physical Design
@ -375,7 +377,7 @@ GND ──→ Common ground
- [ ] Install hardware kill switch inline with 36V battery (NC — press to kill) - [ ] Install hardware kill switch inline with 36V battery (NC — press to kill)
- [ ] Set up ceiling tether point above test area (rated for >15kg) - [ ] Set up ceiling tether point above test area (rated for >15kg)
- [ ] Clear test area: 3m radius, no loose items, shoes on - [ ] Clear test area: 3m radius, no loose items, shoes on
- [ ] Set up PlatformIO project for STM32F745 (STM32 HAL) - [ ] Set up PlatformIO project for ESP32 BALANCE (ESP-IDF)
- [ ] Write MPU-6000 SPI driver (read gyro+accel, complementary filter) - [ ] Write MPU-6000 SPI driver (read gyro+accel, complementary filter)
- [ ] Write PID balance loop with ALL safety checks: - [ ] Write PID balance loop with ALL safety checks:
- ±25° tilt cutoff → disarm, require manual re-arm - ±25° tilt cutoff → disarm, require manual re-arm

View File

@ -1,444 +1,222 @@
# SAUL-TEE System Reference # SAUL-TEE System Reference — SaltyLab ESP32 Architecture
**Rev A — 2026-04-04** *Authoritative source of truth for hardware, pins, protocols, and CAN assignments.*
_Authoritative architecture reference for all agents. Source: hal (Orin), max._ *Spec from hal@Orin, 2026-04-04.*
--- ---
## 1. Robot Overview ## Overview
| Parameter | Value | | Board | Role | MCU | USB chip |
|-----------|-------| |-------|------|-----|----------|
| **Name** | SAUL-TEE | | **ESP32-S3 BALANCE** | PID balance loop, CAN→VESCs, LCD display | ESP32-S3 | CH343 USB-serial |
| **Config** | 4-wheel wagon (replaces 2-wheel self-balancing bot) | | **ESP32-S3 IO** | RC input, motor drivers, sensors, LEDs, peripherals | ESP32-S3 | JTAG USB (native) |
| **Dimensions** | 870 × 510 × 550 mm (L × W × H) |
| **Mass** | ~23 kg |
| **Drive** | 4× hub motors via 2× VESC 6.7 (dual channel each) |
| **Power** | 36V battery bus |
| **AI brain** | Jetson Orin Nano Super (25W) |
| **CAN bus** | 500 kbps, CANable 2.0 USB↔CAN on Orin |
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32F722 / BlackPill are retired. **Robot form factor:** 4-wheel wagon — 870 × 510 × 550 mm, ~23 kg
> New embedded stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** (see §23 below). **Power:** 36 V LiPo, DC-DC → 5 V and 12 V rails
**Orin connection:** CANable2 USB → 500 kbps CAN (same bus as VESCs)
--- ---
## 2. ESP32-S3 BALANCE Board ## ESP32-S3 BALANCE
### Hardware ### Board
| Item | Detail | Waveshare ESP32-S3 Touch LCD 1.28
|------|--------| - GC9A01 round 240×240 LCD
| **Module** | Waveshare ESP32-S3 Touch LCD 1.28 | - CST816S capacitive touch
| **MCU** | ESP32-S3 (dual-core 240 MHz, 512KB SRAM, 8MB flash, 8MB PSRAM) | - QMI8658 6-axis IMU (accel + gyro, SPI)
| **USB** | CH343G USB-UART bridge (UART0 / GPIO43 TX, GPIO44 RX) | - CH343 USB-to-serial chip
| **Display** | 1.28" round GC9A01 240×240 LCD (SPI) |
| **Touch** | CST816S capacitive touch (I2C) |
| **IMU** | QMI8658 6-axis (gyro + accel), I2C on-board |
| **CAN transceiver** | SN65HVD230, external, wired to ESP32-S3 TWAI peripheral |
### Role ### Pin Assignments
- Runs the **PID balance / drive loop** (or stability assist for wagon mode)
- Reads QMI8658 IMU at high rate for tilt / attitude
- Sends drive commands to VESCs over **500 kbps CAN** (VESC native protocol)
- Receives high-level velocity commands from Orin over CAN (0x3000x303)
- Publishes telemetry to Orin over CAN (0x4000x401)
- Bridges ESP32-IO sensor data over **inter-board UART** @ 460800 baud
### On-Board Pin Map (fixed by Waveshare PCB) | Function | GPIO | Notes |
|----------|------|-------|
| **QMI8658 IMU (SPI)** | | |
| SCK | IO39 | |
| MOSI | IO38 | |
| MISO | IO40 | |
| CS | IO41 | |
| INT1 | IO42 | data-ready interrupt |
| **GC9A01 LCD (shares SPI bus)** | | |
| CS | IO12 | |
| DC | IO11 | |
| RST | IO10 | |
| BL | IO9 | PWM backlight |
| **CST816S Touch (I2C)** | | |
| SDA | IO4 | |
| SCL | IO5 | |
| INT | IO6 | |
| RST | IO7 | |
| **CAN — SN65HVD230 transceiver** | | 500 kbps |
| TX | IO43 | → SN65HVD230 TXD |
| RX | IO44 | ← SN65HVD230 RXD |
| **Inter-board UART (to IO board)** | | 460800 baud |
| TX | IO17 | |
| RX | IO18 | |
| Signal | GPIO | Notes | ### Responsibilities
|--------|------|-------| - Read QMI8658 @ 1 kHz (SPI, INT1-driven)
| LCD SCLK | 10 | GC9A01 SPI clock | - Complementary filter → pitch angle
| LCD MOSI | 11 | GC9A01 SPI data | - PID balance loop (configurable Kp / Ki / Kd)
| LCD CS | 9 | GC9A01 chip select | - Send VESC speed commands via CAN (ID 68 = left, ID 56 = right)
| LCD DC | 8 | GC9A01 data/cmd | - Receive Orin velocity+mode commands via CAN (0x3000x303)
| LCD BL | 2 | Backlight PWM | - Receive IO board status (arming, RC, faults) via UART protocol
| Touch SDA | 6 | CST816S / I2C-0 | - Drive GC9A01 LCD: pitch, speed, battery %, error state
| Touch SCL | 7 | CST816S / I2C-0 | - Enforce tilt cutoff at ±25°; IWDG 50 ms timeout
| Touch INT | 5 | Active-low interrupt | - Publish telemetry on CAN 0x4000x401 at 10 Hz
| Touch RST | 4 | Active-low reset |
| IMU SDA | 6 | QMI8658 shares I2C-0 |
| IMU SCL | 7 | QMI8658 shares I2C-0 |
| IMU INT1 | 3 | Data-ready interrupt |
| CH343 USB TX | 43 | UART0 TX (USB serial) |
| CH343 USB RX | 44 | UART0 RX (USB serial) |
### External Wiring (user-soldered, confirm with calipers/multimeter)
| Signal | GPIO | Notes |
|--------|------|-------|
| CAN TX | **TBD** | → SN65HVD230 D pin; use free header GPIO |
| CAN RX | **TBD** | ← SN65HVD230 R pin; use free header GPIO |
| Inter-board UART TX | **TBD** | → ESP32-IO UART RX @ 460800 |
| Inter-board UART RX | **TBD** | ← ESP32-IO UART TX @ 460800 |
> ⚠️ **Verify GPIO assignments in `esp32/balance/src/config.h` before writing firmware.**
### QMI8658 Details
| Item | Value |
|------|-------|
| I2C address | 0x6A (SA0 pin low) or 0x6B (SA0 high) |
| Gyro full-scale | ±2048 °/s (configurable ±16/32/64/128/256/512/1024/2048) |
| Accel full-scale | ±16 g (configurable ±2/4/8/16) |
| Output data rate | Up to 7174.4 Hz |
| Interface | I2C or SPI (board routes I2C) |
--- ---
## 3. ESP32-S3 IO Board ## ESP32-S3 IO
### Hardware ### Board
| Item | Detail | Bare ESP32-S3 devkit (JTAG USB)
|------|--------|
| **Module** | ESP32-S3 bare dev board (JTAG USB) |
| **MCU** | ESP32-S3 (dual-core 240 MHz) |
| **USB** | Built-in USB-JTAG/Serial (no external bridge) |
### Role ### Pin Assignments
- All **physical I/O**: motors, RC, sensors, indicators, accessories
- TBS Crossfire primary RC (UART0)
- ELRS failover RC (UART2)
- BTS7960 half-bridge motor drivers (4-wheel PWM drive)
- NFC, barometer, ToF range sensors (I2C)
- WS2812B LED strip
- Horn, headlight, cooling fan, buzzer
### Pin Map | Function | GPIO | Notes |
|----------|------|-------|
| **TBS Crossfire RC — UART0 (primary)** | | |
| RX | IO44 | CRSF frames from Crossfire RX |
| TX | IO43 | telemetry to Crossfire TX |
| **ELRS failover — UART2** | | active if CRSF absent >100 ms |
| RX | IO16 | |
| TX | IO17 | |
| **BTS7960 Motor Driver — Left** | | |
| RPWM | IO1 | forward PWM |
| LPWM | IO2 | reverse PWM |
| R_EN | IO3 | right enable |
| L_EN | IO4 | left enable |
| **BTS7960 Motor Driver — Right** | | |
| RPWM | IO5 | |
| LPWM | IO6 | |
| R_EN | IO7 | |
| L_EN | IO8 | |
| **I2C bus** | | |
| SDA | IO11 | |
| SCL | IO12 | |
| NFC (PN532 or similar) | I2C | |
| Barometer (BMP280/BMP388) | I2C | |
| ToF (VL53L0X/VL53L1X) | I2C | |
| **WS2812B LEDs** | | |
| Data | IO13 | |
| **Outputs** | | |
| Horn / buzzer | IO14 | PWM tone |
| Headlight | IO15 | PWM or digital |
| Fan | IO16 | (if ELRS not fitted on UART2) |
| **Inputs** | | |
| Arming button | IO9 | active-low, hold 3 s to arm |
| Kill switch sense | IO10 | hardware estop detect |
| **Inter-board UART (to BALANCE board)** | | 460800 baud |
| TX | IO18 | |
| RX | IO21 | |
| Signal | GPIO | Protocol | Notes | ### Responsibilities
|--------|------|----------|-------| - Parse CRSF frames (TBS Crossfire, primary)
| **TBS Crossfire RX** UART TX | 43 | CRSF @ 420000 baud | Telemetry out to TBS TX module | - Parse ELRS frames (failover, activates if no CRSF for >100 ms)
| **TBS Crossfire RX** UART RX | 44 | CRSF @ 420000 baud | UART0 — RC frames in | - Drive BTS7960 left/right PWM motor drivers
| **ELRS failover** UART TX | **TBD** | CRSF @ 420000 baud | UART2 | - Read NFC, barometer, ToF via I2C
| **ELRS failover** UART RX | **TBD** | CRSF @ 420000 baud | UART2 | - Drive WS2812B LEDs (armed/fault/idle patterns)
| **Inter-board UART TX** | **TBD** | Binary @ 460800 | → BALANCE UART RX | - Control horn, headlight, fan, buzzer
| **Inter-board UART RX** | **TBD** | Binary @ 460800 | ← BALANCE UART TX | - Manage arming: hold button 3 s while upright → send ARM to BALANCE
| **BTS7960 — FL** RPWM | **TBD** | PWM | Front-left forward | - Monitor kill switch input → immediate motor off + FAULT frame
| **BTS7960 — FL** LPWM | **TBD** | PWM | Front-left reverse | - Forward RC + sensor data to BALANCE via binary UART protocol
| **BTS7960 — FL** R_EN | **TBD** | GPIO | Enable H | - Report faults and RC-loss upstream
| **BTS7960 — FL** L_EN | **TBD** | GPIO | Enable H |
| **BTS7960 — FR** RPWM | **TBD** | PWM | Front-right forward |
| **BTS7960 — FR** LPWM | **TBD** | PWM | Front-right reverse |
| **BTS7960 — FR** R_EN | **TBD** | GPIO | Enable H |
| **BTS7960 — FR** L_EN | **TBD** | GPIO | Enable H |
| **BTS7960 — RL** RPWM | **TBD** | PWM | Rear-left forward |
| **BTS7960 — RL** LPWM | **TBD** | PWM | Rear-left reverse |
| **BTS7960 — RL** R_EN | **TBD** | GPIO | Enable H |
| **BTS7960 — RL** L_EN | **TBD** | GPIO | Enable H |
| **BTS7960 — RR** RPWM | **TBD** | PWM | Rear-right forward |
| **BTS7960 — RR** LPWM | **TBD** | PWM | Rear-right reverse |
| **BTS7960 — RR** R_EN | **TBD** | GPIO | Enable H |
| **BTS7960 — RR** L_EN | **TBD** | GPIO | Enable H |
| **I2C SDA** | **TBD** | I2C | NFC + baro + ToF shared bus |
| **I2C SCL** | **TBD** | I2C | NFC + baro + ToF shared bus |
| **WS2812B data** | **TBD** | RMT | LED strip |
| **Horn** | **TBD** | GPIO/PWM | MOSFET or relay |
| **Headlight** | **TBD** | GPIO/PWM | MOSFET |
| **Fan** | **TBD** | PWM | ESC/electronics cooling |
| **Buzzer** | **TBD** | GPIO/PWM | Piezo or active buzzer |
> ⚠️ **TBD pin assignments** — to be confirmed in `esp32/io/src/config.h` once wiring is set.
### I2C Peripherals
| Device | Address | Function |
|--------|---------|----------|
| NFC module | 0x24 (PN532) or 0x28 | NFC tag read/write |
| Barometer | 0x76 (BMP280/BMP388) | Altitude + temp |
| ToF range | 0x29 (VL53L0X) or 0x52 (VL53L4CD) | Proximity/obstacle |
--- ---
## 4. Inter-Board UART Protocol (BALANCE ↔ IO) ## Inter-Board Binary Protocol (UART @ 460800 baud)
### Link Parameters
| Parameter | Value |
|-----------|-------|
| Baud rate | 460800 |
| Format | 8N1 |
| Direction | Full-duplex |
### Frame Format
``` ```
Byte 0: 0xAA (start-of-frame magic) [0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8]
Byte 1: LEN (payload length in bytes, uint8)
Byte 2: TYPE (message type, uint8)
Byte 3…N: PAYLOAD (LEN bytes)
Byte N+1: CRC8 (CRC-8/MAXIM over bytes 1..N)
``` ```
- `0xAA` — start byte
- `LEN` — payload length in bytes (uint8)
- `TYPE` — message type (uint8)
- `CRC8` — CRC-8/MAXIM over TYPE + PAYLOAD bytes
### Message Types (draft — confirm in firmware) ### IO → BALANCE Messages
| Type | Direction | Payload | Description | | TYPE | Name | Payload | Description |
|------|-----------|---------|-------------| |------|------|---------|-------------|
| 0x01 | IO → BAL | `[ch1:u16][ch2:u16]…[ch16:u16][flags:u8]` | RC channels (16× uint16, 10002000 µs) + status flags | | 0x01 | RC_CMD | int16 throttle, int16 steer, uint8 flags | flags: bit0=armed, bit1=kill |
| 0x02 | IO → BAL | `[sensor_id:u8][value:f32]` | Sensor reading (ToF, baro, etc.) | | 0x02 | SENSOR | uint16 tof_mm, int16 baro_delta_pa, uint8 nfc_present | |
| 0x03 | IO → BAL | `[nfc_uid:u8×7][len:u8]` | NFC tag detected | | 0x03 | FAULT | uint8 fault_flags | bit0=rc_loss, bit1=motor_fault, bit2=estop |
| 0x10 | BAL → IO | `[leds:u8][horn:u8][light:u8][fan:u8][buzz:u8]` | Actuator commands |
| 0x11 | BAL → IO | `[state:u8][pitch:f32][speed:f32]` | Status for LED animations |
| 0xFF | both | `[uptime_ms:u32]` | Heartbeat (watchdog reset) |
> Frame types are **draft** — refer to `esp32/shared/protocol.h` for authoritative definitions. ### BALANCE → IO Messages
### CRC-8 Polynomial | TYPE | Name | Payload | Description |
``` |------|------|---------|-------------|
Poly: 0x31 (CRC-8/MAXIM, also called CRC-8/1-Wire) | 0x10 | STATE | int16 pitch_x100, int16 pid_out, uint8 error_state | |
Init: 0x00 | 0x11 | LED_CMD | uint8 pattern, uint8 r, uint8 g, uint8 b | |
RefIn/RefOut: true | 0x12 | BUZZER | uint8 tone_id, uint16 duration_ms | |
XorOut: 0x00
```
--- ---
## 5. CAN Bus ## CAN Bus — 500 kbps
### Bus Parameters ### Node Assignments
| Parameter | Value |
|-----------|-------|
| Bit rate | 500 kbps |
| Topology | Single bus; all nodes share CANH/CANL + GND |
| Termination | 120 Ω at each end of the bus |
| Orin interface | CANable 2.0 USB↔CAN → `/dev/canable0` (or `can0` after `ip link`) |
### Node Addresses | Node | CAN ID | Role |
|------|--------|------|
| VESC Left motor | **68** | Receives speed/duty via VESC CAN protocol |
| VESC Right motor | **56** | Receives speed/duty via VESC CAN protocol |
| ESP32-S3 BALANCE | — | Sends VESC commands; publishes telemetry |
| Jetson Orin (CANable2) | — | Sends velocity commands; receives telemetry |
| Node | CAN ID / Role | ### Frame Table
|------|--------------|
| VESC left motor | ID **68** (0x44) — FSESC 6.7 Pro Mini Dual, left channel |
| VESC right motor | ID **56** (0x38) — FSESC 6.7 Pro Mini Dual, right channel |
| ESP32-S3 BALANCE | transmits telemetry 0x4000x401; receives Orin cmds 0x3000x303 |
| Orin (CANable2) | transmits cmds 0x3000x303; receives telemetry 0x4000x401 |
### Orin → Robot Command Frames (0x3000x303) | CAN ID | Direction | Description | Rate |
|--------|-----------|-------------|------|
| CAN ID | DLC | Payload | Description | | 0x300 | Orin → BALANCE | Velocity cmd: int16 speed_mmps, int16 steer_mrad | 20 Hz |
|--------|-----|---------|-------------| | 0x301 | Orin → BALANCE | PID tuning: float Kp, float Ki, float Kd (3×4B IEEE-754) | on demand |
| **0x300** | 8 | `[speed:i16][steer:i16][mode:u8][flags:u8][_:u16]` | Drive command (speed/steer ±1000, mode byte) | | 0x302 | Orin → BALANCE | Mode: uint8 (0=off, 1=balance, 2=manual, 3=estop) | on demand |
| **0x301** | 1 | `[arm:u8]` | Arm/disarm (0x01 = arm, 0x00 = disarm) | | 0x303 | Orin → BALANCE | Config: uint16 tilt_limit_x100, uint16 max_speed_mmps | on demand |
| **0x302** | 8 | `[kp:f16][ki:f16][kd:f16][_:u16]` | PID update (half-float) | | 0x400 | BALANCE → Orin | Telemetry A: int16 pitch_x100, int16 pid_out, int16 speed_mmps, uint8 state | 10 Hz |
| **0x303** | 1 | `[0xE5]` | Emergency stop (magic byte, cuts all motors) | | 0x401 | BALANCE → Orin | Telemetry B: int16 vesc_l_rpm, int16 vesc_r_rpm, uint16 battery_mv, uint8 faults | 10 Hz |
### Robot → Orin Telemetry Frames (0x4000x401)
| CAN ID | DLC | Payload | Description |
|--------|-----|---------|-------------|
| **0x400** | 8 | `[pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8]` | Attitude + drive state |
| **0x401** | 4 | `[vbat:u16][fault_code:u8][rssi:i8]` | Battery voltage (mV), fault, RC RSSI |
### VESC Native CAN (standard VESC protocol)
ESP32-S3 BALANCE sends VESC commands using the standard VESC CAN protocol:
| Frame type | CAN ID formula | Notes |
|------------|---------------|-------|
| SET_DUTY | `(0x00 << 8) | VESC_ID` | Duty cycle 1.0..+1.0 × 100000 |
| SET_CURRENT | `(0x01 << 8) | VESC_ID` | Current in mA |
| SET_RPM | `(0x03 << 8) | VESC_ID` | Electrical RPM |
| SET_CURRENT_BRAKE | `(0x02 << 8) | VESC_ID` | Braking current in mA |
| STATUS_1 | `(0x09 << 8) | VESC_ID` | ERPMs + current + duty (rx) |
| STATUS_4 | `(0x10 << 8) | VESC_ID` | Temp + input voltage + input current (rx) |
--- ---
## 6. RC Channel Mapping ## RC Channel Mapping (TBS Crossfire / ELRS CRSF)
### Primary: TBS Crossfire (UART0 on ESP32-IO, CRSF @ 420000 baud) | CH | Function | Range (µs) | Notes |
|----|----------|------------|-------|
| 1 | Steer (Roll) | 9882012 | ±100% → ±max steer |
| 2 | Throttle (Pitch) | 9882012 | forward / back speed |
| 3 | Spare | 9882012 | |
| 4 | Spare | 9882012 | |
| 5 | ARM switch | <1500=disarm, >1500=arm | SB on TX |
| 6 | **ESTOP** | <1500=normal, >1500=kill | SC on TX — checked first every loop |
| 7 | Speed limit | 9882012 | maps to 10100% speed cap |
| 8 | Spare | | |
| Channel | Function | Range | Notes | **RC loss:** No valid CRSF frame >100 ms → IO sends FAULT(rc_loss) → BALANCE cuts motors.
|---------|----------|-------|-------|
| CH1 | Roll / Steer | 10002000 µs | Left stick X (mode 2) |
| CH2 | Pitch / Speed | 10002000 µs | Left stick Y |
| CH3 | Throttle | 10002000 µs | Right stick Y |
| CH4 | Yaw | 10002000 µs | Right stick X |
| CH5 | Arm | 1000 / 2000 µs | 2-pos switch — <1200=DISARM, >1800=ARM |
| CH6 | Drive mode | 1000/1500/2000 µs | 3-pos: RC / Assisted / Autonomous |
| CH7 | Speed limit | 10002000 µs | Analog knob, scales max speed |
| CH8 | Aux / Horn | 1000 / 2000 µs | Momentary for horn; held = klaxon |
### Failover: ELRS (UART2 on ESP32-IO, CRSF @ 420000 baud)
Same channel mapping as TBS Crossfire. IO board switches to ELRS automatically if Crossfire link lost for > 300 ms.
### Failsafe
- **RC lost > 300 ms**: motors stop, DISARM state, wait for link restoration
- **Arm switch** must be cycled to DISARM→ARM to re-arm after failsafe
--- ---
## 7. Serial Commands (Orin → ESP32-S3 BALANCE via CAN 0x300) ## Safety Invariants
All high-level commands from the Orin go over CAN. The BALANCE board also accepts direct USB-serial commands on its CH343 port for bench debugging: 1. **Motors NEVER spin on power-on** — 3 s button hold required while upright
2. **Tilt cutoff ±25°** — immediate motor zero, manual re-arm required
3. **IWDG 50 ms** — firmware hang → motors cut
4. **ESTOP RC channel** checked first in every loop iteration
5. **Orin CAN timeout 500 ms** → revert to RC-only mode
6. **Speed hard cap** — start at 10%, increase in 10% increments only after stable tethered testing
7. **Never untethered** until stable for 5+ continuous minutes tethered
---
## USB Debug Commands (both boards, serial console)
``` ```
# Arm / disarm help list commands
ARM status print pitch, PID state, CAN stats, UART stats
DISARM pid <Kp> <Ki> <Kd> set PID gains
arm arm (if upright and safe)
# Set drive (speed -1000..1000, steer -1000..1000) disarm disarm immediately
DRIVE <speed> <steer> estop emergency stop (requires re-arm)
tilt_limit <deg> set tilt cutoff angle (default 25)
# Set mode speed_limit <pct> set speed cap percentage (default 10)
MODE RC # full RC passthrough can_stats CAN bus counters (tx/rx/errors/busoff)
MODE ASSISTED # Orin sends velocity, BALANCE does stability uart_stats inter-board UART frame counters
MODE AUTO # full autonomous (Orin CAN commands only) reboot soft reboot
# PID update
PID <kp> <ki> <kd>
# Emergency stop (same as CAN 0x303)
ESTOP
# Status query
STATUS # returns JSON telemetry line
# IMU calibration
IMU CAL
``` ```
---
## 8. System Wiring Diagram
```
┌──────────────────────────────────────────────────────────┐
│ JETSON ORIN NANO SUPER │
│ (Top plate, 25W) │
│ │
│ USB-A ─── CANable 2.0 USB↔CAN (can0, 500 kbps) │
│ USB-A ─── RealSense D435i (USB 3.1) │
│ USB-A ─── RPLIDAR A1M8 (CP2102, 115200) │
│ USB-C ─── SIM7600A 4G/LTE modem │
│ CSI-A ─── 2× IMX219 cameras │
│ CSI-B ─── 2× IMX219 cameras │
│ 40-pin ── ReSpeaker 2-Mic HAT │
└──────────────────────┬───────────────────────────────────┘
│ USB-A
│ CANable 2.0
│ 500 kbps CAN
┌────────────────────────────────┴──────────────────────────────────┐
│ CAN BUS (CANH / CANL) │
│ 120Ω ───┤ (all nodes) ├─── 120Ω │
└───┬──────────────────────────┬───────────────────┬────────────────┘
│ │ │
▼ ▼ ▼
┌─────────────────────┐ ┌─────────────────────┐ (CAN ID 56/68)
│ ESP32-S3 BALANCE │ │ VESC left (ID 68)│ VESC right (ID 56)
│ Waveshare LCD 1.28 │ │ FSESC 6.7 Pro Mini│
│ │ │ Dual │
│ QMI8658 IMU (I2C) │ └────────┬────────────┘
│ SN65HVD230 (CAN) │ │ Phase wires
│ CH343 USB (debug) │ ┌─────┴────────────────┐
│ │ │ Hub motors (4×) │
│ UART ─────────────────┐ │ FL / FR / RL / RR │
└─────────────────────┘ │ └──────────────────────┘
↑ 460800 baud binary │
↓ inter-board protocol │
┌─────────────────────────┘
│ ESP32-S3 IO (bare board)
│ JTAG USB (debug)
│ UART0 ── TBS Crossfire RX (CRSF, 420000)
│ UART2 ── ELRS receiver (CRSF failover, 420000)
│ PWM ──── 4× BTS7960 motor drivers
│ I2C ──── NFC + Baro + ToF sensors
│ GPIO ─── WS2812B LEDs
│ GPIO ─── Horn / Headlight / Fan / Buzzer
└─────────────────────────────────────────────
```
---
## 9. Power Architecture
```
36V BATTERY
├── VESCs (36V direct) ──── 4× Hub motors
├── BTS7960 boards (36V → motor logic 5V via onboard reg)
├── DC-DC 12V ──── Fan / Headlight / Accessories
└── DC-DC 5V ─┬── Jetson Orin (USB-C PD or barrel)
├── ESP32-S3 BALANCE (USB 5V or VIN)
├── ESP32-S3 IO (USB 5V or VIN)
├── TBS Crossfire RX (5V)
├── ELRS RX (5V)
├── WS2812B strip (5V)
├── RPLIDAR A1M8 (5V via USB)
└── Sensors / NFC / ToF / Baro (3.3V LDO from ESP32)
```
---
## 10. Device Nodes (Orin)
| Device | Node | Notes |
|--------|------|-------|
| CANable 2.0 | `can0` | `ip link set can0 up type can bitrate 500000` |
| RealSense D435i | `/dev/bus/usb/...` | realsense-ros driver |
| RPLIDAR A1M8 | `/dev/rplidar` | udev symlink from ttyUSB* |
| SIM7600A 4G | `/dev/ttyUSB02` | AT, PPP, GNSS NMEA |
| IMX219 cameras | `/dev/video0,2,4,6` | CSI-A: 0/2, CSI-B: 4/6 |
| ESP32-S3 BAL debug | `/dev/ttyACM0` or `/dev/esp32-balance` | CH343 CDC |
| ESP32-S3 IO debug | `/dev/ttyACM1` or `/dev/esp32-io` | JTAG/CDC |
---
## 11. Safety Systems
| System | Trigger | Action |
|--------|---------|--------|
| **HW kill switch** | Big red button (NC inline with 36V) | Cuts all power instantly |
| **Tilt cutoff** | `|pitch| > 25°` | Motors off → DISARM, manual re-arm required |
| **RC failsafe** | No RC frame > 300 ms | Motors off → DISARM |
| **CAN watchdog** | No Orin heartbeat > 500 ms | Drop to RC-only mode |
| **ESTOP CAN** | CAN frame 0x303 (magic 0xE5) | Immediate motor off, DISARM |
| **ESTOP inter-board** | Type 0xFF heartbeat missing > 200 ms | IO board stops BTS7960 enables |
| **Startup arming** | Cold power-on | Motors NEVER spin; need deliberate ARM |
---
## 12. Firmware Repository Layout
```
esp32/
├── balance/ — ESP32-S3 BALANCE firmware (PlatformIO)
│ ├── src/
│ │ ├── main.cpp
│ │ ├── config.h ← GPIO assignments live here
│ │ ├── imu_qmi8658.cpp/.h
│ │ ├── can_vesc.cpp/.h
│ │ └── protocol.cpp/.h
│ └── platformio.ini
├── io/ — ESP32-S3 IO firmware (PlatformIO)
│ ├── src/
│ │ ├── main.cpp
│ │ ├── config.h ← GPIO assignments live here
│ │ ├── rc_crsf.cpp/.h
│ │ ├── motor_bts7960.cpp/.h
│ │ └── protocol.cpp/.h
│ └── platformio.ini
└── shared/
└── protocol.h — inter-board frame types (authoritative)
src/ — LEGACY STM32 firmware (ARCHIVED — do not extend)
include/ — LEGACY STM32 headers (ARCHIVED — do not extend)
```
---
## 13. Open Questions / TBDs
| Item | Owner | Status |
|------|-------|--------|
| GPIO assignments for CAN TX/RX on BALANCE board | sl-firmware | **TBD** |
| GPIO assignments for IO board (all external pins) | sl-firmware | **TBD** |
| Inter-board protocol message type table (finalized) | sl-firmware | **TBD** |
| BTS7960 wiring — 4WD vs 2WD mode (all 4 or front 2 only) | max | **TBD** |
| UWB ESP-NOW receiver — BALANCE or IO board? | sl-uwb + max | **TBD** |
| VESC CAN IDs confirmed (68/56 left/right) | max | ✅ Confirmed |
| Robot dimensions 870×510×550 mm, 23 kg | max | ✅ Confirmed |
---
_Last updated: 2026-04-04. Contact max or hal on MQTT for corrections._

View File

@ -2,7 +2,7 @@
<html> <html>
<head> <head>
<meta charset="utf-8"> <meta charset="utf-8">
<title>GEPRC GEP-F722-45A AIO — Board Layout</title> <title>GEPRC GEP-F722-45A AIO — Board Layout (Legacy / Archived)</title>
<style> <style>
* { margin: 0; padding: 0; box-sizing: border-box; } * { margin: 0; padding: 0; box-sizing: border-box; }
body { background: #1a1a2e; color: #eee; font-family: 'Courier New', monospace; display: flex; flex-direction: column; align-items: center; padding: 20px; } body { background: #1a1a2e; color: #eee; font-family: 'Courier New', monospace; display: flex; flex-direction: column; align-items: center; padding: 20px; }
@ -112,8 +112,8 @@ h1 { color: #e94560; margin-bottom: 5px; font-size: 1.4em; }
</style> </style>
</head> </head>
<body> <body>
<h1>🤖 GEPRC GEP-F722-45A AIO — SaltyLab Pinout</h1> <h1>🤖 GEPRC GEP-F722-45A AIO — SaltyLab Pinout (Legacy / Archived)</h1>
<p class="subtitle">STM32F722RET6 + ICM-42688-P | Betaflight target: GEPR-GEPRC_F722_AIO</p> <p class="subtitle">ESP32RET6 + ICM-42688-P | Betaflight target: GEPR-GEPRC_F722_AIO</p>
<div class="container"> <div class="container">
<div class="board-wrap"> <div class="board-wrap">
@ -125,7 +125,7 @@ h1 { color: #e94560; margin-bottom: 5px; font-size: 1.4em; }
<div class="mount br"></div> <div class="mount br"></div>
<!-- MCU --> <!-- MCU -->
<div class="mcu"><div class="dot"></div>STM32<br>F722RET6<br>216MHz</div> <div class="mcu"><div class="dot"></div>ESP32<br>(legacy:<br>F722RET6)</div>
<!-- IMU --> <!-- IMU -->
<div class="imu">ICM<br>42688</div> <div class="imu">ICM<br>42688</div>

View File

@ -14,125 +14,123 @@
│ ORIN NANO SUPER │ │ ORIN NANO SUPER │
│ (Top Plate — 25W) │ │ (Top Plate — 25W) │
│ │ │ │
│ USB-C ──── STM32 CDC (/dev/stm32-bridge, 921600 baud) │ │ USB-A ──── CANable2 USB-CAN adapter (slcan0, 500 kbps) │
│ USB-A ──── ESP32-S3 IO (/dev/esp32-io, 460800 baud) │
│ USB-A1 ─── RealSense D435i (USB 3.1) │ │ USB-A1 ─── RealSense D435i (USB 3.1) │
│ USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │ │ USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │
│ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │ │ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │
│ USB ─────── Leap Motion Controller (hand/gesture tracking) │ │ USB ─────── Leap Motion Controller (hand/gesture tracking) │
│ CSI-A ──── ArduCam adapter → 2× IMX219 (front + left) │ │ CSI-A ──── ArduCam adapter → 2x IMX219 (front + left) │
│ CSI-B ──── ArduCam adapter → 2× IMX219 (rear + right) │ │ CSI-B ──── ArduCam adapter → 2x IMX219 (rear + right) │
│ M.2 ───── 1TB NVMe SSD │ │ M.2 ───── 1TB NVMe SSD │
│ 40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec) │ │ 40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec) │
│ Pin 8 ──┐ │ │ Pin 8 ──┐ │
│ Pin 10 ─┤ UART fallback to FC (ttyTHS0, 921600) │ Pin 10 ─┤ UART fallback to ESP32-S3 BALANCE (ttyTHS0, 460800)
│ Pin 6 ──┘ GND │ │ Pin 6 ──┘ GND │
│ │ │ │
└─────────────────────────────────────────────────────────────────────┘ └─────────────────────────────────────────────────────────────────────┘
│ USB-C (data only) │ UART fallback (3 wires) │ USB-A (CANable2) │ UART fallback (3 wires)
│ 921600 baud │ 921600 baud, 3.3V │ SocketCAN slcan0 │ 460800 baud, 3.3V
│ 500 kbps │
▼ ▼ ▼ ▼
┌─────────────────────────────────────────────────────────────────────┐ ┌─────────────────────────────────────────────────────────────────────┐
MAMBA F722S (FC) ESP32-S3 BALANCE
(Middle Plate — foam mounted) (Waveshare Touch LCD 1.28, Middle Plate)
│ │ │ │
│ USB-C ──── Orin (CDC serial, primary link) │ │ CAN bus ──── CANable2 → Orin (primary link, ISO 11898) │
│ │ │ UART0 ──── Orin UART fallback (460800 baud, 3.3V) │
│ USART2 (PA2=TX, PA3=RX) ──── Hoverboard ESC (26400 baud) │ │ UART1 ──── VESC Left (CAN ID 56) via UART/CAN bridge │
│ UART4 (PA0=TX, PA1=RX) ──── ELRS RX (CRSF, 420000 baud) │ │ UART2 ──── VESC Right (CAN ID 68) via UART/CAN bridge │
│ USART6 (PC6=TX, PC7=RX) ──── Orin UART fallback │ │ I2C ──── QMI8658 IMU (onboard, 6-DOF accel+gyro) │
│ UART5 (PC12=TX, PD2=RX) ─── Debug (optional) │ │ SPI ──── GC9A01 LCD (onboard, 240x240 round display) │
│ │ │ GPIO ──── WS2812B LED strip │
│ SPI1 ─── MPU6000 IMU (on-board, CW270) │ │ GPIO ──── Buzzer │
│ I2C1 ─── BMP280 baro (on-board, disabled) │ │ ADC ──── Battery voltage divider │
│ ADC ──── Battery voltage (PC1) + Current (PC3) │
│ PB3 ──── WS2812B LED strip │
│ PB2 ──── Buzzer │
│ │ │ │
└─────────────────────────────────────────────────────────────────────┘ └─────────────────────────────────────────────────────────────────────┘
│ USART2 │ UART4 │ CAN bus (ISO 11898) │ UART (460800 baud)
│ PA2=TX → ESC RX │ PA0=TX → ELRS TX │ 500 kbps │
│ PA3=RX ← ESC TX │ PA1=RX ← ELRS RX
│ GND ─── GND │ GND ─── GND
▼ ▼ ▼ ▼
┌────────────────────────┐ ┌──────────────────────────┐ ┌────────────────────────┐ ┌──────────────────────────┐
│ HOVERBOARD ESC │ │ ELRS 2.4GHz RX │ │ VESC Left (ID 56) │ │ VESC Right (ID 68) │
│ (Bottom Plate) │ │ (beside FC) │ │ (Bottom Plate) │ │ (Bottom Plate) │
│ │ │ │
│ BLDC hub motor │ │ BLDC hub motor │
│ CAN 500 kbps │ │ CAN 500 kbps │
│ FOC current control │ │ FOC current control │
│ VESC Status 1 (0x900) │ │ VESC Status 1 (0x910) │
│ │ │ │ │ │ │ │
│ 2× BLDC hub motors │ │ CRSF protocol │
│ 26400 baud UART │ │ 420000 baud │
│ Frame: [0xABCD] │ │ BetaFPV 1W TX → RX │
│ [steer][speed][csum] │ │ CH3=speed CH4=steer │
│ │ │ CH5=arm CH6=mode │
└────────────────────────┘ └──────────────────────────┘ └────────────────────────┘ └──────────────────────────┘
│ │
┌────┴────┐ LEFT MOTOR RIGHT MOTOR
▼ ▼ ```
🛞 LEFT RIGHT 🛞
MOTOR MOTOR
## Wire-by-Wire Connections ## Wire-by-Wire Connections
### 1. Orin ↔ FC (Primary: USB CDC) ### 1. Orin <-> ESP32-S3 BALANCE (Primary: CAN Bus via CANable2)
| From | To | Wire Color | Notes | | From | To | Wire | Notes |
|------|----|-----------|-------| |------|----|------|-------|
| Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus | | Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps |
| CANable2 CAN-H | ESP32-S3 BALANCE CAN-H | twisted pair | ISO 11898 differential |
| CANable2 CAN-L | ESP32-S3 BALANCE CAN-L | twisted pair | ISO 11898 differential |
- Device: `/dev/ttyACM0` → symlink `/dev/stm32-bridge` - Interface: SocketCAN `slcan0`, 500 kbps
- Baud: 921600, 8N1 - Device node: `/dev/canable2` (via udev, symlink to ttyUSBx)
- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC) - Protocol: CAN frames --- ORIN_CMD_DRIVE (0x300), ORIN_CMD_MODE (0x301), ORIN_CMD_ESTOP (0x302)
- Telemetry: BALANCE_STATUS (0x400), BALANCE_VESC (0x401), BALANCE_IMU (0x402), BALANCE_BATTERY (0x403)
### 2. Orin ↔ FC (Fallback: Hardware UART) ### 2. Orin <-> ESP32-S3 BALANCE (Fallback: Hardware UART)
| Orin Pin | Signal | FC Pin | FC Signal | | Orin Pin | Signal | ESP32-S3 Pin | Notes |
|----------|--------|--------|-----------| |----------|--------|--------------|-------|
| Pin 8 | TXD0 | PC7 | USART6 RX | | Pin 8 | TXD0 | GPIO17 (UART0 RX) | Orin TX -> BALANCE RX |
| Pin 10 | RXD0 | PC6 | USART6 TX | | Pin 10 | RXD0 | GPIO18 (UART0 TX) | Orin RX <- BALANCE TX |
| Pin 6 | GND | GND | GND | | Pin 6 | GND | GND | Common ground |
- Jetson device: `/dev/ttyTHS0` - Jetson device: `/dev/ttyTHS0`
- Baud: 921600, 8N1 - Baud: 460800, 8N1
- Voltage: 3.3V both sides (no level shifter needed) - Voltage: 3.3V both sides (no level shifter needed)
- **Cross-connect:** Orin TX → FC RX, Orin RX ← FC TX - Cross-connect: Orin TX -> BALANCE RX, Orin RX <- BALANCE TX
### 3. FC ↔ Hoverboard ESC ### 3. Orin <-> ESP32-S3 IO (USB Serial)
| FC Pin | Signal | ESC Pin | Notes | | From | To | Notes |
|--------|--------|---------|-------| |------|----|-------|
| PA2 | USART2 TX | RX | FC sends speed/steer commands | | Orin USB-A | ESP32-S3 IO USB-C | USB cable, /dev/esp32-io |
| PA3 | USART2 RX | TX | ESC sends feedback (optional) |
- Device node: `/dev/esp32-io` (udev symlink)
- Baud: 460800, 8N1
- Protocol: Binary frames `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
- Use: IO expansion, GPIO control, sensor polling
### 4. ESP32-S3 BALANCE <-> VESC Motors (CAN Bus)
| BALANCE Pin | Signal | VESC Pin | Notes |
|-------------|--------|----------|-------|
| GPIO21 | CAN-H | CAN-H | ISO 11898 differential pair |
| GPIO22 | CAN-L | CAN-L | ISO 11898 differential pair |
| GND | GND | GND | Common ground | | GND | GND | GND | Common ground |
- Baud: 26400, 8N1 - Baud: 500 kbps CAN
- Protocol: Binary frame — `[0xABCD][steer:int16][speed:int16][checksum:uint16]` - VESC Left: CAN ID 56, VESC Right: CAN ID 68
- Speed range: -1000 to +1000 - Commands: COMM_SET_RPM, COMM_SET_CURRENT, COMM_SET_DUTY
- **Keep wires short and twisted** (EMI from ESC) - Telemetry: VESC Status 1 at 50 Hz (RPM, current, duty)
### 4. FC ↔ ELRS Receiver
| FC Pin | Signal | ELRS Pin | Notes |
|--------|--------|----------|-------|
| PA0 | UART4 TX | RX | Telemetry to TX (optional) |
| PA1 | UART4 RX | TX | CRSF frames from RX |
| GND | GND | GND | Common ground |
| 5V | — | VCC | Power ELRS from 5V bus |
- Baud: 420000 (CRSF protocol)
- Failsafe: disarm after 300ms without frame
### 5. Power Distribution ### 5. Power Distribution
``` ```
BATTERY (36V) ──┬── Hoverboard ESC (36V direct) BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
├── VESC Right (36V direct -> BLDC right motor)
├── 5V BEC/regulator ──┬── Orin (USB-C PD or barrel jack) ├── 5V BEC/regulator ──┬── Orin (USB-C PD or barrel jack)
│ ├── FC (via USB or 5V pad) │ ├── ESP32-S3 BALANCE (5V via USB-C)
│ ├── ELRS RX (5V) │ ├── ESP32-S3 IO (5V via USB-C)
│ ├── WS2812B LEDs (5V) │ ├── WS2812B LEDs (5V)
│ └── RPLIDAR (5V via USB) │ └── RPLIDAR (5V via USB)
└── Battery monitor ──── FC ADC (PC1=voltage, PC3=current) └── Battery monitor ──── ESP32-S3 BALANCE ADC (voltage divider)
``` ```
### 6. Sensors on Orin (USB/CSI) ### 6. Sensors on Orin (USB/CSI)
@ -143,20 +141,36 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
| RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` | | RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` |
| IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` | | IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` |
| IMX219 rear+right | MIPI CSI-2 | CSI-B (J8) | `/dev/video4,6` | | IMX219 rear+right | MIPI CSI-2 | CSI-B (J8) | `/dev/video4,6` |
| 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` | | 1TB NVMe | PCIe Gen3 x4 | M.2 Key M | `/dev/nvme0n1` |
| CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
## FC UART Summary (MAMBA F722S — OBSOLETE) ## FC UART Summary (MAMBA F722S — OBSOLETE)
| UART | Pins | Baud | Assignment | Notes | | Interface | Pins | Baud/Rate | Assignment | Notes |
|------|------|------|------------|-------| |-----------|------|-----------|------------|-------|
| USART1 | PB6=TX, PB7=RX | — | SmartAudio/VTX | Unused in SaltyLab | | UART0 | GPIO17=RX, GPIO18=TX | 460800 | Orin UART fallback | 3.3V, cross-connect |
| USART2 | PA2=TX, PA3=RX | 26400 | Hoverboard ESC | Binary motor commands | | UART1 | GPIO19=RX, GPIO20=TX | 115200 | Debug serial | Optional |
| USART3 | PB10=TX, PB11=RX | — | Available | Was SBUS default | | CAN (TWAI) | GPIO21=H, GPIO22=L | 500 kbps | CAN bus (VESCs + Orin) | SN65HVD230 transceiver |
| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control | | I2C | GPIO4=SDA, GPIO5=SCL | 400 kHz | QMI8658 IMU (addr 0x6B) | Onboard |
| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional | | SPI | GPIO36=MOSI, GPIO37=SCLK, GPIO35=CS | 40 MHz | GC9A01 LCD (onboard) | 240x240 round |
| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link | | USB CDC | USB-C | 460800 | Orin USB fallback | /dev/esp32-balance |
| USB CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` |
## CAN Frame ID Map
| CAN ID | Direction | Name | Contents |
|--------|-----------|------|----------|
| 0x300 | Orin -> BALANCE | ORIN_CMD_DRIVE | left_rpm_f32, right_rpm_f32 (8 bytes LE) |
| 0x301 | Orin -> BALANCE | ORIN_CMD_MODE | mode byte (0=IDLE, 1=DRIVE, 2=ESTOP) |
| 0x302 | Orin -> BALANCE | ORIN_CMD_ESTOP | flags byte (bit0=stop, bit1=clear) |
| 0x400 | BALANCE -> Orin | BALANCE_STATUS | pitch x10:i16, motor_cmd:u16, vbat_mv:u16, state:u8, flags:u8 |
| 0x401 | BALANCE -> Orin | BALANCE_VESC | l_rpm x10:i16, r_rpm x10:i16, l_cur x10:i16, r_cur x10:i16 |
| 0x402 | BALANCE -> Orin | BALANCE_IMU | pitch x100:i16, roll x100:i16, yaw x100:i16, ax x100:i16, ay x100:i16, az x100:i16 |
| 0x403 | BALANCE -> Orin | BALANCE_BATTERY | vbat_mv:u16, current_ma:i16, soc_pct:u8 |
| 0x900+ID | VESC Left -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 |
| 0x910+ID | VESC Right -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 |
VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44).
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header) ### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
@ -174,57 +188,59 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
| Pin 2, 4 | 5V | Power | | Pin 2, 4 | 5V | Power |
| Pin 6, 9 | GND | Ground | | Pin 6, 9 | GND | Ground |
- **Codec:** Wolfson WM8960 (I2C addr 0x1A) - Codec: Wolfson WM8960 (I2C addr 0x1A)
- **Mics:** 2× MEMS (left + right) — basic stereo / sound localization - Mics: 2x MEMS (left + right) --- basic stereo / sound localization
- **Speaker:** 3W class-D amp output (JST connector) - Speaker: 3W class-D amp output (JST connector)
- **Headset:** 3.5mm TRRS jack - Headset: 3.5mm TRRS jack
- **Requires:** WM8960 device tree overlay for Jetson (community port) - Requires: WM8960 device tree overlay for Jetson (community port)
- **Use:** Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements - Use: Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
### 8. SIM7600A 4G/LTE HAT (via USB) ### 8. SIM7600A 4G/LTE HAT (via USB)
| Connection | Detail | | Connection | Detail |
|-----------|--------| |-----------|--------|
| Interface | USB (micro-B on HAT USB-A/C on Orin) | | Interface | USB (micro-B on HAT -> USB-A/C on Orin) |
| Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) | | Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) |
| Power | 5V from USB or separate 5V supply (peak 2A during TX) | | Power | 5V from USB or separate 5V supply (peak 2A during TX) |
| SIM | Nano-SIM slot on HAT | | SIM | Nano-SIM slot on HAT |
| Antenna | 4G LTE + GPS/GNSS (external SMA antennas mount high on chassis) | | Antenna | 4G LTE + GPS/GNSS (external SMA antennas --- mount high on chassis) |
- **Data:** PPP or QMI for internet connectivity - Data: PPP or QMI for internet connectivity
- **GPS/GNSS:** Built-in receiver, NMEA sentences on ttyUSB2 — outdoor positioning - GPS/GNSS: Built-in receiver, NMEA sentences on ttyUSB2 --- outdoor positioning
- **AT commands:** `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix) - AT commands: `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
- **Connected via USB** (not 40-pin) — avoids UART conflict with FC fallback, flexible antenna placement - Connected via USB (not 40-pin) --- avoids UART conflict with BALANCE fallback, flexible antenna placement
- **Use:** Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control - Use: Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
### 10. Leap Motion Controller (USB) ### 9. Leap Motion Controller (USB)
| Connection | Detail | | Connection | Detail |
|-----------|--------| |-----------|--------|
| Interface | USB 3.0 (micro-B on controller USB-A on Orin) | | Interface | USB 3.0 (micro-B on controller -> USB-A on Orin) |
| Power | ~0.5W | | Power | ~0.5W |
| Range | ~80cm, 150° FOV | | Range | ~80cm, 150 deg FOV |
| SDK | Ultraleap Gemini V5+ (Linux ARM64 support) | | SDK | Ultraleap Gemini V5+ (Linux ARM64 support) |
| ROS2 | `leap_motion_ros2` wrapper available | | ROS2 | `leap_motion_ros2` wrapper available |
- **2× IR cameras + 3× IR LEDs** tracks all 10 fingers in 3D, sub-mm precision - 2x IR cameras + 3x IR LEDs --- tracks all 10 fingers in 3D, sub-mm precision
- **Mount:** Forward-facing on sensor tower or upward on Orin plate - Mount: Forward-facing on sensor tower or upward on Orin plate
- **Use:** Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos - Use: Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
- **Combined with ReSpeaker:** Voice + gesture control with zero hardware in hand - Combined with ReSpeaker: Voice + gesture control with zero hardware in hand
### 11. Power Budget (USB) ### 10. Power Budget (USB)
| Device | Interface | Power Draw | | Device | Interface | Power Draw |
|--------|-----------|------------| |--------|-----------|------------|
| STM32 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) | | CANable2 USB-CAN | USB-A | ~0.5W |
| ESP32-S3 BALANCE | USB-C | ~0.8W (WiFi off) |
| ESP32-S3 IO | USB-C | ~0.5W |
| RealSense D435i | USB-A | ~1.5W (3.5W peak) | | RealSense D435i | USB-A | ~1.5W (3.5W peak) |
| RPLIDAR A1M8 | USB-A | ~2.6W (motor on) | | RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
| SIM7600A | USB | ~1W idle, 3W TX peak | | SIM7600A | USB | ~1W idle, 3W TX peak |
| Leap Motion | USB | ~0.5W | | Leap Motion | USB-A | ~0.5W |
| ReSpeaker HAT | 40-pin | ~0.5W | | ReSpeaker HAT | 40-pin | ~0.5W |
| **Total USB** | | **~6.5W typical, ~10.5W peak** | | **Total USB** | | **~7.9W typical, ~11W peak** |
Orin Nano Super delivers up to 25W USB peripherals are well within budget. Orin Nano Super delivers up to 25W --- USB peripherals are well within budget.
--- ---
@ -232,38 +248,37 @@ Orin Nano Super delivers up to 25W — USB peripherals are well within budget.
``` ```
┌──────────────┐ ┌──────────────┐
ELRS TX │ (in your hand) RC TX │ (in your hand)
│ (2.4GHz) │ │ (2.4GHz) │
└──────┬───────┘ └──────┬───────┘
│ radio │ radio
┌──────▼───────┐ ┌──────▼───────┐
ELRS RX │ CRSF 420kbaud RC RX │ CRSF 420kbaud (future)
└──────┬───────┘ └──────┬───────┘
│ UART4 │ UART
┌────────────▼────────────┐ ┌────────────▼────────────┐
│ MAMBA F722S │ │ ESP32-S3 BALANCE │
│ (Waveshare LCD 1.28) │
│ │ │ │
MPU6000 → Balance PID QMI8658 -> Balance PID
CRSF → Mode Manager RC -> Mode Manager
│ Safety Monitor │ │ Safety Monitor │
│ │ │ │
└──┬──────────┬───────────┘ └──┬──────────┬───────────┘
USART2 ─────┘ └───── USB CDC / USART6 CAN 500kbps─┘ └───── CAN bus / UART fallback
26400 baud 921600 baud
│ │ │ │
┌────┴────────────┐
┌────────────────┐ ┌───────────────────┐ │ CAN bus (500k) │ ┌───────────────────┐
│ Hoverboard ESC │ │ Orin Nano Super │ ├─ VESC Left 56 │ │ Orin Nano Super │
│ │ │ │ └─ VESC Right 68 │ │ │
│ L motor R motor│ │ SLAM / Nav2 / AI │ │ │ │ SLAM / Nav2 / AI │
│ 🛞 🛞 │ │ Person following │ ▼ ▼ │ Person following │
└────────────────┘ │ Voice commands │ LEFT RIGHT │ Voice commands │
│ 4G telemetry │ MOTOR MOTOR │ 4G telemetry │
└──┬──────────┬───────┘ └──┬──────────┬───────┘
│ │ │ │
┌──────────▼─┐ ┌────▼──────────┐ ┌──────────▼─┐ ┌────▼──────────┐
│ ReSpeaker │ │ SIM7600A │ │ ReSpeaker │ │ SIM7600A │
│ 2-Mic HAT │ │ 4G/LTE + GPS │ │ 2-Mic HAT │ │ 4G/LTE + GPS │
│ 🎤 🔊 │ │ 📡 🛰️ │
└────────────┘ └───────────────┘ └────────────┘ └───────────────┘
``` ```

View File

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

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. 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 BALANCE firmware.
## Behavior Tree Sequence ## Behavior Tree Sequence

View File

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

View File

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

View File

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

View File

@ -56,7 +56,7 @@ sudo jtop
|-----------|----------|------------|----------|-----------|-------| |-----------|----------|------------|----------|-----------|-------|
| RealSense D435i | 0.3 | 1.5 | 3.5 | USB 3.1 | Peak during boot/init | | 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 | | 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 bridge | 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 | | 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** | | | | **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) ├─► DC-DC Buck → 5V 6A ──► Jetson Orin barrel jack (30W)
│ (e.g., XL4016E1) │ (e.g., XL4016E1)
├─► DC-DC Buck → 5V 3A ──► STM32 + logic 5V rail ├─► DC-DC Buck → 5V 3A ──► ESP32 + logic 5V rail
└─► Hoverboard ESC ──► Hub motors (48V loop) └─► Hoverboard ESC ──► Hub motors (48V loop)
``` ```

View File

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

View File

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

View File

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

View File

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

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

View File

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

View File

@ -14,7 +14,7 @@ Alert levels (SoC thresholds):
5% EMERGENCY publish zero /cmd_vel, disarm, log + alert 5% EMERGENCY publish zero /cmd_vel, disarm, log + alert
SoC source priority: SoC source priority:
1. soc_pct field from STM32 BATTERY telemetry (fuel gauge or lookup on STM32) 1. soc_pct field from ESP32 BATTERY telemetry (fuel gauge or lookup on ESP32 BALANCE)
2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known 2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known
Parameters (config/battery_params.yaml): Parameters (config/battery_params.yaml):
@ -320,7 +320,7 @@ class BatteryNode(Node):
self._speed_limit_pub.publish(msg) self._speed_limit_pub.publish(msg)
def _execute_safe_stop(self) -> None: def _execute_safe_stop(self) -> None:
"""Send zero /cmd_vel and disarm the STM32.""" """Send zero /cmd_vel and disarm the ESP32 BALANCE."""
self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming") self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming")
# Publish zero velocity # Publish zero velocity
zero_twist = Twist() zero_twist = Twist()

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

View File

@ -1,260 +0,0 @@
"""stm32_protocol.py — Inter-board UART frame codec (ESP32 BALANCE ↔ ESP32 IO).
File name retained for import compatibility. This module implements the binary
serial protocol that runs between the two ESP32-S3 embedded boards.
Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 (2026-04-04)
Physical: UART @ 460800 baud, 8N1
Frame layout:
HEADER LEN TYPE PAYLOAD CRC8
0xAA 1 B 1 B LEN bytes 1 B
CRC8 covers: LEN + TYPE + PAYLOAD (polynomial 0x07, init 0x00).
Max payload: 64 bytes. No ETX byte.
Note: the Orin communicates with ESP32 BALANCE via CAN (CANable2/slcan0),
NOT via this serial protocol. See mamba_protocol.py for the CAN frame codec.
Message types IO BALANCE:
0x01 RC_CHANNELS raw RC channel values (CRSF or ELRS)
0x02 SENSORS barometer + ToF data
Message types BALANCE IO:
0x10 LED_CMD LED strip pattern [pattern u8][r u8][g u8][b u8]
0x11 OUTPUT_CMD horn/buzzer/headlight/fan [flags u8][headlight u8][fan u8]
0x12 MOTOR_CMD BTS7960 aux motor [motor_a i16 BE][motor_b i16 BE]
0x20 HEARTBEAT status keepalive [state u8][error_flags u8]
RC channel mapping (CH1CH8, CRSF range 1721811, centre 992):
CH1 Steer CH2 Drive CH3 Throttle CH4 Yaw
CH5 Arm CH6 Mode CH7 E-stop CH8 Spare
"""
from __future__ import annotations
import struct
from dataclasses import dataclass, field
from enum import IntEnum
from typing import Optional
# ── Frame constants ───────────────────────────────────────────────────────────
FRAME_HEADER = 0xAA
MAX_PAYLOAD_LEN = 64
BAUD_RATE = 460800 # inter-board UART
# ── Message type codes ────────────────────────────────────────────────────────
class IOMsg(IntEnum):
RC_CHANNELS = 0x01
SENSORS = 0x02
class BalMsg(IntEnum):
LED_CMD = 0x10
OUTPUT_CMD = 0x11
MOTOR_CMD = 0x12
HEARTBEAT = 0x20
# ── Parsed message dataclasses ────────────────────────────────────────────────
@dataclass
class RcChannels:
"""RC channel values from TBS Crossfire (primary) or ELRS (failover).
CRSF range: 1721811 (centre 992).
"""
channels: list = field(default_factory=lambda: [992] * 8)
source: int = 0 # 0 = CRSF, 1 = ELRS failover
@dataclass
class SensorData:
pressure_pa: float = 0.0 # Pascal (barometer)
temperature_c: float = 0.0 # °C
tof_mm: int = 0 # Time-of-flight distance mm
@dataclass
class LedCmd:
pattern: int = 0
r: int = 0
g: int = 0
b: int = 0
@dataclass
class OutputCmd:
horn: bool = False
buzzer: bool = False
headlight: int = 0 # 0255 PWM
fan: int = 0 # 0255 PWM
@dataclass
class MotorCmd:
motor_a: int = 0 # -255..+255 (BTS7960 channel A)
motor_b: int = 0 # -255..+255 (BTS7960 channel B)
@dataclass
class Heartbeat:
state: int = 0 # 0=IDLE 1=RUNNING 2=FAULT
error_flags: int = 0
InterboardMsg = RcChannels | SensorData | LedCmd | OutputCmd | MotorCmd | Heartbeat
# ── CRC-8 (polynomial 0x07, init 0x00) ───────────────────────────────────────
def _crc8(data: bytes) -> int:
crc = 0
for byte in data:
crc ^= byte
for _ in range(8):
crc = ((crc << 1) ^ 0x07) if (crc & 0x80) else (crc << 1)
crc &= 0xFF
return crc
# ── Frame encoder ─────────────────────────────────────────────────────────────
def _build_frame(msg_type: int, payload: bytes) -> bytes:
assert len(payload) <= MAX_PAYLOAD_LEN
crc_data = bytes([len(payload), msg_type]) + payload
return bytes([FRAME_HEADER, len(payload), msg_type]) + payload + bytes([_crc8(crc_data)])
def encode_heartbeat(state: int = 0, error_flags: int = 0) -> bytes:
return _build_frame(BalMsg.HEARTBEAT, struct.pack("BB", state & 0xFF, error_flags & 0xFF))
def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes:
return _build_frame(BalMsg.LED_CMD, struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF))
def encode_output_cmd(horn: bool, buzzer: bool, headlight: int, fan: int) -> bytes:
flags = (int(horn) & 1) | ((int(buzzer) & 1) << 1)
return _build_frame(BalMsg.OUTPUT_CMD, struct.pack("BBB", flags, headlight & 0xFF, fan & 0xFF))
def encode_motor_cmd(motor_a: int, motor_b: int) -> bytes:
a = max(-255, min(255, int(motor_a)))
b = max(-255, min(255, int(motor_b)))
return _build_frame(BalMsg.MOTOR_CMD, struct.pack(">hh", a, b))
# ── Streaming frame parser ────────────────────────────────────────────────────
class _St(IntEnum):
HDR = 0
LEN = 1
TYPE = 2
PAY = 3
CRC = 4
class FrameParser:
"""Byte-by-byte streaming parser for inter-board frames.
Usage::
parser = FrameParser()
for b in incoming:
msg = parser.feed(b)
if msg is not None:
handle(msg)
"""
def __init__(self):
self._s = _St.HDR
self._len = 0
self._type = 0
self._pay = bytearray()
self.frames_ok = 0
self.frames_error = 0
def reset(self):
self._s = _St.HDR
self._pay = bytearray()
def feed(self, byte: int) -> Optional[InterboardMsg | tuple]:
s = self._s
if s == _St.HDR:
if byte == FRAME_HEADER:
self._s = _St.LEN
return None
if s == _St.LEN:
if byte > MAX_PAYLOAD_LEN:
self.frames_error += 1; self.reset(); return None
self._len = byte
self._s = _St.TYPE
return None
if s == _St.TYPE:
self._type = byte
self._pay = bytearray()
self._s = _St.PAY if self._len > 0 else _St.CRC
return None
if s == _St.PAY:
self._pay.append(byte)
if len(self._pay) == self._len:
self._s = _St.CRC
return None
if s == _St.CRC:
self.reset()
expected = _crc8(bytes([self._len, self._type]) + self._pay)
if byte != expected:
self.frames_error += 1; return None
self.frames_ok += 1
return _decode(self._type, bytes(self._pay))
self.reset(); return None
# ── Message decoder ───────────────────────────────────────────────────────────
def _decode(type_code: int, payload: bytes) -> Optional[InterboardMsg | tuple]:
try:
if type_code == IOMsg.RC_CHANNELS:
if len(payload) < 17: return None
ch = list(struct.unpack_from("<8H", payload))
return RcChannels(channels=ch, source=payload[16])
if type_code == IOMsg.SENSORS:
if len(payload) < 10: return None
pres, temp, tof = struct.unpack_from("<ffH", payload)
return SensorData(pressure_pa=pres, temperature_c=temp, tof_mm=tof)
if type_code == BalMsg.LED_CMD:
if len(payload) < 4: return None
pat, r, g, b = struct.unpack_from("BBBB", payload)
return LedCmd(pattern=pat, r=r, g=g, b=b)
if type_code == BalMsg.OUTPUT_CMD:
if len(payload) < 3: return None
flags, headlight, fan = struct.unpack_from("BBB", payload)
return OutputCmd(horn=bool(flags & 1), buzzer=bool(flags & 2),
headlight=headlight, fan=fan)
if type_code == BalMsg.MOTOR_CMD:
if len(payload) < 4: return None
a, b = struct.unpack_from(">hh", payload)
return MotorCmd(motor_a=a, motor_b=b)
if type_code == BalMsg.HEARTBEAT:
if len(payload) < 2: return None
state, flags = struct.unpack_from("BB", payload)
return Heartbeat(state=state, error_flags=flags)
except struct.error:
return None
return (type_code, payload)

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

View File

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

View File

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

View File

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

View File

@ -29,7 +29,7 @@ setup(
zip_safe=True, zip_safe=True,
maintainer="sl-jetson", maintainer="sl-jetson",
maintainer_email="sl-jetson@saltylab.local", maintainer_email="sl-jetson@saltylab.local",
description="STM32 USB CDC → ROS2 serial bridge for saltybot", description="ESP32 USB CDC → ROS2 serial bridge for saltybot",
license="MIT", license="MIT",
tests_require=["pytest"], tests_require=["pytest"],
entry_points={ entry_points={
@ -41,8 +41,8 @@ setup(
# Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate # Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate
"cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main", "cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main",
"remote_estop_node = saltybot_bridge.remote_estop_node:main", "remote_estop_node = saltybot_bridge.remote_estop_node:main",
# ESP32-IO inter-board bridge node # Binary-framed ESP32 BALANCE command node (Issue #119)
"esp32_io_bridge_node = saltybot_bridge.esp32_io_bridge_node:main", "stm32_cmd_node = saltybot_bridge.stm32_cmd_node:main",
# Battery management node (Issue #125) # Battery management node (Issue #125)
"battery_node = saltybot_bridge.battery_node:main", "battery_node = saltybot_bridge.battery_node:main",
# Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685) # Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685)

View File

@ -1,5 +1,5 @@
""" """
Unit tests for JetsonSTM32 command serialization logic. Unit tests for JetsonESP32 BALANCE command serialization logic.
Tests Twistspeed/steer conversion and frame formatting. Tests Twistspeed/steer conversion and frame formatting.
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
""" """

View File

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

View File

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

View File

@ -19,7 +19,7 @@
# inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding) # inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
# DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer) # 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 bridge consumes this topic.
bt_navigator: bt_navigator:
ros__parameters: ros__parameters:

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -1,7 +1,7 @@
""" """
wheel_odom_node.py Differential drive wheel encoder odometry (Issue #184). 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 bridge, integrates
differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz. differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz.
Optionally broadcasts the odom base_link TF transform. Optionally broadcasts the odom base_link TF transform.

View File

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

View File

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

View File

@ -1 +1 @@
"""SaltyBot CAN bridge package — ESP32-S3 BALANCE controller and VESC telemetry via python-can.""" """SaltyBot CAN bridge package — ESP32 IO motor controller and VESC telemetry via python-can."""

View File

@ -1,216 +0,0 @@
#!/usr/bin/env python3
"""balance_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE.
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
CAN bus: 500 kbps, standard 11-bit IDs, CANable 2.0 (slcan0 / can0) on Orin.
Orin ESP32 BALANCE (commands)
0x300 DRIVE 8 B [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16]
0x301 ARM 1 B [arm:u8] 0x00=DISARM 0x01=ARM
0x302 PID_SET 8 B [kp:f16 BE][ki:f16 BE][kd:f16 BE][_:u16]
0x303 ESTOP 1 B [0xE5] magic byte cuts all motors immediately
ESP32 BALANCE Orin (telemetry)
0x400 ATTITUDE 8 B [pitch:f16 BE][speed:f16 BE][yaw_rate:f16 BE][state:u8][flags:u8]
0x401 BATTERY 4 B [vbat_mv:u16 BE][fault_code:u8][rssi:i8]
speed/steer range: 1000..+1000 (motor units). f16 = IEEE 754 half-precision.
VESC standard extended CAN (29-bit IDs = packet_type<<8 | node_id):
Left VESC node ID = 56 (0x38)
Right VESC node ID = 68 (0x44)
STATUS_1 cmd=9 erpm i32 BE, current i16 (/10 A), duty i16 (/1000)
STATUS_4 cmd=16 temp_fet i16 (/10 °C), temp_mot i16 (/10 °C), cur_in i16 (/10 A)
STATUS_5 cmd=27 tacho i32, vbat i16 (/10 V)
"""
import struct
from dataclasses import dataclass
# ── CAN IDs ───────────────────────────────────────────────────────────────────
# Orin → ESP32 BALANCE
ORIN_CMD_DRIVE: int = 0x300
ORIN_CMD_ARM: int = 0x301
ORIN_CMD_PID: int = 0x302
ORIN_CMD_ESTOP: int = 0x303
# ESP32 BALANCE → Orin
ESP32_TELEM_ATTITUDE: int = 0x400
ESP32_TELEM_BATTERY: int = 0x401
# Backward-compat aliases used by other nodes
FC_STATUS: int = ESP32_TELEM_ATTITUDE
FC_VESC: int = ESP32_TELEM_BATTERY
# VESC node IDs
VESC_LEFT_ID: int = 56
VESC_RIGHT_ID: int = 68
# VESC packet types
VESC_STATUS_1: int = 9
VESC_STATUS_4: int = 16
VESC_STATUS_5: int = 27
# ── Mode constants (DRIVE frame mode byte) ─────────────────────────────────────
MODE_IDLE: int = 0 # RC passthrough, Orin not injecting
MODE_DRIVE: int = 1 # Orin velocity commands
MODE_AUTONOMOUS: int = 2 # full autonomy
MODE_ESTOP: int = 2 # alias
# ESTOP magic byte
_ESTOP_MAGIC: int = 0xE5
# ── Struct formats (big-endian) ────────────────────────────────────────────────
_FMT_DRIVE = ">hhBBH" # i16 speed, i16 steer, u8 mode, u8 flags, u16 pad
_FMT_PID = ">eeeH" # f16 kp, f16 ki, f16 kd, u16 pad
_FMT_ATTITUDE = ">eeeBB" # f16 pitch, f16 speed, f16 yaw_rate, u8 state, u8 flags
_FMT_BATTERY = ">HBb" # u16 vbat_mv, u8 fault_code, i8 rssi
# ── Data classes ──────────────────────────────────────────────────────────────
@dataclass
class AttitudeTelemetry:
"""Decoded ATTITUDE (0x400) from ESP32 BALANCE."""
pitch_deg: float = 0.0 # degrees, half-float
speed: float = 0.0 # m/s, half-float
yaw_rate: float = 0.0 # rad/s, half-float
state: int = 0 # 0=IDLE 1=RUNNING 2=FAULT
flags: int = 0 # error bitmask
@dataclass
class BatteryTelemetry:
"""Decoded BATTERY (0x401) from ESP32 BALANCE."""
vbat_mv: int = 0 # millivolts
fault_code: int = 0 # 0 = OK
rssi: int = 0 # RC signal dBm (signed)
@dataclass
class PidGains:
"""Balance PID gains."""
kp: float = 0.0
ki: float = 0.0
kd: float = 0.0
@dataclass
class VescStatus1:
"""Decoded VESC STATUS (cmd=9) — direct from VESC."""
node_id: int = 0
erpm: float = 0.0
current: float = 0.0 # A
duty: float = 0.0 # -1.0..+1.0
@dataclass
class VescStatus4:
"""Decoded VESC STATUS_4 (cmd=16)."""
node_id: int = 0
temp_fet_c: float = 0.0
temp_motor_c: float = 0.0
current_in: float = 0.0
@dataclass
class VescStatus5:
"""Decoded VESC STATUS_5 (cmd=27)."""
node_id: int = 0
tacho: int = 0
vbat_v: float = 0.0
# ── Orin → BALANCE encoders ───────────────────────────────────────────────────
def encode_drive_cmd(speed: int, steer: int,
mode: int = MODE_DRIVE, flags: int = 0) -> bytes:
"""Encode ORIN_CMD_DRIVE (0x300) — 8 bytes.
speed: 1000..+1000 motor units (positive = forward)
steer: 1000..+1000 motor units (positive = right)
mode: MODE_IDLE / MODE_DRIVE / MODE_AUTONOMOUS
"""
speed = max(-1000, min(1000, int(speed)))
steer = max(-1000, min(1000, int(steer)))
return struct.pack(_FMT_DRIVE, speed, steer, mode & 0xFF, flags & 0xFF, 0)
def encode_arm_cmd(arm: bool) -> bytes:
"""Encode ORIN_CMD_ARM (0x301) — 1 byte."""
return struct.pack("B", 0x01 if arm else 0x00)
def encode_pid_cmd(kp: float, ki: float, kd: float) -> bytes:
"""Encode ORIN_CMD_PID (0x302) — 8 bytes (3× half-float + 2-byte pad)."""
return struct.pack(_FMT_PID, float(kp), float(ki), float(kd), 0)
def encode_estop_cmd() -> bytes:
"""Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5."""
return struct.pack("B", _ESTOP_MAGIC)
# ── BALANCE → Orin decoders ───────────────────────────────────────────────────
def decode_attitude(data: bytes) -> AttitudeTelemetry:
"""Decode ATTITUDE (0x400) — 8 bytes."""
if len(data) < 8:
raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}")
pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data)
return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate,
state=state, flags=flags)
def decode_battery(data: bytes) -> BatteryTelemetry:
"""Decode BATTERY (0x401) — 4 bytes."""
if len(data) < 4:
raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}")
vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data)
return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi)
# Backward-compat aliases
def decode_fc_status(data: bytes) -> AttitudeTelemetry:
return decode_attitude(data)
def decode_fc_vesc(data: bytes) -> BatteryTelemetry:
return decode_battery(data)
# ── VESC CAN helpers ─────────────────────────────────────────────────────────
def decode_vesc_can_id(can_id: int) -> tuple:
"""Split a VESC extended CAN ID into (packet_type, node_id)."""
return (can_id >> 8) & 0xFF, can_id & 0xFF
def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1:
"""Decode VESC STATUS (cmd=9): erpm i32, current i16(/10), duty i16(/1000)."""
erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8])
return VescStatus1(node_id=node_id, erpm=float(erpm),
current=cur_x10 / 10.0, duty=duty_x1000 / 1000.0)
def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4:
"""Decode VESC STATUS_4 (cmd=16): temp_fet, temp_mot, cur_in (all /10)."""
tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8])
return VescStatus4(node_id=node_id, temp_fet_c=tfet / 10.0,
temp_motor_c=tmot / 10.0, current_in=cur_in / 10.0)
def decode_vesc_status5(node_id: int, data: bytes) -> VescStatus5:
"""Decode VESC STATUS_5 (cmd=27): tacho i32, vbat i16 (/10 V)."""
tacho, vbat_x10 = struct.unpack_from(">ih", data[:6])
return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0)
def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple:
"""Encode VESC SET_RPM command. Returns (extended_can_id, payload)."""
can_id = (3 << 8) | (node_id & 0xFF)
return can_id, struct.pack(">i", int(rpm))

View File

@ -15,7 +15,7 @@ setup(
zip_safe=True, zip_safe=True,
maintainer="sl-controls", maintainer="sl-controls",
maintainer_email="sl-controls@saltylab.local", maintainer_email="sl-controls@saltylab.local",
description="CAN bus bridge for ESP32-S3 BALANCE controller and VESC telemetry", description="CAN bus bridge for ESP32 IO motor controller and VESC telemetry",
license="MIT", license="MIT",
tests_require=["pytest"], tests_require=["pytest"],
entry_points={ entry_points={

View File

@ -1,28 +1,28 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
protocol_defs.py CAN message ID constants and frame builders/parsers for the protocol_defs.py CAN message ID constants and frame builders/parsers for the
OrinMambaVESC integration test suite. OrinESP32 IOVESC integration test suite.
All IDs and payload formats are derived from: All IDs and payload formats are derived from:
include/orin_can.h OrinFC (Mamba) protocol include/orin_can.h OrinFC (ESP32 IO) protocol
include/vesc_can.h VESC CAN protocol include/vesc_can.h VESC CAN protocol
saltybot_can_bridge/mamba_protocol.py existing bridge constants saltybot_can_bridge/mamba_protocol.py existing bridge constants
CAN IDs used in tests CAN IDs used in tests
--------------------- ---------------------
Orin FC (Mamba) commands (standard 11-bit, matching orin_can.h): Orin FC (ESP32 IO) commands (standard 11-bit, matching orin_can.h):
ORIN_CMD_HEARTBEAT 0x300 ORIN_CMD_HEARTBEAT 0x300
ORIN_CMD_DRIVE 0x301 int16 speed (1000..+1000), int16 steer (1000..+1000) ORIN_CMD_DRIVE 0x301 int16 speed (1000..+1000), int16 steer (1000..+1000)
ORIN_CMD_MODE 0x302 uint8 mode byte ORIN_CMD_MODE 0x302 uint8 mode byte
ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR) ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR)
FC (Mamba) Orin telemetry (standard 11-bit, matching orin_can.h): FC (ESP32 IO) Orin telemetry (standard 11-bit, matching orin_can.h):
FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t) FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t)
FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t) FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t)
FC_IMU 0x402 8 bytes FC_IMU 0x402 8 bytes
FC_BARO 0x403 8 bytes FC_BARO 0x403 8 bytes
Mamba VESC internal commands (matching mamba_protocol.py): ESP32 IO VESC internal commands (matching mamba_protocol.py):
MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian 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_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop)
MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop
@ -36,7 +36,7 @@ import struct
from typing import Tuple from typing import Tuple
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Orin → FC (Mamba) command IDs (from orin_can.h) # Orin → FC (ESP32 IO) command IDs (from orin_can.h)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
ORIN_CMD_HEARTBEAT: int = 0x300 ORIN_CMD_HEARTBEAT: int = 0x300
@ -45,7 +45,7 @@ ORIN_CMD_MODE: int = 0x302
ORIN_CMD_ESTOP: int = 0x303 ORIN_CMD_ESTOP: int = 0x303
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# FC (Mamba) → Orin telemetry IDs (from orin_can.h) # FC (ESP32 IO) → Orin telemetry IDs (from orin_can.h)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
FC_STATUS: int = 0x400 FC_STATUS: int = 0x400
@ -54,7 +54,7 @@ FC_IMU: int = 0x402
FC_BARO: int = 0x403 FC_BARO: int = 0x403
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Mamba → VESC internal command IDs (from mamba_protocol.py) # ESP32 IO → VESC internal command IDs (from mamba_protocol.py)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
MAMBA_CMD_VELOCITY: int = 0x100 MAMBA_CMD_VELOCITY: int = 0x100
@ -136,7 +136,7 @@ def build_estop_cmd(action: int = 1) -> bytes:
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Frame builders — Mamba velocity commands (mamba_protocol.py encoding) # Frame builders — ESP32 IO velocity commands (mamba_protocol.py encoding)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:

View File

@ -14,7 +14,7 @@ setup(
zip_safe=True, zip_safe=True,
maintainer="sl-jetson", maintainer="sl-jetson",
maintainer_email="sl-jetson@saltylab.local", maintainer_email="sl-jetson@saltylab.local",
description="End-to-end CAN integration tests for Orin↔Mamba↔VESC full loop", description="End-to-end CAN integration tests for Orin↔ESP32 IO↔VESC full loop",
license="MIT", license="MIT",
tests_require=["pytest"], tests_require=["pytest"],
entry_points={ entry_points={

View File

@ -3,7 +3,7 @@
test_drive_command.py Integration tests for the drive command path. test_drive_command.py Integration tests for the drive command path.
Tests verify: Tests verify:
DRIVE cmd Mamba receives velocity command frame mock VESC status response DRIVE cmd ESP32 IO receives velocity command frame mock VESC status response
FC_VESC broadcast contains correct RPMs. FC_VESC broadcast contains correct RPMs.
All tests run without real hardware or a running ROS2 system. All tests run without real hardware or a running ROS2 system.
@ -61,7 +61,7 @@ def _send_drive(bus, left_mps: float, right_mps: float) -> None:
class TestDriveForward: class TestDriveForward:
def test_drive_forward_velocity_frame_sent(self, mock_can_bus): 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 Inject DRIVE cmd (1.0 m/s, 1.0 m/s) verify ESP32 IO receives
a MAMBA_CMD_VELOCITY frame with correct payload. a MAMBA_CMD_VELOCITY frame with correct payload.
""" """
_send_drive(mock_can_bus, 1.0, 1.0) _send_drive(mock_can_bus, 1.0, 1.0)
@ -84,7 +84,7 @@ class TestDriveForward:
def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus): def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus):
""" """
Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct. 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.) (In the real loop ESP32 IO computes RPM from m/s and broadcasts FC_VESC.)
This test checks the FC_VESC frame format and parser. This test checks the FC_VESC frame format and parser.
""" """
# Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics) # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics)

View File

@ -47,7 +47,7 @@ class VescStatusAggregator:
2. Builds an FC_VESC broadcast payload 2. Builds an FC_VESC broadcast payload
3. Injects the FC_VESC frame onto the mock bus 3. Injects the FC_VESC frame onto the mock bus
This represents the Mamba Orin telemetry path. This represents the ESP32 IO Orin telemetry path.
""" """
def __init__(self, bus: MockCANBus): def __init__(self, bus: MockCANBus):

View File

@ -90,7 +90,7 @@ class HeartbeatSimulator:
def _simulate_estop_on_timeout(bus: MockCANBus) -> None: def _simulate_estop_on_timeout(bus: MockCANBus) -> None:
""" """
Simulate the firmware-side logic: when heartbeat timeout expires, Simulate the firmware-side logic: when heartbeat timeout expires,
the FC sends an e-stop command by setting estop mode on the Mamba bus. the FC sends an e-stop command by setting estop mode on the ESP32 IO bus.
We model this as the bridge sending zero velocity + ESTOP mode. We model this as the bridge sending zero velocity + ESTOP mode.
""" """

View File

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

View File

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

View File

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

View File

@ -138,7 +138,7 @@ class DiagnosticsNode(Node):
self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {}) self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {})
def _check_stm32(self): def _check_stm32(self):
self.hardware_checks["stm32"] = ("OK", "STM32 bridge online", {}) self.hardware_checks["stm32"] = ("OK", "ESP32 bridge online", {})
def _check_servos(self): def _check_servos(self):
try: try:

View File

@ -7,7 +7,7 @@
# ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.2 # 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) # 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 BALANCE AUTONOMOUS mode gate.
# Do not run this node without the cmd_vel bridge running on the same robot. # Do not run this node without the cmd_vel bridge running on the same robot.
# ── Follow geometry ──────────────────────────────────────────────────────────── # ── Follow geometry ────────────────────────────────────────────────────────────
@ -70,5 +70,5 @@ control_rate: 20.0 # Hz — lower than cmd_vel bridge (50Hz) by desig
# ── Mode integration ────────────────────────────────────────────────────────── # ── Mode integration ──────────────────────────────────────────────────────────
# Master enable for the follow controller. When false, node publishes zero cmd_vel. # Master enable for the follow controller. When false, node publishes zero cmd_vel.
# Toggle at runtime: ros2 param set /person_follower follow_enabled false # 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 BALANCE AUTONOMOUS mode (md=2).
follow_enabled: true follow_enabled: true

View File

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

View File

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

View File

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

View File

@ -1,7 +1,7 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
"""gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548). """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 BALANCE.
Implements smooth trapezoidal motion profiles with configurable axis limits. Implements smooth trapezoidal motion profiles with configurable axis limits.
Subscribed topics: Subscribed topics:

View File

@ -1,14 +1,14 @@
"""jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548). """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 side).
Command type (Jetson STM32): Command type (Jetson ESP32 BALANCE):
0x0B GIMBAL_POS int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes) 0x0B GIMBAL_POS int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes)
pan_x10 = pan_deg * 10 (±1500 for ±150°) pan_x10 = pan_deg * 10 (±1500 for ±150°)
tilt_x10 = tilt_deg * 10 (±450 for ±45°) tilt_x10 = tilt_deg * 10 (±450 for ±45°)
speed = servo speed register 04095 (0 = max) speed = servo speed register 04095 (0 = max)
Telemetry type (STM32 Jetson): Telemetry type (ESP32 BALANCE Jetson):
0x84 GIMBAL_STATE int16 pan_x10 + int16 tilt_x10 + 0x84 GIMBAL_STATE int16 pan_x10 + int16 tilt_x10 +
uint16 pan_speed_raw + uint16 tilt_speed_raw + uint16 pan_speed_raw + uint16 tilt_speed_raw +
uint8 torque_en + uint8 rx_err_pct (10 bytes) uint8 torque_en + uint8 rx_err_pct (10 bytes)
@ -31,8 +31,8 @@ ETX = 0x03
# ── Command / telemetry type codes ───────────────────────────────────────────── # ── Command / telemetry type codes ─────────────────────────────────────────────
CMD_GIMBAL_POS = 0x0B # Jetson → STM32: set pan/tilt target CMD_GIMBAL_POS = 0x0B # Jetson → ESP32 BALANCE: set pan/tilt target
TLM_GIMBAL_STATE = 0x84 # STM32 → Jetson: measured state TLM_GIMBAL_STATE = 0x84 # ESP32 BALANCE → Jetson: measured state
# Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed. # 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)) # Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))

View File

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

View File

@ -13,7 +13,7 @@ Topic graph
In RC mode (blend_alpha 0) the node publishes Twist(0,0) so the bridge 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 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 BALANCE is in RC_MANUAL.
The bridge's existing ESC ramp handles hardware-level smoothing; The bridge's existing ESC ramp handles hardware-level smoothing;
the blend_alpha here provides the higher-level cmd_vel policy ramp. the blend_alpha here provides the higher-level cmd_vel policy ramp.

View File

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

View File

@ -9,7 +9,7 @@ Inputs
axes[stick_axes...] Roll/Pitch/Throttle/Yaw override detection axes[stick_axes...] Roll/Pitch/Throttle/Yaw override detection
/saltybot/balance_state (std_msgs/String JSON) /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 BALANCE mode.
<slam_fix_topic> (geometry_msgs/PoseWithCovarianceStamped) <slam_fix_topic> (geometry_msgs/PoseWithCovarianceStamped)
Any message received within slam_fix_timeout_s SLAM fix valid. 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_joy_t: float = 0.0 # monotonic; 0 = never
self._last_slam_t: float = 0.0 self._last_slam_t: float = 0.0
self._joy_axes: list = [] self._joy_axes: list = []
self._stm32_mode: int = 0 # from balance_state JSON self._esp32_mode: int = 0 # from balance_state JSON
# ── QoS ─────────────────────────────────────────────────────────────── # ── QoS ───────────────────────────────────────────────────────────────
best_effort = QoSProfile( best_effort = QoSProfile(
@ -187,7 +187,7 @@ class ModeSwitchNode(Node):
data = json.loads(msg.data) data = json.loads(msg.data)
# "mode" is a label string; map back to int for reference # "mode" is a label string; map back to int for reference
mode_label = data.get("mode", "RC_MANUAL") 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) "AUTONOMOUS": 2}.get(mode_label, 0)
except (json.JSONDecodeError, TypeError): except (json.JSONDecodeError, TypeError):
pass pass

View File

@ -1,8 +1,8 @@
vesc_can_odometry: vesc_can_odometry:
ros__parameters: ros__parameters:
# ── CAN motor IDs (used for CAN addressing) ─────────────────────────────── # ── CAN motor IDs (used for CAN addressing) ───────────────────────────────
left_can_id: 56 # left motor VESC CAN ID (Mamba F722S) left_can_id: 56 # left motor VESC CAN ID (ESP32 BALANCE)
right_can_id: 68 # right motor VESC CAN ID (Mamba F722S) right_can_id: 68 # right motor VESC CAN ID (ESP32 BALANCE)
# ── State topic names (must match VESC telemetry publisher) ────────────── # ── State topic names (must match VESC telemetry publisher) ──────────────
left_state_topic: /vesc/left/state left_state_topic: /vesc/left/state

View File

@ -12,7 +12,7 @@
# Hardware: # Hardware:
# IMU: RealSense D435i BMI055 → /imu/data # IMU: RealSense D435i BMI055 → /imu/data
# GPS: SIM7600X cellular → /gps/fix (±2.5 m CEP) # GPS: SIM7600X cellular → /gps/fix (±2.5 m CEP)
# Odom: STM32 wheel encoders → /odom # Odom: ESP32 BALANCE wheel encoders → /odom
# RTK: ZED-F9P (optional) → /gps/fix (±2 cm CEP when use_rtk: true) # RTK: ZED-F9P (optional) → /gps/fix (±2 cm CEP when use_rtk: true)
# ── Local EKF: fuses wheel odometry + IMU in odom frame ────────────────────── # ── Local EKF: fuses wheel odometry + IMU in odom frame ──────────────────────

View File

@ -70,8 +70,8 @@ class ParameterServer(Node):
"""Load parameter definitions from config file""" """Load parameter definitions from config file"""
defs = { defs = {
'hardware': { 'hardware': {
'serial_port': ParamInfo('serial_port', '/dev/stm32-bridge', 'string', 'serial_port': ParamInfo('serial_port', '/dev/esp32-bridge', 'string',
'hardware', description='STM32 bridge serial port'), 'hardware', description='ESP32 bridge serial port'),
'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware', 'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware',
min_val=9600, max_val=3000000, min_val=9600, max_val=3000000,
description='Serial baud rate'), description='Serial baud rate'),

View File

@ -370,7 +370,7 @@ class PIDAutotuneNode(Node):
ser.write(frame_set) ser.write(frame_set)
time.sleep(0.05) # allow FC to process PID_SET time.sleep(0.05) # allow FC to process PID_SET
ser.write(frame_save) ser.write(frame_save)
# Flash erase takes ~1s on STM32F7; wait for it # Flash erase takes ~1s on ESP32; wait for it
time.sleep(1.5) time.sleep(1.5)
self.get_logger().info( self.get_logger().info(

View File

@ -9,7 +9,7 @@
# #
# GPS source: SIM7600X → /gps/fix (NavSatFix, ±2.5m CEP) — PR #65 # GPS source: SIM7600X → /gps/fix (NavSatFix, ±2.5m CEP) — PR #65
# Heading: D435i IMU → /imu/data, converted yaw → route waypoint heading_deg # Heading: D435i IMU → /imu/data, converted yaw → route waypoint heading_deg
# Odometry: STM32 wheel encoders → /odom # Odometry: ESP32 BALANCE wheel encoders → /odom
# UWB: /uwb/target (follow-me reference, logged for context) # UWB: /uwb/target (follow-me reference, logged for context)
route_recorder: route_recorder:

View File

@ -10,7 +10,7 @@ Depends on:
saltybot-nav2 container (Nav2 action server /navigate_through_poses) saltybot-nav2 container (Nav2 action server /navigate_through_poses)
saltybot_cellular (/gps/fix from SIM7600X GPS PR #65) saltybot_cellular (/gps/fix from SIM7600X GPS PR #65)
saltybot_uwb (/uwb/target PR #66, used for context during recording) saltybot_uwb (/uwb/target PR #66, used for context during recording)
STM32 bridge (/odom from wheel encoders) ESP32 bridge (/odom from wheel encoders)
D435i (/imu/data for heading) D435i (/imu/data for heading)
Usage record a route: Usage record a route:

View File

@ -5,7 +5,7 @@ Hardware
SaltyRover: 4-wheel ground robot with individual brushless ESCs. SaltyRover: 4-wheel ground robot with individual brushless ESCs.
ESCs controlled via PWM (servo-style 10002000 µs pulses). ESCs controlled via PWM (servo-style 10002000 µs pulses).
Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge. Communication: USB CDC serial to ESP32 BALANCE or Raspberry Pi Pico GPIO PWM bridge.
ESC channel assignments (configurable): ESC channel assignments (configurable):
CH1 = left-front CH1 = left-front

View File

@ -39,6 +39,6 @@ safety_zone:
# ── cmd_vel topics ─────────────────────────────────────────────────────── # ── cmd_vel topics ───────────────────────────────────────────────────────
# Safety zone node intercepts cmd_vel from upstream, overrides to zero on estop. # Safety zone node intercepts cmd_vel from upstream, overrides to zero on estop.
# Typical chain: # Typical chain:
# cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → STM32 # cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → ESP32 BALANCE
cmd_vel_input_topic: /cmd_vel_input # upstream velocity (remap as needed) cmd_vel_input_topic: /cmd_vel_input # upstream velocity (remap as needed)
cmd_vel_output_topic: /cmd_vel # downstream (to STM32 bridge) cmd_vel_output_topic: /cmd_vel # downstream (to ESP32 bridge)

View File

@ -10,7 +10,7 @@
# ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0 # ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0
# #
# Data flow: # Data flow:
# person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → STM32 # person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE
# ── Controller ───────────────────────────────────────────────────────────────── # ── Controller ─────────────────────────────────────────────────────────────────
control_rate: 50.0 # Hz — 50ms tick, same as cmd_vel_bridge control_rate: 50.0 # Hz — 50ms tick, same as cmd_vel_bridge
@ -83,11 +83,11 @@ ride:
target_vel_max: 15.0 # m/s — cap; EUC max ~30 km/h = 8.3 m/s typical target_vel_max: 15.0 # m/s — cap; EUC max ~30 km/h = 8.3 m/s typical
# ── Notes ───────────────────────────────────────────────────────────────────── # ── Notes ─────────────────────────────────────────────────────────────────────
# 1. To enable ride profile, the Jetson → STM32 cmd_vel_bridge must also be # 1. To enable ride profile, the Jetson → ESP32 BALANCE cmd_vel_bridge must also be
# reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150 # reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150
# at ride speed (slower ramp = smoother balance). # at ride speed (slower ramp = smoother balance).
# #
# 2. The STM32 balance PID gains likely need retuning for ride speed. Expect # 2. The ESP32 BALANCE balance PID gains likely need retuning for ride speed. Expect
# increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s. # increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s.
# #
# 3. Test sequence recommendation: # 3. Test sequence recommendation:

View File

@ -10,7 +10,7 @@ cmd_vel_bridge with matching limits:
ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0 ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0
Prerequisite node pipeline: Prerequisite node pipeline:
person_follower /cmd_vel_raw [speed_controller] /cmd_vel cmd_vel_bridge STM32 person_follower /cmd_vel_raw [speed_controller] /cmd_vel cmd_vel_bridge ESP32 BALANCE
Usage: Usage:
# Defaults (walk profile initially, adapts via UWB + GPS): # Defaults (walk profile initially, adapts via UWB + GPS):

View File

@ -5,7 +5,7 @@ Hardware
SaltyTank: tracked robot with left/right independent brushless ESCs. SaltyTank: tracked robot with left/right independent brushless ESCs.
ESCs controlled via PWM (servo-style 10002000 µs pulses). ESCs controlled via PWM (servo-style 10002000 µs pulses).
Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge. Communication: USB CDC serial to ESP32 BALANCE or Raspberry Pi Pico GPIO PWM bridge.
ESC channel assignments (configurable): ESC channel assignments (configurable):
CH1 = left-front (or left-track in 2WD/tracked mode) CH1 = left-front (or left-track in 2WD/tracked mode)

View File

@ -298,7 +298,7 @@ class TestBatteryMonitoring(unittest.TestCase):
rclpy.spin_once(self.node, timeout_sec=0.1) rclpy.spin_once(self.node, timeout_sec=0.1)
def test_01_battery_topic_advertised(self): def test_01_battery_topic_advertised(self):
"""Battery topic must be advertised (from STM32 bridge).""" """Battery topic must be advertised (from ESP32 bridge)."""
self._spin(5.0) self._spin(5.0)
all_topics = {name for name, _ in self.node.get_topic_names_and_types()} all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
@ -327,7 +327,7 @@ class TestBatteryMonitoring(unittest.TestCase):
self.node.destroy_subscription(sub) self.node.destroy_subscription(sub)
if not received: if not received:
pytest.skip("Battery data not publishing (STM32 bridge may be disabled in test mode)") pytest.skip("Battery data not publishing (ESP32 bridge may be disabled in test mode)")
class TestDockingServices(unittest.TestCase): class TestDockingServices(unittest.TestCase):

View File

@ -1,5 +1,5 @@
# VESC CAN Telemetry Node — SaltyBot dual FSESC 6.7 Pro (FW 6.6) # VESC CAN Telemetry Node — SaltyBot dual FSESC 6.7 Pro (FW 6.6)
# SocketCAN interface: can0 (SN65HVD230 transceiver on MAMBA F722S CAN2) # SocketCAN interface: can0 (SN65HVD230 transceiver on ESP32 BALANCE CAN2)
vesc_telemetry: vesc_telemetry:
ros__parameters: ros__parameters:

View File

@ -8,7 +8,7 @@ monitor_speed = 115200
board_build.mcu = stm32f722ret6 board_build.mcu = stm32f722ret6
board_build.f_cpu = 216000000L board_build.f_cpu = 216000000L
build_flags = build_flags =
-DSTM32F722xx -DESP32xx
-DUSE_HAL_DRIVER -DUSE_HAL_DRIVER
-DHSE_VALUE=8000000U -DHSE_VALUE=8000000U
-DUSE_USB_FS -DUSE_USB_FS

View File

@ -16,7 +16,7 @@
| Depth Cam | Intel RealSense D435i — 848×480 @ 90fps, BMI055 IMU | | Depth Cam | Intel RealSense D435i — 848×480 @ 90fps, BMI055 IMU |
| LIDAR | RPLIDAR A1M8 — 360° 2D, 12m range, ~5.5 Hz | | LIDAR | RPLIDAR A1M8 — 360° 2D, 12m range, ~5.5 Hz |
| Wide Cams | 4× IMX219 160° CSI — front/right/rear/left 90° intervals *(arriving)* | | Wide Cams | 4× IMX219 160° CSI — front/right/rear/left 90° intervals *(arriving)* |
| FC | STM32F722 — UART bridge `/dev/ttyACM0` @ 921600 | | FC | ESP32 — UART bridge `/dev/ttyACM0` @ 921600 |
--- ---
@ -76,7 +76,7 @@ Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x)
Nav2 stack (Phase 2b) Nav2 stack (Phase 2b)
20Hz costmap 20Hz costmap
/cmd_vel → STM32 /cmd_vel → ESP32 BALANCE
4× IMX219 CSI (Phase 2c — pending hardware) 4× IMX219 CSI (Phase 2c — pending hardware)
front/right/rear/left 160° front/right/rear/left 160°

View File

@ -22,7 +22,7 @@ Requirements:
dfu-util >= 0.9 installed and in PATH dfu-util >= 0.9 installed and in PATH
Dual-bank note: Dual-bank note:
STM32F722 has single-bank 512 KB flash; hardware A/B rollback is not ESP32 has single-bank 512 KB flash; hardware A/B rollback is not
supported. Rollback is implemented here by saving a backup of the supported. Rollback is implemented here by saving a backup of the
previous binary (.firmware_backup.bin) before each flash. previous binary (.firmware_backup.bin) before each flash.
""" """
@ -36,11 +36,11 @@ import subprocess
import sys import sys
import time import time
# ---- STM32F722 flash constants ---- # ---- ESP32 flash constants ----
FLASH_BASE = 0x08000000 FLASH_BASE = 0x08000000
FLASH_SIZE = 0x80000 # 512 KB FLASH_SIZE = 0x80000 # 512 KB
# ---- DFU device defaults (STM32 system bootloader) ---- # ---- DFU device defaults (ESP32/STM32 system bootloader) ----
DFU_VID = 0x0483 # STMicroelectronics DFU_VID = 0x0483 # STMicroelectronics
DFU_PID = 0xDF11 # DFU mode DFU_PID = 0xDF11 # DFU mode
@ -62,16 +62,16 @@ def crc32_file(path: str) -> int:
def stm32_crc32(data: bytes) -> int: def stm32_crc32(data: bytes) -> int:
""" """
Compute CRC-32/MPEG-2 matching STM32F7 hardware CRC unit. Compute CRC-32/MPEG-2 matching ESP32 hardware CRC unit.
STM32 algorithm: ESP32/STM32 algorithm:
Polynomial : 0x04C11DB7 Polynomial : 0x04C11DB7
Initial : 0xFFFFFFFF Initial : 0xFFFFFFFF
Width : 32 bits Width : 32 bits
Reflection : none (MSB-first) Reflection : none (MSB-first)
Feed size : 32-bit words from flash (little-endian CPU read) Feed size : 32-bit words from flash (little-endian CPU read)
When the STM32 reads a flash word it gets a little-endian uint32; When the ESP32 BALANCE reads a flash word it gets a little-endian uint32;
the hardware CRC unit processes bits[31:24] first, then [23:16], the hardware CRC unit processes bits[31:24] first, then [23:16],
[15:8], [7:0]. This Python implementation replicates that behaviour. [15:8], [7:0]. This Python implementation replicates that behaviour.
@ -214,11 +214,11 @@ def main() -> int:
f'({FLASH_SIZE} bytes)', file=sys.stderr) f'({FLASH_SIZE} bytes)', file=sys.stderr)
return 1 return 1
# STM32 hardware CRC (for cross-checking with firmware telemetry) # ESP32/STM32 hardware CRC (for cross-checking with firmware telemetry)
with open(target, 'rb') as fh: with open(target, 'rb') as fh:
bin_data = fh.read() bin_data = fh.read()
crc_hw = stm32_crc32(bin_data.ljust(FLASH_SIZE, b'\xff')) crc_hw = stm32_crc32(bin_data.ljust(FLASH_SIZE, b'\xff'))
print(f'CRC-32 : 0x{crc_hw:08X} (MPEG-2 / STM32 HW, padded to {FLASH_SIZE // 1024} KB)') print(f'CRC-32 : 0x{crc_hw:08X} (MPEG-2 / ESP32/STM32 HW, padded to {FLASH_SIZE // 1024} KB)')
# Save backup before flashing (skip when rolling back) # Save backup before flashing (skip when rolling back)
if not args.rollback: if not args.rollback:

View File

@ -4,7 +4,7 @@ test_bno055_data.py — Issue #135: BNO055 driver unit tests.
Tests data scaling, byte parsing, calibration status extraction, Tests data scaling, byte parsing, calibration status extraction,
calibration offset packing/unpacking, and temperature handling. calibration offset packing/unpacking, and temperature handling.
No HAL or STM32 hardware required pure Python logic. No HAL or STM32/ESP32 hardware required pure Python logic.
""" """
import struct import struct

View File

@ -4,7 +4,7 @@ test_jlink_frames.py — Issue #120: JLink binary protocol unit tests.
Tests CRC16-XModem, frame building, frame parsing (state machine), Tests CRC16-XModem, frame building, frame parsing (state machine),
command payload encoding, and telemetry frame layout. command payload encoding, and telemetry frame layout.
No HAL or STM32 hardware required pure Python logic. No HAL or STM32/ESP32 hardware required pure Python logic.
""" """
import struct import struct

View File

@ -3,7 +3,7 @@ test_ota.py — OTA firmware update utilities (Issue #124)
Tests: Tests:
- CRC-32/ISO-HDLC (crc32_file / binascii.crc32) - CRC-32/ISO-HDLC (crc32_file / binascii.crc32)
- CRC-32/MPEG-2 (stm32_crc32 matches STM32F7 hardware CRC unit) - CRC-32/MPEG-2 (stm32_crc32 matches ESP32 hardware CRC unit)
- CRC-16/XMODEM (_crc16_xmodem JLink frame integrity) - CRC-16/XMODEM (_crc16_xmodem JLink frame integrity)
- DFU_ENTER frame (JLINK_CMD_DFU_ENTER = 0x06, no payload) - DFU_ENTER frame (JLINK_CMD_DFU_ENTER = 0x06, no payload)
- Safety constants (BKP index, flash region, magic value) - Safety constants (BKP index, flash region, magic value)
@ -233,8 +233,8 @@ class TestOtaConstants:
BNO055_BKP_RANGE = range(0, 7) BNO055_BKP_RANGE = range(0, 7)
assert OTA_DFU_BKP_IDX not in BNO055_BKP_RANGE assert OTA_DFU_BKP_IDX not in BNO055_BKP_RANGE
def test_bkp_idx_valid_stm32f7(self): def test_bkp_idx_valid_esp32(self):
"""STM32F7 has 32 backup registers (BKP0RBKP31R).""" """ESP32 has 32 backup registers (BKP0RBKP31R)."""
OTA_DFU_BKP_IDX = 15 OTA_DFU_BKP_IDX = 15
assert 0 <= OTA_DFU_BKP_IDX <= 31 assert 0 <= OTA_DFU_BKP_IDX <= 31
@ -252,7 +252,7 @@ class TestOtaConstants:
assert DFU_VID == 0x0483 assert DFU_VID == 0x0483
def test_dfu_pid_dfu_mode(self): def test_dfu_pid_dfu_mode(self):
"""Default PID = 0xDF11 (STM32 DFU mode).""" """Default PID = 0xDF11 (ESP32 DFU mode)."""
assert DFU_PID == 0xDF11 assert DFU_PID == 0xDF11
def test_bkp_idx_not_zero(self): def test_bkp_idx_not_zero(self):

View File

@ -471,7 +471,7 @@ class TestJlinkProtocol:
# Tests: Wake latency and IWDG budget # Tests: Wake latency and IWDG budget
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
class TestWakeLatencyBudget: class TestWakeLatencyBudget:
# STM32F722 STOP-mode wakeup: HSI ready ~2 ms + PLL lock ~2 ms ≈ 4 ms # ESP32 STOP-mode wakeup: HSI ready ~2 ms + PLL lock ~2 ms ≈ 4 ms
ESTIMATED_WAKE_MS = 10 # conservative upper bound ESTIMATED_WAKE_MS = 10 # conservative upper bound
def test_wake_latency_within_50ms(self): def test_wake_latency_within_50ms(self):
@ -493,7 +493,7 @@ class TestWakeLatencyBudget:
assert PM_FADE_MS < PM_IDLE_TIMEOUT_MS assert PM_FADE_MS < PM_IDLE_TIMEOUT_MS
def test_stop_mode_wake_much_less_than_50ms(self): def test_stop_mode_wake_much_less_than_50ms(self):
# PLL startup on STM32F7: HSI on (0 ms, already running) + # PLL startup on ESP32: HSI on (0 ms, already running) +
# PLL lock ~2 ms + SysTick re-init ~0.1 ms ≈ 3 ms # PLL lock ~2 ms + SysTick re-init ~0.1 ms ≈ 3 ms
pll_lock_ms = 3 pll_lock_ms = 3
overhead_ms = 1 overhead_ms = 1
@ -539,7 +539,7 @@ class TestHardwareConstants:
assert 216 / 2 == 108 assert 216 / 2 == 108
def test_flash_latency_7_required_at_216mhz(self): def test_flash_latency_7_required_at_216mhz(self):
"""STM32F7 at 2.7-3.3 V: 7 wait states for 210-216 MHz.""" """ESP32 at 2.7-3.3 V: 7 wait states for 210-216 MHz."""
FLASH_LATENCY = 7 FLASH_LATENCY = 7
assert FLASH_LATENCY == 7 assert FLASH_LATENCY == 7

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — CAN Monitor</title> <title>Saltybot — CAN Monitor</title>
<link rel="stylesheet" href="can_monitor_panel.css"> <link rel="stylesheet" href="can_monitor_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — System Diagnostics</title> <title>Saltybot — System Diagnostics</title>
<link rel="stylesheet" href="diagnostics_panel.css"> <link rel="stylesheet" href="diagnostics_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>
@ -112,7 +112,7 @@
<div class="temp-bar-track"><div class="temp-bar-fill" id="gpu-temp-bar" style="width:0%"></div></div> <div class="temp-bar-track"><div class="temp-bar-fill" id="gpu-temp-bar" style="width:0%"></div></div>
</div> </div>
<div class="temp-box" id="board-temp-box"> <div class="temp-box" id="board-temp-box">
<div class="temp-label">Board / STM32</div> <div class="temp-label">Board / ESP32</div>
<div class="temp-value" id="board-temp-val"></div> <div class="temp-value" id="board-temp-val"></div>
<div class="temp-bar-track"><div class="temp-bar-fill" id="board-temp-bar" style="width:0%"></div></div> <div class="temp-bar-track"><div class="temp-bar-fill" id="board-temp-bar" style="width:0%"></div></div>
</div> </div>

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — Event Log</title> <title>Saltybot — Event Log</title>
<link rel="stylesheet" href="event_log_panel.css"> <link rel="stylesheet" href="event_log_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — Gamepad Teleop</title> <title>Saltybot — Gamepad Teleop</title>
<link rel="stylesheet" href="gamepad_panel.css"> <link rel="stylesheet" href="gamepad_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -6,7 +6,7 @@
<title>Saltybot — Gimbal Control</title> <title>Saltybot — Gimbal Control</title>
<link rel="stylesheet" href="gimbal_panel.css"> <link rel="stylesheet" href="gimbal_panel.css">
<!-- roslib from CDN --> <!-- roslib from CDN -->
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
<style> <style>
/* Cam button active state (can't use CSS-only with JS-toggled class without Tailwind) */ /* Cam button active state (can't use CSS-only with JS-toggled class without Tailwind) */
.cam-btn { padding: 3px 10px; border-radius: 4px; border: 1px solid #1e3a5f; .cam-btn { padding: 3px 10px; border-radius: 4px; border: 1px solid #1e3a5f;

View File

@ -7,7 +7,7 @@
<link rel="stylesheet" href="gps_map_panel.css"> <link rel="stylesheet" href="gps_map_panel.css">
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css" /> <link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css" />
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script> <script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — Dashboard</title> <title>Saltybot — Dashboard</title>
<link rel="stylesheet" href="dashboard.css"> <link rel="stylesheet" href="dashboard.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — Map View</title> <title>Saltybot — Map View</title>
<link rel="stylesheet" href="map_panel.css"> <link rel="stylesheet" href="map_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -5,7 +5,7 @@
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0"> <meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — Settings</title> <title>Saltybot — Settings</title>
<link rel="stylesheet" href="settings_panel.css"> <link rel="stylesheet" href="settings_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script> <script src="https://cdn.jsdelivr.net/npm/roslib@1.4.0/build/roslib.min.js"></script>
</head> </head>
<body> <body>

View File

@ -151,14 +151,14 @@ export function ControlMode({ subscribe }) {
<div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">BALANCE STATE</div> <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">BALANCE STATE</div>
<div className="grid grid-cols-2 gap-2 text-xs"> <div className="grid grid-cols-2 gap-2 text-xs">
<div> <div>
<span className="text-gray-600">STM32 State: </span> <span className="text-gray-600">ESP32 State: </span>
<span className={`font-bold ${ <span className={`font-bold ${
balState.state === 'ARMED' ? 'text-green-400' : balState.state === 'ARMED' ? 'text-green-400' :
balState.state === 'TILT FAULT' ? 'text-red-400' : 'text-gray-400' balState.state === 'TILT FAULT' ? 'text-red-400' : 'text-gray-400'
}`}>{balState.state}</span> }`}>{balState.state}</span>
</div> </div>
<div> <div>
<span className="text-gray-600">STM32 Mode: </span> <span className="text-gray-600">ESP32 Mode: </span>
<span className="text-cyan-400">{balState.mode}</span> <span className="text-cyan-400">{balState.mode}</span>
</div> </div>
<div> <div>

View File

@ -287,7 +287,7 @@ function FirmwareView({ firmwareInfo, startFirmwareWatch, connected }) {
useEffect(() => { if (!connected) return; const unsub = startFirmwareWatch(); return unsub; }, [connected, startFirmwareWatch]); useEffect(() => { if (!connected) return; const unsub = startFirmwareWatch(); return unsub; }, [connected, startFirmwareWatch]);
const formatUptime = (s) => { if (!s) return '—'; return `${Math.floor(s/3600)}h ${Math.floor((s%3600)/60)}m`; }; const formatUptime = (s) => { if (!s) return '—'; return `${Math.floor(s/3600)}h ${Math.floor((s%3600)/60)}m`; };
const rows = [ const rows = [
['STM32 Firmware', firmwareInfo?.stm32Version ?? '—'], ['ESP32 Firmware', firmwareInfo?.esp32Version ?? '—'],
['Jetson SW', firmwareInfo?.jetsonVersion ?? '—'], ['Jetson SW', firmwareInfo?.jetsonVersion ?? '—'],
['Last OTA Update',firmwareInfo?.lastOtaDate ?? '—'], ['Last OTA Update',firmwareInfo?.lastOtaDate ?? '—'],
['Hostname', firmwareInfo?.hostname ?? '—'], ['Hostname', firmwareInfo?.hostname ?? '—'],
@ -310,7 +310,7 @@ function FirmwareView({ firmwareInfo, startFirmwareWatch, connected }) {
</div> </div>
{!firmwareInfo && connected && ( {!firmwareInfo && connected && (
<div className="text-amber-700 text-xs border border-amber-900 rounded p-2"> <div className="text-amber-700 text-xs border border-amber-900 rounded p-2">
Waiting for /diagnostics Ensure firmware diagnostics keys (stm32_fw_version etc.) are published. Waiting for /diagnostics Ensure firmware diagnostics keys (esp32_fw_version etc.) are published.
</div> </div>
)} )}
</div> </div>

View File

@ -48,7 +48,7 @@ function loadRobots() {
const EMPTY_DATA = () => ({ const EMPTY_DATA = () => ({
state: null, // 'DISARMED'|'ARMED'|'TILT FAULT' state: null, // 'DISARMED'|'ARMED'|'TILT FAULT'
stm32Mode: null, // 'MANUAL'|'ASSISTED' esp32Mode: null, // 'MANUAL'|'ASSISTED'
controlMode: null, // 'RC'|'RAMP_TO_AUTO'|'AUTO'|'RAMP_TO_RC' controlMode: null, // 'RC'|'RAMP_TO_AUTO'|'AUTO'|'RAMP_TO_RC'
blendAlpha: 0, blendAlpha: 0,
pipeline: null, // 'idle'|'listening'|'thinking'|'speaking' pipeline: null, // 'idle'|'listening'|'thinking'|'speaking'
@ -153,7 +153,7 @@ export function useFleet() {
const d = JSON.parse(msg.data); const d = JSON.parse(msg.data);
_update(id, { _update(id, {
state: d.state, state: d.state,
stm32Mode: d.mode, esp32Mode: d.mode,
pitch: d.pitch_deg ?? 0, pitch: d.pitch_deg ?? 0,
motorCmd: d.motor_cmd ?? 0, motorCmd: d.motor_cmd ?? 0,
lastSeen: Date.now(), lastSeen: Date.now(),

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