feat(arch): implement SAUL-TEE ESP32 protocol specs from hal reference doc
Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md (hal, 2026-04-04) stm32_protocol.py — rewritten for inter-board UART protocol (ESP32 BALANCE ↔ IO): - Frame: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud (was STX/ETX/CRC16) - CRC-8 poly 0x07 over LEN+TYPE+PAYLOAD - New message types: RC_CHANNELS(0x01), SENSORS(0x02), LED_CMD(0x10), OUTPUT_CMD(0x11), MOTOR_CMD(0x12), HEARTBEAT(0x20) mamba_protocol.py — updated CAN IDs and frame formats: - Orin→BALANCE: DRIVE(0x300) f32×2 LE, MODE(0x301), ESTOP(0x302), LED(0x303) - BALANCE→Orin: FC_STATUS(0x400) pitch/vbat/state, FC_VESC(0x401) rpm/current - VESC node IDs: Left=56, Right=68 (authoritative per §8) - VESC extended frames: STATUS1(cmd=9), STATUS4(cmd=16), STATUS5(cmd=27) - Replaced old MAMBA_CMD_*/MAMBA_TELEM_* constants can_bridge_node.py — updated to use new IDs: - ORIN_CMD_DRIVE/MODE/ESTOP replace MAMBA_CMD_VELOCITY/MODE/ESTOP - FC_STATUS handler: publishes pitch→/can/imu, vbat_mv→/can/battery - FC_VESC handler: publishes rpm/cur→/can/vesc/left|right/state - VESC STATUS1 extended frames decoded per node ID (56/68) - Removed PID CAN command (not in new spec) CLAUDE.md — updated with ESP32-S3 BALANCE/IO hardware summary + key protocols Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
19be6bbe11
commit
308be74330
@ -1,138 +1,129 @@
|
||||
# SaltyLab / SAUL-TEE Wiring Reference
|
||||
# SaltyLab Wiring Diagram
|
||||
|
||||
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired.
|
||||
> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
|
||||
> **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only.
|
||||
|
||||
---
|
||||
|
||||
## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
## System Overview
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ 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 +134,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)
|
||||
## ESP32-S3 BALANCE UART/CAN Summary
|
||||
|
||||
| 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 +181,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 +241,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,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,22 +1,21 @@
|
||||
#!/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 (CANable2 / 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 §4 & §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 (0x302)
|
||||
|
||||
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
|
||||
/can/imu sensor_msgs/Imu from FC_STATUS (0x400) pitch
|
||||
/can/battery sensor_msgs/BatteryState from FC_STATUS (0x400) vbat_mv
|
||||
/can/vesc/left/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
||||
/can/vesc/right/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
||||
/can/connection_status std_msgs/String "connected" | "disconnected"
|
||||
|
||||
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
|
||||
@ -30,30 +29,36 @@ import can
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from rcl_interfaces.msg import SetParametersResult
|
||||
|
||||
from sensor_msgs.msg import BatteryState, Imu
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
|
||||
from saltybot_can_bridge.mamba_protocol import (
|
||||
MAMBA_CMD_ESTOP,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_TELEM_BATTERY,
|
||||
MAMBA_TELEM_IMU,
|
||||
VESC_TELEM_STATE,
|
||||
ORIN_CAN_ID_FC_PID_ACK,
|
||||
ORIN_CAN_ID_PID_SET,
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
# Orin → BALANCE command IDs
|
||||
ORIN_CMD_DRIVE,
|
||||
ORIN_CMD_MODE,
|
||||
ORIN_CMD_ESTOP,
|
||||
# BALANCE → Orin telemetry IDs
|
||||
FC_STATUS,
|
||||
FC_VESC,
|
||||
# VESC node IDs
|
||||
VESC_LEFT_ID,
|
||||
VESC_RIGHT_ID,
|
||||
VESC_STATUS_1,
|
||||
# Mode constants
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
MODE_IDLE,
|
||||
encode_estop_cmd,
|
||||
# Encoders
|
||||
encode_drive_cmd,
|
||||
encode_mode_cmd,
|
||||
encode_velocity_cmd,
|
||||
encode_pid_set_cmd,
|
||||
decode_battery_telem,
|
||||
decode_imu_telem,
|
||||
decode_pid_ack,
|
||||
decode_vesc_state,
|
||||
encode_estop_cmd,
|
||||
encode_led_cmd,
|
||||
# Decoders
|
||||
decode_fc_status,
|
||||
decode_fc_vesc,
|
||||
decode_vesc_status1,
|
||||
decode_vesc_can_id,
|
||||
)
|
||||
|
||||
# Reconnect attempt interval when CAN bus is lost
|
||||
@ -64,29 +69,21 @@ _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) # 56
|
||||
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) # 68
|
||||
self.declare_parameter("command_timeout_s", 0.5)
|
||||
self.declare_parameter("pid/kp", 0.0)
|
||||
self.declare_parameter("pid/ki", 0.0)
|
||||
self.declare_parameter("pid/kd", 0.0)
|
||||
|
||||
self._iface: str = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
|
||||
self._mamba_id: int = self.get_parameter("mamba_can_id").value
|
||||
self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
|
||||
self._pid_kp: float = self.get_parameter("pid/kp").value
|
||||
self._pid_ki: float = self.get_parameter("pid/ki").value
|
||||
self._pid_kd: float = self.get_parameter("pid/kd").value
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────
|
||||
self._bus: Optional[can.BusABC] = None
|
||||
@ -110,7 +107,6 @@ class CanBridgeNode(Node):
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
||||
self.add_on_set_parameters_callback(self._on_set_parameters)
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||
@ -128,33 +124,8 @@ 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}"
|
||||
)
|
||||
|
||||
# -- 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:
|
||||
@ -212,29 +183,21 @@ class CanBridgeNode(Node):
|
||||
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.
|
||||
# Differential drive decomposition (positive angular = CCW = left turn).
|
||||
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")
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(left_mps, right_mps), "cmd_vel")
|
||||
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
|
||||
|
||||
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")
|
||||
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "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_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode")
|
||||
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
|
||||
|
||||
# ── Watchdog ──────────────────────────────────────────────────────────
|
||||
|
||||
@ -244,14 +207,8 @@ class CanBridgeNode(Node):
|
||||
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"
|
||||
)
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "watchdog zero-vel")
|
||||
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle")
|
||||
|
||||
# ── CAN send helper ───────────────────────────────────────────────────
|
||||
|
||||
@ -305,24 +262,28 @@ class CanBridgeNode(Node):
|
||||
arb_id = frame.arbitration_id
|
||||
data = bytes(frame.data)
|
||||
|
||||
# VESC STATUS_1 CAN IDs: (VESC_STATUS_1 << 8) | node_id
|
||||
_vesc_left_status1 = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||
_vesc_right_status1 = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||
|
||||
try:
|
||||
if arb_id == MAMBA_TELEM_IMU:
|
||||
self._handle_imu(data, frame.timestamp)
|
||||
if arb_id == FC_STATUS:
|
||||
self._handle_fc_status(data)
|
||||
|
||||
elif arb_id == MAMBA_TELEM_BATTERY:
|
||||
self._handle_battery(data, frame.timestamp)
|
||||
elif arb_id == FC_VESC:
|
||||
self._handle_fc_vesc(data)
|
||||
|
||||
elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
|
||||
self._handle_vesc_state(data, frame.timestamp, side="left")
|
||||
elif arb_id == _vesc_left_status1:
|
||||
telem = decode_vesc_status1(self._left_vesc_id, data)
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
||||
self._pub_vesc_left.publish(msg)
|
||||
|
||||
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}"
|
||||
)
|
||||
elif arb_id == _vesc_right_status1:
|
||||
telem = decode_vesc_status1(self._right_vesc_id, data)
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
||||
self._pub_vesc_right.publish(msg)
|
||||
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
@ -331,51 +292,38 @@ class CanBridgeNode(Node):
|
||||
|
||||
# ── Frame handlers ────────────────────────────────────────────────────
|
||||
|
||||
def _handle_imu(self, data: bytes, timestamp: float) -> None:
|
||||
telem = decode_imu_telem(data)
|
||||
def _handle_fc_status(self, data: bytes) -> None:
|
||||
"""FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags."""
|
||||
telem = decode_fc_status(data)
|
||||
|
||||
msg = Imu()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = "imu_link"
|
||||
# Publish pitch as IMU (orientation only — yaw/roll unknown from FC_STATUS)
|
||||
imu_msg = Imu()
|
||||
imu_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
imu_msg.header.frame_id = "imu_link"
|
||||
# Only pitch is available; publish as angular velocity placeholder
|
||||
imu_msg.angular_velocity.y = telem.pitch_deg # degrees, not rad/s
|
||||
imu_msg.orientation_covariance[0] = -1.0 # covariance unknown
|
||||
self._pub_imu.publish(imu_msg)
|
||||
|
||||
msg.linear_acceleration.x = telem.accel_x
|
||||
msg.linear_acceleration.y = telem.accel_y
|
||||
msg.linear_acceleration.z = telem.accel_z
|
||||
# Publish battery (vbat_mv → volts)
|
||||
bat_msg = BatteryState()
|
||||
bat_msg.header.stamp = imu_msg.header.stamp
|
||||
bat_msg.voltage = telem.vbat_mv / 1000.0
|
||||
bat_msg.present = True
|
||||
bat_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
self._pub_battery.publish(bat_msg)
|
||||
|
||||
msg.angular_velocity.x = telem.gyro_x
|
||||
msg.angular_velocity.y = telem.gyro_y
|
||||
msg.angular_velocity.z = telem.gyro_z
|
||||
def _handle_fc_vesc(self, data: bytes) -> None:
|
||||
"""FC_VESC (0x401): left/right RPM and current aggregated by BALANCE."""
|
||||
telem = decode_fc_vesc(data)
|
||||
|
||||
# Covariance unknown; mark as -1 per REP-145
|
||||
msg.orientation_covariance[0] = -1.0
|
||||
left_msg = Float32MultiArray()
|
||||
left_msg.data = [telem.left_rpm, 0.0, 0.0, telem.left_cur]
|
||||
self._pub_vesc_left.publish(left_msg)
|
||||
|
||||
self._pub_imu.publish(msg)
|
||||
|
||||
def _handle_battery(self, data: bytes, timestamp: float) -> None:
|
||||
telem = decode_battery_telem(data)
|
||||
|
||||
msg = BatteryState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.voltage = telem.voltage
|
||||
msg.current = telem.current
|
||||
msg.present = True
|
||||
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
|
||||
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)
|
||||
right_msg = Float32MultiArray()
|
||||
right_msg.data = [telem.right_rpm, 0.0, 0.0, telem.right_cur]
|
||||
self._pub_vesc_right.publish(right_msg)
|
||||
|
||||
# ── Status helper ─────────────────────────────────────────────────────
|
||||
|
||||
@ -390,14 +338,8 @@ class CanBridgeNode(Node):
|
||||
"""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, 0.0), "shutdown")
|
||||
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown")
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
|
||||
@ -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