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

Merge pull request 'feat(arch): CAN/UART bridge update for SAUL-TEE ESP32 architecture' (#714) from sl-controls/esp32-can-bridge-update into main

Browse Source
This commit is contained in:
sl-jetson 2026-04-04 09:00:10 -04:00
commit fe84ff6039
7 changed files with 422 additions and 1287 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

283
docs/wiring-diagram.md
View File

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

38
jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml
View File

@ -1,30 +1,16 @@
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119) # stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
# Binary-framed Jetson↔ESP32 bridge at 921600 baud. # Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud.
# Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]
# Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
# ── Serial port ──────────────────────────────────────────────────────────────── # ── Serial port ────────────────────────────────────────────────────────────────
# Use /dev/esp32-bridge if the udev rule is applied: # Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", # ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout" serial_port: /dev/esp32-io
serial_port: /dev/ttyACM0 baud_rate: 460800
baud_rate: 921600 reconnect_delay: 2.0 # seconds between reconnect attempts
reconnect_delay: 2.0 # seconds between USB reconnect attempts
# ── Heartbeat ───────────────────────────────────────────────────────────────── # ── Heartbeat ─────────────────────────────────────────────────────────────────
# HEARTBEAT frame sent every heartbeat_period seconds. # HEARTBEAT (0x20) sent every heartbeat_period.
# STM32 fires watchdog and reverts to safe state if no frame received for 500ms. # ESP32 IO watchdog fires if no heartbeat for ~500 ms.
heartbeat_period: 0.2 # 200ms → well within 500ms ESP32 BALANCE watchdog heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog
# ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
# If no /cmd_vel message received for watchdog_timeout seconds,
# send SPEED_STEER(0,0) to stop the robot.
watchdog_timeout: 0.5 # 500ms
# ── Twist velocity scaling ────────────────────────────────────────────────────
# speed = clamp(linear.x * speed_scale, -1000, 1000) (m/s → ESC units)
# steer = clamp(angular.z * steer_scale, -1000, 1000) (rad/s → ESC units)
#
# Default: 1 m/s → 1000 ESC units, ±2 rad/s → ±1000 steer.
# Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction.
# Tune speed_scale to set the physical top speed.
speed_scale: 1000.0
steer_scale: -500.0

28
jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
View File

@ -1,14 +1,14 @@
"""stm32_cmd.launch.py — Launch the binary-framed ESP32 BALANCE command node (Issue #119). """stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node.
Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol).
Handles RC monitoring, sensor data, LED/output commands.
Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node.
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
Usage: Usage:
# Default (binary protocol, bidirectional):
ros2 launch saltybot_bridge stm32_cmd.launch.py ros2 launch saltybot_bridge stm32_cmd.launch.py
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
# Override serial port:
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
# Custom velocity scales:
ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
""" """
import os import os
@ -24,12 +24,9 @@ def generate_launch_description() -> LaunchDescription:
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml") params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
return LaunchDescription([ return LaunchDescription([
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"), DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"),
DeclareLaunchArgument("baud_rate", default_value="921600"), DeclareLaunchArgument("baud_rate", default_value="460800"),
DeclareLaunchArgument("speed_scale", default_value="1000.0"), DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
Node( Node(
package="saltybot_bridge", package="saltybot_bridge",
@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription:
{ {
"serial_port": LaunchConfiguration("serial_port"), "serial_port": LaunchConfiguration("serial_port"),
"baud_rate": LaunchConfiguration("baud_rate"), "baud_rate": LaunchConfiguration("baud_rate"),
"speed_scale": LaunchConfiguration("speed_scale"),
"steer_scale": LaunchConfiguration("steer_scale"),
"watchdog_timeout": LaunchConfiguration("watchdog_timeout"),
"heartbeat_period": LaunchConfiguration("heartbeat_period"), "heartbeat_period": LaunchConfiguration("heartbeat_period"),
}, },
], ],

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

@ -1,45 +1,32 @@
"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge. """stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.
Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud. inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
TX commands (Jetson → STM32): This node is NOT the primary drive path (that is CAN via can_bridge_node).
SPEED_STEER — 50 Hz from /cmd_vel subscription It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
HEARTBEAT — 200 ms timer (ESP32 BALANCE 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. Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud
RX telemetry (STM32 → Jetson): RX from ESP32 IO:
IMU → /saltybot/imu (sensor_msgs/Imu) RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON)
BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON) SENSORS (0x02) → /saltybot/sensors (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 TX to ESP32 IO:
continuously retries at reconnect_delay interval. LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON)
OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON)
This node owns /dev/ttyACM0 exclusively — do NOT run alongside HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive)
serial_bridge_node or saltybot_cmd_node on the same port.
Parameters (config/stm32_cmd_params.yaml): Parameters (config/stm32_cmd_params.yaml):
serial_port /dev/ttyACM0 serial_port /dev/esp32-io
baud_rate 921600 baud_rate 460800
reconnect_delay 2.0 (seconds) reconnect_delay 2.0
heartbeat_period 0.2 (seconds) heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms)
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 from __future__ import annotations
import json import json
import math
import threading import threading
import time import time
@ -50,119 +37,69 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
import serial import serial
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 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_msgs.msg import String
from std_srvs.srv import SetBool, Trigger
from .stm32_protocol import ( from .stm32_protocol import (
BAUD_RATE,
FrameParser, FrameParser,
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, RcChannels,
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode, SensorData,
encode_pid_update, encode_heartbeat,
encode_led_cmd,
encode_output_cmd,
) )
# ── Constants ─────────────────────────────────────────────────────────────────
IMU_FRAME_ID = "imu_link"
_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
def _clamp(v: float, lo: float, hi: float) -> float:
return max(lo, min(hi, v))
# ── Node ──────────────────────────────────────────────────────────────────────
class Stm32CmdNode(Node): class Stm32CmdNode(Node):
"""Binary-framed Jetson↔ESP32 bridge node.""" """Orin ↔ ESP32-S3 IO auxiliary bridge node."""
def __init__(self) -> None: def __init__(self) -> None:
super().__init__("stm32_cmd_node") super().__init__("stm32_cmd_node")
# ── Parameters ──────────────────────────────────────────────────────── # ── Parameters ────────────────────────────────────────────────────
self.declare_parameter("serial_port", "/dev/ttyACM0") self.declare_parameter("serial_port", "/dev/esp32-io")
self.declare_parameter("baud_rate", 921600) self.declare_parameter("baud_rate", BAUD_RATE)
self.declare_parameter("reconnect_delay", 2.0) self.declare_parameter("reconnect_delay", 2.0)
self.declare_parameter("heartbeat_period", 0.2) 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 self._port_name = self.get_parameter("serial_port").value
baud = self.get_parameter("baud_rate").value self._baud = self.get_parameter("baud_rate").value
self._reconnect_delay = self.get_parameter("reconnect_delay").value self._reconnect_delay = self.get_parameter("reconnect_delay").value
self._hb_period = self.get_parameter("heartbeat_period").value self._hb_period = self.get_parameter("heartbeat_period").value
self._wd_timeout = self.get_parameter("watchdog_timeout").value
self._speed_scale = self.get_parameter("speed_scale").value
self._steer_scale = self.get_parameter("steer_scale").value
# ── QoS ─────────────────────────────────────────────────────────────── # ── QoS ───────────────────────────────────────────────────────────
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST, depth=10,
)
rel_qos = QoSProfile( rel_qos = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE, reliability=ReliabilityPolicy.RELIABLE,
history=HistoryPolicy.KEEP_LAST, depth=10, history=HistoryPolicy.KEEP_LAST, depth=10,
) )
# ── Publishers ──────────────────────────────────────────────────────── # ── Publishers ────────────────────────────────────────────────────
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos) self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos)
self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos) self._sens_pub = self.create_publisher(String, "/saltybot/sensors", rel_qos)
self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos) self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", 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 ─────────────────────────────────────────────────────── # ── Subscriptions ─────────────────────────────────────────────────
self._cmd_vel_sub = self.create_subscription( self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos)
Twist, "/cmd_vel", self._on_cmd_vel, rel_qos, self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
)
self._pid_sub = self.create_subscription(
String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
)
# ── Services ────────────────────────────────────────────────────────── # ── Serial state ──────────────────────────────────────────────────
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: serial.Serial | None = None
self._ser_lock = threading.Lock() self._ser_lock = threading.Lock()
self._parser = FrameParser() self._parser = FrameParser()
self._rx_count = 0
# ── TX state ────────────────────────────────────────────────────────── # ── Open serial and start timers ──────────────────────────────────
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() 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)
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) self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb)
# Watchdog check (fires at 2× watchdog_timeout for quick detection) self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
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( self.get_logger().info(
f"stm32_cmd_node started — {port} @ {baud} baud | " f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
) )
# ── Serial management ───────────────────────────────────────────────────── # ── Serial management ─────────────────────────────────────────────────
def _open_serial(self) -> bool: def _open_serial(self) -> bool:
with self._ser_lock: with self._ser_lock:
@ -170,7 +107,7 @@ class Stm32CmdNode(Node):
self._ser = serial.Serial( self._ser = serial.Serial(
port=self._port_name, port=self._port_name,
baudrate=self._baud, baudrate=self._baud,
timeout=0.005, # non-blocking reads timeout=0.005,
write_timeout=0.1, write_timeout=0.1,
) )
self._ser.reset_input_buffer() self._ser.reset_input_buffer()
@ -185,17 +122,7 @@ class Stm32CmdNode(Node):
self._ser = None self._ser = None
return False 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: def _write(self, data: bytes) -> bool:
"""Thread-safe serial write. Returns False if port is not open."""
with self._ser_lock: with self._ser_lock:
if self._ser is None or not self._ser.is_open: if self._ser is None or not self._ser.is_open:
return False return False
@ -207,16 +134,15 @@ class Stm32CmdNode(Node):
self._ser = None self._ser = None
return False return False
# ── RX — read callback ──────────────────────────────────────────────────── # ── RX ────────────────────────────────────────────────────────────────
def _read_cb(self) -> None: def _read_cb(self) -> None:
"""Read bytes from serial and feed them to the frame parser."""
raw: bytes | None = None raw: bytes | None = None
reconnect_needed = False reconnect = False
with self._ser_lock: with self._ser_lock:
if self._ser is None or not self._ser.is_open: if self._ser is None or not self._ser.is_open:
reconnect_needed = True reconnect = True
else: else:
try: try:
n = self._ser.in_waiting n = self._ser.in_waiting
@ -225,9 +151,9 @@ class Stm32CmdNode(Node):
except serial.SerialException as exc: except serial.SerialException as exc:
self.get_logger().error(f"Serial read error: {exc}") self.get_logger().error(f"Serial read error: {exc}")
self._ser = None self._ser = None
reconnect_needed = True reconnect = True
if reconnect_needed: if reconnect:
self.get_logger().warn( self.get_logger().warn(
"Serial disconnected — will retry", "Serial disconnected — will retry",
throttle_duration_sec=self._reconnect_delay, throttle_duration_sec=self._reconnect_delay,
@ -240,230 +166,105 @@ class Stm32CmdNode(Node):
return return
for byte in raw: for byte in raw:
frame = self._parser.feed(byte) msg = self._parser.feed(byte)
if frame is not None: if msg is not None:
self._rx_frame_count += 1 self._rx_count += 1
self._dispatch_frame(frame) self._dispatch(msg)
def _dispatch_frame(self, frame) -> None: def _dispatch(self, msg) -> None:
"""Route a decoded frame to the appropriate publisher."""
now = self.get_clock().now().to_msg() now = self.get_clock().now().to_msg()
ts = f"{now.sec}.{now.nanosec:09d}"
if isinstance(frame, ImuFrame): if isinstance(msg, RcChannels):
self._publish_imu(frame, now) out = String()
out.data = json.dumps({
"channels": msg.channels,
"source": msg.source,
"ts": ts,
})
self._rc_pub.publish(out)
elif isinstance(frame, BatteryFrame): elif isinstance(msg, SensorData):
self._publish_battery(frame, now) out = String()
out.data = json.dumps({
"pressure_pa": msg.pressure_pa,
"temperature_c": msg.temperature_c,
"tof_mm": msg.tof_mm,
"ts": ts,
})
self._sens_pub.publish(out)
elif isinstance(frame, MotorRpmFrame): elif isinstance(msg, tuple):
self._publish_motor_rpm(frame, now) type_code, _ = msg
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
elif isinstance(frame, ArmStateFrame): # ── TX ────────────────────────────────────────────────────────────────
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 ESP32 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"ESP32 BALANCE 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: def _heartbeat_cb(self) -> None:
"""Send HEARTBEAT every heartbeat_period (default 200ms)."""
self._write(encode_heartbeat()) self._write(encode_heartbeat())
def _watchdog_cb(self) -> None: def _on_leds(self, msg: String) -> None:
"""Send zero-speed if /cmd_vel silent for watchdog_timeout seconds.""" """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
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: try:
data = json.loads(msg.data) d = json.loads(msg.data)
kp = float(data["kp"]) frame = encode_led_cmd(
ki = float(data["ki"]) int(d.get("pattern", 0)),
kd = float(data["kd"]) int(d.get("r", 0)),
int(d.get("g", 0)),
int(d.get("b", 0)),
)
except (ValueError, KeyError, json.JSONDecodeError) as exc: except (ValueError, KeyError, json.JSONDecodeError) as exc:
self.get_logger().error(f"Bad PID update JSON: {exc}") self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
return return
frame = encode_pid_update(kp, ki, kd) self._write(frame)
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 _on_outputs(self, msg: String) -> None:
"""Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
try:
d = json.loads(msg.data)
frame = encode_output_cmd(
bool(d.get("horn", False)),
bool(d.get("buzzer", False)),
int(d.get("headlight", 0)),
int(d.get("fan", 0)),
)
except (ValueError, KeyError, json.JSONDecodeError) as exc:
self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}")
return
self._write(frame)
def _svc_arm(self, request: SetBool.Request, response: SetBool.Response): # ── Diagnostics ───────────────────────────────────────────────────────
"""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: def _publish_diagnostics(self) -> None:
diag = DiagnosticArray() diag = DiagnosticArray()
diag.header.stamp = self.get_clock().now().to_msg() diag.header.stamp = self.get_clock().now().to_msg()
status = DiagnosticStatus() status = DiagnosticStatus()
status.name = "saltybot/stm32_cmd_node" status.name = "saltybot/esp32_io_bridge"
status.hardware_id = "esp32" status.hardware_id = "esp32-s3-io"
port_ok = self._ser is not None and self._ser.is_open port_ok = self._ser is not None and self._ser.is_open
if port_ok: status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
status.level = DiagnosticStatus.OK status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
status.message = "Serial OK" status.values = [
else: KeyValue(key="serial_port", value=self._port_name),
status.level = DiagnosticStatus.ERROR KeyValue(key="baud_rate", value=str(self._baud)),
status.message = f"Serial disconnected: {self._port_name}" KeyValue(key="port_open", value=str(port_ok)),
KeyValue(key="rx_frames", value=str(self._rx_count)),
wd_age = time.monotonic() - self._last_cmd_t KeyValue(key="rx_errors", value=str(self._parser.frames_error)),
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) diag.status.append(status)
self._diag_pub.publish(diag) self._diag_pub.publish(diag)
# ── Lifecycle ───────────────────────────────────────────────────────────── # ── Lifecycle ─────────────────────────────────────────────────────────
def destroy_node(self) -> None: def destroy_node(self) -> None:
# Send zero-speed + disarm on shutdown self._write(encode_heartbeat(state=0))
self._write(encode_speed_steer(0, 0)) with self._ser_lock:
self._write(encode_arm(False)) if self._ser and self._ser.is_open:
self._close_serial() try:
self._ser.close()
except Exception:
pass
self._ser = None
super().destroy_node() super().destroy_node()

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

@ -1,336 +0,0 @@
"""stm32_protocol.py — Binary frame codec for Jetson↔ESP32 BALANCE communication.
# TODO(esp32-migration): This protocol was designed for STM32F722 USB CDC.
# When ESP32 BALANCE protocol is defined, update frame layout and baud rate.
Issue #119: defines the binary serial protocol between the Jetson Nano and the
ESP32 BALANCE over USB CDC @ 921600 baud.
# TODO(esp32-migration): update when ESP32 BALANCE protocol is defined.
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 → ESP32 BALANCE):
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 (ESP32 BALANCE → 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 (ESP32 BALANCE → 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 ESP32 BALANCE 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)

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

@ -1,27 +1,34 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32 IO motor can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
controller and VESC motor controllers over CAN bus. board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps).
The node opens the SocketCAN interface (slcan0 by default), spawns a background Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
reader thread to process incoming telemetry, and exposes the following interface:
Subscriptions Subscriptions
------------- -------------
/cmd_vel geometry_msgs/Twist → VESC speed commands (CAN) /cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300)
/estop std_msgs/Bool → ESP32 IO e-stop (CAN) /estop std_msgs/Bool → ORIN_CMD_ESTOP (0x303)
/saltybot/arm std_msgs/Bool → ORIN_CMD_ARM (0x301)
Publications Publications
------------ ------------
/can/imu sensor_msgs/Imu ESP32 IO IMU telemetry /saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state
/can/battery sensor_msgs/BatteryState ESP32 IO battery telemetry /saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude
/can/vesc/left/state std_msgs/Float32MultiArray Left VESC state /can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi
/can/vesc/right/state std_msgs/Float32MultiArray Right VESC state /can/vesc/left/state std_msgs/Float32MultiArray VESC STATUS_1 left
/can/connection_status std_msgs/String "connected" | "disconnected" /can/vesc/right/state std_msgs/Float32MultiArray VESC STATUS_1 right
/can/connection_status std_msgs/String "connected" | "disconnected"
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 Parameters
----------
can_interface str CAN socket name (default: slcan0)
speed_scale float /cmd_vel linear.x (m/s) → motor units (default: 1000.0)
steer_scale float /cmd_vel angular.z (rad/s) → motor units (default: -500.0)
command_timeout_s float watchdog zero-vel threshold (default: 0.5)
""" """
import json
import threading import threading
import time import time
from typing import Optional from typing import Optional
@ -30,90 +37,77 @@ import can
import rclpy import rclpy
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from rclpy.node import Node from rclpy.node import Node
from rcl_interfaces.msg import SetParametersResult from sensor_msgs.msg import BatteryState
from sensor_msgs.msg import BatteryState, Imu
from std_msgs.msg import Bool, Float32MultiArray, String from std_msgs.msg import Bool, Float32MultiArray, String
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.balance_protocol import (
MAMBA_CMD_ESTOP, ORIN_CMD_DRIVE,
MAMBA_CMD_MODE, ORIN_CMD_ARM,
MAMBA_CMD_VELOCITY, ORIN_CMD_ESTOP,
MAMBA_TELEM_BATTERY, ESP32_TELEM_ATTITUDE,
MAMBA_TELEM_IMU, ESP32_TELEM_BATTERY,
VESC_TELEM_STATE, VESC_LEFT_ID,
ORIN_CAN_ID_FC_PID_ACK, VESC_RIGHT_ID,
ORIN_CAN_ID_PID_SET, VESC_STATUS_1,
MODE_DRIVE, MODE_DRIVE,
MODE_ESTOP,
MODE_IDLE, MODE_IDLE,
encode_drive_cmd,
encode_arm_cmd,
encode_estop_cmd, encode_estop_cmd,
encode_mode_cmd, decode_attitude,
encode_velocity_cmd, decode_battery,
encode_pid_set_cmd, decode_vesc_status1,
decode_battery_telem,
decode_imu_telem,
decode_pid_ack,
decode_vesc_state,
) )
# Reconnect attempt interval when CAN bus is lost # Reconnect attempt interval when CAN bus is lost
_RECONNECT_INTERVAL_S: float = 5.0 _RECONNECT_INTERVAL_S: float = 5.0
# Watchdog timer tick rate (Hz) # Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent
_WATCHDOG_HZ: float = 10.0 _WATCHDOG_HZ: float = 10.0
class CanBridgeNode(Node): class CanBridgeNode(Node):
"""CAN bus bridge between Orin ROS2 and ESP32 IO / VESC controllers.""" """CAN bus bridge between Orin ROS2 and ESP32 BALANCE / VESC controllers."""
def __init__(self) -> None: def __init__(self) -> None:
super().__init__("can_bridge_node") super().__init__("can_bridge_node")
# ── Parameters ──────────────────────────────────────────────────── # ── Parameters ────────────────────────────────────────────────────
self.declare_parameter("can_interface", "slcan0") self.declare_parameter("can_interface", "slcan0")
self.declare_parameter("left_vesc_can_id", 56) self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
self.declare_parameter("right_vesc_can_id", 68) self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
self.declare_parameter("mamba_can_id", 1) self.declare_parameter("speed_scale", 1000.0)
self.declare_parameter("steer_scale", -500.0)
self.declare_parameter("command_timeout_s", 0.5) self.declare_parameter("command_timeout_s", 0.5)
self.declare_parameter("pid/kp", 0.0)
self.declare_parameter("pid/ki", 0.0)
self.declare_parameter("pid/kd", 0.0)
self._iface: str = self.get_parameter("can_interface").value self._iface = self.get_parameter("can_interface").value
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value self._left_vesc_id = self.get_parameter("left_vesc_can_id").value
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
self._mamba_id: int = self.get_parameter("mamba_can_id").value self._speed_scale = self.get_parameter("speed_scale").value
self._cmd_timeout: float = self.get_parameter("command_timeout_s").value self._steer_scale = self.get_parameter("steer_scale").value
self._pid_kp: float = self.get_parameter("pid/kp").value self._cmd_timeout = self.get_parameter("command_timeout_s").value
self._pid_ki: float = self.get_parameter("pid/ki").value
self._pid_kd: float = self.get_parameter("pid/kd").value
# ── State ───────────────────────────────────────────────────────── # ── State ─────────────────────────────────────────────────────────
self._bus: Optional[can.BusABC] = None self._bus: Optional[can.BusABC] = None
self._connected: bool = False self._connected: bool = False
self._last_cmd_time: float = time.monotonic() self._last_cmd_time: float = time.monotonic()
self._lock = threading.Lock() # protects _bus / _connected self._lock = threading.Lock()
# ── Publishers ──────────────────────────────────────────────────── # ── Publishers ────────────────────────────────────────────────────
self._pub_imu = self.create_publisher(Imu, "/can/imu", 10) self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10)
self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10)
self._pub_vesc_left = self.create_publisher( self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
Float32MultiArray, "/can/vesc/left/state", 10 self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state", 10)
) self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state", 10)
self._pub_vesc_right = self.create_publisher( self._pub_status = self.create_publisher(String, "/can/connection_status", 10)
Float32MultiArray, "/can/vesc/right/state", 10
)
self._pub_status = self.create_publisher(
String, "/can/connection_status", 10
)
# ── Subscriptions ───────────────────────────────────────────────── # ── Subscriptions ─────────────────────────────────────────────────
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
self.create_subscription(Bool, "/estop", self._estop_cb, 10) self.create_subscription(Bool, "/estop", self._estop_cb, 10)
self.add_on_set_parameters_callback(self._on_set_parameters) self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
# ── Timers ──────────────────────────────────────────────────────── # ── Timers ────────────────────────────────────────────────────────
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb) self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb)
# ── Open CAN ────────────────────────────────────────────────────── # ── Open CAN ──────────────────────────────────────────────────────
@ -128,46 +122,17 @@ class CanBridgeNode(Node):
self.get_logger().info( self.get_logger().info(
f"can_bridge_node ready — iface={self._iface} " f"can_bridge_node ready — iface={self._iface} "
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
f"mamba={self._mamba_id}" f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
) )
# -- PID parameter callback (Issue #693) --
def _on_set_parameters(self, params) -> SetParametersResult:
"""Send new PID gains over CAN when pid/* params change."""
for p in params:
if p.name == "pid/kp":
self._pid_kp = float(p.value)
elif p.name == "pid/ki":
self._pid_ki = float(p.value)
elif p.name == "pid/kd":
self._pid_kd = float(p.value)
else:
continue
try:
payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
self.get_logger().info(
f"PID gains sent: Kp={self._pid_kp:.2f} "
f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
)
except ValueError as exc:
return SetParametersResult(successful=False, reason=str(exc))
return SetParametersResult(successful=True)
# ── Connection management ────────────────────────────────────────────── # ── Connection management ──────────────────────────────────────────────
def _try_connect(self) -> None: def _try_connect(self) -> None:
"""Attempt to open the CAN interface; silently skip if already connected."""
with self._lock: with self._lock:
if self._connected: if self._connected:
return return
try: try:
bus = can.interface.Bus( self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
channel=self._iface,
bustype="socketcan",
)
self._bus = bus
self._connected = True self._connected = True
self.get_logger().info(f"CAN bus connected: {self._iface}") self.get_logger().info(f"CAN bus connected: {self._iface}")
self._publish_status("connected") self._publish_status("connected")
@ -180,12 +145,10 @@ class CanBridgeNode(Node):
self._publish_status("disconnected") self._publish_status("disconnected")
def _reconnect_cb(self) -> None: def _reconnect_cb(self) -> None:
"""Periodic timer: try to reconnect when disconnected."""
if not self._connected: if not self._connected:
self._try_connect() self._try_connect()
def _handle_can_error(self, exc: Exception, context: str) -> None: def _handle_can_error(self, exc: Exception, context: str) -> None:
"""Mark bus as disconnected on any CAN error."""
self.get_logger().warning(f"CAN error in {context}: {exc}") self.get_logger().warning(f"CAN error in {context}: {exc}")
with self._lock: with self._lock:
if self._bus is not None: if self._bus is not None:
@ -200,73 +163,51 @@ class CanBridgeNode(Node):
# ── ROS callbacks ───────────────────────────────────────────────────── # ── ROS callbacks ─────────────────────────────────────────────────────
def _cmd_vel_cb(self, msg: Twist) -> None: def _cmd_vel_cb(self, msg: Twist) -> None:
"""Convert /cmd_vel Twist to VESC speed commands over CAN.""" """Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN."""
self._last_cmd_time = time.monotonic() self._last_cmd_time = time.monotonic()
if not self._connected: if not self._connected:
return return
speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale)))
# Differential drive decomposition — individual wheel speeds in m/s. steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
# The VESC nodes interpret linear velocity directly; angular is handled self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
# by the sign difference between left and right.
linear = msg.linear.x
angular = msg.angular.z
# Forward left = forward right for pure translation; for rotation
# left slows and right speeds up (positive angular = CCW = left turn).
# The ESP32 IO velocity command carries both wheels independently.
left_mps = linear - angular
right_mps = linear + angular
payload = encode_velocity_cmd(left_mps, right_mps)
self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
# Keep ESP32 IO in DRIVE mode while receiving commands
self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
def _estop_cb(self, msg: Bool) -> None: def _estop_cb(self, msg: Bool) -> None:
"""Forward /estop to ESP32 IO over CAN.""" """Forward /estop to ESP32 BALANCE over CAN."""
if not self._connected: if not self._connected:
return return
payload = encode_estop_cmd(msg.data)
self._send_can(MAMBA_CMD_ESTOP, payload, "estop")
if msg.data: if msg.data:
self._send_can( self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode" self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
) else:
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 IO") # Clear estop: send DISARM then re-ARM (let operator decide to re-arm)
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "estop_clear")
def _arm_cb(self, msg: Bool) -> None:
"""Forward /saltybot/arm to ORIN_CMD_ARM."""
if not self._connected:
return
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(msg.data), "arm")
self.get_logger().info(f"ARM command: {'ARM' if msg.data else 'DISARM'}")
# ── Watchdog ────────────────────────────────────────────────────────── # ── Watchdog ──────────────────────────────────────────────────────────
def _watchdog_cb(self) -> None: def _watchdog_cb(self) -> None:
"""If no /cmd_vel arrives within the timeout, send zero velocity.""" """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive)."""
if not self._connected: if not self._connected:
return return
elapsed = time.monotonic() - self._last_cmd_time if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
if elapsed > self._cmd_timeout: self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
self._send_can(
MAMBA_CMD_VELOCITY,
encode_velocity_cmd(0.0, 0.0),
"watchdog zero-vel",
)
self._send_can(
MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
)
# ── CAN send helper ─────────────────────────────────────────────────── # ── CAN send helper ───────────────────────────────────────────────────
def _send_can(self, arb_id: int, data: bytes, context: str) -> None: def _send_can(self, arb_id: int, data: bytes, context: str,
"""Send a standard CAN frame; handle errors gracefully.""" extended: bool = False) -> None:
with self._lock: with self._lock:
if not self._connected or self._bus is None: if not self._connected or self._bus is None:
return return
bus = self._bus bus = self._bus
msg = can.Message(arbitration_id=arb_id, data=data,
msg = can.Message( is_extended_id=extended)
arbitration_id=arb_id,
data=data,
is_extended_id=False,
)
try: try:
bus.send(msg, timeout=0.05) bus.send(msg, timeout=0.05)
except can.CanError as exc: except can.CanError as exc:
@ -275,55 +216,41 @@ class CanBridgeNode(Node):
# ── Background CAN reader ───────────────────────────────────────────── # ── Background CAN reader ─────────────────────────────────────────────
def _reader_loop(self) -> None: def _reader_loop(self) -> None:
"""
Blocking CAN read loop executed in a daemon thread.
Dispatches incoming frames to the appropriate handler.
"""
while rclpy.ok(): while rclpy.ok():
with self._lock: with self._lock:
connected = self._connected connected, bus = self._connected, self._bus
bus = self._bus
if not connected or bus is None: if not connected or bus is None:
time.sleep(0.1) time.sleep(0.1)
continue continue
try: try:
frame = bus.recv(timeout=0.5) frame = bus.recv(timeout=0.5)
except can.CanError as exc: except can.CanError as exc:
self._handle_can_error(exc, "reader_loop recv") self._handle_can_error(exc, "reader_loop recv")
continue continue
if frame is None: if frame is None:
# Timeout — no frame within 0.5 s, loop again
continue continue
self._dispatch_frame(frame) self._dispatch_frame(frame)
def _dispatch_frame(self, frame: can.Message) -> None: def _dispatch_frame(self, frame: can.Message) -> None:
"""Route an incoming CAN frame to the correct publisher."""
arb_id = frame.arbitration_id arb_id = frame.arbitration_id
data = bytes(frame.data) data = bytes(frame.data)
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
try: try:
if arb_id == MAMBA_TELEM_IMU: if arb_id == ESP32_TELEM_ATTITUDE:
self._handle_imu(data, frame.timestamp) self._handle_attitude(data)
elif arb_id == ESP32_TELEM_BATTERY:
elif arb_id == MAMBA_TELEM_BATTERY: self._handle_battery(data)
self._handle_battery(data, frame.timestamp) elif arb_id == vesc_l:
t = decode_vesc_status1(self._left_vesc_id, data)
elif arb_id == VESC_TELEM_STATE + self._left_vesc_id: m = Float32MultiArray()
self._handle_vesc_state(data, frame.timestamp, side="left") m.data = [t.erpm, t.duty, 0.0, t.current]
self._pub_vesc_left.publish(m)
elif arb_id == VESC_TELEM_STATE + self._right_vesc_id: elif arb_id == vesc_r:
self._handle_vesc_state(data, frame.timestamp, side="right") t = decode_vesc_status1(self._right_vesc_id, data)
m = Float32MultiArray()
elif arb_id == ORIN_CAN_ID_FC_PID_ACK: m.data = [t.erpm, t.duty, 0.0, t.current]
gains = decode_pid_ack(data) self._pub_vesc_right.publish(m)
self.get_logger().debug(
f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
)
except Exception as exc: except Exception as exc:
self.get_logger().warning( self.get_logger().warning(
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}" f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
@ -331,52 +258,36 @@ class CanBridgeNode(Node):
# ── Frame handlers ──────────────────────────────────────────────────── # ── Frame handlers ────────────────────────────────────────────────────
def _handle_imu(self, data: bytes, timestamp: float) -> None: _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
telem = decode_imu_telem(data)
msg = Imu() def _handle_attitude(self, data: bytes) -> None:
msg.header.stamp = self.get_clock().now().to_msg() """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
msg.header.frame_id = "imu_link" t = decode_attitude(data)
now = self.get_clock().now().to_msg()
msg.linear_acceleration.x = telem.accel_x payload = {
msg.linear_acceleration.y = telem.accel_y "pitch_deg": round(t.pitch_deg, 2),
msg.linear_acceleration.z = telem.accel_z "speed_mps": round(t.speed, 3),
"yaw_rate": round(t.yaw_rate, 3),
msg.angular_velocity.x = telem.gyro_x "state": t.state,
msg.angular_velocity.y = telem.gyro_y "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
msg.angular_velocity.z = telem.gyro_z "flags": t.flags,
"ts": f"{now.sec}.{now.nanosec:09d}",
# Covariance unknown; mark as -1 per REP-145 }
msg.orientation_covariance[0] = -1.0 msg = String()
msg.data = json.dumps(payload)
self._pub_imu.publish(msg) self._pub_attitude.publish(msg)
self._pub_balance.publish(msg) # keep /saltybot/balance_state alive
def _handle_battery(self, data: bytes, timestamp: float) -> None:
telem = decode_battery_telem(data)
def _handle_battery(self, data: bytes) -> None:
"""BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
t = decode_battery(data)
msg = BatteryState() msg = BatteryState()
msg.header.stamp = self.get_clock().now().to_msg() msg.header.stamp = self.get_clock().now().to_msg()
msg.voltage = telem.voltage msg.voltage = t.vbat_mv / 1000.0
msg.current = telem.current
msg.present = True msg.present = True
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
self._pub_battery.publish(msg) self._pub_battery.publish(msg)
def _handle_vesc_state(
self, data: bytes, timestamp: float, side: str
) -> None:
telem = decode_vesc_state(data)
msg = Float32MultiArray()
# Layout: [erpm, duty, voltage, current]
msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
if side == "left":
self._pub_vesc_left.publish(msg)
else:
self._pub_vesc_right.publish(msg)
# ── Status helper ───────────────────────────────────────────────────── # ── Status helper ─────────────────────────────────────────────────────
def _publish_status(self, status: str) -> None: def _publish_status(self, status: str) -> None:
@ -387,17 +298,10 @@ class CanBridgeNode(Node):
# ── Shutdown ────────────────────────────────────────────────────────── # ── Shutdown ──────────────────────────────────────────────────────────
def destroy_node(self) -> None: def destroy_node(self) -> None:
"""Send zero velocity and shut down the CAN bus cleanly."""
if self._connected and self._bus is not None: if self._connected and self._bus is not None:
try: try:
self._send_can( self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
MAMBA_CMD_VELOCITY, self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown")
encode_velocity_cmd(0.0, 0.0),
"shutdown",
)
self._send_can(
MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
)
except Exception: except Exception:
pass pass
try: try:
@ -407,8 +311,6 @@ class CanBridgeNode(Node):
super().destroy_node() super().destroy_node()
# ---------------------------------------------------------------------------
def main(args=None) -> None: def main(args=None) -> None:
rclpy.init(args=args) rclpy.init(args=args)
node = CanBridgeNode() node = CanBridgeNode()

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

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