From 308be743303d254fa29fb2f4f0667ff48ff5aae6 Mon Sep 17 00:00:00 2001 From: sl-webui Date: Sat, 4 Apr 2026 08:25:24 -0400 Subject: [PATCH] feat(arch): implement SAUL-TEE ESP32 protocol specs from hal reference doc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- docs/wiring-diagram.md | 296 +++++++-------- .../saltybot_bridge/stm32_protocol.py | 336 ------------------ .../saltybot_can_bridge/can_bridge_node.py | 244 +++++-------- .../saltybot_can_bridge/mamba_protocol.py | 227 ------------ 4 files changed, 245 insertions(+), 858 deletions(-) delete mode 100644 jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py delete mode 100644 jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py diff --git a/docs/wiring-diagram.md b/docs/wiring-diagram.md index aaa15f5..94a39b6 100644 --- a/docs/wiring-diagram.md +++ b/docs/wiring-diagram.md @@ -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 │ - └──┬──────────┬───────┘ - │ │ - ┌──────────▼─┐ ┌────▼──────────┐ - │ ReSpeaker │ │ SIM7600A │ - │ 2-Mic HAT │ │ 4G/LTE + GPS │ - │ 🎤 🔊 │ │ 📡 🛰️ │ - └────────────┘ └───────────────┘ + ┌────┴────────────┐ ▼ + │ 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 │ + └────────────┘ └───────────────┘ ``` diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py deleted file mode 100644 index b7a4e18..0000000 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py +++ /dev/null @@ -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) diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py index 4f29667..2869aab 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py @@ -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) @@ -127,34 +123,9 @@ class CanBridgeNode(Node): self.get_logger().info( f"can_bridge_node ready — iface={self._iface} " - f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " - f"mamba={self._mamba_id}" + f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_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. - left_mps = linear - angular + # 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: diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py deleted file mode 100644 index 3639d08..0000000 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py +++ /dev/null @@ -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)