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.
|
## System Overview
|
||||||
> 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)
|
|
||||||
|
|
||||||
```
|
```
|
||||||
┌─────────────────────────────────────────────────────────────────────┐
|
┌─────────────────────────────────────────────────────────────────────┐
|
||||||
│ ORIN NANO SUPER │
|
│ ORIN NANO SUPER │
|
||||||
│ (Top Plate — 25W) │
|
│ (Top Plate — 25W) │
|
||||||
│ │
|
│ │
|
||||||
│ USB-C ──── STM32 CDC (/dev/stm32-bridge, 921600 baud) │
|
│ USB-A ──── CANable2 USB-CAN adapter (slcan0, 500 kbps) │
|
||||||
|
│ USB-A ──── ESP32-S3 IO (/dev/esp32-io, 460800 baud) │
|
||||||
│ USB-A1 ─── RealSense D435i (USB 3.1) │
|
│ USB-A1 ─── RealSense D435i (USB 3.1) │
|
||||||
│ USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │
|
│ USB-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │
|
||||||
│ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │
|
│ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │
|
||||||
│ USB ─────── Leap Motion Controller (hand/gesture tracking) │
|
│ USB ─────── Leap Motion Controller (hand/gesture tracking) │
|
||||||
│ CSI-A ──── ArduCam adapter → 2× IMX219 (front + left) │
|
│ CSI-A ──── ArduCam adapter → 2x IMX219 (front + left) │
|
||||||
│ CSI-B ──── ArduCam adapter → 2× IMX219 (rear + right) │
|
│ CSI-B ──── ArduCam adapter → 2x IMX219 (rear + right) │
|
||||||
│ M.2 ───── 1TB NVMe SSD │
|
│ M.2 ───── 1TB NVMe SSD │
|
||||||
│ 40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec) │
|
│ 40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec) │
|
||||||
│ Pin 8 ──┐ │
|
│ Pin 8 ──┐ │
|
||||||
│ Pin 10 ─┤ UART fallback to FC (ttyTHS0, 921600) │
|
│ Pin 10 ─┤ UART fallback to ESP32-S3 BALANCE (ttyTHS0, 460800) │
|
||||||
│ Pin 6 ──┘ GND │
|
│ Pin 6 ──┘ GND │
|
||||||
│ │
|
│ │
|
||||||
└─────────────────────────────────────────────────────────────────────┘
|
└─────────────────────────────────────────────────────────────────────┘
|
||||||
│ USB-C (data only) │ UART fallback (3 wires)
|
│ USB-A (CANable2) │ UART fallback (3 wires)
|
||||||
│ 921600 baud │ 921600 baud, 3.3V
|
│ SocketCAN slcan0 │ 460800 baud, 3.3V
|
||||||
|
│ 500 kbps │
|
||||||
▼ ▼
|
▼ ▼
|
||||||
┌─────────────────────────────────────────────────────────────────────┐
|
┌─────────────────────────────────────────────────────────────────────┐
|
||||||
│ MAMBA F722S (FC) │
|
│ ESP32-S3 BALANCE │
|
||||||
│ (Middle Plate — foam mounted) │
|
│ (Waveshare Touch LCD 1.28, Middle Plate) │
|
||||||
│ │
|
│ │
|
||||||
│ USB-C ──── Orin (CDC serial, primary link) │
|
│ CAN bus ──── CANable2 → Orin (primary link, ISO 11898) │
|
||||||
│ │
|
│ UART0 ──── Orin UART fallback (460800 baud, 3.3V) │
|
||||||
│ USART2 (PA2=TX, PA3=RX) ──── Hoverboard ESC (26400 baud) │
|
│ UART1 ──── VESC Left (CAN ID 56) via UART/CAN bridge │
|
||||||
│ UART4 (PA0=TX, PA1=RX) ──── ELRS RX (CRSF, 420000 baud) │
|
│ UART2 ──── VESC Right (CAN ID 68) via UART/CAN bridge │
|
||||||
│ USART6 (PC6=TX, PC7=RX) ──── Orin UART fallback │
|
│ I2C ──── QMI8658 IMU (onboard, 6-DOF accel+gyro) │
|
||||||
│ UART5 (PC12=TX, PD2=RX) ─── Debug (optional) │
|
│ SPI ──── GC9A01 LCD (onboard, 240x240 round display) │
|
||||||
│ │
|
│ GPIO ──── WS2812B LED strip │
|
||||||
│ SPI1 ─── MPU6000 IMU (on-board, CW270) │
|
│ GPIO ──── Buzzer │
|
||||||
│ I2C1 ─── BMP280 baro (on-board, disabled) │
|
│ ADC ──── Battery voltage divider │
|
||||||
│ ADC ──── Battery voltage (PC1) + Current (PC3) │
|
|
||||||
│ PB3 ──── WS2812B LED strip │
|
|
||||||
│ PB2 ──── Buzzer │
|
|
||||||
│ │
|
│ │
|
||||||
└─────────────────────────────────────────────────────────────────────┘
|
└─────────────────────────────────────────────────────────────────────┘
|
||||||
│ USART2 │ UART4
|
│ CAN bus (ISO 11898) │ UART (460800 baud)
|
||||||
│ PA2=TX → ESC RX │ PA0=TX → ELRS TX
|
│ 500 kbps │
|
||||||
│ PA3=RX ← ESC TX │ PA1=RX ← ELRS RX
|
|
||||||
│ GND ─── GND │ GND ─── GND
|
|
||||||
▼ ▼
|
▼ ▼
|
||||||
┌────────────────────────┐ ┌──────────────────────────┐
|
┌────────────────────────┐ ┌──────────────────────────┐
|
||||||
│ HOVERBOARD ESC │ │ ELRS 2.4GHz RX │
|
│ VESC Left (ID 56) │ │ VESC Right (ID 68) │
|
||||||
│ (Bottom Plate) │ │ (beside FC) │
|
│ (Bottom Plate) │ │ (Bottom Plate) │
|
||||||
|
│ │ │ │
|
||||||
|
│ BLDC hub motor │ │ BLDC hub motor │
|
||||||
|
│ CAN 500 kbps │ │ CAN 500 kbps │
|
||||||
|
│ FOC current control │ │ FOC current control │
|
||||||
|
│ VESC Status 1 (0x900) │ │ VESC Status 1 (0x910) │
|
||||||
│ │ │ │
|
│ │ │ │
|
||||||
│ 2× BLDC hub motors │ │ CRSF protocol │
|
|
||||||
│ 26400 baud UART │ │ 420000 baud │
|
|
||||||
│ Frame: [0xABCD] │ │ BetaFPV 1W TX → RX │
|
|
||||||
│ [steer][speed][csum] │ │ CH3=speed CH4=steer │
|
|
||||||
│ │ │ CH5=arm CH6=mode │
|
|
||||||
└────────────────────────┘ └──────────────────────────┘
|
└────────────────────────┘ └──────────────────────────┘
|
||||||
│
|
│ │
|
||||||
┌────┴────┐
|
LEFT MOTOR RIGHT MOTOR
|
||||||
▼ ▼
|
```
|
||||||
🛞 LEFT RIGHT 🛞
|
|
||||||
MOTOR MOTOR
|
|
||||||
|
|
||||||
|
|
||||||
## Wire-by-Wire Connections
|
## Wire-by-Wire Connections
|
||||||
|
|
||||||
### 1. Orin ↔ FC (Primary: USB CDC)
|
### 1. Orin <-> ESP32-S3 BALANCE (Primary: CAN Bus via CANable2)
|
||||||
|
|
||||||
| From | To | Wire Color | Notes |
|
| From | To | Wire | Notes |
|
||||||
|------|----|-----------|-------|
|
|------|----|------|-------|
|
||||||
| Orin USB-C port | FC USB-C port | USB cable | Data only, FC powered from 5V bus |
|
| Orin USB-A | CANable2 USB | USB cable | SocketCAN slcan0 @ 500 kbps |
|
||||||
|
| CANable2 CAN-H | ESP32-S3 BALANCE CAN-H | twisted pair | ISO 11898 differential |
|
||||||
|
| CANable2 CAN-L | ESP32-S3 BALANCE CAN-L | twisted pair | ISO 11898 differential |
|
||||||
|
|
||||||
- Device: `/dev/ttyACM0` → symlink `/dev/stm32-bridge`
|
- Interface: SocketCAN `slcan0`, 500 kbps
|
||||||
- Baud: 921600, 8N1
|
- Device node: `/dev/canable2` (via udev, symlink to ttyUSBx)
|
||||||
- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
|
- Protocol: CAN frames --- ORIN_CMD_DRIVE (0x300), ORIN_CMD_MODE (0x301), ORIN_CMD_ESTOP (0x302)
|
||||||
|
- Telemetry: BALANCE_STATUS (0x400), BALANCE_VESC (0x401), BALANCE_IMU (0x402), BALANCE_BATTERY (0x403)
|
||||||
|
|
||||||
### 2. Orin ↔ FC (Fallback: Hardware UART)
|
### 2. Orin <-> ESP32-S3 BALANCE (Fallback: Hardware UART)
|
||||||
|
|
||||||
| Orin Pin | Signal | FC Pin | FC Signal |
|
| Orin Pin | Signal | ESP32-S3 Pin | Notes |
|
||||||
|----------|--------|--------|-----------|
|
|----------|--------|--------------|-------|
|
||||||
| Pin 8 | TXD0 | PC7 | USART6 RX |
|
| Pin 8 | TXD0 | GPIO17 (UART0 RX) | Orin TX -> BALANCE RX |
|
||||||
| Pin 10 | RXD0 | PC6 | USART6 TX |
|
| Pin 10 | RXD0 | GPIO18 (UART0 TX) | Orin RX <- BALANCE TX |
|
||||||
| Pin 6 | GND | GND | GND |
|
| Pin 6 | GND | GND | Common ground |
|
||||||
|
|
||||||
- Jetson device: `/dev/ttyTHS0`
|
- Jetson device: `/dev/ttyTHS0`
|
||||||
- Baud: 921600, 8N1
|
- Baud: 460800, 8N1
|
||||||
- Voltage: 3.3V both sides (no level shifter needed)
|
- Voltage: 3.3V both sides (no level shifter needed)
|
||||||
- **Cross-connect:** Orin TX → FC RX, Orin RX ← FC TX
|
- Cross-connect: Orin TX -> BALANCE RX, Orin RX <- BALANCE TX
|
||||||
|
|
||||||
### 3. FC ↔ Hoverboard ESC
|
### 3. Orin <-> ESP32-S3 IO (USB Serial)
|
||||||
|
|
||||||
| FC Pin | Signal | ESC Pin | Notes |
|
| From | To | Notes |
|
||||||
|--------|--------|---------|-------|
|
|------|----|-------|
|
||||||
| PA2 | USART2 TX | RX | FC sends speed/steer commands |
|
| Orin USB-A | ESP32-S3 IO USB-C | USB cable, /dev/esp32-io |
|
||||||
| PA3 | USART2 RX | TX | ESC sends feedback (optional) |
|
|
||||||
|
- Device node: `/dev/esp32-io` (udev symlink)
|
||||||
|
- Baud: 460800, 8N1
|
||||||
|
- Protocol: Binary frames `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
|
||||||
|
- Use: IO expansion, GPIO control, sensor polling
|
||||||
|
|
||||||
|
### 4. ESP32-S3 BALANCE <-> VESC Motors (CAN Bus)
|
||||||
|
|
||||||
|
| BALANCE Pin | Signal | VESC Pin | Notes |
|
||||||
|
|-------------|--------|----------|-------|
|
||||||
|
| GPIO21 | CAN-H | CAN-H | ISO 11898 differential pair |
|
||||||
|
| GPIO22 | CAN-L | CAN-L | ISO 11898 differential pair |
|
||||||
| GND | GND | GND | Common ground |
|
| GND | GND | GND | Common ground |
|
||||||
|
|
||||||
- Baud: 26400, 8N1
|
- Baud: 500 kbps CAN
|
||||||
- Protocol: Binary frame — `[0xABCD][steer:int16][speed:int16][checksum:uint16]`
|
- VESC Left: CAN ID 56, VESC Right: CAN ID 68
|
||||||
- Speed range: -1000 to +1000
|
- Commands: COMM_SET_RPM, COMM_SET_CURRENT, COMM_SET_DUTY
|
||||||
- **Keep wires short and twisted** (EMI from ESC)
|
- Telemetry: VESC Status 1 at 50 Hz (RPM, current, duty)
|
||||||
|
|
||||||
### 4. FC ↔ ELRS Receiver
|
|
||||||
|
|
||||||
| FC Pin | Signal | ELRS Pin | Notes |
|
|
||||||
|--------|--------|----------|-------|
|
|
||||||
| PA0 | UART4 TX | RX | Telemetry to TX (optional) |
|
|
||||||
| PA1 | UART4 RX | TX | CRSF frames from RX |
|
|
||||||
| GND | GND | GND | Common ground |
|
|
||||||
| 5V | — | VCC | Power ELRS from 5V bus |
|
|
||||||
|
|
||||||
- Baud: 420000 (CRSF protocol)
|
|
||||||
- Failsafe: disarm after 300ms without frame
|
|
||||||
|
|
||||||
### 5. Power Distribution
|
### 5. Power Distribution
|
||||||
|
|
||||||
```
|
```
|
||||||
BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
|
||||||
|
├── VESC Right (36V direct -> BLDC right motor)
|
||||||
│
|
│
|
||||||
├── 5V BEC/regulator ──┬── Orin (USB-C PD or barrel jack)
|
├── 5V BEC/regulator ──┬── Orin (USB-C PD or barrel jack)
|
||||||
│ ├── FC (via USB or 5V pad)
|
│ ├── ESP32-S3 BALANCE (5V via USB-C)
|
||||||
│ ├── ELRS RX (5V)
|
│ ├── ESP32-S3 IO (5V via USB-C)
|
||||||
│ ├── WS2812B LEDs (5V)
|
│ ├── WS2812B LEDs (5V)
|
||||||
│ └── RPLIDAR (5V via USB)
|
│ └── RPLIDAR (5V via USB)
|
||||||
│
|
│
|
||||||
└── Battery monitor ──── FC ADC (PC1=voltage, PC3=current)
|
└── Battery monitor ──── ESP32-S3 BALANCE ADC (voltage divider)
|
||||||
```
|
```
|
||||||
|
|
||||||
### 6. Sensors on Orin (USB/CSI)
|
### 6. Sensors on Orin (USB/CSI)
|
||||||
@ -143,20 +134,36 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
|||||||
| RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` |
|
| RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` |
|
||||||
| IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` |
|
| IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` |
|
||||||
| IMX219 rear+right | MIPI CSI-2 | CSI-B (J8) | `/dev/video4,6` |
|
| IMX219 rear+right | MIPI CSI-2 | CSI-B (J8) | `/dev/video4,6` |
|
||||||
| 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
|
| 1TB NVMe | PCIe Gen3 x4 | M.2 Key M | `/dev/nvme0n1` |
|
||||||
|
| CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
|
||||||
|
|
||||||
|
|
||||||
## FC UART Summary (MAMBA F722S — OBSOLETE)
|
## ESP32-S3 BALANCE UART/CAN Summary
|
||||||
|
|
||||||
| UART | Pins | Baud | Assignment | Notes |
|
| Interface | Pins | Baud/Rate | Assignment | Notes |
|
||||||
|------|------|------|------------|-------|
|
|-----------|------|-----------|------------|-------|
|
||||||
| USART1 | PB6=TX, PB7=RX | — | SmartAudio/VTX | Unused in SaltyLab |
|
| UART0 | GPIO17=RX, GPIO18=TX | 460800 | Orin UART fallback | 3.3V, cross-connect |
|
||||||
| USART2 | PA2=TX, PA3=RX | 26400 | Hoverboard ESC | Binary motor commands |
|
| UART1 | GPIO19=RX, GPIO20=TX | 115200 | Debug serial | Optional |
|
||||||
| USART3 | PB10=TX, PB11=RX | — | Available | Was SBUS default |
|
| CAN (TWAI) | GPIO21=H, GPIO22=L | 500 kbps | CAN bus (VESCs + Orin) | SN65HVD230 transceiver |
|
||||||
| UART4 | PA0=TX, PA1=RX | 420000 | ELRS RX (CRSF) | RC control |
|
| I2C | GPIO4=SDA, GPIO5=SCL | 400 kHz | QMI8658 IMU (addr 0x6B) | Onboard |
|
||||||
| UART5 | PC12=TX, PD2=RX | 115200 | Debug serial | Optional |
|
| SPI | GPIO36=MOSI, GPIO37=SCLK, GPIO35=CS | 40 MHz | GC9A01 LCD (onboard) | 240x240 round |
|
||||||
| USART6 | PC6=TX, PC7=RX | 921600 | Jetson UART | Fallback link |
|
| USB CDC | USB-C | 460800 | Orin USB fallback | /dev/esp32-balance |
|
||||||
| USB CDC | USB-C | 921600 | Jetson primary | `/dev/stm32-bridge` |
|
|
||||||
|
## CAN Frame ID Map
|
||||||
|
|
||||||
|
| CAN ID | Direction | Name | Contents |
|
||||||
|
|--------|-----------|------|----------|
|
||||||
|
| 0x300 | Orin -> BALANCE | ORIN_CMD_DRIVE | left_rpm_f32, right_rpm_f32 (8 bytes LE) |
|
||||||
|
| 0x301 | Orin -> BALANCE | ORIN_CMD_MODE | mode byte (0=IDLE, 1=DRIVE, 2=ESTOP) |
|
||||||
|
| 0x302 | Orin -> BALANCE | ORIN_CMD_ESTOP | flags byte (bit0=stop, bit1=clear) |
|
||||||
|
| 0x400 | BALANCE -> Orin | BALANCE_STATUS | pitch x10:i16, motor_cmd:u16, vbat_mv:u16, state:u8, flags:u8 |
|
||||||
|
| 0x401 | BALANCE -> Orin | BALANCE_VESC | l_rpm x10:i16, r_rpm x10:i16, l_cur x10:i16, r_cur x10:i16 |
|
||||||
|
| 0x402 | BALANCE -> Orin | BALANCE_IMU | pitch x100:i16, roll x100:i16, yaw x100:i16, ax x100:i16, ay x100:i16, az x100:i16 |
|
||||||
|
| 0x403 | BALANCE -> Orin | BALANCE_BATTERY | vbat_mv:u16, current_ma:i16, soc_pct:u8 |
|
||||||
|
| 0x900+ID | VESC Left -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 |
|
||||||
|
| 0x910+ID | VESC Right -> | VESC_STATUS_1 | erpm:i32, current x10:i16, duty x1000:i16 |
|
||||||
|
|
||||||
|
VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44).
|
||||||
|
|
||||||
|
|
||||||
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
|
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
|
||||||
@ -174,57 +181,59 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
|
|||||||
| Pin 2, 4 | 5V | Power |
|
| Pin 2, 4 | 5V | Power |
|
||||||
| Pin 6, 9 | GND | Ground |
|
| Pin 6, 9 | GND | Ground |
|
||||||
|
|
||||||
- **Codec:** Wolfson WM8960 (I2C addr 0x1A)
|
- Codec: Wolfson WM8960 (I2C addr 0x1A)
|
||||||
- **Mics:** 2× MEMS (left + right) — basic stereo / sound localization
|
- Mics: 2x MEMS (left + right) --- basic stereo / sound localization
|
||||||
- **Speaker:** 3W class-D amp output (JST connector)
|
- Speaker: 3W class-D amp output (JST connector)
|
||||||
- **Headset:** 3.5mm TRRS jack
|
- Headset: 3.5mm TRRS jack
|
||||||
- **Requires:** WM8960 device tree overlay for Jetson (community port)
|
- Requires: WM8960 device tree overlay for Jetson (community port)
|
||||||
- **Use:** Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
|
- Use: Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
|
||||||
|
|
||||||
### 8. SIM7600A 4G/LTE HAT (via USB)
|
### 8. SIM7600A 4G/LTE HAT (via USB)
|
||||||
|
|
||||||
| Connection | Detail |
|
| Connection | Detail |
|
||||||
|-----------|--------|
|
|-----------|--------|
|
||||||
| Interface | USB (micro-B on HAT → USB-A/C on Orin) |
|
| Interface | USB (micro-B on HAT -> USB-A/C on Orin) |
|
||||||
| Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) |
|
| Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) |
|
||||||
| Power | 5V from USB or separate 5V supply (peak 2A during TX) |
|
| Power | 5V from USB or separate 5V supply (peak 2A during TX) |
|
||||||
| SIM | Nano-SIM slot on HAT |
|
| SIM | Nano-SIM slot on HAT |
|
||||||
| Antenna | 4G LTE + GPS/GNSS (external SMA antennas — mount high on chassis) |
|
| Antenna | 4G LTE + GPS/GNSS (external SMA antennas --- mount high on chassis) |
|
||||||
|
|
||||||
- **Data:** PPP or QMI for internet connectivity
|
- Data: PPP or QMI for internet connectivity
|
||||||
- **GPS/GNSS:** Built-in receiver, NMEA sentences on ttyUSB2 — outdoor positioning
|
- GPS/GNSS: Built-in receiver, NMEA sentences on ttyUSB2 --- outdoor positioning
|
||||||
- **AT commands:** `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
|
- AT commands: `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
|
||||||
- **Connected via USB** (not 40-pin) — avoids UART conflict with FC fallback, flexible antenna placement
|
- Connected via USB (not 40-pin) --- avoids UART conflict with BALANCE fallback, flexible antenna placement
|
||||||
- **Use:** Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
|
- Use: Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
|
||||||
|
|
||||||
### 10. Leap Motion Controller (USB)
|
### 9. Leap Motion Controller (USB)
|
||||||
|
|
||||||
| Connection | Detail |
|
| Connection | Detail |
|
||||||
|-----------|--------|
|
|-----------|--------|
|
||||||
| Interface | USB 3.0 (micro-B on controller → USB-A on Orin) |
|
| Interface | USB 3.0 (micro-B on controller -> USB-A on Orin) |
|
||||||
| Power | ~0.5W |
|
| Power | ~0.5W |
|
||||||
| Range | ~80cm, 150° FOV |
|
| Range | ~80cm, 150 deg FOV |
|
||||||
| SDK | Ultraleap Gemini V5+ (Linux ARM64 support) |
|
| SDK | Ultraleap Gemini V5+ (Linux ARM64 support) |
|
||||||
| ROS2 | `leap_motion_ros2` wrapper available |
|
| ROS2 | `leap_motion_ros2` wrapper available |
|
||||||
|
|
||||||
- **2× IR cameras + 3× IR LEDs** — tracks all 10 fingers in 3D, sub-mm precision
|
- 2x IR cameras + 3x IR LEDs --- tracks all 10 fingers in 3D, sub-mm precision
|
||||||
- **Mount:** Forward-facing on sensor tower or upward on Orin plate
|
- Mount: Forward-facing on sensor tower or upward on Orin plate
|
||||||
- **Use:** Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
|
- Use: Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
|
||||||
- **Combined with ReSpeaker:** Voice + gesture control with zero hardware in hand
|
- Combined with ReSpeaker: Voice + gesture control with zero hardware in hand
|
||||||
|
|
||||||
### 11. Power Budget (USB)
|
### 10. Power Budget (USB)
|
||||||
|
|
||||||
| Device | Interface | Power Draw |
|
| Device | Interface | Power Draw |
|
||||||
|--------|-----------|------------|
|
|--------|-----------|------------|
|
||||||
| STM32 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
|
| CANable2 USB-CAN | USB-A | ~0.5W |
|
||||||
|
| ESP32-S3 BALANCE | USB-C | ~0.8W (WiFi off) |
|
||||||
|
| ESP32-S3 IO | USB-C | ~0.5W |
|
||||||
| RealSense D435i | USB-A | ~1.5W (3.5W peak) |
|
| RealSense D435i | USB-A | ~1.5W (3.5W peak) |
|
||||||
| RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
|
| RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
|
||||||
| SIM7600A | USB | ~1W idle, 3W TX peak |
|
| SIM7600A | USB | ~1W idle, 3W TX peak |
|
||||||
| Leap Motion | USB | ~0.5W |
|
| Leap Motion | USB-A | ~0.5W |
|
||||||
| ReSpeaker HAT | 40-pin | ~0.5W |
|
| ReSpeaker HAT | 40-pin | ~0.5W |
|
||||||
| **Total USB** | | **~6.5W typical, ~10.5W peak** |
|
| **Total USB** | | **~7.9W typical, ~11W peak** |
|
||||||
|
|
||||||
Orin Nano Super delivers up to 25W — USB peripherals are well within budget.
|
Orin Nano Super delivers up to 25W --- USB peripherals are well within budget.
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@ -232,38 +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) │
|
│ (2.4GHz) │
|
||||||
└──────┬───────┘
|
└──────┬───────┘
|
||||||
│ radio
|
│ radio
|
||||||
┌──────▼───────┐
|
┌──────▼───────┐
|
||||||
│ ELRS RX │ CRSF 420kbaud
|
│ RC RX │ CRSF 420kbaud (future)
|
||||||
└──────┬───────┘
|
└──────┬───────┘
|
||||||
│ UART4
|
│ UART
|
||||||
┌────────────▼────────────┐
|
┌────────────▼────────────┐
|
||||||
│ MAMBA F722S │
|
│ ESP32-S3 BALANCE │
|
||||||
|
│ (Waveshare LCD 1.28) │
|
||||||
│ │
|
│ │
|
||||||
│ MPU6000 → Balance PID │
|
│ QMI8658 -> Balance PID │
|
||||||
│ CRSF → Mode Manager │
|
│ RC -> Mode Manager │
|
||||||
│ Safety Monitor │
|
│ Safety Monitor │
|
||||||
│ │
|
│ │
|
||||||
└──┬──────────┬───────────┘
|
└──┬──────────┬───────────┘
|
||||||
USART2 ─────┘ └───── USB CDC / USART6
|
CAN 500kbps─┘ └───── CAN bus / UART fallback
|
||||||
26400 baud 921600 baud
|
|
||||||
│ │
|
│ │
|
||||||
▼ ▼
|
┌────┴────────────┐ ▼
|
||||||
┌────────────────┐ ┌───────────────────┐
|
│ CAN bus (500k) │ ┌───────────────────┐
|
||||||
│ Hoverboard ESC │ │ Orin Nano Super │
|
├─ VESC Left 56 │ │ Orin Nano Super │
|
||||||
│ │ │ │
|
└─ VESC Right 68 │ │ │
|
||||||
│ L motor R motor│ │ SLAM / Nav2 / AI │
|
│ │ │ SLAM / Nav2 / AI │
|
||||||
│ 🛞 🛞 │ │ Person following │
|
▼ ▼ │ Person following │
|
||||||
└────────────────┘ │ Voice commands │
|
LEFT RIGHT │ Voice commands │
|
||||||
│ 4G telemetry │
|
MOTOR MOTOR │ 4G telemetry │
|
||||||
└──┬──────────┬───────┘
|
└──┬──────────┬───────┘
|
||||||
│ │
|
│ │
|
||||||
┌──────────▼─┐ ┌────▼──────────┐
|
┌──────────▼─┐ ┌────▼──────────┐
|
||||||
│ ReSpeaker │ │ SIM7600A │
|
│ ReSpeaker │ │ SIM7600A │
|
||||||
│ 2-Mic HAT │ │ 4G/LTE + GPS │
|
│ 2-Mic HAT │ │ 4G/LTE + GPS │
|
||||||
│ 🎤 🔊 │ │ 📡 🛰️ │
|
|
||||||
└────────────┘ └───────────────┘
|
└────────────┘ └───────────────┘
|
||||||
```
|
```
|
||||||
|
|||||||
@ -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
|
#!/usr/bin/env python3
|
||||||
"""
|
"""
|
||||||
can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32 IO motor
|
can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
|
||||||
controller and VESC motor controllers over CAN bus.
|
board and VESC motor controllers over CAN bus (CANable2 / slcan0, 500 kbps).
|
||||||
|
|
||||||
The node opens the SocketCAN interface (slcan0 by default), spawns a background
|
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §4 & §6 (2026-04-04)
|
||||||
reader thread to process incoming telemetry, and exposes the following interface:
|
|
||||||
|
|
||||||
Subscriptions
|
Subscriptions
|
||||||
-------------
|
-------------
|
||||||
/cmd_vel geometry_msgs/Twist → VESC speed commands (CAN)
|
/cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300)
|
||||||
/estop std_msgs/Bool → ESP32 IO e-stop (CAN)
|
/estop std_msgs/Bool → ORIN_CMD_ESTOP (0x302)
|
||||||
|
|
||||||
Publications
|
Publications
|
||||||
------------
|
------------
|
||||||
/can/imu sensor_msgs/Imu ESP32 IO IMU telemetry
|
/can/imu sensor_msgs/Imu from FC_STATUS (0x400) pitch
|
||||||
/can/battery sensor_msgs/BatteryState ESP32 IO battery telemetry
|
/can/battery sensor_msgs/BatteryState from FC_STATUS (0x400) vbat_mv
|
||||||
/can/vesc/left/state std_msgs/Float32MultiArray Left VESC state
|
/can/vesc/left/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
||||||
/can/vesc/right/state std_msgs/Float32MultiArray Right VESC state
|
/can/vesc/right/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
||||||
/can/connection_status std_msgs/String "connected" | "disconnected"
|
/can/connection_status std_msgs/String "connected" | "disconnected"
|
||||||
|
|
||||||
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
|
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
|
||||||
@ -30,30 +29,36 @@ import can
|
|||||||
import rclpy
|
import rclpy
|
||||||
from geometry_msgs.msg import Twist
|
from geometry_msgs.msg import Twist
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from rcl_interfaces.msg import SetParametersResult
|
|
||||||
from sensor_msgs.msg import BatteryState, Imu
|
from sensor_msgs.msg import BatteryState, Imu
|
||||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||||
|
|
||||||
from saltybot_can_bridge.mamba_protocol import (
|
from saltybot_can_bridge.balance_protocol import (
|
||||||
MAMBA_CMD_ESTOP,
|
# Orin → BALANCE command IDs
|
||||||
MAMBA_CMD_MODE,
|
ORIN_CMD_DRIVE,
|
||||||
MAMBA_CMD_VELOCITY,
|
ORIN_CMD_MODE,
|
||||||
MAMBA_TELEM_BATTERY,
|
ORIN_CMD_ESTOP,
|
||||||
MAMBA_TELEM_IMU,
|
# BALANCE → Orin telemetry IDs
|
||||||
VESC_TELEM_STATE,
|
FC_STATUS,
|
||||||
ORIN_CAN_ID_FC_PID_ACK,
|
FC_VESC,
|
||||||
ORIN_CAN_ID_PID_SET,
|
# VESC node IDs
|
||||||
|
VESC_LEFT_ID,
|
||||||
|
VESC_RIGHT_ID,
|
||||||
|
VESC_STATUS_1,
|
||||||
|
# Mode constants
|
||||||
MODE_DRIVE,
|
MODE_DRIVE,
|
||||||
MODE_ESTOP,
|
MODE_ESTOP,
|
||||||
MODE_IDLE,
|
MODE_IDLE,
|
||||||
encode_estop_cmd,
|
# Encoders
|
||||||
|
encode_drive_cmd,
|
||||||
encode_mode_cmd,
|
encode_mode_cmd,
|
||||||
encode_velocity_cmd,
|
encode_estop_cmd,
|
||||||
encode_pid_set_cmd,
|
encode_led_cmd,
|
||||||
decode_battery_telem,
|
# Decoders
|
||||||
decode_imu_telem,
|
decode_fc_status,
|
||||||
decode_pid_ack,
|
decode_fc_vesc,
|
||||||
decode_vesc_state,
|
decode_vesc_status1,
|
||||||
|
decode_vesc_can_id,
|
||||||
)
|
)
|
||||||
|
|
||||||
# Reconnect attempt interval when CAN bus is lost
|
# Reconnect attempt interval when CAN bus is lost
|
||||||
@ -64,29 +69,21 @@ _WATCHDOG_HZ: float = 10.0
|
|||||||
|
|
||||||
|
|
||||||
class CanBridgeNode(Node):
|
class CanBridgeNode(Node):
|
||||||
"""CAN bus bridge between Orin ROS2 and ESP32 IO / VESC controllers."""
|
"""CAN bus bridge between Orin ROS2 and ESP32 BALANCE / VESC controllers."""
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__("can_bridge_node")
|
super().__init__("can_bridge_node")
|
||||||
|
|
||||||
# ── Parameters ────────────────────────────────────────────────────
|
# ── Parameters ────────────────────────────────────────────────────
|
||||||
self.declare_parameter("can_interface", "slcan0")
|
self.declare_parameter("can_interface", "slcan0")
|
||||||
self.declare_parameter("left_vesc_can_id", 56)
|
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) # 56
|
||||||
self.declare_parameter("right_vesc_can_id", 68)
|
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) # 68
|
||||||
self.declare_parameter("mamba_can_id", 1)
|
|
||||||
self.declare_parameter("command_timeout_s", 0.5)
|
self.declare_parameter("command_timeout_s", 0.5)
|
||||||
self.declare_parameter("pid/kp", 0.0)
|
|
||||||
self.declare_parameter("pid/ki", 0.0)
|
|
||||||
self.declare_parameter("pid/kd", 0.0)
|
|
||||||
|
|
||||||
self._iface: str = self.get_parameter("can_interface").value
|
self._iface: str = self.get_parameter("can_interface").value
|
||||||
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").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._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._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 ─────────────────────────────────────────────────────────
|
# ── State ─────────────────────────────────────────────────────────
|
||||||
self._bus: Optional[can.BusABC] = None
|
self._bus: Optional[can.BusABC] = None
|
||||||
@ -110,7 +107,6 @@ class CanBridgeNode(Node):
|
|||||||
# ── Subscriptions ─────────────────────────────────────────────────
|
# ── Subscriptions ─────────────────────────────────────────────────
|
||||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||||
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
||||||
self.add_on_set_parameters_callback(self._on_set_parameters)
|
|
||||||
|
|
||||||
# ── Timers ────────────────────────────────────────────────────────
|
# ── Timers ────────────────────────────────────────────────────────
|
||||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||||
@ -128,33 +124,8 @@ class CanBridgeNode(Node):
|
|||||||
self.get_logger().info(
|
self.get_logger().info(
|
||||||
f"can_bridge_node ready — iface={self._iface} "
|
f"can_bridge_node ready — iface={self._iface} "
|
||||||
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id}"
|
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id}"
|
||||||
f"mamba={self._mamba_id}"
|
|
||||||
)
|
)
|
||||||
|
|
||||||
# -- PID parameter callback (Issue #693) --
|
|
||||||
|
|
||||||
def _on_set_parameters(self, params) -> SetParametersResult:
|
|
||||||
"""Send new PID gains over CAN when pid/* params change."""
|
|
||||||
for p in params:
|
|
||||||
if p.name == "pid/kp":
|
|
||||||
self._pid_kp = float(p.value)
|
|
||||||
elif p.name == "pid/ki":
|
|
||||||
self._pid_ki = float(p.value)
|
|
||||||
elif p.name == "pid/kd":
|
|
||||||
self._pid_kd = float(p.value)
|
|
||||||
else:
|
|
||||||
continue
|
|
||||||
try:
|
|
||||||
payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
|
|
||||||
self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
|
|
||||||
self.get_logger().info(
|
|
||||||
f"PID gains sent: Kp={self._pid_kp:.2f} "
|
|
||||||
f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
|
|
||||||
)
|
|
||||||
except ValueError as exc:
|
|
||||||
return SetParametersResult(successful=False, reason=str(exc))
|
|
||||||
return SetParametersResult(successful=True)
|
|
||||||
|
|
||||||
# ── Connection management ──────────────────────────────────────────────
|
# ── Connection management ──────────────────────────────────────────────
|
||||||
|
|
||||||
def _try_connect(self) -> None:
|
def _try_connect(self) -> None:
|
||||||
@ -212,29 +183,21 @@ class CanBridgeNode(Node):
|
|||||||
linear = msg.linear.x
|
linear = msg.linear.x
|
||||||
angular = msg.angular.z
|
angular = msg.angular.z
|
||||||
|
|
||||||
# Forward left = forward right for pure translation; for rotation
|
# Differential drive decomposition (positive angular = CCW = left turn).
|
||||||
# left slows and right speeds up (positive angular = CCW = left turn).
|
|
||||||
# The ESP32 IO velocity command carries both wheels independently.
|
|
||||||
left_mps = linear - angular
|
left_mps = linear - angular
|
||||||
right_mps = linear + angular
|
right_mps = linear + angular
|
||||||
|
|
||||||
payload = encode_velocity_cmd(left_mps, right_mps)
|
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(left_mps, right_mps), "cmd_vel")
|
||||||
self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
|
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
|
||||||
|
|
||||||
# Keep ESP32 IO in DRIVE mode while receiving commands
|
|
||||||
self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
|
|
||||||
|
|
||||||
def _estop_cb(self, msg: Bool) -> None:
|
def _estop_cb(self, msg: Bool) -> None:
|
||||||
"""Forward /estop to ESP32 IO over CAN."""
|
"""Forward /estop to ESP32 BALANCE over CAN."""
|
||||||
if not self._connected:
|
if not self._connected:
|
||||||
return
|
return
|
||||||
payload = encode_estop_cmd(msg.data)
|
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "estop")
|
||||||
self._send_can(MAMBA_CMD_ESTOP, payload, "estop")
|
|
||||||
if msg.data:
|
if msg.data:
|
||||||
self._send_can(
|
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode")
|
||||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
|
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
|
||||||
)
|
|
||||||
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 IO")
|
|
||||||
|
|
||||||
# ── Watchdog ──────────────────────────────────────────────────────────
|
# ── Watchdog ──────────────────────────────────────────────────────────
|
||||||
|
|
||||||
@ -244,14 +207,8 @@ class CanBridgeNode(Node):
|
|||||||
return
|
return
|
||||||
elapsed = time.monotonic() - self._last_cmd_time
|
elapsed = time.monotonic() - self._last_cmd_time
|
||||||
if elapsed > self._cmd_timeout:
|
if elapsed > self._cmd_timeout:
|
||||||
self._send_can(
|
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "watchdog zero-vel")
|
||||||
MAMBA_CMD_VELOCITY,
|
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle")
|
||||||
encode_velocity_cmd(0.0, 0.0),
|
|
||||||
"watchdog zero-vel",
|
|
||||||
)
|
|
||||||
self._send_can(
|
|
||||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── CAN send helper ───────────────────────────────────────────────────
|
# ── CAN send helper ───────────────────────────────────────────────────
|
||||||
|
|
||||||
@ -305,24 +262,28 @@ class CanBridgeNode(Node):
|
|||||||
arb_id = frame.arbitration_id
|
arb_id = frame.arbitration_id
|
||||||
data = bytes(frame.data)
|
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:
|
try:
|
||||||
if arb_id == MAMBA_TELEM_IMU:
|
if arb_id == FC_STATUS:
|
||||||
self._handle_imu(data, frame.timestamp)
|
self._handle_fc_status(data)
|
||||||
|
|
||||||
elif arb_id == MAMBA_TELEM_BATTERY:
|
elif arb_id == FC_VESC:
|
||||||
self._handle_battery(data, frame.timestamp)
|
self._handle_fc_vesc(data)
|
||||||
|
|
||||||
elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
|
elif arb_id == _vesc_left_status1:
|
||||||
self._handle_vesc_state(data, frame.timestamp, side="left")
|
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:
|
elif arb_id == _vesc_right_status1:
|
||||||
self._handle_vesc_state(data, frame.timestamp, side="right")
|
telem = decode_vesc_status1(self._right_vesc_id, data)
|
||||||
|
msg = Float32MultiArray()
|
||||||
elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
|
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
||||||
gains = decode_pid_ack(data)
|
self._pub_vesc_right.publish(msg)
|
||||||
self.get_logger().debug(
|
|
||||||
f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
|
|
||||||
)
|
|
||||||
|
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
self.get_logger().warning(
|
self.get_logger().warning(
|
||||||
@ -331,51 +292,38 @@ class CanBridgeNode(Node):
|
|||||||
|
|
||||||
# ── Frame handlers ────────────────────────────────────────────────────
|
# ── Frame handlers ────────────────────────────────────────────────────
|
||||||
|
|
||||||
def _handle_imu(self, data: bytes, timestamp: float) -> None:
|
def _handle_fc_status(self, data: bytes) -> None:
|
||||||
telem = decode_imu_telem(data)
|
"""FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags."""
|
||||||
|
telem = decode_fc_status(data)
|
||||||
|
|
||||||
msg = Imu()
|
# Publish pitch as IMU (orientation only — yaw/roll unknown from FC_STATUS)
|
||||||
msg.header.stamp = self.get_clock().now().to_msg()
|
imu_msg = Imu()
|
||||||
msg.header.frame_id = "imu_link"
|
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
|
# Publish battery (vbat_mv → volts)
|
||||||
msg.linear_acceleration.y = telem.accel_y
|
bat_msg = BatteryState()
|
||||||
msg.linear_acceleration.z = telem.accel_z
|
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
|
def _handle_fc_vesc(self, data: bytes) -> None:
|
||||||
msg.angular_velocity.y = telem.gyro_y
|
"""FC_VESC (0x401): left/right RPM and current aggregated by BALANCE."""
|
||||||
msg.angular_velocity.z = telem.gyro_z
|
telem = decode_fc_vesc(data)
|
||||||
|
|
||||||
# Covariance unknown; mark as -1 per REP-145
|
left_msg = Float32MultiArray()
|
||||||
msg.orientation_covariance[0] = -1.0
|
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)
|
right_msg = Float32MultiArray()
|
||||||
|
right_msg.data = [telem.right_rpm, 0.0, 0.0, telem.right_cur]
|
||||||
def _handle_battery(self, data: bytes, timestamp: float) -> None:
|
self._pub_vesc_right.publish(right_msg)
|
||||||
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)
|
|
||||||
|
|
||||||
# ── Status helper ─────────────────────────────────────────────────────
|
# ── Status helper ─────────────────────────────────────────────────────
|
||||||
|
|
||||||
@ -390,14 +338,8 @@ class CanBridgeNode(Node):
|
|||||||
"""Send zero velocity and shut down the CAN bus cleanly."""
|
"""Send zero velocity and shut down the CAN bus cleanly."""
|
||||||
if self._connected and self._bus is not None:
|
if self._connected and self._bus is not None:
|
||||||
try:
|
try:
|
||||||
self._send_can(
|
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "shutdown")
|
||||||
MAMBA_CMD_VELOCITY,
|
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown")
|
||||||
encode_velocity_cmd(0.0, 0.0),
|
|
||||||
"shutdown",
|
|
||||||
)
|
|
||||||
self._send_can(
|
|
||||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
|
|
||||||
)
|
|
||||||
except Exception:
|
except Exception:
|
||||||
pass
|
pass
|
||||||
try:
|
try:
|
||||||
|
|||||||
@ -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