cleanup: Remove all Mamba/STM32/BlackPill references — ESP32-S3 only
- Renamed mamba_protocol.py → balance_protocol.py; updated all importers - can_bridge_node.py rewritten to use balance_protocol.py API (ESP32-S3 BALANCE) - test_can_bridge.py rewritten to test actual balance_protocol.py constants/functions - All STM32/Mamba references in Python, YAML, Markdown, shell scripts replaced: * Hardware: MAMBA F722S → ESP32-S3 BALANCE/IO * Device paths: /dev/stm32-bridge → /dev/esp32-io * Node names: stm32_serial_bridge → esp32_io_serial_bridge * hardware_id: stm32f722 → esp32s3-balance/esp32s3-io - C/C++ src/include/lib/test files: added DEPRECATED header comment - Covers: saltybot_bridge, saltybot_can_bridge, saltybot_can_e2e_test, saltybot_bringup, saltybot_diagnostics, saltybot_mode_switch, and all chassis, docs, scripts, and project files Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
ec4527b8f3
commit
cea3eaff97
@ -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-S3 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-S3 (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-S3 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)
|
||||||
|
|||||||
@ -1,12 +1,12 @@
|
|||||||
# SaltyLab Firmware — Agent Playbook
|
# SaltyLab Firmware — Agent Playbook
|
||||||
|
|
||||||
## Project
|
## Project
|
||||||
Self-balancing two-wheeled robot: STM32F722 flight controller, hoverboard hub motors, Jetson Nano for AI/SLAM.
|
Self-balancing two-wheeled robot: Orin Nano Super, 2x VESC (IDs 68 left / 56 right), ESP32-S3 BALANCE, ESP32-S3 IO.
|
||||||
|
|
||||||
## Team
|
## Team
|
||||||
| Agent | Role | Focus |
|
| Agent | Role | Focus |
|
||||||
|-------|------|-------|
|
|-------|------|-------|
|
||||||
| **sl-firmware** | Embedded Firmware Lead | STM32 HAL, USB CDC debugging, SPI/UART, PlatformIO, DFU bootloader |
|
| **sl-firmware** | Embedded Firmware Lead | ESP32-S3/ESP-IDF, USB CDC debugging, SPI/UART, Arduino/ESP-IDF, esptool.py |
|
||||||
| **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
|
| **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
|
||||||
| **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 |
|
| **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 |
|
||||||
|
|
||||||
|
|||||||
12
TEAM.md
12
TEAM.md
@ -1,7 +1,7 @@
|
|||||||
# 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.
|
Self-balancing two-wheeled robot using a drone flight controller (ESP32-S3), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM.
|
||||||
|
|
||||||
## Current Status
|
## Current Status
|
||||||
- **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand
|
- **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand
|
||||||
@ -14,10 +14,10 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
|
|||||||
|
|
||||||
### 1. Embedded Firmware Engineer (Lead)
|
### 1. Embedded Firmware Engineer (Lead)
|
||||||
**Must-have:**
|
**Must-have:**
|
||||||
- Deep STM32 HAL experience (F7 series specifically)
|
- Deep ESP32-S3 HAL experience (F7 series specifically)
|
||||||
- USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts)
|
- USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts)
|
||||||
- SPI + UART + USB coexistence on STM32
|
- SPI + UART + USB coexistence on ESP32-S3
|
||||||
- PlatformIO or bare-metal STM32 toolchain
|
- PlatformIO or bare-metal ESP32-S3 toolchain
|
||||||
- DFU bootloader implementation
|
- DFU bootloader implementation
|
||||||
|
|
||||||
**Nice-to-have:**
|
**Nice-to-have:**
|
||||||
@ -25,7 +25,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
|
|||||||
- PID control loop tuning for balance robots
|
- 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 ESP32-S3 USB issues before — this is not a software logic bug, it's a hardware peripheral interaction issue.
|
||||||
|
|
||||||
### 2. Control Systems / Robotics Engineer
|
### 2. Control Systems / Robotics Engineer
|
||||||
**Must-have:**
|
**Must-have:**
|
||||||
@ -61,7 +61,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-S3 BALANCE (ESP32-S3, ICM-42688-P) |
|
||||||
| 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 |
|
||||||
|
|||||||
@ -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: ESP32-S3 (ESP32-S3 BALANCE FC)
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|||||||
@ -56,7 +56,7 @@
|
|||||||
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 FC mount (ESP32-S3 BALANCE)
|
||||||
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 FC onto standoffs; secure with M3×6 BHCS. Snug only — do not over-torque.
|
||||||
3. Orient USB-C port toward front of robot for cable access.
|
3. Orient USB-C port toward front of robot for cable access.
|
||||||
|
|||||||
@ -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 | BALANCE board standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE vibration isolation |
|
||||||
| 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
|
| 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
|
||||||
@ -90,7 +90,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
|
|||||||
|
|
||||||
| # | 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-S3 BALANCE FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front |
|
||||||
| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation |
|
| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation |
|
||||||
| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads |
|
| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads |
|
||||||
| 16 | Jetson Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
|
| 16 | Jetson Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
|
||||||
|
|||||||
@ -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-S3 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
|
||||||
| ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C |
|
| 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 | — |
|
||||||
|
|
||||||
|
|||||||
@ -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-S3 IO 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)
|
- ESP32-S3 IO
|
||||||
- Available UART: USART3 (PB10=TX, PB11=RX)
|
- Available UART: USART3 (PB10=TX, PB11=RX)
|
||||||
- Clock: 216 MHz
|
- Clock: 216 MHz
|
||||||
|
|
||||||
|
|||||||
@ -32,7 +32,7 @@ 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 ESP32-S3 BALANCE (ICM-42688-P IMU) | ✅ Have — balance brain |
|
||||||
| 1x Jetson Nano + Noctua fan | ✅ Have |
|
| 1x Jetson Nano + Noctua fan | ✅ Have |
|
||||||
| 1x RealSense D435i | ✅ Have |
|
| 1x RealSense D435i | ✅ Have |
|
||||||
| 1x RPLIDAR A1M8 | ✅ Have |
|
| 1x RPLIDAR A1M8 | ✅ Have |
|
||||||
@ -50,13 +50,13 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
|
|||||||
| 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART |
|
| 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:** ESP32-S3 (Xtensa LX7 dual-core 240MHz, 512KB SRAM, 8MB flash)
|
||||||
- **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
|
- **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 + ESP-IDF)
|
||||||
- **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 +95,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 ESP32-S3 BALANCE board has an ICM-42688-P IMU, proper voltage regulation, and CAN bus interface. We write a lean custom balance firmware using ESP-IDF.
|
||||||
|
|
||||||
### Architecture
|
### Architecture
|
||||||
```
|
```
|
||||||
@ -142,7 +142,7 @@ GND ──→ GND
|
|||||||
5V ←── 5V
|
5V ←── 5V
|
||||||
```
|
```
|
||||||
|
|
||||||
### Custom Firmware (STM32 C)
|
### Custom Firmware (ESP32-S3 C/C++)
|
||||||
|
|
||||||
```c
|
```c
|
||||||
// Core balance loop — runs in timer interrupt @ 1-8kHz
|
// Core balance loop — runs in timer interrupt @ 1-8kHz
|
||||||
@ -280,8 +280,8 @@ GND ──→ Common ground
|
|||||||
```
|
```
|
||||||
|
|
||||||
### Dev Tools
|
### Dev Tools
|
||||||
- **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD
|
- **Flashing:** esptool.py via USB (bootloader mode)
|
||||||
- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE
|
- **IDE:** PlatformIO + ESP-IDF, or Arduino IDE with ESP32 core
|
||||||
- **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
|
- **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
|
||||||
|
|
||||||
## Physical Design
|
## Physical Design
|
||||||
@ -375,7 +375,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-S3 (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
|
||||||
|
|||||||
@ -7,7 +7,7 @@
|
|||||||
│ ORIN NANO SUPER │
|
│ ORIN NANO SUPER │
|
||||||
│ (Top Plate — 25W) │
|
│ (Top Plate — 25W) │
|
||||||
│ │
|
│ │
|
||||||
│ USB-C ──── STM32 CDC (/dev/stm32-bridge, 921600 baud) │
|
│ USB-C ──── ESP32-S3 IO CDC (/dev/esp32-io, 921600 baud) │
|
||||||
│ USB-A1 ─── RealSense D435i (USB 3.1) │
|
│ USB-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) │
|
||||||
@ -25,7 +25,7 @@
|
|||||||
│ 921600 baud │ 921600 baud, 3.3V
|
│ 921600 baud │ 921600 baud, 3.3V
|
||||||
▼ ▼
|
▼ ▼
|
||||||
┌─────────────────────────────────────────────────────────────────────┐
|
┌─────────────────────────────────────────────────────────────────────┐
|
||||||
│ MAMBA F722S (FC) │
|
│ ESP32-S3 BALANCE / IO (FC) │
|
||||||
│ (Middle Plate — foam mounted) │
|
│ (Middle Plate — foam mounted) │
|
||||||
│ │
|
│ │
|
||||||
│ USB-C ──── Orin (CDC serial, primary link) │
|
│ USB-C ──── Orin (CDC serial, primary link) │
|
||||||
@ -72,7 +72,7 @@
|
|||||||
|------|----|-----------|-------|
|
|------|----|-----------|-------|
|
||||||
| Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus |
|
| Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus |
|
||||||
|
|
||||||
- Device: `/dev/ttyACM0` → symlink `/dev/stm32-bridge`
|
- Device: `/dev/ttyACM0` → symlink `/dev/esp32-io`
|
||||||
- Baud: 921600, 8N1
|
- Baud: 921600, 8N1
|
||||||
- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
|
- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
|
||||||
|
|
||||||
@ -139,7 +139,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
|||||||
| 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
|
| 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
|
||||||
|
|
||||||
|
|
||||||
## FC UART Summary (MAMBA F722S)
|
## FC UART Summary (ESP32-S3 IO)
|
||||||
|
|
||||||
| UART | Pins | Baud | Assignment | Notes |
|
| UART | Pins | Baud | Assignment | Notes |
|
||||||
|------|------|------|------------|-------|
|
|------|------|------|------------|-------|
|
||||||
@ -149,7 +149,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
|||||||
| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control |
|
| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control |
|
||||||
| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
|
| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
|
||||||
| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
|
| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
|
||||||
| USB CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` |
|
| USB CDC | USB-C | 921600 | Jetson primary | `/dev/esp32-io` |
|
||||||
|
|
||||||
|
|
||||||
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
|
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
|
||||||
@ -209,7 +209,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
|||||||
|
|
||||||
| Device | Interface | Power Draw |
|
| Device | Interface | Power Draw |
|
||||||
|--------|-----------|------------|
|
|--------|-----------|------------|
|
||||||
| STM32 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
|
| ESP32-S3 IO (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
|
||||||
| RealSense D435i | USB-A | ~1.5W (3.5W peak) |
|
| 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 |
|
||||||
@ -234,7 +234,7 @@ Orin Nano Super delivers up to 25W — USB peripherals are well within budget.
|
|||||||
└──────┬───────┘
|
└──────┬───────┘
|
||||||
│ UART4
|
│ UART4
|
||||||
┌────────────▼────────────┐
|
┌────────────▼────────────┐
|
||||||
│ MAMBA F722S │
|
│ ESP32-S3 BALANCE │
|
||||||
│ │
|
│ │
|
||||||
│ MPU6000 → Balance PID │
|
│ MPU6000 → Balance PID │
|
||||||
│ CRSF → Mode Manager │
|
│ CRSF → Mode Manager │
|
||||||
|
|||||||
2
flash.sh
2
flash.sh
@ -1,7 +1,7 @@
|
|||||||
#!/bin/bash
|
#!/bin/bash
|
||||||
# Flash SaltyLab — auto-reboot to DFU if serial port exists
|
# Flash SaltyLab — auto-reboot to DFU if serial port exists
|
||||||
PORT=/dev/cu.usbmodemSALTY0011
|
PORT=/dev/cu.usbmodemSALTY0011
|
||||||
FW=.pio/build/f722/firmware.bin
|
FW=.pio/build/esp32s3/firmware.bin
|
||||||
|
|
||||||
if [ -e "$PORT" ]; then
|
if [ -e "$PORT" ]; then
|
||||||
echo "Sending reboot-to-DFU..."
|
echo "Sending reboot-to-DFU..."
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef BARO_H
|
#ifndef BARO_H
|
||||||
#define BARO_H
|
#define BARO_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef BATTERY_H
|
#ifndef BATTERY_H
|
||||||
#define BATTERY_H
|
#define BATTERY_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
/*
|
/*
|
||||||
* battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533)
|
* battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533)
|
||||||
*
|
*
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef BNO055_H
|
#ifndef BNO055_H
|
||||||
#define BNO055_H
|
#define BNO055_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef BUZZER_H
|
#ifndef BUZZER_H
|
||||||
#define BUZZER_H
|
#define BUZZER_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef CONFIG_H
|
#ifndef CONFIG_H
|
||||||
#define CONFIG_H
|
#define CONFIG_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
/*
|
/*
|
||||||
* face_lcd.h — STM32 LCD Display Driver for Face Animations
|
* face_lcd.h — STM32 LCD Display Driver for Face Animations
|
||||||
*
|
*
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef FAN_H
|
#ifndef FAN_H
|
||||||
#define FAN_H
|
#define FAN_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef FAULT_HANDLER_H
|
#ifndef FAULT_HANDLER_H
|
||||||
#define FAULT_HANDLER_H
|
#define FAULT_HANDLER_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef I2C1_H
|
#ifndef I2C1_H
|
||||||
#define I2C1_H
|
#define I2C1_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef JETSON_CMD_H
|
#ifndef JETSON_CMD_H
|
||||||
#define JETSON_CMD_H
|
#define JETSON_CMD_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef JETSON_UART_H
|
#ifndef JETSON_UART_H
|
||||||
#define JETSON_UART_H
|
#define JETSON_UART_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef JLINK_H
|
#ifndef JLINK_H
|
||||||
#define JLINK_H
|
#define JLINK_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef ORIN_CAN_H
|
#ifndef ORIN_CAN_H
|
||||||
#define ORIN_CAN_H
|
#define ORIN_CAN_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef OTA_H
|
#ifndef OTA_H
|
||||||
#define OTA_H
|
#define OTA_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef PID_FLASH_H
|
#ifndef PID_FLASH_H
|
||||||
#define PID_FLASH_H
|
#define PID_FLASH_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef POWER_MGMT_H
|
#ifndef POWER_MGMT_H
|
||||||
#define POWER_MGMT_H
|
#define POWER_MGMT_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef UART_PROTOCOL_H
|
#ifndef UART_PROTOCOL_H
|
||||||
#define UART_PROTOCOL_H
|
#define UART_PROTOCOL_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef ULTRASONIC_H
|
#ifndef ULTRASONIC_H
|
||||||
#define ULTRASONIC_H
|
#define ULTRASONIC_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef VESC_CAN_H
|
#ifndef VESC_CAN_H
|
||||||
#define VESC_CAN_H
|
#define VESC_CAN_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef WATCHDOG_H
|
#ifndef WATCHDOG_H
|
||||||
#define WATCHDOG_H
|
#define WATCHDOG_H
|
||||||
|
|
||||||
|
|||||||
@ -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) |
|
| Motor controller | ESP32-S3 BALANCE (CAN bus 500 kbps) |
|
||||||
|
|
||||||
## 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-S3)
|
||||||
├── README.md # This file
|
├── README.md # This file
|
||||||
├── docs/
|
├── docs/
|
||||||
│ ├── pinout.md # GPIO/I2C/UART pinout reference
|
│ ├── pinout.md # GPIO/I2C/UART pinout reference
|
||||||
|
|||||||
@ -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-S3 firmware.
|
||||||
|
|
||||||
## Behavior Tree Sequence
|
## Behavior Tree Sequence
|
||||||
|
|
||||||
|
|||||||
@ -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-S3 bridge consumes this topic.
|
||||||
|
|
||||||
bt_navigator:
|
bt_navigator:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
|
|||||||
@ -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-S3 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-S3 CDC) ----------------------
|
||||||
# Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to STM32.
|
# Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3.
|
||||||
# Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
|
# 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
|
||||||
|
|||||||
@ -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-S3 BALANCE + CAN Bus + 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-S3 BALANCE (CAN Bus — Primary)
|
||||||
|
|
||||||
The STM32 acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**.
|
The ESP32-S3 BALANCE acts as a real-time motor + IMU controller. Communication is via **CAN bus (CANable2, SocketCAN)**.
|
||||||
|
|
||||||
### USB CDC Connection
|
### USB CDC Connection
|
||||||
| Connection | Detail |
|
| Connection | Detail |
|
||||||
|-----------|--------|
|
|-----------|--------|
|
||||||
| Interface | USB Micro-B on STM32 dev board → USB-A on Jetson |
|
| Interface | CANable2 USB → USB-A on Jetson (SocketCAN slcan0) |
|
||||||
| Device node | `/dev/ttyACM0` → symlink `/dev/stm32-bridge` (via udev) |
|
| Device node | `/dev/ttyUSB0` → symlink `/dev/canable2` (via udev); SocketCAN `slcan0` |
|
||||||
| Baud rate | 921600 (configured in STM32 firmware) |
|
| Baud rate | 500 kbps CAN bus |
|
||||||
| Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) |
|
| 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 | CANable2/ESP32 | 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 differential CAN signals (ISO 11898)
|
||||||
|
|
||||||
```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-S3 BALANCE bridge node):**
|
||||||
| ROS2 Topic | Direction | Content |
|
| ROS2 Topic | Direction | Content |
|
||||||
|-----------|-----------|---------
|
|-----------|-----------|---------
|
||||||
| `/saltybot/imu` | STM32→Jetson | IMU data (accel, gyro) at 50Hz |
|
| `/saltybot/imu` | ESP32-S3→Jetson | IMU data (accel, gyro) at 50Hz |
|
||||||
| `/saltybot/balance_state` | STM32→Jetson | Motor cmd, pitch, state |
|
| `/saltybot/balance_state` | ESP32-S3→Jetson | Motor cmd, pitch, state |
|
||||||
| `/cmd_vel` | Jetson→STM32 | Velocity commands → `C<spd>,<str>\n` |
|
| `/cmd_vel` | Jetson→ESP32-S3 | Velocity commands → `C<spd>,<str>\n` |
|
||||||
| `/saltybot/estop` | Jetson→STM32 | Emergency stop |
|
| `/saltybot/estop` | Jetson→ESP32-S3 | Emergency stop |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@ -266,7 +266,7 @@ sudo mkdir -p /mnt/nvme
|
|||||||
|------|------|----------|
|
|------|------|----------|
|
||||||
| USB-A (top, blue) | USB 3.1 Gen 1 | RealSense D435i |
|
| USB-A (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) | CANable2 USB 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-S3 BALANCE (fallback) |
|
||||||
| 10 | RXD0 | 3.3V | UART RX ← STM32 (fallback) |
|
| 10 | RXD0 | 3.3V | UART RX ← ESP32-S3 BALANCE (fallback) |
|
||||||
| USB-A ×2 | — | 5V | D435i, RPLIDAR |
|
| USB-A ×2 | — | 5V | D435i, RPLIDAR |
|
||||||
| USB-C | — | 5V | STM32 CDC |
|
| USB-C | — | 5V | CANable2 USB |
|
||||||
| CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left |
|
| CSI-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-S3 BALANCE (CANable2 USB)
|
||||||
KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
||||||
SYMLINK+="stm32-bridge", MODE="0666"
|
SYMLINK+="esp32-balance", MODE="0666"
|
||||||
|
|
||||||
# Intel RealSense D435i
|
# Intel RealSense D435i
|
||||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \
|
SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \
|
||||||
|
|||||||
@ -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-S3 BALANCE | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V |
|
||||||
| 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active |
|
| 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-S3 + logic 5V rail
|
||||||
│
|
│
|
||||||
└─► Hoverboard ESC ──► Hub motors (48V loop)
|
└─► Hoverboard ESC ──► Hub motors (48V loop)
|
||||||
```
|
```
|
||||||
|
|||||||
@ -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-balance 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-S3 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
|
||||||
|
|||||||
@ -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-S3 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-balance 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-S3 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-S3 jetson heartbeat timeout for consistency.
|
||||||
cmd_vel_timeout: 0.5 # seconds
|
cmd_vel_timeout: 0.5 # seconds
|
||||||
|
|||||||
@ -1,18 +1,18 @@
|
|||||||
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
|
# esp32_cmd_params.yaml — Configuration for esp32_cmd_node (Issue #119)
|
||||||
# Binary-framed Jetson↔STM32 bridge at 921600 baud.
|
# Binary-framed Jetson↔ESP32-S3 BALANCE bridge at 460800 baud.
|
||||||
|
|
||||||
# ── Serial port ────────────────────────────────────────────────────────────────
|
# ── Serial port ────────────────────────────────────────────────────────────────
|
||||||
# Use /dev/stm32-bridge if the udev rule is applied:
|
# Use /dev/esp32-balance if the udev rule is applied:
|
||||||
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
|
# SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
|
||||||
# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
|
# SYMLINK+="esp32-balance", MODE="0660", GROUP="dialout"
|
||||||
serial_port: /dev/ttyACM0
|
serial_port: /dev/esp32-balance
|
||||||
baud_rate: 921600
|
baud_rate: 460800
|
||||||
reconnect_delay: 2.0 # seconds between USB reconnect attempts
|
reconnect_delay: 2.0 # seconds between USB reconnect attempts
|
||||||
|
|
||||||
# ── Heartbeat ─────────────────────────────────────────────────────────────────
|
# ── Heartbeat ─────────────────────────────────────────────────────────────────
|
||||||
# HEARTBEAT frame sent every heartbeat_period seconds.
|
# HEARTBEAT frame sent every heartbeat_period seconds.
|
||||||
# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
|
# ESP32-S3 fires watchdog and reverts to safe state if no frame received for 500ms.
|
||||||
heartbeat_period: 0.2 # 200ms → well within 500ms STM32 watchdog
|
heartbeat_period: 0.2 # 200ms → well within 500ms ESP32 watchdog
|
||||||
|
|
||||||
# ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
|
# ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
|
||||||
# If no /cmd_vel message received for watchdog_timeout seconds,
|
# If no /cmd_vel message received for watchdog_timeout seconds,
|
||||||
@ -1,6 +1,6 @@
|
|||||||
remote_estop_node:
|
remote_estop_node:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
serial_port: /dev/stm32-bridge
|
serial_port: /dev/esp32-balance
|
||||||
baud_rate: 921600
|
baud_rate: 921600
|
||||||
mqtt_host: "mqtt.example.com"
|
mqtt_host: "mqtt.example.com"
|
||||||
mqtt_port: 1883
|
mqtt_port: 1883
|
||||||
|
|||||||
@ -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-S3 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-S3 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)"),
|
||||||
|
|||||||
@ -1,10 +1,10 @@
|
|||||||
"""
|
"""
|
||||||
cmd_vel_bridge.launch.py — Nav2 cmd_vel → STM32 autonomous drive bridge.
|
cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32-S3 autonomous drive bridge.
|
||||||
|
|
||||||
Starts cmd_vel_bridge_node, which owns the serial port exclusively and provides:
|
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-S3 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-S3 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-S3 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)"),
|
||||||
|
|||||||
@ -1,14 +1,14 @@
|
|||||||
"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119).
|
"""esp32_cmd.launch.py — Launch the binary-framed ESP32 command node (Issue #119).
|
||||||
|
|
||||||
Usage:
|
Usage:
|
||||||
# Default (binary protocol, bidirectional):
|
# Default (binary protocol, bidirectional):
|
||||||
ros2 launch saltybot_bridge stm32_cmd.launch.py
|
ros2 launch saltybot_bridge esp32_cmd.launch.py
|
||||||
|
|
||||||
# Override serial port:
|
# Override serial port:
|
||||||
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
|
ros2 launch saltybot_bridge esp32_cmd.launch.py serial_port:=/dev/ttyACM1
|
||||||
|
|
||||||
# Custom velocity scales:
|
# Custom velocity scales:
|
||||||
ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
|
ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
@ -21,11 +21,11 @@ from launch_ros.actions import Node
|
|||||||
|
|
||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
pkg = get_package_share_directory("saltybot_bridge")
|
pkg = get_package_share_directory("saltybot_bridge")
|
||||||
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
|
params_file = os.path.join(pkg, "config", "esp32_cmd_params.yaml")
|
||||||
|
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
DeclareLaunchArgument("serial_port", default_value="/dev/esp32-balance"),
|
||||||
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
DeclareLaunchArgument("baud_rate", default_value="460800"),
|
||||||
DeclareLaunchArgument("speed_scale", default_value="1000.0"),
|
DeclareLaunchArgument("speed_scale", default_value="1000.0"),
|
||||||
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
|
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
|
||||||
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
|
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
|
||||||
@ -33,8 +33,8 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
|
|
||||||
Node(
|
Node(
|
||||||
package="saltybot_bridge",
|
package="saltybot_bridge",
|
||||||
executable="stm32_cmd_node",
|
executable="esp32_cmd_node",
|
||||||
name="stm32_cmd_node",
|
name="esp32_cmd_node",
|
||||||
output="screen",
|
output="screen",
|
||||||
emulate_tty=True,
|
emulate_tty=True,
|
||||||
parameters=[
|
parameters=[
|
||||||
@ -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-S3 firmware transmitting JSON telemetry frames (50 Hz)
|
||||||
- ROS2 environment sourced (source install/setup.bash)
|
- ROS2 environment sourced (source install/setup.bash)
|
||||||
|
|
||||||
Note:
|
Note:
|
||||||
|
|||||||
@ -1,6 +1,6 @@
|
|||||||
"""battery_node.py — Battery management for saltybot (Issue #125).
|
"""battery_node.py — Battery management for saltybot (Issue #125).
|
||||||
|
|
||||||
Subscribes to /saltybot/telemetry/battery (JSON from stm32_cmd_node) and:
|
Subscribes to /saltybot/telemetry/battery (JSON from esp32_io_cmd_node) and:
|
||||||
- Publishes sensor_msgs/BatteryState on /saltybot/battery
|
- Publishes sensor_msgs/BatteryState on /saltybot/battery
|
||||||
- Publishes JSON alerts on /saltybot/battery/alert at threshold crossings
|
- Publishes JSON alerts on /saltybot/battery/alert at threshold crossings
|
||||||
- Reduces speed limit at low SoC via /saltybot/speed_limit (std_msgs/Float32)
|
- Reduces speed limit at low SoC via /saltybot/speed_limit (std_msgs/Float32)
|
||||||
@ -14,7 +14,7 @@ Alert levels (SoC thresholds):
|
|||||||
5% EMERGENCY — publish zero /cmd_vel, disarm, log + alert
|
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-S3 IO BATTERY telemetry (fuel gauge or lookup on ESP32-S3 IO)
|
||||||
2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known
|
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-S3 IO."""
|
||||||
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()
|
||||||
|
|||||||
@ -1,5 +1,5 @@
|
|||||||
"""
|
"""
|
||||||
cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 drive command bridge.
|
cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 BALANCE drive command bridge.
|
||||||
|
|
||||||
Extends the basic saltybot_cmd_node with four additions required for safe
|
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-S3 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-S3 IO reverts steer to 0 after 500ms silence.
|
||||||
|
|
||||||
Telemetry (50 Hz from STM32):
|
Telemetry (50 Hz from ESP32-S3 IO):
|
||||||
Same RX/publish pipeline as saltybot_cmd_node.
|
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._balance_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-S3 IO 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._balance_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-S3 IO
|
||||||
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-S3 IO 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._balance_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)
|
||||||
@ -410,7 +410,7 @@ class CmdVelBridgeNode(Node):
|
|||||||
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-S3 IO IMU fault: errno={errno}")
|
||||||
|
|
||||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
|||||||
@ -1,8 +1,8 @@
|
|||||||
"""
|
"""
|
||||||
remote_estop_node.py -- Remote e-stop bridge: MQTT -> STM32 USB CDC
|
remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32-S3 IO USB CDC
|
||||||
|
|
||||||
{"kill": true} -> writes 'E\n' to STM32 (ESTOP_REMOTE, immediate motor cutoff)
|
{"kill": true} -> writes 'E\n' to ESP32-S3 IO (ESTOP_REMOTE, immediate motor cutoff)
|
||||||
{"kill": false} -> writes 'Z\n' to STM32 (clear latch, robot can re-arm)
|
{"kill": false} -> writes 'Z\n' to ESP32-S3 IO (clear latch, robot can re-arm)
|
||||||
|
|
||||||
Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in
|
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-io')
|
||||||
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)
|
||||||
|
|||||||
@ -1,20 +1,20 @@
|
|||||||
"""
|
"""
|
||||||
saltybot_cmd_node — full bidirectional STM32↔Jetson bridge
|
saltybot_cmd_node — full bidirectional ESP32-S3 IO↔Jetson bridge
|
||||||
Combines telemetry RX (from serial_bridge_node) with drive command TX.
|
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-S3 IO):
|
||||||
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-S3 IO
|
||||||
Heartbeat timer (200ms) → H\\n → STM32
|
Heartbeat timer (200ms) → H\\n → ESP32-S3 IO
|
||||||
|
|
||||||
Protocol:
|
Protocol:
|
||||||
H\\n — heartbeat. STM32 reverts steer to 0 if gap > 500ms.
|
H\\n — heartbeat. ESP32-S3 IO reverts steer to 0 if gap > 500ms.
|
||||||
C<spd>,<str>\\n — drive command. speed/steer: -1000..+1000 integers.
|
C<spd>,<str>\\n — drive command. speed/steer: -1000..+1000 integers.
|
||||||
C command also refreshes STM32 heartbeat timer.
|
C command also refreshes ESP32-S3 IO heartbeat timer.
|
||||||
|
|
||||||
Twist mapping (configurable via ROS2 params):
|
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-S3 IO 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)
|
||||||
@ -298,7 +298,7 @@ class SaltybotCmdNode(Node):
|
|||||||
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-S3 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-S3 IO reverts steer to 0 if gap > 500ms."""
|
||||||
self._write(b"H\n")
|
self._write(b"H\n")
|
||||||
|
|
||||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||||
|
|||||||
@ -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-S3 IO 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_io_serial_bridge")
|
||||||
|
|
||||||
# ── Parameters ────────────────────────────────────────────────────────
|
# ── Parameters ────────────────────────────────────────────────────────
|
||||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
||||||
@ -83,11 +83,11 @@ 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-S3 IO 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(
|
||||||
f"stm32_serial_bridge started — {port} @ {baud} baud"
|
f"esp32_io_serial_bridge started — {port} @ {baud} baud"
|
||||||
)
|
)
|
||||||
|
|
||||||
# ── Serial management ─────────────────────────────────────────────────────
|
# ── Serial management ─────────────────────────────────────────────────────
|
||||||
@ -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-S3 IO 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-S3 IO 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.
|
||||||
|
|
||||||
@ -297,7 +297,7 @@ class SerialBridgeNode(Node):
|
|||||||
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-S3 IO reported IMU fault: errno={errno}")
|
||||||
|
|
||||||
def destroy_node(self):
|
def destroy_node(self):
|
||||||
self._close_serial()
|
self._close_serial()
|
||||||
|
|||||||
@ -1,483 +0,0 @@
|
|||||||
"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge.
|
|
||||||
|
|
||||||
Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
|
|
||||||
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
|
|
||||||
|
|
||||||
TX commands (Jetson → STM32):
|
|
||||||
SPEED_STEER — 50 Hz from /cmd_vel subscription
|
|
||||||
HEARTBEAT — 200 ms timer (STM32 watchdog fires at 500 ms)
|
|
||||||
ARM — via /saltybot/arm service
|
|
||||||
SET_MODE — via /saltybot/set_mode service
|
|
||||||
PID_UPDATE — via /saltybot/pid_update topic
|
|
||||||
|
|
||||||
Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
|
|
||||||
|
|
||||||
RX telemetry (STM32 → Jetson):
|
|
||||||
IMU → /saltybot/imu (sensor_msgs/Imu)
|
|
||||||
BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON)
|
|
||||||
MOTOR_RPM → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
|
|
||||||
ARM_STATE → /saltybot/arm_state (std_msgs/String JSON)
|
|
||||||
ERROR → /saltybot/error (std_msgs/String JSON)
|
|
||||||
All frames → /diagnostics (diagnostic_msgs/DiagnosticArray)
|
|
||||||
|
|
||||||
Auto-reconnect: USB disconnect is detected when serial.read() raises; node
|
|
||||||
continuously retries at reconnect_delay interval.
|
|
||||||
|
|
||||||
This node owns /dev/ttyACM0 exclusively — do NOT run alongside
|
|
||||||
serial_bridge_node or saltybot_cmd_node on the same port.
|
|
||||||
|
|
||||||
Parameters (config/stm32_cmd_params.yaml):
|
|
||||||
serial_port /dev/ttyACM0
|
|
||||||
baud_rate 921600
|
|
||||||
reconnect_delay 2.0 (seconds)
|
|
||||||
heartbeat_period 0.2 (seconds)
|
|
||||||
watchdog_timeout 0.5 (seconds — no /cmd_vel → send zero-speed)
|
|
||||||
speed_scale 1000.0 (linear.x m/s → ESC units)
|
|
||||||
steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention)
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
import threading
|
|
||||||
import time
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
|
||||||
|
|
||||||
import serial
|
|
||||||
|
|
||||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
|
||||||
from geometry_msgs.msg import Twist
|
|
||||||
from sensor_msgs.msg import Imu
|
|
||||||
from std_msgs.msg import String
|
|
||||||
from std_srvs.srv import SetBool, Trigger
|
|
||||||
|
|
||||||
from .stm32_protocol import (
|
|
||||||
FrameParser,
|
|
||||||
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
|
||||||
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
|
|
||||||
encode_pid_update,
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Constants ─────────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
IMU_FRAME_ID = "imu_link"
|
|
||||||
_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
|
||||||
|
|
||||||
|
|
||||||
def _clamp(v: float, lo: float, hi: float) -> float:
|
|
||||||
return max(lo, min(hi, v))
|
|
||||||
|
|
||||||
|
|
||||||
# ── Node ──────────────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
class Stm32CmdNode(Node):
|
|
||||||
"""Binary-framed Jetson↔STM32 bridge node."""
|
|
||||||
|
|
||||||
def __init__(self) -> None:
|
|
||||||
super().__init__("stm32_cmd_node")
|
|
||||||
|
|
||||||
# ── Parameters ────────────────────────────────────────────────────────
|
|
||||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
|
||||||
self.declare_parameter("baud_rate", 921600)
|
|
||||||
self.declare_parameter("reconnect_delay", 2.0)
|
|
||||||
self.declare_parameter("heartbeat_period", 0.2)
|
|
||||||
self.declare_parameter("watchdog_timeout", 0.5)
|
|
||||||
self.declare_parameter("speed_scale", 1000.0)
|
|
||||||
self.declare_parameter("steer_scale", -500.0)
|
|
||||||
|
|
||||||
port = self.get_parameter("serial_port").value
|
|
||||||
baud = self.get_parameter("baud_rate").value
|
|
||||||
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
|
||||||
self._hb_period = self.get_parameter("heartbeat_period").value
|
|
||||||
self._wd_timeout = self.get_parameter("watchdog_timeout").value
|
|
||||||
self._speed_scale = self.get_parameter("speed_scale").value
|
|
||||||
self._steer_scale = self.get_parameter("steer_scale").value
|
|
||||||
|
|
||||||
# ── QoS ───────────────────────────────────────────────────────────────
|
|
||||||
sensor_qos = QoSProfile(
|
|
||||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
|
||||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
|
||||||
)
|
|
||||||
rel_qos = QoSProfile(
|
|
||||||
reliability=ReliabilityPolicy.RELIABLE,
|
|
||||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Publishers ────────────────────────────────────────────────────────
|
|
||||||
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos)
|
|
||||||
self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos)
|
|
||||||
self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos)
|
|
||||||
self._battery_pub = self.create_publisher(String, "/saltybot/telemetry/battery", rel_qos)
|
|
||||||
self._rpm_pub = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
|
|
||||||
self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos)
|
|
||||||
|
|
||||||
# ── Subscribers ───────────────────────────────────────────────────────
|
|
||||||
self._cmd_vel_sub = self.create_subscription(
|
|
||||||
Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
|
|
||||||
)
|
|
||||||
self._pid_sub = self.create_subscription(
|
|
||||||
String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Services ──────────────────────────────────────────────────────────
|
|
||||||
self._arm_srv = self.create_service(SetBool, "/saltybot/arm", self._svc_arm)
|
|
||||||
self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode)
|
|
||||||
|
|
||||||
# ── Serial state ──────────────────────────────────────────────────────
|
|
||||||
self._port_name = port
|
|
||||||
self._baud = baud
|
|
||||||
self._ser: serial.Serial | None = None
|
|
||||||
self._ser_lock = threading.Lock()
|
|
||||||
self._parser = FrameParser()
|
|
||||||
|
|
||||||
# ── TX state ──────────────────────────────────────────────────────────
|
|
||||||
self._last_speed = 0
|
|
||||||
self._last_steer = 0
|
|
||||||
self._last_cmd_t = time.monotonic()
|
|
||||||
self._watchdog_sent = False # tracks whether we already sent zero
|
|
||||||
|
|
||||||
# ── Diagnostics state ──────────────────────────────────────────────────
|
|
||||||
self._last_arm_state = -1
|
|
||||||
self._last_battery_mv = 0
|
|
||||||
self._rx_frame_count = 0
|
|
||||||
|
|
||||||
# ── Open serial and start timers ──────────────────────────────────────
|
|
||||||
self._open_serial()
|
|
||||||
|
|
||||||
# Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean)
|
|
||||||
self._read_timer = self.create_timer(0.005, self._read_cb)
|
|
||||||
# Heartbeat TX
|
|
||||||
self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb)
|
|
||||||
# Watchdog check (fires at 2× watchdog_timeout for quick detection)
|
|
||||||
self._wd_timer = self.create_timer(self._wd_timeout / 2, self._watchdog_cb)
|
|
||||||
# Periodic diagnostics
|
|
||||||
self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
|
|
||||||
|
|
||||||
self.get_logger().info(
|
|
||||||
f"stm32_cmd_node started — {port} @ {baud} baud | "
|
|
||||||
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Serial management ─────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _open_serial(self) -> bool:
|
|
||||||
with self._ser_lock:
|
|
||||||
try:
|
|
||||||
self._ser = serial.Serial(
|
|
||||||
port=self._port_name,
|
|
||||||
baudrate=self._baud,
|
|
||||||
timeout=0.005, # non-blocking reads
|
|
||||||
write_timeout=0.1,
|
|
||||||
)
|
|
||||||
self._ser.reset_input_buffer()
|
|
||||||
self._parser.reset()
|
|
||||||
self.get_logger().info(f"Serial open: {self._port_name}")
|
|
||||||
return True
|
|
||||||
except serial.SerialException as exc:
|
|
||||||
self.get_logger().error(
|
|
||||||
f"Cannot open {self._port_name}: {exc}",
|
|
||||||
throttle_duration_sec=self._reconnect_delay,
|
|
||||||
)
|
|
||||||
self._ser = None
|
|
||||||
return False
|
|
||||||
|
|
||||||
def _close_serial(self) -> None:
|
|
||||||
with self._ser_lock:
|
|
||||||
if self._ser and self._ser.is_open:
|
|
||||||
try:
|
|
||||||
self._ser.close()
|
|
||||||
except Exception:
|
|
||||||
pass
|
|
||||||
self._ser = None
|
|
||||||
|
|
||||||
def _write(self, data: bytes) -> bool:
|
|
||||||
"""Thread-safe serial write. Returns False if port is not open."""
|
|
||||||
with self._ser_lock:
|
|
||||||
if self._ser is None or not self._ser.is_open:
|
|
||||||
return False
|
|
||||||
try:
|
|
||||||
self._ser.write(data)
|
|
||||||
return True
|
|
||||||
except serial.SerialException as exc:
|
|
||||||
self.get_logger().error(f"Serial write error: {exc}")
|
|
||||||
self._ser = None
|
|
||||||
return False
|
|
||||||
|
|
||||||
# ── RX — read callback ────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _read_cb(self) -> None:
|
|
||||||
"""Read bytes from serial and feed them to the frame parser."""
|
|
||||||
raw: bytes | None = None
|
|
||||||
reconnect_needed = False
|
|
||||||
|
|
||||||
with self._ser_lock:
|
|
||||||
if self._ser is None or not self._ser.is_open:
|
|
||||||
reconnect_needed = True
|
|
||||||
else:
|
|
||||||
try:
|
|
||||||
n = self._ser.in_waiting
|
|
||||||
if n:
|
|
||||||
raw = self._ser.read(n)
|
|
||||||
except serial.SerialException as exc:
|
|
||||||
self.get_logger().error(f"Serial read error: {exc}")
|
|
||||||
self._ser = None
|
|
||||||
reconnect_needed = True
|
|
||||||
|
|
||||||
if reconnect_needed:
|
|
||||||
self.get_logger().warn(
|
|
||||||
"Serial disconnected — will retry",
|
|
||||||
throttle_duration_sec=self._reconnect_delay,
|
|
||||||
)
|
|
||||||
time.sleep(self._reconnect_delay)
|
|
||||||
self._open_serial()
|
|
||||||
return
|
|
||||||
|
|
||||||
if not raw:
|
|
||||||
return
|
|
||||||
|
|
||||||
for byte in raw:
|
|
||||||
frame = self._parser.feed(byte)
|
|
||||||
if frame is not None:
|
|
||||||
self._rx_frame_count += 1
|
|
||||||
self._dispatch_frame(frame)
|
|
||||||
|
|
||||||
def _dispatch_frame(self, frame) -> None:
|
|
||||||
"""Route a decoded frame to the appropriate publisher."""
|
|
||||||
now = self.get_clock().now().to_msg()
|
|
||||||
|
|
||||||
if isinstance(frame, ImuFrame):
|
|
||||||
self._publish_imu(frame, now)
|
|
||||||
|
|
||||||
elif isinstance(frame, BatteryFrame):
|
|
||||||
self._publish_battery(frame, now)
|
|
||||||
|
|
||||||
elif isinstance(frame, MotorRpmFrame):
|
|
||||||
self._publish_motor_rpm(frame, now)
|
|
||||||
|
|
||||||
elif isinstance(frame, ArmStateFrame):
|
|
||||||
self._publish_arm_state(frame, now)
|
|
||||||
|
|
||||||
elif isinstance(frame, ErrorFrame):
|
|
||||||
self._publish_error(frame, now)
|
|
||||||
|
|
||||||
elif isinstance(frame, tuple):
|
|
||||||
type_code, payload = frame
|
|
||||||
self.get_logger().debug(
|
|
||||||
f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}"
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Telemetry publishers ──────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _publish_imu(self, frame: ImuFrame, stamp) -> None:
|
|
||||||
msg = Imu()
|
|
||||||
msg.header.stamp = stamp
|
|
||||||
msg.header.frame_id = IMU_FRAME_ID
|
|
||||||
|
|
||||||
# orientation: unknown — signal with -1 in first covariance
|
|
||||||
msg.orientation_covariance[0] = -1.0
|
|
||||||
|
|
||||||
msg.angular_velocity.x = math.radians(frame.pitch_deg)
|
|
||||||
msg.angular_velocity.y = math.radians(frame.roll_deg)
|
|
||||||
msg.angular_velocity.z = math.radians(frame.yaw_deg)
|
|
||||||
cov = math.radians(0.3) ** 2 # ±0.3° noise estimate from STM32 BMI088
|
|
||||||
msg.angular_velocity_covariance[0] = cov
|
|
||||||
msg.angular_velocity_covariance[4] = cov
|
|
||||||
msg.angular_velocity_covariance[8] = cov
|
|
||||||
|
|
||||||
msg.linear_acceleration.x = frame.accel_x
|
|
||||||
msg.linear_acceleration.y = frame.accel_y
|
|
||||||
msg.linear_acceleration.z = frame.accel_z
|
|
||||||
acov = 0.05 ** 2 # ±0.05 m/s² noise
|
|
||||||
msg.linear_acceleration_covariance[0] = acov
|
|
||||||
msg.linear_acceleration_covariance[4] = acov
|
|
||||||
msg.linear_acceleration_covariance[8] = acov
|
|
||||||
|
|
||||||
self._imu_pub.publish(msg)
|
|
||||||
|
|
||||||
def _publish_battery(self, frame: BatteryFrame, stamp) -> None:
|
|
||||||
payload = {
|
|
||||||
"voltage_v": round(frame.voltage_mv / 1000.0, 3),
|
|
||||||
"voltage_mv": frame.voltage_mv,
|
|
||||||
"current_ma": frame.current_ma,
|
|
||||||
"soc_pct": frame.soc_pct,
|
|
||||||
"charging": frame.current_ma < -100,
|
|
||||||
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
|
|
||||||
}
|
|
||||||
self._last_battery_mv = frame.voltage_mv
|
|
||||||
msg = String()
|
|
||||||
msg.data = json.dumps(payload)
|
|
||||||
self._battery_pub.publish(msg)
|
|
||||||
|
|
||||||
def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None:
|
|
||||||
payload = {
|
|
||||||
"left_rpm": frame.left_rpm,
|
|
||||||
"right_rpm": frame.right_rpm,
|
|
||||||
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
|
|
||||||
}
|
|
||||||
msg = String()
|
|
||||||
msg.data = json.dumps(payload)
|
|
||||||
self._rpm_pub.publish(msg)
|
|
||||||
|
|
||||||
def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None:
|
|
||||||
label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})")
|
|
||||||
if frame.state != self._last_arm_state:
|
|
||||||
self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})")
|
|
||||||
self._last_arm_state = frame.state
|
|
||||||
|
|
||||||
payload = {
|
|
||||||
"state": frame.state,
|
|
||||||
"state_label": label,
|
|
||||||
"error_flags": frame.error_flags,
|
|
||||||
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
|
|
||||||
}
|
|
||||||
msg = String()
|
|
||||||
msg.data = json.dumps(payload)
|
|
||||||
self._arm_pub.publish(msg)
|
|
||||||
|
|
||||||
def _publish_error(self, frame: ErrorFrame, stamp) -> None:
|
|
||||||
self.get_logger().error(
|
|
||||||
f"STM32 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
|
|
||||||
)
|
|
||||||
payload = {
|
|
||||||
"error_code": frame.error_code,
|
|
||||||
"subcode": frame.subcode,
|
|
||||||
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
|
|
||||||
}
|
|
||||||
msg = String()
|
|
||||||
msg.data = json.dumps(payload)
|
|
||||||
self._error_pub.publish(msg)
|
|
||||||
|
|
||||||
# ── TX — command send ─────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _on_cmd_vel(self, msg: Twist) -> None:
|
|
||||||
"""Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz."""
|
|
||||||
speed = int(_clamp(msg.linear.x * self._speed_scale, -1000.0, 1000.0))
|
|
||||||
steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0))
|
|
||||||
self._last_speed = speed
|
|
||||||
self._last_steer = steer
|
|
||||||
self._last_cmd_t = time.monotonic()
|
|
||||||
self._watchdog_sent = False
|
|
||||||
|
|
||||||
frame = encode_speed_steer(speed, steer)
|
|
||||||
if not self._write(frame):
|
|
||||||
self.get_logger().warn(
|
|
||||||
"SPEED_STEER dropped — serial not open",
|
|
||||||
throttle_duration_sec=2.0,
|
|
||||||
)
|
|
||||||
|
|
||||||
def _heartbeat_cb(self) -> None:
|
|
||||||
"""Send HEARTBEAT every heartbeat_period (default 200ms)."""
|
|
||||||
self._write(encode_heartbeat())
|
|
||||||
|
|
||||||
def _watchdog_cb(self) -> None:
|
|
||||||
"""Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
|
|
||||||
if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
|
|
||||||
if not self._watchdog_sent:
|
|
||||||
self.get_logger().warn(
|
|
||||||
f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
|
|
||||||
)
|
|
||||||
self._watchdog_sent = True
|
|
||||||
self._last_speed = 0
|
|
||||||
self._last_steer = 0
|
|
||||||
self._write(encode_speed_steer(0, 0))
|
|
||||||
|
|
||||||
def _on_pid_update(self, msg: String) -> None:
|
|
||||||
"""Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
|
|
||||||
try:
|
|
||||||
data = json.loads(msg.data)
|
|
||||||
kp = float(data["kp"])
|
|
||||||
ki = float(data["ki"])
|
|
||||||
kd = float(data["kd"])
|
|
||||||
except (ValueError, KeyError, json.JSONDecodeError) as exc:
|
|
||||||
self.get_logger().error(f"Bad PID update JSON: {exc}")
|
|
||||||
return
|
|
||||||
frame = encode_pid_update(kp, ki, kd)
|
|
||||||
if self._write(frame):
|
|
||||||
self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
|
|
||||||
else:
|
|
||||||
self.get_logger().warn("PID_UPDATE dropped — serial not open")
|
|
||||||
|
|
||||||
# ── Services ──────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _svc_arm(self, request: SetBool.Request, response: SetBool.Response):
|
|
||||||
"""SetBool(True) = arm, SetBool(False) = disarm."""
|
|
||||||
arm = request.data
|
|
||||||
frame = encode_arm(arm)
|
|
||||||
ok = self._write(frame)
|
|
||||||
response.success = ok
|
|
||||||
response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open"
|
|
||||||
self.get_logger().info(
|
|
||||||
f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}"
|
|
||||||
)
|
|
||||||
return response
|
|
||||||
|
|
||||||
def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response):
|
|
||||||
"""SetBool: data maps to mode byte (True=1, False=0)."""
|
|
||||||
mode = 1 if request.data else 0
|
|
||||||
frame = encode_set_mode(mode)
|
|
||||||
ok = self._write(frame)
|
|
||||||
response.success = ok
|
|
||||||
response.message = f"mode={mode}" if ok else "serial not open"
|
|
||||||
return response
|
|
||||||
|
|
||||||
# ── Diagnostics ───────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _publish_diagnostics(self) -> None:
|
|
||||||
diag = DiagnosticArray()
|
|
||||||
diag.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
|
|
||||||
status = DiagnosticStatus()
|
|
||||||
status.name = "saltybot/stm32_cmd_node"
|
|
||||||
status.hardware_id = "stm32f722"
|
|
||||||
|
|
||||||
port_ok = self._ser is not None and self._ser.is_open
|
|
||||||
if port_ok:
|
|
||||||
status.level = DiagnosticStatus.OK
|
|
||||||
status.message = "Serial OK"
|
|
||||||
else:
|
|
||||||
status.level = DiagnosticStatus.ERROR
|
|
||||||
status.message = f"Serial disconnected: {self._port_name}"
|
|
||||||
|
|
||||||
wd_age = time.monotonic() - self._last_cmd_t
|
|
||||||
status.values = [
|
|
||||||
KeyValue(key="serial_port", value=self._port_name),
|
|
||||||
KeyValue(key="port_open", value=str(port_ok)),
|
|
||||||
KeyValue(key="rx_frames", value=str(self._rx_frame_count)),
|
|
||||||
KeyValue(key="rx_errors", value=str(self._parser.frames_error)),
|
|
||||||
KeyValue(key="last_speed", value=str(self._last_speed)),
|
|
||||||
KeyValue(key="last_steer", value=str(self._last_steer)),
|
|
||||||
KeyValue(key="cmd_vel_age_s", value=f"{wd_age:.2f}"),
|
|
||||||
KeyValue(key="battery_mv", value=str(self._last_battery_mv)),
|
|
||||||
KeyValue(key="arm_state", value=_ARM_LABEL.get(self._last_arm_state, "?")),
|
|
||||||
]
|
|
||||||
diag.status.append(status)
|
|
||||||
self._diag_pub.publish(diag)
|
|
||||||
|
|
||||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def destroy_node(self) -> None:
|
|
||||||
# Send zero-speed + disarm on shutdown
|
|
||||||
self._write(encode_speed_steer(0, 0))
|
|
||||||
self._write(encode_arm(False))
|
|
||||||
self._close_serial()
|
|
||||||
super().destroy_node()
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None) -> None:
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = Stm32CmdNode()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@ -1,332 +0,0 @@
|
|||||||
"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
|
|
||||||
|
|
||||||
Issue #119: defines the binary serial protocol between the Jetson Nano and the
|
|
||||||
STM32F722 flight controller over USB CDC @ 921600 baud.
|
|
||||||
|
|
||||||
Frame layout (all multi-byte fields are big-endian):
|
|
||||||
┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
|
|
||||||
│ STX │ TYPE │ LEN │ PAYLOAD │ CRC16 │ ETX │
|
|
||||||
│ 0x02 │ 1B │ 1B │ LEN bytes │ 2B BE │ 0x03 │
|
|
||||||
└──────┴──────┴──────┴──────────────────┴───────────┴──────┘
|
|
||||||
|
|
||||||
CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
|
|
||||||
CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
|
|
||||||
|
|
||||||
Command types (Jetson → STM32):
|
|
||||||
0x01 HEARTBEAT — no payload (len=0)
|
|
||||||
0x02 SPEED_STEER — int16 speed + int16 steer (len=4) range: -1000..+1000
|
|
||||||
0x03 ARM — uint8 (0=disarm, 1=arm) (len=1)
|
|
||||||
0x04 SET_MODE — uint8 mode (len=1)
|
|
||||||
0x05 PID_UPDATE — float32 kp + ki + kd (len=12)
|
|
||||||
|
|
||||||
Telemetry types (STM32 → Jetson):
|
|
||||||
0x10 IMU — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12)
|
|
||||||
0x11 BATTERY — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5)
|
|
||||||
0x12 MOTOR_RPM — int16 left_rpm + int16 right_rpm (len=4)
|
|
||||||
0x13 ARM_STATE — uint8 state + uint8 error_flags (len=2)
|
|
||||||
0x14 ERROR — uint8 error_code + uint8 subcode (len=2)
|
|
||||||
|
|
||||||
Usage:
|
|
||||||
# Encoding (Jetson → STM32)
|
|
||||||
frame = encode_speed_steer(300, -150)
|
|
||||||
ser.write(frame)
|
|
||||||
|
|
||||||
# Decoding (STM32 → Jetson), one byte at a time
|
|
||||||
parser = FrameParser()
|
|
||||||
for byte in incoming_bytes:
|
|
||||||
result = parser.feed(byte)
|
|
||||||
if result is not None:
|
|
||||||
handle_frame(result)
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import struct
|
|
||||||
from dataclasses import dataclass
|
|
||||||
from enum import IntEnum
|
|
||||||
from typing import Optional
|
|
||||||
|
|
||||||
# ── Frame constants ───────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
STX = 0x02
|
|
||||||
ETX = 0x03
|
|
||||||
MAX_PAYLOAD_LEN = 64 # hard limit; any frame larger is corrupt
|
|
||||||
|
|
||||||
|
|
||||||
# ── Command / telemetry type codes ────────────────────────────────────────────
|
|
||||||
|
|
||||||
class CmdType(IntEnum):
|
|
||||||
HEARTBEAT = 0x01
|
|
||||||
SPEED_STEER = 0x02
|
|
||||||
ARM = 0x03
|
|
||||||
SET_MODE = 0x04
|
|
||||||
PID_UPDATE = 0x05
|
|
||||||
|
|
||||||
|
|
||||||
class TelType(IntEnum):
|
|
||||||
IMU = 0x10
|
|
||||||
BATTERY = 0x11
|
|
||||||
MOTOR_RPM = 0x12
|
|
||||||
ARM_STATE = 0x13
|
|
||||||
ERROR = 0x14
|
|
||||||
|
|
||||||
|
|
||||||
# ── Parsed telemetry objects ──────────────────────────────────────────────────
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class ImuFrame:
|
|
||||||
pitch_deg: float # degrees (positive = forward tilt)
|
|
||||||
roll_deg: float
|
|
||||||
yaw_deg: float
|
|
||||||
accel_x: float # m/s²
|
|
||||||
accel_y: float
|
|
||||||
accel_z: float
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class BatteryFrame:
|
|
||||||
voltage_mv: int # millivolts (e.g. 11100 = 11.1 V)
|
|
||||||
current_ma: int # milliamps (negative = charging)
|
|
||||||
soc_pct: int # state of charge 0–100 (from STM32 fuel gauge or lookup)
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class MotorRpmFrame:
|
|
||||||
left_rpm: int
|
|
||||||
right_rpm: int
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class ArmStateFrame:
|
|
||||||
state: int # 0=DISARMED 1=ARMED 2=TILT_FAULT
|
|
||||||
error_flags: int # bitmask
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class ErrorFrame:
|
|
||||||
error_code: int
|
|
||||||
subcode: int
|
|
||||||
|
|
||||||
|
|
||||||
# Union type for decoded results
|
|
||||||
TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame
|
|
||||||
|
|
||||||
|
|
||||||
# ── CRC16 CCITT ───────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
|
|
||||||
"""CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR."""
|
|
||||||
crc = init
|
|
||||||
for byte in data:
|
|
||||||
crc ^= byte << 8
|
|
||||||
for _ in range(8):
|
|
||||||
if crc & 0x8000:
|
|
||||||
crc = (crc << 1) ^ 0x1021
|
|
||||||
else:
|
|
||||||
crc <<= 1
|
|
||||||
crc &= 0xFFFF
|
|
||||||
return crc
|
|
||||||
|
|
||||||
|
|
||||||
# ── Frame encoder ─────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _build_frame(cmd_type: int, payload: bytes) -> bytes:
|
|
||||||
"""Assemble a complete binary frame with CRC16."""
|
|
||||||
assert len(payload) <= MAX_PAYLOAD_LEN, "Payload too large"
|
|
||||||
length = len(payload)
|
|
||||||
header = bytes([cmd_type, length])
|
|
||||||
crc = _crc16_ccitt(header + payload)
|
|
||||||
return bytes([STX, cmd_type, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
|
|
||||||
|
|
||||||
|
|
||||||
def encode_heartbeat() -> bytes:
|
|
||||||
"""HEARTBEAT frame — no payload."""
|
|
||||||
return _build_frame(CmdType.HEARTBEAT, b"")
|
|
||||||
|
|
||||||
|
|
||||||
def encode_speed_steer(speed: int, steer: int) -> bytes:
|
|
||||||
"""SPEED_STEER frame — int16 speed + int16 steer, both in -1000..+1000."""
|
|
||||||
speed = max(-1000, min(1000, int(speed)))
|
|
||||||
steer = max(-1000, min(1000, int(steer)))
|
|
||||||
return _build_frame(CmdType.SPEED_STEER, struct.pack(">hh", speed, steer))
|
|
||||||
|
|
||||||
|
|
||||||
def encode_arm(arm: bool) -> bytes:
|
|
||||||
"""ARM frame — 0=disarm, 1=arm."""
|
|
||||||
return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
|
|
||||||
|
|
||||||
|
|
||||||
def encode_set_mode(mode: int) -> bytes:
|
|
||||||
"""SET_MODE frame — mode byte."""
|
|
||||||
return _build_frame(CmdType.SET_MODE, struct.pack("B", mode & 0xFF))
|
|
||||||
|
|
||||||
|
|
||||||
def encode_pid_update(kp: float, ki: float, kd: float) -> bytes:
|
|
||||||
"""PID_UPDATE frame — three float32 values."""
|
|
||||||
return _build_frame(CmdType.PID_UPDATE, struct.pack(">fff", kp, ki, kd))
|
|
||||||
|
|
||||||
|
|
||||||
# ── Frame decoder (state-machine parser) ─────────────────────────────────────
|
|
||||||
|
|
||||||
class ParserState(IntEnum):
|
|
||||||
WAIT_STX = 0
|
|
||||||
WAIT_TYPE = 1
|
|
||||||
WAIT_LEN = 2
|
|
||||||
PAYLOAD = 3
|
|
||||||
CRC_HI = 4
|
|
||||||
CRC_LO = 5
|
|
||||||
WAIT_ETX = 6
|
|
||||||
|
|
||||||
|
|
||||||
class ParseError(Exception):
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
class FrameParser:
|
|
||||||
"""Byte-by-byte streaming parser for STM32 telemetry frames.
|
|
||||||
|
|
||||||
Feed individual bytes via feed(); returns a decoded TelemetryFrame (or raw
|
|
||||||
bytes tuple) when a complete valid frame is received.
|
|
||||||
|
|
||||||
Thread-safety: single-threaded — wrap in a lock if shared across threads.
|
|
||||||
|
|
||||||
Usage::
|
|
||||||
parser = FrameParser()
|
|
||||||
for b in incoming:
|
|
||||||
result = parser.feed(b)
|
|
||||||
if result is not None:
|
|
||||||
process(result)
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self) -> None:
|
|
||||||
self._state = ParserState.WAIT_STX
|
|
||||||
self._type = 0
|
|
||||||
self._length = 0
|
|
||||||
self._payload = bytearray()
|
|
||||||
self._crc_rcvd = 0
|
|
||||||
self.frames_ok = 0
|
|
||||||
self.frames_error = 0
|
|
||||||
|
|
||||||
def reset(self) -> None:
|
|
||||||
"""Reset parser to initial state (call after error or port reconnect)."""
|
|
||||||
self._state = ParserState.WAIT_STX
|
|
||||||
self._payload = bytearray()
|
|
||||||
|
|
||||||
def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
|
|
||||||
"""Process one byte. Returns decoded frame on success, None otherwise.
|
|
||||||
|
|
||||||
On CRC error, increments frames_error and resets. The return value on
|
|
||||||
success is a dataclass (ImuFrame, BatteryFrame, etc.) or a
|
|
||||||
(type_code, raw_payload) tuple for unknown type codes.
|
|
||||||
"""
|
|
||||||
s = self._state
|
|
||||||
|
|
||||||
if s == ParserState.WAIT_STX:
|
|
||||||
if byte == STX:
|
|
||||||
self._state = ParserState.WAIT_TYPE
|
|
||||||
return None
|
|
||||||
|
|
||||||
if s == ParserState.WAIT_TYPE:
|
|
||||||
self._type = byte
|
|
||||||
self._state = ParserState.WAIT_LEN
|
|
||||||
return None
|
|
||||||
|
|
||||||
if s == ParserState.WAIT_LEN:
|
|
||||||
self._length = byte
|
|
||||||
self._payload = bytearray()
|
|
||||||
if self._length > MAX_PAYLOAD_LEN:
|
|
||||||
# Corrupt frame — too big; reset
|
|
||||||
self.frames_error += 1
|
|
||||||
self.reset()
|
|
||||||
return None
|
|
||||||
if self._length == 0:
|
|
||||||
self._state = ParserState.CRC_HI
|
|
||||||
else:
|
|
||||||
self._state = ParserState.PAYLOAD
|
|
||||||
return None
|
|
||||||
|
|
||||||
if s == ParserState.PAYLOAD:
|
|
||||||
self._payload.append(byte)
|
|
||||||
if len(self._payload) == self._length:
|
|
||||||
self._state = ParserState.CRC_HI
|
|
||||||
return None
|
|
||||||
|
|
||||||
if s == ParserState.CRC_HI:
|
|
||||||
self._crc_rcvd = byte << 8
|
|
||||||
self._state = ParserState.CRC_LO
|
|
||||||
return None
|
|
||||||
|
|
||||||
if s == ParserState.CRC_LO:
|
|
||||||
self._crc_rcvd |= byte
|
|
||||||
self._state = ParserState.WAIT_ETX
|
|
||||||
return None
|
|
||||||
|
|
||||||
if s == ParserState.WAIT_ETX:
|
|
||||||
self.reset() # always reset so we look for next STX
|
|
||||||
if byte != ETX:
|
|
||||||
self.frames_error += 1
|
|
||||||
return None
|
|
||||||
|
|
||||||
# Verify CRC
|
|
||||||
crc_data = bytes([self._type, self._length]) + self._payload
|
|
||||||
expected = _crc16_ccitt(crc_data)
|
|
||||||
if expected != self._crc_rcvd:
|
|
||||||
self.frames_error += 1
|
|
||||||
return None
|
|
||||||
|
|
||||||
# Decode
|
|
||||||
self.frames_ok += 1
|
|
||||||
return _decode_telemetry(self._type, bytes(self._payload))
|
|
||||||
|
|
||||||
# Should never reach here
|
|
||||||
self.reset()
|
|
||||||
return None
|
|
||||||
|
|
||||||
|
|
||||||
# ── Telemetry decoder ─────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
|
|
||||||
"""Decode a validated telemetry payload into a typed dataclass."""
|
|
||||||
try:
|
|
||||||
if type_code == TelType.IMU:
|
|
||||||
if len(payload) < 12:
|
|
||||||
return None
|
|
||||||
p, r, y, ax, ay, az = struct.unpack_from(">hhhhhh", payload)
|
|
||||||
return ImuFrame(
|
|
||||||
pitch_deg=p / 100.0,
|
|
||||||
roll_deg=r / 100.0,
|
|
||||||
yaw_deg=y / 100.0,
|
|
||||||
accel_x=ax / 100.0,
|
|
||||||
accel_y=ay / 100.0,
|
|
||||||
accel_z=az / 100.0,
|
|
||||||
)
|
|
||||||
|
|
||||||
if type_code == TelType.BATTERY:
|
|
||||||
if len(payload) < 5:
|
|
||||||
return None
|
|
||||||
v_mv, i_ma, soc = struct.unpack_from(">HhB", payload)
|
|
||||||
return BatteryFrame(voltage_mv=v_mv, current_ma=i_ma, soc_pct=soc)
|
|
||||||
|
|
||||||
if type_code == TelType.MOTOR_RPM:
|
|
||||||
if len(payload) < 4:
|
|
||||||
return None
|
|
||||||
left, right = struct.unpack_from(">hh", payload)
|
|
||||||
return MotorRpmFrame(left_rpm=left, right_rpm=right)
|
|
||||||
|
|
||||||
if type_code == TelType.ARM_STATE:
|
|
||||||
if len(payload) < 2:
|
|
||||||
return None
|
|
||||||
state, flags = struct.unpack_from("BB", payload)
|
|
||||||
return ArmStateFrame(state=state, error_flags=flags)
|
|
||||||
|
|
||||||
if type_code == TelType.ERROR:
|
|
||||||
if len(payload) < 2:
|
|
||||||
return None
|
|
||||||
code, sub = struct.unpack_from("BB", payload)
|
|
||||||
return ErrorFrame(error_code=code, subcode=sub)
|
|
||||||
|
|
||||||
except struct.error:
|
|
||||||
return None
|
|
||||||
|
|
||||||
# Unknown telemetry type — return raw
|
|
||||||
return (type_code, payload)
|
|
||||||
@ -13,7 +13,7 @@ setup(
|
|||||||
"launch/bridge.launch.py",
|
"launch/bridge.launch.py",
|
||||||
"launch/cmd_vel_bridge.launch.py",
|
"launch/cmd_vel_bridge.launch.py",
|
||||||
"launch/remote_estop.launch.py",
|
"launch/remote_estop.launch.py",
|
||||||
"launch/stm32_cmd.launch.py",
|
"launch/esp32_cmd.launch.py",
|
||||||
"launch/battery.launch.py",
|
"launch/battery.launch.py",
|
||||||
"launch/uart_bridge.launch.py",
|
"launch/uart_bridge.launch.py",
|
||||||
]),
|
]),
|
||||||
@ -21,7 +21,7 @@ setup(
|
|||||||
"config/bridge_params.yaml",
|
"config/bridge_params.yaml",
|
||||||
"config/cmd_vel_bridge_params.yaml",
|
"config/cmd_vel_bridge_params.yaml",
|
||||||
"config/estop_params.yaml",
|
"config/estop_params.yaml",
|
||||||
"config/stm32_cmd_params.yaml",
|
"config/esp32_cmd_params.yaml",
|
||||||
"config/battery_params.yaml",
|
"config/battery_params.yaml",
|
||||||
]),
|
]),
|
||||||
],
|
],
|
||||||
@ -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-S3 USB serial → ROS2 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",
|
||||||
# Binary-framed STM32 command node (Issue #119)
|
# Binary-framed ESP32-S3 IO command node (Issue #119)
|
||||||
"stm32_cmd_node = saltybot_bridge.stm32_cmd_node:main",
|
"esp32_cmd_node = saltybot_bridge.esp32_cmd_node:main",
|
||||||
# Battery management node (Issue #125)
|
# Battery 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)
|
||||||
|
|||||||
@ -1,5 +1,5 @@
|
|||||||
"""
|
"""
|
||||||
Unit tests for Jetson→STM32 command serialization logic.
|
Unit tests for Jetson→ESP32-S3 command serialization logic.
|
||||||
Tests Twist→speed/steer conversion and frame formatting.
|
Tests Twist→speed/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
|
||||||
"""
|
"""
|
||||||
|
|||||||
@ -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, balance_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 balance_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)
|
||||||
|
|||||||
@ -1,4 +1,4 @@
|
|||||||
"""test_stm32_cmd_node.py — Unit tests for Stm32CmdNode with mock serial port.
|
"""test_esp32_cmd_node.py — Unit tests for Esp32CmdNode with mock serial port.
|
||||||
|
|
||||||
Tests:
|
Tests:
|
||||||
- Serial open/close lifecycle
|
- Serial open/close lifecycle
|
||||||
@ -12,7 +12,7 @@ Tests:
|
|||||||
- Zero-speed sent on node shutdown
|
- Zero-speed sent on node shutdown
|
||||||
- CRC errors counted correctly
|
- CRC errors counted correctly
|
||||||
|
|
||||||
Run with: pytest test/test_stm32_cmd_node.py -v
|
Run with: pytest test/test_esp32_cmd_node.py -v
|
||||||
No ROS2 runtime required — uses mock Node infrastructure.
|
No ROS2 runtime required — uses mock Node infrastructure.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
@ -29,7 +29,7 @@ import pytest
|
|||||||
|
|
||||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
||||||
|
|
||||||
from saltybot_bridge.stm32_protocol import (
|
from saltybot_bridge.esp32_protocol import (
|
||||||
STX, ETX, CmdType, TelType,
|
STX, ETX, CmdType, TelType,
|
||||||
encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update,
|
encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update,
|
||||||
_build_frame, _crc16_ccitt,
|
_build_frame, _crc16_ccitt,
|
||||||
@ -219,10 +219,10 @@ class TestMockSerialTX:
|
|||||||
class TestMockSerialRX:
|
class TestMockSerialRX:
|
||||||
"""Test RX parsing path using MockSerial with pre-loaded telemetry data."""
|
"""Test RX parsing path using MockSerial with pre-loaded telemetry data."""
|
||||||
|
|
||||||
from saltybot_bridge.stm32_protocol import FrameParser
|
from saltybot_bridge.esp32_protocol import FrameParser
|
||||||
|
|
||||||
def test_rx_imu_frame(self):
|
def test_rx_imu_frame(self):
|
||||||
from saltybot_bridge.stm32_protocol import FrameParser, ImuFrame
|
from saltybot_bridge.esp32_protocol import FrameParser, ImuFrame
|
||||||
raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981)
|
raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981)
|
||||||
ms = MockSerial(rx_data=raw)
|
ms = MockSerial(rx_data=raw)
|
||||||
parser = FrameParser()
|
parser = FrameParser()
|
||||||
@ -241,7 +241,7 @@ class TestMockSerialRX:
|
|||||||
assert f.accel_z == pytest.approx(9.81)
|
assert f.accel_z == pytest.approx(9.81)
|
||||||
|
|
||||||
def test_rx_battery_frame(self):
|
def test_rx_battery_frame(self):
|
||||||
from saltybot_bridge.stm32_protocol import FrameParser, BatteryFrame
|
from saltybot_bridge.esp32_protocol import FrameParser, BatteryFrame
|
||||||
raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45)
|
raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45)
|
||||||
ms = MockSerial(rx_data=raw)
|
ms = MockSerial(rx_data=raw)
|
||||||
parser = FrameParser()
|
parser = FrameParser()
|
||||||
@ -257,7 +257,7 @@ class TestMockSerialRX:
|
|||||||
assert f.soc_pct == 45
|
assert f.soc_pct == 45
|
||||||
|
|
||||||
def test_rx_multiple_frames_in_one_read(self):
|
def test_rx_multiple_frames_in_one_read(self):
|
||||||
from saltybot_bridge.stm32_protocol import FrameParser
|
from saltybot_bridge.esp32_protocol import FrameParser
|
||||||
raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes())
|
raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes())
|
||||||
ms = MockSerial(rx_data=raw)
|
ms = MockSerial(rx_data=raw)
|
||||||
parser = FrameParser()
|
parser = FrameParser()
|
||||||
@ -271,7 +271,7 @@ class TestMockSerialRX:
|
|||||||
assert parser.frames_error == 0
|
assert parser.frames_error == 0
|
||||||
|
|
||||||
def test_rx_bad_crc_counted_as_error(self):
|
def test_rx_bad_crc_counted_as_error(self):
|
||||||
from saltybot_bridge.stm32_protocol import FrameParser
|
from saltybot_bridge.esp32_protocol import FrameParser
|
||||||
raw = bytearray(_arm_state_frame_bytes(state=1))
|
raw = bytearray(_arm_state_frame_bytes(state=1))
|
||||||
raw[-3] ^= 0xFF # corrupt CRC
|
raw[-3] ^= 0xFF # corrupt CRC
|
||||||
ms = MockSerial(rx_data=bytes(raw))
|
ms = MockSerial(rx_data=bytes(raw))
|
||||||
@ -282,7 +282,7 @@ class TestMockSerialRX:
|
|||||||
assert parser.frames_error == 1
|
assert parser.frames_error == 1
|
||||||
|
|
||||||
def test_rx_resync_after_corrupt_byte(self):
|
def test_rx_resync_after_corrupt_byte(self):
|
||||||
from saltybot_bridge.stm32_protocol import FrameParser, ArmStateFrame
|
from saltybot_bridge.esp32_protocol import FrameParser, ArmStateFrame
|
||||||
garbage = b"\xDE\xAD\x00\x00"
|
garbage = b"\xDE\xAD\x00\x00"
|
||||||
valid = _arm_state_frame_bytes(state=1)
|
valid = _arm_state_frame_bytes(state=1)
|
||||||
ms = MockSerial(rx_data=garbage + valid)
|
ms = MockSerial(rx_data=garbage + valid)
|
||||||
@ -1,4 +1,4 @@
|
|||||||
"""test_stm32_protocol.py — Unit tests for binary STM32 frame codec.
|
"""test_esp32_protocol.py — Unit tests for binary ESP32-S3 frame codec.
|
||||||
|
|
||||||
Tests:
|
Tests:
|
||||||
- CRC16-CCITT correctness
|
- CRC16-CCITT correctness
|
||||||
@ -12,7 +12,7 @@ Tests:
|
|||||||
- Speed/steer clamping in encode_speed_steer
|
- Speed/steer clamping in encode_speed_steer
|
||||||
- Round-trip encode → decode for all known telemetry types
|
- Round-trip encode → decode for all known telemetry types
|
||||||
|
|
||||||
Run with: pytest test/test_stm32_protocol.py -v
|
Run with: pytest test/test_esp32_protocol.py -v
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
@ -25,7 +25,7 @@ import os
|
|||||||
# ── Path setup (no ROS2 install needed) ──────────────────────────────────────
|
# ── Path setup (no ROS2 install needed) ──────────────────────────────────────
|
||||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
||||||
|
|
||||||
from saltybot_bridge.stm32_protocol import (
|
from saltybot_bridge.esp32_protocol import (
|
||||||
STX, ETX,
|
STX, ETX,
|
||||||
CmdType, TelType,
|
CmdType, TelType,
|
||||||
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
||||||
@ -1,5 +1,5 @@
|
|||||||
"""
|
"""
|
||||||
Unit tests for STM32 telemetry parsing logic.
|
Unit tests for ESP32-S3 telemetry parsing logic.
|
||||||
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
|||||||
@ -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-S3 IO bridge consumes this topic.
|
||||||
|
|
||||||
bt_navigator:
|
bt_navigator:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
|
|||||||
@ -2,12 +2,12 @@
|
|||||||
# Master configuration for full stack bringup
|
# Master configuration for full stack bringup
|
||||||
|
|
||||||
# ────────────────────────────────────────────────────────────────────────────
|
# ────────────────────────────────────────────────────────────────────────────
|
||||||
# HARDWARE — STM32 Bridge & Motor Control
|
# HARDWARE — ESP32-S3 IO Bridge & Motor Control
|
||||||
# ────────────────────────────────────────────────────────────────────────────
|
# ────────────────────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
saltybot_bridge_node:
|
saltybot_bridge_node:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
serial_port: "/dev/stm32-bridge"
|
serial_port: "/dev/esp32-io"
|
||||||
baud_rate: 921600
|
baud_rate: 921600
|
||||||
timeout: 0.05
|
timeout: 0.05
|
||||||
reconnect_delay: 2.0
|
reconnect_delay: 2.0
|
||||||
|
|||||||
@ -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-S3 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-S3 IO bridge (serial port owner — must be first)
|
||||||
t= 2s cmd_vel bridge (consumes /cmd_vel, needs STM32 bridge up)
|
t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 IO bridge up)
|
||||||
t= 2s sensors (RPLIDAR + RealSense)
|
t= 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-S3 IO bridge must be up before cmd_vel bridge sends any command.
|
||||||
• cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
|
• cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
|
||||||
• STM32 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
|
• ESP32-S3 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
|
||||||
until STM32 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
|
until ESP32-S3 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel.
|
||||||
• follow_enabled:=false disables person follower without stopping the node.
|
• 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-S3 IO)
|
||||||
/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-S3 IO 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-S3 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-S3 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-S3 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,
|
||||||
|
|||||||
@ -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_io_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-S3 IO 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_io_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_io_port_arg = DeclareLaunchArgument(
|
||||||
"stm32_port",
|
"esp32_io_port",
|
||||||
default_value="/dev/stm32-bridge",
|
default_value="/dev/esp32-io",
|
||||||
description="STM32 USART bridge serial device",
|
description="ESP32-S3 IO serial device",
|
||||||
)
|
)
|
||||||
|
|
||||||
uwb_port_a_arg = DeclareLaunchArgument(
|
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_io_port = LaunchConfiguration("esp32_io_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-S3 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-S3 bidirectional bridge (JLINK USART1)
|
||||||
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-S3 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=0–4s) ───────────────────────────
|
# ── GROUP A: Drivers (all profiles, t=0–4s) ───────────────────────────
|
||||||
robot_description,
|
robot_description,
|
||||||
stm32_bridge,
|
esp32_bridge,
|
||||||
sensors,
|
sensors,
|
||||||
motor_daemon,
|
motor_daemon,
|
||||||
sensor_health,
|
sensor_health,
|
||||||
|
|||||||
@ -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-S3 IO encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles
|
||||||
this by detecting jumps larger than half the int32 range and adjusting by the
|
this by detecting jumps larger than half the int32 range and adjusting by the
|
||||||
full range:
|
full range:
|
||||||
|
|
||||||
|
|||||||
@ -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_io_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-S3 IO 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_io_port: str = "/dev/esp32-io"
|
||||||
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-S3 IO 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_io_bridge=True,
|
||||||
enable_sensors=True,
|
enable_sensors=True,
|
||||||
enable_motor_daemon=True,
|
enable_motor_daemon=True,
|
||||||
enable_imu=True,
|
enable_imu=True,
|
||||||
|
|||||||
@ -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-S3 IO 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.
|
||||||
|
|
||||||
|
|||||||
@ -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-S3 IO 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-S3 IO 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-io
|
||||||
```
|
```
|
||||||
|
|
||||||
## Performance Baseline
|
## Performance Baseline
|
||||||
|
|||||||
@ -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_io_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_io_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_io_port_set(self):
|
||||||
p = _minimal()
|
p = _minimal()
|
||||||
assert p.stm32_port.startswith("/dev/")
|
assert p.esp32_io_port.startswith("/dev/")
|
||||||
|
|
||||||
def test_uwb_ports_set(self):
|
def test_uwb_ports_set(self):
|
||||||
p = _full()
|
p = _full()
|
||||||
|
|||||||
@ -10,7 +10,7 @@
|
|||||||
- Sensors:
|
- Sensors:
|
||||||
* RPLIDAR A1M8 (360° scanning LiDAR)
|
* RPLIDAR A1M8 (360° scanning LiDAR)
|
||||||
* RealSense D435i (RGB-D camera + IMU)
|
* RealSense D435i (RGB-D camera + IMU)
|
||||||
* BNO055 (9-DOF IMU, STM32 FC)
|
* ICM-42688-P (9-DOF IMU, ESP32-S3 BALANCE)
|
||||||
- Actuators:
|
- Actuators:
|
||||||
* 2x differential drive motors
|
* 2x differential drive motors
|
||||||
* Pan/Tilt servos for camera
|
* Pan/Tilt servos for camera
|
||||||
@ -120,7 +120,7 @@
|
|||||||
<child link="right_wheel_link" />
|
<child link="right_wheel_link" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<!-- IMU Link (STM32 FC BNO055, mounted on main board) -->
|
<!-- IMU Link (ESP32-S3 BALANCE ICM-42688-P, mounted on main board) -->
|
||||||
<link name="imu_link">
|
<link name="imu_link">
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass value="0.01" />
|
<mass value="0.01" />
|
||||||
|
|||||||
@ -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-S3, 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
|
||||||
|
|||||||
@ -6,7 +6,7 @@ startup_checks:
|
|||||||
- realsense
|
- realsense
|
||||||
- vesc
|
- vesc
|
||||||
- jabra_microphone
|
- jabra_microphone
|
||||||
- stm32_bridge
|
- esp32_bridge
|
||||||
- servos
|
- servos
|
||||||
- wifi
|
- wifi
|
||||||
- gps
|
- gps
|
||||||
|
|||||||
@ -89,7 +89,7 @@ class DiagnosticsNode(Node):
|
|||||||
self._check_realsense()
|
self._check_realsense()
|
||||||
self._check_vesc()
|
self._check_vesc()
|
||||||
self._check_jabra()
|
self._check_jabra()
|
||||||
self._check_stm32()
|
self._check_esp32()
|
||||||
self._check_servos()
|
self._check_servos()
|
||||||
self._check_wifi()
|
self._check_wifi()
|
||||||
self._check_gps()
|
self._check_gps()
|
||||||
@ -137,8 +137,8 @@ class DiagnosticsNode(Node):
|
|||||||
except:
|
except:
|
||||||
self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {})
|
self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {})
|
||||||
|
|
||||||
def _check_stm32(self):
|
def _check_esp32(self):
|
||||||
self.hardware_checks["stm32"] = ("OK", "STM32 bridge online", {})
|
self.hardware_checks["esp32"] = ("OK", "ESP32-S3 bridge online", {})
|
||||||
|
|
||||||
def _check_servos(self):
|
def _check_servos(self):
|
||||||
try:
|
try:
|
||||||
|
|||||||
@ -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-S3 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-S3 AUTONOMOUS mode (md=2).
|
||||||
follow_enabled: true
|
follow_enabled: true
|
||||||
|
|||||||
@ -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-S3 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
|
||||||
|
|||||||
@ -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-S3 over JLINK protocol
|
||||||
serial_port: "/dev/ttyTHS1"
|
serial_port: "/dev/ttyTHS1"
|
||||||
baud_rate: 921600
|
baud_rate: 921600
|
||||||
|
|
||||||
|
|||||||
@ -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-S3",
|
||||||
)
|
)
|
||||||
pan_limit_arg = DeclareLaunchArgument(
|
pan_limit_arg = DeclareLaunchArgument(
|
||||||
"pan_limit_deg",
|
"pan_limit_deg",
|
||||||
|
|||||||
@ -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-S3.
|
||||||
Implements smooth trapezoidal motion profiles with configurable axis limits.
|
Implements smooth trapezoidal motion profiles with configurable axis limits.
|
||||||
|
|
||||||
Subscribed topics:
|
Subscribed topics:
|
||||||
|
|||||||
@ -1,19 +1,19 @@
|
|||||||
"""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-S3 side).
|
||||||
|
|
||||||
Command type (Jetson → STM32):
|
Command type (Jetson → ESP32-S3):
|
||||||
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 0–4095 (0 = max)
|
speed = servo speed register 0–4095 (0 = max)
|
||||||
|
|
||||||
Telemetry type (STM32 → Jetson):
|
Telemetry type (ESP32-S3 → Jetson):
|
||||||
0x84 GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
|
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)
|
||||||
|
|
||||||
Frame format (shared with stm32_protocol.py):
|
Frame format (shared with esp32_protocol.py):
|
||||||
[STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
|
[STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
|
||||||
CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes.
|
CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD 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-S3: set pan/tilt target
|
||||||
TLM_GIMBAL_STATE = 0x84 # STM32 → Jetson: measured state
|
TLM_GIMBAL_STATE = 0x84 # ESP32-S3 → Jetson: measured state
|
||||||
|
|
||||||
# Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
|
# 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))
|
||||||
|
|||||||
@ -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-S3 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)
|
||||||
|
|||||||
@ -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-S3 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.
|
||||||
|
|||||||
@ -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-S3 executing RC pilot commands; Jetson cmd_vel blocked.
|
||||||
"RAMP_TO_AUTO" — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
|
"RAMP_TO_AUTO" — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
|
||||||
"AUTO" — STM32 executing Jetson cmd_vel; RC sticks idle.
|
"AUTO" — ESP32-S3 executing Jetson cmd_vel; RC sticks idle.
|
||||||
"RAMP_TO_RC" — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
|
"RAMP_TO_RC" — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
|
||||||
|
|
||||||
Blend alpha
|
Blend alpha
|
||||||
|
|||||||
@ -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-S3 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
|
||||||
|
|||||||
@ -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-S3 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 ──────────────────────
|
||||||
|
|||||||
@ -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-S3 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'),
|
||||||
|
|||||||
@ -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-S3 IO; wait for it
|
||||||
time.sleep(1.5)
|
time.sleep(1.5)
|
||||||
|
|
||||||
self.get_logger().info(
|
self.get_logger().info(
|
||||||
|
|||||||
@ -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-S3 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:
|
||||||
|
|||||||
@ -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-S3 bridge (/odom from wheel encoders)
|
||||||
D435i (/imu/data for heading)
|
D435i (/imu/data for heading)
|
||||||
|
|
||||||
Usage — record a route:
|
Usage — record a route:
|
||||||
|
|||||||
@ -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 1000–2000 µs pulses).
|
ESCs controlled via PWM (servo-style 1000–2000 µs pulses).
|
||||||
Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge.
|
Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge.
|
||||||
|
|
||||||
ESC channel assignments (configurable):
|
ESC channel assignments (configurable):
|
||||||
CH1 = left-front
|
CH1 = left-front
|
||||||
|
|||||||
@ -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-S3
|
||||||
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-S3 bridge)
|
||||||
|
|||||||
@ -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-S3
|
||||||
|
|
||||||
# ── 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-S3 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-S3 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:
|
||||||
|
|||||||
@ -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-S3
|
||||||
|
|
||||||
Usage:
|
Usage:
|
||||||
# Defaults (walk profile initially, adapts via UWB + GPS):
|
# Defaults (walk profile initially, adapts via UWB + GPS):
|
||||||
|
|||||||
@ -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 1000–2000 µs pulses).
|
ESCs controlled via PWM (servo-style 1000–2000 µs pulses).
|
||||||
Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge.
|
Communication: USB CDC serial to ESP32-S3 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)
|
||||||
|
|||||||
@ -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-S3 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-S3 bridge may be disabled in test mode)")
|
||||||
|
|
||||||
|
|
||||||
class TestDockingServices(unittest.TestCase):
|
class TestDockingServices(unittest.TestCase):
|
||||||
|
|||||||
@ -107,9 +107,9 @@ cat > /etc/udev/rules.d/99-saltybot.rules << 'EOF'
|
|||||||
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 Virtual COM)
|
# ESP32-S3 IO USB CDC (Espressif Virtual COM)
|
||||||
KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
||||||
SYMLINK+="stm32-bridge", MODE="0666"
|
SYMLINK+="esp32-io", MODE="0666"
|
||||||
|
|
||||||
# Intel RealSense D435i
|
# Intel RealSense D435i
|
||||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \
|
SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \
|
||||||
@ -143,7 +143,7 @@ if grep -q "console=ttyTCU0" /boot/extlinux/extlinux.conf 2>/dev/null; then
|
|||||||
echo "[i] Serial console is on ttyTCU0 (debug UART) — ttyTHS0 is free."
|
echo "[i] Serial console is on ttyTCU0 (debug UART) — ttyTHS0 is free."
|
||||||
else
|
else
|
||||||
echo "[!] Check /boot/extlinux/extlinux.conf — ensure ttyTHS0 is not used"
|
echo "[!] Check /boot/extlinux/extlinux.conf — ensure ttyTHS0 is not used"
|
||||||
echo " as a serial console if you need it for STM32 UART fallback."
|
echo " as a serial console if you need it for ESP32-S3 IO UART fallback."
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# ── Check CSI camera drivers ──────────────────────────────────────────────────
|
# ── Check CSI camera drivers ──────────────────────────────────────────────────
|
||||||
|
|||||||
@ -16,7 +16,7 @@
|
|||||||
# CAN_IFACE — SocketCAN name (default: slcan0)
|
# CAN_IFACE — SocketCAN name (default: slcan0)
|
||||||
# CAN_BITRATE — bit rate (default: 500000)
|
# CAN_BITRATE — bit rate (default: 500000)
|
||||||
#
|
#
|
||||||
# Mamba CAN ID: 1 (0x001)
|
# ESP32-S3 BALANCE CAN ID: 1 (0x001)
|
||||||
# VESC left: 56 (0x038)
|
# VESC left: 56 (0x038)
|
||||||
# VESC right: 68 (0x044)
|
# VESC right: 68 (0x044)
|
||||||
|
|
||||||
@ -108,7 +108,7 @@ cmd_verify() {
|
|||||||
if ! ip link show "$IFACE" &>/dev/null; then
|
if ! ip link show "$IFACE" &>/dev/null; then
|
||||||
die "$IFACE is not up — run '$0 up' first"
|
die "$IFACE is not up — run '$0 up' first"
|
||||||
fi
|
fi
|
||||||
log "Listening on $IFACE — expecting frames from Mamba (0x001), VESC left (0x038), VESC right (0x044)"
|
log "Listening on $IFACE — expecting frames from ESP32-S3 BALANCE (0x001), VESC left (0x038), VESC right (0x044)"
|
||||||
log "Press Ctrl-C to stop."
|
log "Press Ctrl-C to stop."
|
||||||
exec candump "$IFACE"
|
exec candump "$IFACE"
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @file usbd_cdc.h
|
* @file usbd_cdc.h
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
#ifndef USBD_CONF_H
|
#ifndef USBD_CONF_H
|
||||||
#define USBD_CONF_H
|
#define USBD_CONF_H
|
||||||
|
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @file usbd_core.h
|
* @file usbd_core.h
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @file usbd_req.h
|
* @file usbd_req.h
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
/* DEPRECATED: STM32/Mamba firmware -- replaced by ESP32-S3. Do not modify. */
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @file usbd_def.h
|
* @file usbd_def.h
|
||||||
|
|||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user