Merge pull request 'feat(arch): CAN/UART bridge update for SAUL-TEE ESP32 architecture' (#714) from sl-controls/esp32-can-bridge-update into main
This commit is contained in:
commit
fe84ff6039
@ -14,125 +14,123 @@
|
||||
│ ORIN NANO SUPER │
|
||||
│ (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-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │
|
||||
│ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │
|
||||
│ USB ─────── Leap Motion Controller (hand/gesture tracking) │
|
||||
│ CSI-A ──── ArduCam adapter → 2× IMX219 (front + left) │
|
||||
│ CSI-B ──── ArduCam adapter → 2× IMX219 (rear + right) │
|
||||
│ CSI-A ──── ArduCam adapter → 2x IMX219 (front + left) │
|
||||
│ CSI-B ──── ArduCam adapter → 2x IMX219 (rear + right) │
|
||||
│ M.2 ───── 1TB NVMe SSD │
|
||||
│ 40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec) │
|
||||
│ Pin 8 ──┐ │
|
||||
│ Pin 10 ─┤ UART fallback to FC (ttyTHS0, 921600) │
|
||||
│ Pin 10 ─┤ UART fallback to ESP32-S3 BALANCE (ttyTHS0, 460800) │
|
||||
│ Pin 6 ──┘ GND │
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
│ USB-C (data only) │ UART fallback (3 wires)
|
||||
│ 921600 baud │ 921600 baud, 3.3V
|
||||
│ USB-A (CANable2) │ UART fallback (3 wires)
|
||||
│ SocketCAN slcan0 │ 460800 baud, 3.3V
|
||||
│ 500 kbps │
|
||||
▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ MAMBA F722S (FC) │
|
||||
│ (Middle Plate — foam mounted) │
|
||||
│ ESP32-S3 BALANCE │
|
||||
│ (Waveshare Touch LCD 1.28, Middle Plate) │
|
||||
│ │
|
||||
│ USB-C ──── Orin (CDC serial, primary link) │
|
||||
│ │
|
||||
│ USART2 (PA2=TX, PA3=RX) ──── Hoverboard ESC (26400 baud) │
|
||||
│ UART4 (PA0=TX, PA1=RX) ──── ELRS RX (CRSF, 420000 baud) │
|
||||
│ USART6 (PC6=TX, PC7=RX) ──── Orin UART fallback │
|
||||
│ UART5 (PC12=TX, PD2=RX) ─── Debug (optional) │
|
||||
│ │
|
||||
│ SPI1 ─── MPU6000 IMU (on-board, CW270) │
|
||||
│ I2C1 ─── BMP280 baro (on-board, disabled) │
|
||||
│ ADC ──── Battery voltage (PC1) + Current (PC3) │
|
||||
│ PB3 ──── WS2812B LED strip │
|
||||
│ PB2 ──── Buzzer │
|
||||
│ CAN bus ──── CANable2 → Orin (primary link, ISO 11898) │
|
||||
│ UART0 ──── Orin UART fallback (460800 baud, 3.3V) │
|
||||
│ UART1 ──── VESC Left (CAN ID 56) via UART/CAN bridge │
|
||||
│ UART2 ──── VESC Right (CAN ID 68) via UART/CAN bridge │
|
||||
│ I2C ──── QMI8658 IMU (onboard, 6-DOF accel+gyro) │
|
||||
│ SPI ──── GC9A01 LCD (onboard, 240x240 round display) │
|
||||
│ GPIO ──── WS2812B LED strip │
|
||||
│ GPIO ──── Buzzer │
|
||||
│ ADC ──── Battery voltage divider │
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
│ USART2 │ UART4
|
||||
│ PA2=TX → ESC RX │ PA0=TX → ELRS TX
|
||||
│ PA3=RX ← ESC TX │ PA1=RX ← ELRS RX
|
||||
│ GND ─── GND │ GND ─── GND
|
||||
│ CAN bus (ISO 11898) │ UART (460800 baud)
|
||||
│ 500 kbps │
|
||||
▼ ▼
|
||||
┌────────────────────────┐ ┌──────────────────────────┐
|
||||
│ HOVERBOARD ESC │ │ ELRS 2.4GHz RX │
|
||||
│ (Bottom Plate) │ │ (beside FC) │
|
||||
│ VESC Left (ID 56) │ │ VESC Right (ID 68) │
|
||||
│ (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 RIGHT 🛞
|
||||
MOTOR MOTOR
|
||||
│ │
|
||||
LEFT MOTOR RIGHT MOTOR
|
||||
```
|
||||
|
||||
|
||||
## 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 |
|
||||
|------|----|-----------|-------|
|
||||
| Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus |
|
||||
| From | To | Wire | Notes |
|
||||
|------|----|------|-------|
|
||||
| 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`
|
||||
- Baud: 921600, 8N1
|
||||
- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
|
||||
- Interface: SocketCAN `slcan0`, 500 kbps
|
||||
- Device node: `/dev/canable2` (via udev, symlink to ttyUSBx)
|
||||
- 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 |
|
||||
|----------|--------|--------|-----------|
|
||||
| Pin 8 | TXD0 | PC7 | USART6 RX |
|
||||
| Pin 10 | RXD0 | PC6 | USART6 TX |
|
||||
| Pin 6 | GND | GND | GND |
|
||||
| Orin Pin | Signal | ESP32-S3 Pin | Notes |
|
||||
|----------|--------|--------------|-------|
|
||||
| Pin 8 | TXD0 | GPIO17 (UART0 RX) | Orin TX -> BALANCE RX |
|
||||
| Pin 10 | RXD0 | GPIO18 (UART0 TX) | Orin RX <- BALANCE TX |
|
||||
| Pin 6 | GND | GND | Common ground |
|
||||
|
||||
- Jetson device: `/dev/ttyTHS0`
|
||||
- Baud: 921600, 8N1
|
||||
- Baud: 460800, 8N1
|
||||
- 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 |
|
||||
|--------|--------|---------|-------|
|
||||
| PA2 | USART2 TX | RX | FC sends speed/steer commands |
|
||||
| PA3 | USART2 RX | TX | ESC sends feedback (optional) |
|
||||
| From | To | Notes |
|
||||
|------|----|-------|
|
||||
| Orin USB-A | ESP32-S3 IO USB-C | USB cable, /dev/esp32-io |
|
||||
|
||||
- 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 |
|
||||
|
||||
- Baud: 26400, 8N1
|
||||
- Protocol: Binary frame — `[0xABCD][steer:int16][speed:int16][checksum:uint16]`
|
||||
- Speed range: -1000 to +1000
|
||||
- **Keep wires short and twisted** (EMI from ESC)
|
||||
|
||||
### 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
|
||||
- Baud: 500 kbps CAN
|
||||
- VESC Left: CAN ID 56, VESC Right: CAN ID 68
|
||||
- Commands: COMM_SET_RPM, COMM_SET_CURRENT, COMM_SET_DUTY
|
||||
- Telemetry: VESC Status 1 at 50 Hz (RPM, current, duty)
|
||||
|
||||
### 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)
|
||||
│ ├── FC (via USB or 5V pad)
|
||||
│ ├── ELRS RX (5V)
|
||||
│ ├── ESP32-S3 BALANCE (5V via USB-C)
|
||||
│ ├── ESP32-S3 IO (5V via USB-C)
|
||||
│ ├── WS2812B LEDs (5V)
|
||||
│ └── 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)
|
||||
@ -143,20 +141,36 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
||||
| RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` |
|
||||
| IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` |
|
||||
| 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)
|
||||
|
||||
| UART | Pins | Baud | Assignment | Notes |
|
||||
|------|------|------|------------|-------|
|
||||
| USART1 | PB6=TX, PB7=RX | — | SmartAudio/VTX | Unused in SaltyLab |
|
||||
| USART2 | PA2=TX, PA3=RX | 26400 | Hoverboard ESC | Binary motor commands |
|
||||
| USART3 | PB10=TX, PB11=RX | — | Available | Was SBUS default |
|
||||
| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control |
|
||||
| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
|
||||
| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
|
||||
| USB CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` |
|
||||
| Interface | Pins | Baud/Rate | Assignment | Notes |
|
||||
|-----------|------|-----------|------------|-------|
|
||||
| UART0 | GPIO17=RX, GPIO18=TX | 460800 | Orin UART fallback | 3.3V, cross-connect |
|
||||
| UART1 | GPIO19=RX, GPIO20=TX | 115200 | Debug serial | Optional |
|
||||
| CAN (TWAI) | GPIO21=H, GPIO22=L | 500 kbps | CAN bus (VESCs + Orin) | SN65HVD230 transceiver |
|
||||
| I2C | GPIO4=SDA, GPIO5=SCL | 400 kHz | QMI8658 IMU (addr 0x6B) | Onboard |
|
||||
| SPI | GPIO36=MOSI, GPIO37=SCLK, GPIO35=CS | 40 MHz | GC9A01 LCD (onboard) | 240x240 round |
|
||||
| USB CDC | USB-C | 460800 | Orin USB fallback | /dev/esp32-balance |
|
||||
|
||||
## 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)
|
||||
@ -174,57 +188,59 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
||||
| Pin 2, 4 | 5V | Power |
|
||||
| Pin 6, 9 | GND | Ground |
|
||||
|
||||
- **Codec:** Wolfson WM8960 (I2C addr 0x1A)
|
||||
- **Mics:** 2× MEMS (left + right) — basic stereo / sound localization
|
||||
- **Speaker:** 3W class-D amp output (JST connector)
|
||||
- **Headset:** 3.5mm TRRS jack
|
||||
- **Requires:** WM8960 device tree overlay for Jetson (community port)
|
||||
- **Use:** Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
|
||||
- Codec: Wolfson WM8960 (I2C addr 0x1A)
|
||||
- Mics: 2x MEMS (left + right) --- basic stereo / sound localization
|
||||
- Speaker: 3W class-D amp output (JST connector)
|
||||
- Headset: 3.5mm TRRS jack
|
||||
- Requires: WM8960 device tree overlay for Jetson (community port)
|
||||
- Use: Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
|
||||
|
||||
### 8. SIM7600A 4G/LTE HAT (via USB)
|
||||
|
||||
| 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) |
|
||||
| Power | 5V from USB or separate 5V supply (peak 2A during TX) |
|
||||
| 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
|
||||
- **GPS/GNSS:** Built-in receiver, NMEA sentences on ttyUSB2 — outdoor positioning
|
||||
- **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
|
||||
- **Use:** Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
|
||||
- Data: PPP or QMI for internet connectivity
|
||||
- GPS/GNSS: Built-in receiver, NMEA sentences on ttyUSB2 --- outdoor positioning
|
||||
- AT commands: `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
|
||||
- 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
|
||||
|
||||
### 10. Leap Motion Controller (USB)
|
||||
### 9. Leap Motion Controller (USB)
|
||||
|
||||
| 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 |
|
||||
| Range | ~80cm, 150° FOV |
|
||||
| Range | ~80cm, 150 deg FOV |
|
||||
| SDK | Ultraleap Gemini V5+ (Linux ARM64 support) |
|
||||
| ROS2 | `leap_motion_ros2` wrapper available |
|
||||
|
||||
- **2× IR cameras + 3× IR LEDs** — tracks all 10 fingers in 3D, sub-mm precision
|
||||
- **Mount:** Forward-facing on sensor tower or upward on Orin plate
|
||||
- **Use:** Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
|
||||
- **Combined with ReSpeaker:** Voice + gesture control with zero hardware in hand
|
||||
- 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
|
||||
- Use: Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
|
||||
- Combined with ReSpeaker: Voice + gesture control with zero hardware in hand
|
||||
|
||||
### 11. Power Budget (USB)
|
||||
### 10. Power Budget (USB)
|
||||
|
||||
| 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) |
|
||||
| RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
|
||||
| SIM7600A | USB | ~1W idle, 3W TX peak |
|
||||
| Leap Motion | USB | ~0.5W |
|
||||
| Leap Motion | USB-A | ~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) │
|
||||
└──────┬───────┘
|
||||
│ radio
|
||||
┌──────▼───────┐
|
||||
│ ELRS RX │ CRSF 420kbaud
|
||||
│ RC RX │ CRSF 420kbaud (future)
|
||||
└──────┬───────┘
|
||||
│ UART4
|
||||
│ UART
|
||||
┌────────────▼────────────┐
|
||||
│ MAMBA F722S │
|
||||
│ ESP32-S3 BALANCE │
|
||||
│ (Waveshare LCD 1.28) │
|
||||
│ │
|
||||
│ MPU6000 → Balance PID │
|
||||
│ CRSF → Mode Manager │
|
||||
│ QMI8658 -> Balance PID │
|
||||
│ RC -> Mode Manager │
|
||||
│ Safety Monitor │
|
||||
│ │
|
||||
└──┬──────────┬───────────┘
|
||||
USART2 ─────┘ └───── USB CDC / USART6
|
||||
26400 baud 921600 baud
|
||||
CAN 500kbps─┘ └───── CAN bus / UART fallback
|
||||
│ │
|
||||
▼ ▼
|
||||
┌────────────────┐ ┌───────────────────┐
|
||||
│ Hoverboard ESC │ │ Orin Nano Super │
|
||||
│ │ │ │
|
||||
│ L motor R motor│ │ SLAM / Nav2 / AI │
|
||||
│ 🛞 🛞 │ │ Person following │
|
||||
└────────────────┘ │ Voice commands │
|
||||
│ 4G telemetry │
|
||||
┌────┴────────────┐ ▼
|
||||
│ CAN bus (500k) │ ┌───────────────────┐
|
||||
├─ VESC Left 56 │ │ Orin Nano Super │
|
||||
└─ VESC Right 68 │ │ │
|
||||
│ │ │ SLAM / Nav2 / AI │
|
||||
▼ ▼ │ Person following │
|
||||
LEFT RIGHT │ Voice commands │
|
||||
MOTOR MOTOR │ 4G telemetry │
|
||||
└──┬──────────┬───────┘
|
||||
│ │
|
||||
┌──────────▼─┐ ┌────▼──────────┐
|
||||
│ ReSpeaker │ │ SIM7600A │
|
||||
│ 2-Mic HAT │ │ 4G/LTE + GPS │
|
||||
│ 🎤 🔊 │ │ 📡 🛰️ │
|
||||
└────────────┘ └───────────────┘
|
||||
```
|
||||
|
||||
@ -1,30 +1,16 @@
|
||||
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
|
||||
# Binary-framed Jetson↔ESP32 bridge at 921600 baud.
|
||||
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
|
||||
# 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 ────────────────────────────────────────────────────────────────
|
||||
# Use /dev/esp32-bridge if the udev rule is applied:
|
||||
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
|
||||
# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
|
||||
serial_port: /dev/ttyACM0
|
||||
baud_rate: 921600
|
||||
reconnect_delay: 2.0 # seconds between USB reconnect attempts
|
||||
# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
|
||||
# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
|
||||
serial_port: /dev/esp32-io
|
||||
baud_rate: 460800
|
||||
reconnect_delay: 2.0 # seconds between reconnect attempts
|
||||
|
||||
# ── Heartbeat ─────────────────────────────────────────────────────────────────
|
||||
# HEARTBEAT frame sent every heartbeat_period seconds.
|
||||
# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
|
||||
heartbeat_period: 0.2 # 200ms → well within 500ms ESP32 BALANCE 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
|
||||
# HEARTBEAT (0x20) sent every heartbeat_period.
|
||||
# ESP32 IO watchdog fires if no heartbeat for ~500 ms.
|
||||
heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog
|
||||
|
||||
@ -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:
|
||||
# Default (binary protocol, bidirectional):
|
||||
ros2 launch saltybot_bridge stm32_cmd.launch.py
|
||||
|
||||
# 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
|
||||
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
|
||||
"""
|
||||
|
||||
import os
|
||||
@ -24,11 +24,8 @@ def generate_launch_description() -> LaunchDescription:
|
||||
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
||||
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
||||
DeclareLaunchArgument("speed_scale", default_value="1000.0"),
|
||||
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
|
||||
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
|
||||
DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"),
|
||||
DeclareLaunchArgument("baud_rate", default_value="460800"),
|
||||
DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
|
||||
|
||||
Node(
|
||||
@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription:
|
||||
{
|
||||
"serial_port": LaunchConfiguration("serial_port"),
|
||||
"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"),
|
||||
},
|
||||
],
|
||||
|
||||
@ -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
|
||||
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
|
||||
Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
|
||||
inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
|
||||
|
||||
TX commands (Jetson → STM32):
|
||||
SPEED_STEER — 50 Hz from /cmd_vel subscription
|
||||
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
|
||||
This node is NOT the primary drive path (that is CAN via can_bridge_node).
|
||||
It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
|
||||
|
||||
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):
|
||||
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)
|
||||
RX from ESP32 IO:
|
||||
RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON)
|
||||
SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON)
|
||||
|
||||
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.
|
||||
TX to ESP32 IO:
|
||||
LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON)
|
||||
OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON)
|
||||
HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive)
|
||||
|
||||
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)
|
||||
serial_port /dev/esp32-io
|
||||
baud_rate 460800
|
||||
reconnect_delay 2.0
|
||||
heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
|
||||
@ -50,119 +37,69 @@ 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 (
|
||||
BAUD_RATE,
|
||||
FrameParser,
|
||||
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
||||
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
|
||||
encode_pid_update,
|
||||
RcChannels,
|
||||
SensorData,
|
||||
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):
|
||||
"""Binary-framed Jetson↔ESP32 bridge node."""
|
||||
"""Orin ↔ ESP32-S3 IO auxiliary 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)
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/esp32-io")
|
||||
self.declare_parameter("baud_rate", BAUD_RATE)
|
||||
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._port_name = self.get_parameter("serial_port").value
|
||||
self._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,
|
||||
)
|
||||
# ── QoS ───────────────────────────────────────────────────────────
|
||||
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)
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos)
|
||||
self._sens_pub = self.create_publisher(String, "/saltybot/sensors", 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,
|
||||
)
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos)
|
||||
self.create_subscription(String, "/saltybot/outputs", self._on_outputs, 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
|
||||
# ── Serial state ──────────────────────────────────────────────────
|
||||
self._ser: serial.Serial | None = None
|
||||
self._ser_lock = threading.Lock()
|
||||
self._parser = FrameParser()
|
||||
self._rx_count = 0
|
||||
|
||||
# ── 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 ──────────────────────────────────────
|
||||
# ── 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"
|
||||
f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────────
|
||||
# ── Serial management ─────────────────────────────────────────────────
|
||||
|
||||
def _open_serial(self) -> bool:
|
||||
with self._ser_lock:
|
||||
@ -170,7 +107,7 @@ class Stm32CmdNode(Node):
|
||||
self._ser = serial.Serial(
|
||||
port=self._port_name,
|
||||
baudrate=self._baud,
|
||||
timeout=0.005, # non-blocking reads
|
||||
timeout=0.005,
|
||||
write_timeout=0.1,
|
||||
)
|
||||
self._ser.reset_input_buffer()
|
||||
@ -185,17 +122,7 @@ class Stm32CmdNode(Node):
|
||||
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
|
||||
@ -207,16 +134,15 @@ class Stm32CmdNode(Node):
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
# ── RX — read callback ────────────────────────────────────────────────────
|
||||
# ── RX ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _read_cb(self) -> None:
|
||||
"""Read bytes from serial and feed them to the frame parser."""
|
||||
raw: bytes | None = None
|
||||
reconnect_needed = False
|
||||
reconnect = False
|
||||
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
reconnect_needed = True
|
||||
reconnect = True
|
||||
else:
|
||||
try:
|
||||
n = self._ser.in_waiting
|
||||
@ -225,9 +151,9 @@ class Stm32CmdNode(Node):
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial read error: {exc}")
|
||||
self._ser = None
|
||||
reconnect_needed = True
|
||||
reconnect = True
|
||||
|
||||
if reconnect_needed:
|
||||
if reconnect:
|
||||
self.get_logger().warn(
|
||||
"Serial disconnected — will retry",
|
||||
throttle_duration_sec=self._reconnect_delay,
|
||||
@ -240,230 +166,105 @@ class Stm32CmdNode(Node):
|
||||
return
|
||||
|
||||
for byte in raw:
|
||||
frame = self._parser.feed(byte)
|
||||
if frame is not None:
|
||||
self._rx_frame_count += 1
|
||||
self._dispatch_frame(frame)
|
||||
msg = self._parser.feed(byte)
|
||||
if msg is not None:
|
||||
self._rx_count += 1
|
||||
self._dispatch(msg)
|
||||
|
||||
def _dispatch_frame(self, frame) -> None:
|
||||
"""Route a decoded frame to the appropriate publisher."""
|
||||
def _dispatch(self, msg) -> None:
|
||||
now = self.get_clock().now().to_msg()
|
||||
ts = f"{now.sec}.{now.nanosec:09d}"
|
||||
|
||||
if isinstance(frame, ImuFrame):
|
||||
self._publish_imu(frame, now)
|
||||
if isinstance(msg, RcChannels):
|
||||
out = String()
|
||||
out.data = json.dumps({
|
||||
"channels": msg.channels,
|
||||
"source": msg.source,
|
||||
"ts": ts,
|
||||
})
|
||||
self._rc_pub.publish(out)
|
||||
|
||||
elif isinstance(frame, BatteryFrame):
|
||||
self._publish_battery(frame, now)
|
||||
elif isinstance(msg, SensorData):
|
||||
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):
|
||||
self._publish_motor_rpm(frame, now)
|
||||
elif isinstance(msg, tuple):
|
||||
type_code, _ = msg
|
||||
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
|
||||
|
||||
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 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,
|
||||
)
|
||||
# ── TX ────────────────────────────────────────────────────────────────
|
||||
|
||||
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."""
|
||||
def _on_leds(self, msg: String) -> None:
|
||||
"""Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
|
||||
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'}"
|
||||
d = json.loads(msg.data)
|
||||
frame = encode_led_cmd(
|
||||
int(d.get("pattern", 0)),
|
||||
int(d.get("r", 0)),
|
||||
int(d.get("g", 0)),
|
||||
int(d.get("b", 0)),
|
||||
)
|
||||
return response
|
||||
except (ValueError, KeyError, json.JSONDecodeError) as exc:
|
||||
self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
|
||||
return
|
||||
self._write(frame)
|
||||
|
||||
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
|
||||
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)
|
||||
|
||||
# ── Diagnostics ───────────────────────────────────────────────────────────
|
||||
# ── 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 = "esp32"
|
||||
|
||||
status.name = "saltybot/esp32_io_bridge"
|
||||
status.hardware_id = "esp32-s3-io"
|
||||
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.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
|
||||
status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
|
||||
status.values = [
|
||||
KeyValue(key="serial_port", value=self._port_name),
|
||||
KeyValue(key="baud_rate", value=str(self._baud)),
|
||||
KeyValue(key="port_open", value=str(port_ok)),
|
||||
KeyValue(key="rx_frames", value=str(self._rx_frame_count)),
|
||||
KeyValue(key="rx_frames", value=str(self._rx_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 ─────────────────────────────────────────────────────────────
|
||||
# ── 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()
|
||||
self._write(encode_heartbeat(state=0))
|
||||
with self._ser_lock:
|
||||
if self._ser and self._ser.is_open:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
|
||||
@ -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)
|
||||
@ -1,27 +1,34 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32 IO motor
|
||||
controller and VESC motor controllers over CAN bus.
|
||||
can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
|
||||
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
|
||||
reader thread to process incoming telemetry, and exposes the following interface:
|
||||
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
|
||||
|
||||
Subscriptions
|
||||
-------------
|
||||
/cmd_vel geometry_msgs/Twist → VESC speed commands (CAN)
|
||||
/estop std_msgs/Bool → ESP32 IO e-stop (CAN)
|
||||
/cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300)
|
||||
/estop std_msgs/Bool → ORIN_CMD_ESTOP (0x303)
|
||||
/saltybot/arm std_msgs/Bool → ORIN_CMD_ARM (0x301)
|
||||
|
||||
Publications
|
||||
------------
|
||||
/can/imu sensor_msgs/Imu ESP32 IO IMU telemetry
|
||||
/can/battery sensor_msgs/BatteryState ESP32 IO battery telemetry
|
||||
/can/vesc/left/state std_msgs/Float32MultiArray Left VESC state
|
||||
/can/vesc/right/state std_msgs/Float32MultiArray Right VESC state
|
||||
/saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state
|
||||
/saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude
|
||||
/can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi
|
||||
/can/vesc/left/state std_msgs/Float32MultiArray VESC STATUS_1 left
|
||||
/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 time
|
||||
from typing import Optional
|
||||
@ -30,87 +37,74 @@ import can
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from rcl_interfaces.msg import SetParametersResult
|
||||
from sensor_msgs.msg import BatteryState, Imu
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
|
||||
from saltybot_can_bridge.mamba_protocol import (
|
||||
MAMBA_CMD_ESTOP,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_TELEM_BATTERY,
|
||||
MAMBA_TELEM_IMU,
|
||||
VESC_TELEM_STATE,
|
||||
ORIN_CAN_ID_FC_PID_ACK,
|
||||
ORIN_CAN_ID_PID_SET,
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
ORIN_CMD_DRIVE,
|
||||
ORIN_CMD_ARM,
|
||||
ORIN_CMD_ESTOP,
|
||||
ESP32_TELEM_ATTITUDE,
|
||||
ESP32_TELEM_BATTERY,
|
||||
VESC_LEFT_ID,
|
||||
VESC_RIGHT_ID,
|
||||
VESC_STATUS_1,
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
MODE_IDLE,
|
||||
encode_drive_cmd,
|
||||
encode_arm_cmd,
|
||||
encode_estop_cmd,
|
||||
encode_mode_cmd,
|
||||
encode_velocity_cmd,
|
||||
encode_pid_set_cmd,
|
||||
decode_battery_telem,
|
||||
decode_imu_telem,
|
||||
decode_pid_ack,
|
||||
decode_vesc_state,
|
||||
decode_attitude,
|
||||
decode_battery,
|
||||
decode_vesc_status1,
|
||||
)
|
||||
|
||||
# Reconnect attempt interval when CAN bus is lost
|
||||
_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
|
||||
|
||||
|
||||
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:
|
||||
super().__init__("can_bridge_node")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("can_interface", "slcan0")
|
||||
self.declare_parameter("left_vesc_can_id", 56)
|
||||
self.declare_parameter("right_vesc_can_id", 68)
|
||||
self.declare_parameter("mamba_can_id", 1)
|
||||
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
|
||||
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
|
||||
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("pid/kp", 0.0)
|
||||
self.declare_parameter("pid/ki", 0.0)
|
||||
self.declare_parameter("pid/kd", 0.0)
|
||||
|
||||
self._iface: str = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
|
||||
self._mamba_id: int = self.get_parameter("mamba_can_id").value
|
||||
self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
|
||||
self._pid_kp: float = self.get_parameter("pid/kp").value
|
||||
self._pid_ki: float = self.get_parameter("pid/ki").value
|
||||
self._pid_kd: float = self.get_parameter("pid/kd").value
|
||||
self._iface = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
|
||||
self._speed_scale = self.get_parameter("speed_scale").value
|
||||
self._steer_scale = self.get_parameter("steer_scale").value
|
||||
self._cmd_timeout = self.get_parameter("command_timeout_s").value
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────
|
||||
self._bus: Optional[can.BusABC] = None
|
||||
self._connected: bool = False
|
||||
self._last_cmd_time: float = time.monotonic()
|
||||
self._lock = threading.Lock() # protects _bus / _connected
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
|
||||
self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10)
|
||||
self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10)
|
||||
self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 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_status = self.create_publisher(
|
||||
String, "/can/connection_status", 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_status = self.create_publisher(String, "/can/connection_status", 10)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
||||
self.add_on_set_parameters_callback(self._on_set_parameters)
|
||||
self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||
@ -128,46 +122,17 @@ class CanBridgeNode(Node):
|
||||
self.get_logger().info(
|
||||
f"can_bridge_node ready — iface={self._iface} "
|
||||
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
|
||||
f"mamba={self._mamba_id}"
|
||||
f"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 ──────────────────────────────────────────────
|
||||
|
||||
def _try_connect(self) -> None:
|
||||
"""Attempt to open the CAN interface; silently skip if already connected."""
|
||||
with self._lock:
|
||||
if self._connected:
|
||||
return
|
||||
try:
|
||||
bus = can.interface.Bus(
|
||||
channel=self._iface,
|
||||
bustype="socketcan",
|
||||
)
|
||||
self._bus = bus
|
||||
self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
|
||||
self._connected = True
|
||||
self.get_logger().info(f"CAN bus connected: {self._iface}")
|
||||
self._publish_status("connected")
|
||||
@ -180,12 +145,10 @@ class CanBridgeNode(Node):
|
||||
self._publish_status("disconnected")
|
||||
|
||||
def _reconnect_cb(self) -> None:
|
||||
"""Periodic timer: try to reconnect when disconnected."""
|
||||
if not self._connected:
|
||||
self._try_connect()
|
||||
|
||||
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}")
|
||||
with self._lock:
|
||||
if self._bus is not None:
|
||||
@ -200,73 +163,51 @@ class CanBridgeNode(Node):
|
||||
# ── ROS callbacks ─────────────────────────────────────────────────────
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist) -> None:
|
||||
"""Convert /cmd_vel Twist to VESC speed commands over CAN."""
|
||||
"""Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN."""
|
||||
self._last_cmd_time = time.monotonic()
|
||||
|
||||
if not self._connected:
|
||||
return
|
||||
|
||||
# Differential drive decomposition — individual wheel speeds in m/s.
|
||||
# The VESC nodes interpret linear velocity directly; angular is handled
|
||||
# by the sign difference between left and right.
|
||||
linear = msg.linear.x
|
||||
angular = msg.angular.z
|
||||
|
||||
# Forward left = forward right for pure translation; for rotation
|
||||
# left slows and right speeds up (positive angular = CCW = left turn).
|
||||
# The 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")
|
||||
speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale)))
|
||||
steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
|
||||
|
||||
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:
|
||||
return
|
||||
payload = encode_estop_cmd(msg.data)
|
||||
self._send_can(MAMBA_CMD_ESTOP, payload, "estop")
|
||||
if msg.data:
|
||||
self._send_can(
|
||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
|
||||
)
|
||||
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 IO")
|
||||
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
|
||||
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
|
||||
else:
|
||||
# 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 ──────────────────────────────────────────────────────────
|
||||
|
||||
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:
|
||||
return
|
||||
elapsed = time.monotonic() - self._last_cmd_time
|
||||
if elapsed > self._cmd_timeout:
|
||||
self._send_can(
|
||||
MAMBA_CMD_VELOCITY,
|
||||
encode_velocity_cmd(0.0, 0.0),
|
||||
"watchdog zero-vel",
|
||||
)
|
||||
self._send_can(
|
||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
|
||||
)
|
||||
if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
|
||||
|
||||
# ── CAN send helper ───────────────────────────────────────────────────
|
||||
|
||||
def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
|
||||
"""Send a standard CAN frame; handle errors gracefully."""
|
||||
def _send_can(self, arb_id: int, data: bytes, context: str,
|
||||
extended: bool = False) -> None:
|
||||
with self._lock:
|
||||
if not self._connected or self._bus is None:
|
||||
return
|
||||
bus = self._bus
|
||||
|
||||
msg = can.Message(
|
||||
arbitration_id=arb_id,
|
||||
data=data,
|
||||
is_extended_id=False,
|
||||
)
|
||||
msg = can.Message(arbitration_id=arb_id, data=data,
|
||||
is_extended_id=extended)
|
||||
try:
|
||||
bus.send(msg, timeout=0.05)
|
||||
except can.CanError as exc:
|
||||
@ -275,55 +216,41 @@ class CanBridgeNode(Node):
|
||||
# ── Background CAN reader ─────────────────────────────────────────────
|
||||
|
||||
def _reader_loop(self) -> None:
|
||||
"""
|
||||
Blocking CAN read loop executed in a daemon thread.
|
||||
Dispatches incoming frames to the appropriate handler.
|
||||
"""
|
||||
while rclpy.ok():
|
||||
with self._lock:
|
||||
connected = self._connected
|
||||
bus = self._bus
|
||||
|
||||
connected, bus = self._connected, self._bus
|
||||
if not connected or bus is None:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
|
||||
try:
|
||||
frame = bus.recv(timeout=0.5)
|
||||
except can.CanError as exc:
|
||||
self._handle_can_error(exc, "reader_loop recv")
|
||||
continue
|
||||
|
||||
if frame is None:
|
||||
# Timeout — no frame within 0.5 s, loop again
|
||||
continue
|
||||
|
||||
self._dispatch_frame(frame)
|
||||
|
||||
def _dispatch_frame(self, frame: can.Message) -> None:
|
||||
"""Route an incoming CAN frame to the correct publisher."""
|
||||
arb_id = frame.arbitration_id
|
||||
data = bytes(frame.data)
|
||||
|
||||
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||
vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||
try:
|
||||
if arb_id == MAMBA_TELEM_IMU:
|
||||
self._handle_imu(data, frame.timestamp)
|
||||
|
||||
elif arb_id == MAMBA_TELEM_BATTERY:
|
||||
self._handle_battery(data, frame.timestamp)
|
||||
|
||||
elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
|
||||
self._handle_vesc_state(data, frame.timestamp, side="left")
|
||||
|
||||
elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
|
||||
self._handle_vesc_state(data, frame.timestamp, side="right")
|
||||
|
||||
elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
|
||||
gains = decode_pid_ack(data)
|
||||
self.get_logger().debug(
|
||||
f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
|
||||
)
|
||||
|
||||
if arb_id == ESP32_TELEM_ATTITUDE:
|
||||
self._handle_attitude(data)
|
||||
elif arb_id == ESP32_TELEM_BATTERY:
|
||||
self._handle_battery(data)
|
||||
elif arb_id == vesc_l:
|
||||
t = decode_vesc_status1(self._left_vesc_id, data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
self._pub_vesc_left.publish(m)
|
||||
elif arb_id == vesc_r:
|
||||
t = decode_vesc_status1(self._right_vesc_id, data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
self._pub_vesc_right.publish(m)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
|
||||
@ -331,52 +258,36 @@ class CanBridgeNode(Node):
|
||||
|
||||
# ── Frame handlers ────────────────────────────────────────────────────
|
||||
|
||||
def _handle_imu(self, data: bytes, timestamp: float) -> None:
|
||||
telem = decode_imu_telem(data)
|
||||
_STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
|
||||
|
||||
msg = Imu()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = "imu_link"
|
||||
|
||||
msg.linear_acceleration.x = telem.accel_x
|
||||
msg.linear_acceleration.y = telem.accel_y
|
||||
msg.linear_acceleration.z = telem.accel_z
|
||||
|
||||
msg.angular_velocity.x = telem.gyro_x
|
||||
msg.angular_velocity.y = telem.gyro_y
|
||||
msg.angular_velocity.z = telem.gyro_z
|
||||
|
||||
# Covariance unknown; mark as -1 per REP-145
|
||||
msg.orientation_covariance[0] = -1.0
|
||||
|
||||
self._pub_imu.publish(msg)
|
||||
|
||||
def _handle_battery(self, data: bytes, timestamp: float) -> None:
|
||||
telem = decode_battery_telem(data)
|
||||
def _handle_attitude(self, data: bytes) -> None:
|
||||
"""ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
|
||||
t = decode_attitude(data)
|
||||
now = self.get_clock().now().to_msg()
|
||||
payload = {
|
||||
"pitch_deg": round(t.pitch_deg, 2),
|
||||
"speed_mps": round(t.speed, 3),
|
||||
"yaw_rate": round(t.yaw_rate, 3),
|
||||
"state": t.state,
|
||||
"state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
|
||||
"flags": t.flags,
|
||||
"ts": f"{now.sec}.{now.nanosec:09d}",
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload)
|
||||
self._pub_attitude.publish(msg)
|
||||
self._pub_balance.publish(msg) # keep /saltybot/balance_state alive
|
||||
|
||||
def _handle_battery(self, data: bytes) -> None:
|
||||
"""BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
|
||||
t = decode_battery(data)
|
||||
msg = BatteryState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.voltage = telem.voltage
|
||||
msg.current = telem.current
|
||||
msg.voltage = t.vbat_mv / 1000.0
|
||||
msg.present = True
|
||||
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
|
||||
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 ─────────────────────────────────────────────────────
|
||||
|
||||
def _publish_status(self, status: str) -> None:
|
||||
@ -387,17 +298,10 @@ class CanBridgeNode(Node):
|
||||
# ── Shutdown ──────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
"""Send zero velocity and shut down the CAN bus cleanly."""
|
||||
if self._connected and self._bus is not None:
|
||||
try:
|
||||
self._send_can(
|
||||
MAMBA_CMD_VELOCITY,
|
||||
encode_velocity_cmd(0.0, 0.0),
|
||||
"shutdown",
|
||||
)
|
||||
self._send_can(
|
||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
|
||||
)
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
|
||||
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown")
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
@ -407,8 +311,6 @@ class CanBridgeNode(Node):
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = CanBridgeNode()
|
||||
|
||||
@ -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)
|
||||
Loading…
x
Reference in New Issue
Block a user