From 308be743303d254fa29fb2f4f0667ff48ff5aae6 Mon Sep 17 00:00:00 2001 From: sl-webui Date: Sat, 4 Apr 2026 08:25:24 -0400 Subject: [PATCH 1/2] 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) -- 2.47.2 From fda6ab99ff487f85f12659c3445052fe41b79726 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 4 Apr 2026 08:32:00 -0400 Subject: [PATCH 2/2] feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Update CAN and serial bridge code to match authoritative protocol spec from docs/SAUL-TEE-SYSTEM-REFERENCE.md §5-6 (hal, 2026-04-04). mamba_protocol.py (CAN, Orin ↔ ESP32 BALANCE): - 0x300 DRIVE: [speed:i16][steer:i16][mode:u8][flags:u8][_:u16] — combined frame - 0x301 ARM: [arm:u8] - 0x302 PID: [kp:f16][ki:f16][kd:f16][_:u16] — half-float gains - 0x303 ESTOP: [0xE5] — magic byte cut - 0x400 ATTITUDE: [pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8] - 0x401 BATTERY: [vbat_mv:u16][fault_code:u8][rssi:i8] - Add VESC STATUS1/4/5 decode helpers; VESC IDs 56 (left) / 68 (right) can_bridge_node.py: - /cmd_vel → encode_drive_cmd (speed/steer int16, MODE_DRIVE) - /estop → encode_estop_cmd (magic 0xE5); clear → DISARM - /saltybot/arm → encode_arm_cmd (new subscription) - Watchdog sends DRIVE(0,0,MODE_IDLE) when /cmd_vel silent - ATTITUDE (0x400) → /saltybot/attitude + /saltybot/balance_state JSON - BATTERY (0x401) → /can/battery BatteryState - VESC STATUS1 frames → /can/vesc/left|right/state stm32_cmd_node.py — rewritten for inter-board protocol API: - Imports from updated stm32_protocol (BAUD_RATE=460800, new frame types) - RX: RcChannels → /saltybot/rc_channels, SensorData → /saltybot/sensors - TX: encode_led_cmd, encode_output_cmd from /saltybot/leds + /saltybot/outputs - HEARTBEAT (0x20) timer replaces old SPEED_STEER/ARM logic stm32_cmd_params.yaml: serial_port=/dev/esp32-io, baud=460800 stm32_cmd.launch.py: updated defaults and description Co-Authored-By: Claude Sonnet 4.6 --- docs/wiring-diagram.md | 13 +- .../config/stm32_cmd_params.yaml | 38 +- .../launch/stm32_cmd.launch.py | 28 +- .../saltybot_bridge/stm32_cmd_node.py | 447 +++++------------- .../saltybot_can_bridge/can_bridge_node.py | 280 +++++------ 5 files changed, 277 insertions(+), 529 deletions(-) diff --git a/docs/wiring-diagram.md b/docs/wiring-diagram.md index 94a39b6..a60ef1d 100644 --- a/docs/wiring-diagram.md +++ b/docs/wiring-diagram.md @@ -1,6 +1,13 @@ -# SaltyLab Wiring Diagram +# SaltyLab / SAUL-TEE Wiring Reference -## System Overview +> ⚠️ **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) ``` ┌─────────────────────────────────────────────────────────────────────┐ @@ -138,7 +145,7 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor) | CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` | -## ESP32-S3 BALANCE UART/CAN Summary +## FC UART Summary (MAMBA F722S — OBSOLETE) | Interface | Pins | Baud/Rate | Assignment | Notes | |-----------|------|-----------|------------|-------| diff --git a/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml index 842a8bb..cca03a5 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml @@ -1,30 +1,16 @@ -# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119) -# Binary-framed Jetson↔ESP32 bridge at 921600 baud. +# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge) +# Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud. +# Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] +# Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 # ── Serial port ──────────────────────────────────────────────────────────────── -# Use /dev/esp32-bridge if the udev rule is applied: -# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", -# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout" -serial_port: /dev/ttyACM0 -baud_rate: 921600 -reconnect_delay: 2.0 # seconds between USB reconnect attempts +# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md). +# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed. +serial_port: /dev/esp32-io +baud_rate: 460800 +reconnect_delay: 2.0 # seconds between reconnect attempts # ── Heartbeat ───────────────────────────────────────────────────────────────── -# HEARTBEAT frame sent every heartbeat_period seconds. -# STM32 fires watchdog and reverts to safe state if no frame received for 500ms. -heartbeat_period: 0.2 # 200ms → well within 500ms ESP32 BALANCE watchdog - -# ── Watchdog (Jetson-side) ──────────────────────────────────────────────────── -# If no /cmd_vel message received for watchdog_timeout seconds, -# send SPEED_STEER(0,0) to stop the robot. -watchdog_timeout: 0.5 # 500ms - -# ── Twist velocity scaling ──────────────────────────────────────────────────── -# speed = clamp(linear.x * speed_scale, -1000, 1000) (m/s → ESC units) -# steer = clamp(angular.z * steer_scale, -1000, 1000) (rad/s → ESC units) -# -# Default: 1 m/s → 1000 ESC units, ±2 rad/s → ±1000 steer. -# Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction. -# Tune speed_scale to set the physical top speed. -speed_scale: 1000.0 -steer_scale: -500.0 +# HEARTBEAT (0x20) sent every heartbeat_period. +# ESP32 IO watchdog fires if no heartbeat for ~500 ms. +heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py index fa01e6a..5c0f1e0 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py @@ -1,14 +1,14 @@ -"""stm32_cmd.launch.py — Launch the binary-framed ESP32 BALANCE command node (Issue #119). +"""stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node. + +Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol). +Handles RC monitoring, sensor data, LED/output commands. +Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node. + +Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 Usage: - # Default (binary protocol, bidirectional): ros2 launch saltybot_bridge stm32_cmd.launch.py - - # Override serial port: - ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1 - - # Custom velocity scales: - ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0 + ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0 """ import os @@ -24,12 +24,9 @@ def generate_launch_description() -> LaunchDescription: params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml") return LaunchDescription([ - DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"), - DeclareLaunchArgument("baud_rate", default_value="921600"), - DeclareLaunchArgument("speed_scale", default_value="1000.0"), - DeclareLaunchArgument("steer_scale", default_value="-500.0"), - DeclareLaunchArgument("watchdog_timeout", default_value="0.5"), - DeclareLaunchArgument("heartbeat_period", default_value="0.2"), + DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"), + DeclareLaunchArgument("baud_rate", default_value="460800"), + DeclareLaunchArgument("heartbeat_period", default_value="0.2"), Node( package="saltybot_bridge", @@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription: { "serial_port": LaunchConfiguration("serial_port"), "baud_rate": LaunchConfiguration("baud_rate"), - "speed_scale": LaunchConfiguration("speed_scale"), - "steer_scale": LaunchConfiguration("steer_scale"), - "watchdog_timeout": LaunchConfiguration("watchdog_timeout"), "heartbeat_period": LaunchConfiguration("heartbeat_period"), }, ], diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py index 3533418..af16a65 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py @@ -1,45 +1,32 @@ -"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge. +"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node. -Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary -framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud. +Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the +inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5). -TX commands (Jetson → STM32): - SPEED_STEER — 50 Hz from /cmd_vel subscription - HEARTBEAT — 200 ms timer (ESP32 BALANCE watchdog fires at 500 ms) - ARM — via /saltybot/arm service - SET_MODE — via /saltybot/set_mode service - PID_UPDATE — via /saltybot/pid_update topic +This node is NOT the primary drive path (that is CAN via can_bridge_node). +It handles auxiliary I/O: RC monitoring, sensor data, LED/output control. -Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning. +Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud -RX telemetry (STM32 → Jetson): - IMU → /saltybot/imu (sensor_msgs/Imu) - BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON) - MOTOR_RPM → /saltybot/telemetry/motor_rpm (std_msgs/String JSON) - ARM_STATE → /saltybot/arm_state (std_msgs/String JSON) - ERROR → /saltybot/error (std_msgs/String JSON) - All frames → /diagnostics (diagnostic_msgs/DiagnosticArray) +RX from ESP32 IO: + RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON) + SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON) -Auto-reconnect: USB disconnect is detected when serial.read() raises; node -continuously retries at reconnect_delay interval. - -This node owns /dev/ttyACM0 exclusively — do NOT run alongside -serial_bridge_node or saltybot_cmd_node on the same port. +TX to ESP32 IO: + LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON) + OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON) + HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive) Parameters (config/stm32_cmd_params.yaml): - serial_port /dev/ttyACM0 - baud_rate 921600 - reconnect_delay 2.0 (seconds) - heartbeat_period 0.2 (seconds) - watchdog_timeout 0.5 (seconds — no /cmd_vel → send zero-speed) - speed_scale 1000.0 (linear.x m/s → ESC units) - steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention) + serial_port /dev/esp32-io + baud_rate 460800 + reconnect_delay 2.0 + heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms) """ from __future__ import annotations import json -import math import threading import time @@ -50,119 +37,69 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy import serial from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue -from geometry_msgs.msg import Twist -from sensor_msgs.msg import Imu from std_msgs.msg import String -from std_srvs.srv import SetBool, Trigger from .stm32_protocol import ( + BAUD_RATE, FrameParser, - ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, - encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode, - encode_pid_update, + RcChannels, + SensorData, + encode_heartbeat, + encode_led_cmd, + encode_output_cmd, ) -# ── Constants ───────────────────────────────────────────────────────────────── - -IMU_FRAME_ID = "imu_link" -_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} - - -def _clamp(v: float, lo: float, hi: float) -> float: - return max(lo, min(hi, v)) - - -# ── Node ────────────────────────────────────────────────────────────────────── class Stm32CmdNode(Node): - """Binary-framed Jetson↔ESP32 bridge node.""" + """Orin ↔ ESP32-S3 IO auxiliary bridge node.""" def __init__(self) -> None: super().__init__("stm32_cmd_node") - # ── Parameters ──────────────────────────────────────────────────────── - self.declare_parameter("serial_port", "/dev/ttyACM0") - self.declare_parameter("baud_rate", 921600) + # ── Parameters ──────────────────────────────────────────────────── + self.declare_parameter("serial_port", "/dev/esp32-io") + self.declare_parameter("baud_rate", BAUD_RATE) self.declare_parameter("reconnect_delay", 2.0) self.declare_parameter("heartbeat_period", 0.2) - self.declare_parameter("watchdog_timeout", 0.5) - self.declare_parameter("speed_scale", 1000.0) - self.declare_parameter("steer_scale", -500.0) - port = self.get_parameter("serial_port").value - baud = self.get_parameter("baud_rate").value + self._port_name = self.get_parameter("serial_port").value + self._baud = self.get_parameter("baud_rate").value self._reconnect_delay = self.get_parameter("reconnect_delay").value self._hb_period = self.get_parameter("heartbeat_period").value - self._wd_timeout = self.get_parameter("watchdog_timeout").value - self._speed_scale = self.get_parameter("speed_scale").value - self._steer_scale = self.get_parameter("steer_scale").value - # ── QoS ─────────────────────────────────────────────────────────────── - sensor_qos = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, depth=10, - ) + # ── QoS ─────────────────────────────────────────────────────────── rel_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10, ) - # ── Publishers ──────────────────────────────────────────────────────── - self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos) - self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos) - self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos) - self._battery_pub = self.create_publisher(String, "/saltybot/telemetry/battery", rel_qos) - self._rpm_pub = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos) - self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos) + # ── Publishers ──────────────────────────────────────────────────── + self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos) + self._sens_pub = self.create_publisher(String, "/saltybot/sensors", rel_qos) + self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos) - # ── Subscribers ─────────────────────────────────────────────────────── - self._cmd_vel_sub = self.create_subscription( - Twist, "/cmd_vel", self._on_cmd_vel, rel_qos, - ) - self._pid_sub = self.create_subscription( - String, "/saltybot/pid_update", self._on_pid_update, rel_qos, - ) + # ── Subscriptions ───────────────────────────────────────────────── + self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos) + self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos) - # ── Services ────────────────────────────────────────────────────────── - self._arm_srv = self.create_service(SetBool, "/saltybot/arm", self._svc_arm) - self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode) - - # ── Serial state ────────────────────────────────────────────────────── - self._port_name = port - self._baud = baud + # ── Serial state ────────────────────────────────────────────────── self._ser: serial.Serial | None = None - self._ser_lock = threading.Lock() - self._parser = FrameParser() + self._ser_lock = threading.Lock() + self._parser = FrameParser() + self._rx_count = 0 - # ── TX state ────────────────────────────────────────────────────────── - self._last_speed = 0 - self._last_steer = 0 - self._last_cmd_t = time.monotonic() - self._watchdog_sent = False # tracks whether we already sent zero - - # ── Diagnostics state ────────────────────────────────────────────────── - self._last_arm_state = -1 - self._last_battery_mv = 0 - self._rx_frame_count = 0 - - # ── Open serial and start timers ────────────────────────────────────── + # ── Open serial and start timers ────────────────────────────────── self._open_serial() - # Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean) - self._read_timer = self.create_timer(0.005, self._read_cb) - # Heartbeat TX + self._read_timer = self.create_timer(0.005, self._read_cb) self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb) - # Watchdog check (fires at 2× watchdog_timeout for quick detection) - self._wd_timer = self.create_timer(self._wd_timeout / 2, self._watchdog_cb) - # Periodic diagnostics - self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) + self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) self.get_logger().info( - f"stm32_cmd_node started — {port} @ {baud} baud | " - f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms" + f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud" ) - # ── Serial management ───────────────────────────────────────────────────── + # ── Serial management ───────────────────────────────────────────────── def _open_serial(self) -> bool: with self._ser_lock: @@ -170,7 +107,7 @@ class Stm32CmdNode(Node): self._ser = serial.Serial( port=self._port_name, baudrate=self._baud, - timeout=0.005, # non-blocking reads + timeout=0.005, write_timeout=0.1, ) self._ser.reset_input_buffer() @@ -185,17 +122,7 @@ class Stm32CmdNode(Node): self._ser = None return False - def _close_serial(self) -> None: - with self._ser_lock: - if self._ser and self._ser.is_open: - try: - self._ser.close() - except Exception: - pass - self._ser = None - def _write(self, data: bytes) -> bool: - """Thread-safe serial write. Returns False if port is not open.""" with self._ser_lock: if self._ser is None or not self._ser.is_open: return False @@ -207,16 +134,15 @@ class Stm32CmdNode(Node): self._ser = None return False - # ── RX — read callback ──────────────────────────────────────────────────── + # ── RX ──────────────────────────────────────────────────────────────── def _read_cb(self) -> None: - """Read bytes from serial and feed them to the frame parser.""" raw: bytes | None = None - reconnect_needed = False + reconnect = False with self._ser_lock: if self._ser is None or not self._ser.is_open: - reconnect_needed = True + reconnect = True else: try: n = self._ser.in_waiting @@ -225,9 +151,9 @@ class Stm32CmdNode(Node): except serial.SerialException as exc: self.get_logger().error(f"Serial read error: {exc}") self._ser = None - reconnect_needed = True + reconnect = True - if reconnect_needed: + if reconnect: self.get_logger().warn( "Serial disconnected — will retry", throttle_duration_sec=self._reconnect_delay, @@ -240,230 +166,105 @@ class Stm32CmdNode(Node): return for byte in raw: - frame = self._parser.feed(byte) - if frame is not None: - self._rx_frame_count += 1 - self._dispatch_frame(frame) + msg = self._parser.feed(byte) + if msg is not None: + self._rx_count += 1 + self._dispatch(msg) - def _dispatch_frame(self, frame) -> None: - """Route a decoded frame to the appropriate publisher.""" + def _dispatch(self, msg) -> None: now = self.get_clock().now().to_msg() + ts = f"{now.sec}.{now.nanosec:09d}" - if isinstance(frame, ImuFrame): - self._publish_imu(frame, now) + if isinstance(msg, RcChannels): + out = String() + out.data = json.dumps({ + "channels": msg.channels, + "source": msg.source, + "ts": ts, + }) + self._rc_pub.publish(out) - elif isinstance(frame, BatteryFrame): - self._publish_battery(frame, now) + elif isinstance(msg, SensorData): + out = String() + out.data = json.dumps({ + "pressure_pa": msg.pressure_pa, + "temperature_c": msg.temperature_c, + "tof_mm": msg.tof_mm, + "ts": ts, + }) + self._sens_pub.publish(out) - elif isinstance(frame, MotorRpmFrame): - self._publish_motor_rpm(frame, now) + elif isinstance(msg, tuple): + type_code, _ = msg + self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}") - elif isinstance(frame, ArmStateFrame): - self._publish_arm_state(frame, now) - - elif isinstance(frame, ErrorFrame): - self._publish_error(frame, now) - - elif isinstance(frame, tuple): - type_code, payload = frame - self.get_logger().debug( - f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}" - ) - - # ── Telemetry publishers ────────────────────────────────────────────────── - - def _publish_imu(self, frame: ImuFrame, stamp) -> None: - msg = Imu() - msg.header.stamp = stamp - msg.header.frame_id = IMU_FRAME_ID - - # orientation: unknown — signal with -1 in first covariance - msg.orientation_covariance[0] = -1.0 - - msg.angular_velocity.x = math.radians(frame.pitch_deg) - msg.angular_velocity.y = math.radians(frame.roll_deg) - msg.angular_velocity.z = math.radians(frame.yaw_deg) - cov = math.radians(0.3) ** 2 # ±0.3° noise estimate from ESP32 BMI088 - msg.angular_velocity_covariance[0] = cov - msg.angular_velocity_covariance[4] = cov - msg.angular_velocity_covariance[8] = cov - - msg.linear_acceleration.x = frame.accel_x - msg.linear_acceleration.y = frame.accel_y - msg.linear_acceleration.z = frame.accel_z - acov = 0.05 ** 2 # ±0.05 m/s² noise - msg.linear_acceleration_covariance[0] = acov - msg.linear_acceleration_covariance[4] = acov - msg.linear_acceleration_covariance[8] = acov - - self._imu_pub.publish(msg) - - def _publish_battery(self, frame: BatteryFrame, stamp) -> None: - payload = { - "voltage_v": round(frame.voltage_mv / 1000.0, 3), - "voltage_mv": frame.voltage_mv, - "current_ma": frame.current_ma, - "soc_pct": frame.soc_pct, - "charging": frame.current_ma < -100, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - self._last_battery_mv = frame.voltage_mv - msg = String() - msg.data = json.dumps(payload) - self._battery_pub.publish(msg) - - def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None: - payload = { - "left_rpm": frame.left_rpm, - "right_rpm": frame.right_rpm, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._rpm_pub.publish(msg) - - def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None: - label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})") - if frame.state != self._last_arm_state: - self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})") - self._last_arm_state = frame.state - - payload = { - "state": frame.state, - "state_label": label, - "error_flags": frame.error_flags, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._arm_pub.publish(msg) - - def _publish_error(self, frame: ErrorFrame, stamp) -> None: - self.get_logger().error( - f"ESP32 BALANCE error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}" - ) - payload = { - "error_code": frame.error_code, - "subcode": frame.subcode, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._error_pub.publish(msg) - - # ── TX — command send ───────────────────────────────────────────────────── - - def _on_cmd_vel(self, msg: Twist) -> None: - """Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz.""" - speed = int(_clamp(msg.linear.x * self._speed_scale, -1000.0, 1000.0)) - steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0)) - self._last_speed = speed - self._last_steer = steer - self._last_cmd_t = time.monotonic() - self._watchdog_sent = False - - frame = encode_speed_steer(speed, steer) - if not self._write(frame): - self.get_logger().warn( - "SPEED_STEER dropped — serial not open", - throttle_duration_sec=2.0, - ) + # ── TX ──────────────────────────────────────────────────────────────── def _heartbeat_cb(self) -> None: - """Send HEARTBEAT every heartbeat_period (default 200ms).""" self._write(encode_heartbeat()) - def _watchdog_cb(self) -> None: - """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds.""" - if time.monotonic() - self._last_cmd_t >= self._wd_timeout: - if not self._watchdog_sent: - self.get_logger().warn( - f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed" - ) - self._watchdog_sent = True - self._last_speed = 0 - self._last_steer = 0 - self._write(encode_speed_steer(0, 0)) - - def _on_pid_update(self, msg: String) -> None: - """Parse JSON /saltybot/pid_update and send PID_UPDATE frame.""" + def _on_leds(self, msg: String) -> None: + """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD.""" try: - data = json.loads(msg.data) - kp = float(data["kp"]) - ki = float(data["ki"]) - kd = float(data["kd"]) + d = json.loads(msg.data) + frame = encode_led_cmd( + int(d.get("pattern", 0)), + int(d.get("r", 0)), + int(d.get("g", 0)), + int(d.get("b", 0)), + ) except (ValueError, KeyError, json.JSONDecodeError) as exc: - self.get_logger().error(f"Bad PID update JSON: {exc}") + self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}") return - frame = encode_pid_update(kp, ki, kd) - if self._write(frame): - self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}") - else: - self.get_logger().warn("PID_UPDATE dropped — serial not open") + self._write(frame) - # ── Services ────────────────────────────────────────────────────────────── + def _on_outputs(self, msg: String) -> None: + """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}.""" + try: + d = json.loads(msg.data) + frame = encode_output_cmd( + bool(d.get("horn", False)), + bool(d.get("buzzer", False)), + int(d.get("headlight", 0)), + int(d.get("fan", 0)), + ) + except (ValueError, KeyError, json.JSONDecodeError) as exc: + self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}") + return + self._write(frame) - def _svc_arm(self, request: SetBool.Request, response: SetBool.Response): - """SetBool(True) = arm, SetBool(False) = disarm.""" - arm = request.data - frame = encode_arm(arm) - ok = self._write(frame) - response.success = ok - response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open" - self.get_logger().info( - f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}" - ) - return response - - def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response): - """SetBool: data maps to mode byte (True=1, False=0).""" - mode = 1 if request.data else 0 - frame = encode_set_mode(mode) - ok = self._write(frame) - response.success = ok - response.message = f"mode={mode}" if ok else "serial not open" - return response - - # ── Diagnostics ─────────────────────────────────────────────────────────── + # ── Diagnostics ─────────────────────────────────────────────────────── def _publish_diagnostics(self) -> None: diag = DiagnosticArray() diag.header.stamp = self.get_clock().now().to_msg() - status = DiagnosticStatus() - status.name = "saltybot/stm32_cmd_node" - status.hardware_id = "esp32" - + status.name = "saltybot/esp32_io_bridge" + status.hardware_id = "esp32-s3-io" port_ok = self._ser is not None and self._ser.is_open - if port_ok: - status.level = DiagnosticStatus.OK - status.message = "Serial OK" - else: - status.level = DiagnosticStatus.ERROR - status.message = f"Serial disconnected: {self._port_name}" - - wd_age = time.monotonic() - self._last_cmd_t - status.values = [ - KeyValue(key="serial_port", value=self._port_name), - KeyValue(key="port_open", value=str(port_ok)), - KeyValue(key="rx_frames", value=str(self._rx_frame_count)), - KeyValue(key="rx_errors", value=str(self._parser.frames_error)), - KeyValue(key="last_speed", value=str(self._last_speed)), - KeyValue(key="last_steer", value=str(self._last_steer)), - KeyValue(key="cmd_vel_age_s", value=f"{wd_age:.2f}"), - KeyValue(key="battery_mv", value=str(self._last_battery_mv)), - KeyValue(key="arm_state", value=_ARM_LABEL.get(self._last_arm_state, "?")), + status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR + status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}" + status.values = [ + KeyValue(key="serial_port", value=self._port_name), + KeyValue(key="baud_rate", value=str(self._baud)), + KeyValue(key="port_open", value=str(port_ok)), + KeyValue(key="rx_frames", value=str(self._rx_count)), + KeyValue(key="rx_errors", value=str(self._parser.frames_error)), ] diag.status.append(status) self._diag_pub.publish(diag) - # ── Lifecycle ───────────────────────────────────────────────────────────── + # ── Lifecycle ───────────────────────────────────────────────────────── def destroy_node(self) -> None: - # Send zero-speed + disarm on shutdown - self._write(encode_speed_steer(0, 0)) - self._write(encode_arm(False)) - self._close_serial() + self._write(encode_heartbeat(state=0)) + with self._ser_lock: + if self._ser and self._ser.is_open: + try: + self._ser.close() + except Exception: + pass + self._ser = None super().destroy_node() 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 2869aab..0db02cb 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,26 +1,34 @@ #!/usr/bin/env python3 """ 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). +board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps). -Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §4 & §6 (2026-04-04) +Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04) Subscriptions ------------- - /cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300) - /estop std_msgs/Bool → ORIN_CMD_ESTOP (0x302) + /cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300) + /estop std_msgs/Bool → ORIN_CMD_ESTOP (0x303) + /saltybot/arm std_msgs/Bool → ORIN_CMD_ARM (0x301) Publications ------------ - /can/imu sensor_msgs/Imu 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" + /saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state + /saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude + /can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi + /can/vesc/left/state std_msgs/Float32MultiArray VESC STATUS_1 left + /can/vesc/right/state std_msgs/Float32MultiArray VESC STATUS_1 right + /can/connection_status std_msgs/String "connected" | "disconnected" -Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 +Parameters +---------- + can_interface str CAN socket name (default: slcan0) + speed_scale float /cmd_vel linear.x (m/s) → motor units (default: 1000.0) + steer_scale float /cmd_vel angular.z (rad/s) → motor units (default: -500.0) + command_timeout_s float watchdog zero-vel threshold (default: 0.5) """ +import json import threading import time from typing import Optional @@ -29,42 +37,32 @@ import can import rclpy from geometry_msgs.msg import Twist from rclpy.node import Node - -from sensor_msgs.msg import BatteryState, Imu +from sensor_msgs.msg import BatteryState from std_msgs.msg import Bool, Float32MultiArray, String from saltybot_can_bridge.balance_protocol import ( - # Orin → BALANCE command IDs ORIN_CMD_DRIVE, - ORIN_CMD_MODE, + ORIN_CMD_ARM, ORIN_CMD_ESTOP, - # BALANCE → Orin telemetry IDs - FC_STATUS, - FC_VESC, - # VESC node IDs + ESP32_TELEM_ATTITUDE, + ESP32_TELEM_BATTERY, VESC_LEFT_ID, VESC_RIGHT_ID, VESC_STATUS_1, - # Mode constants MODE_DRIVE, - MODE_ESTOP, MODE_IDLE, - # Encoders encode_drive_cmd, - encode_mode_cmd, + encode_arm_cmd, encode_estop_cmd, - encode_led_cmd, - # Decoders - decode_fc_status, - decode_fc_vesc, + decode_attitude, + decode_battery, decode_vesc_status1, - decode_vesc_can_id, ) # Reconnect attempt interval when CAN bus is lost _RECONNECT_INTERVAL_S: float = 5.0 -# Watchdog timer tick rate (Hz) +# Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent _WATCHDOG_HZ: float = 10.0 @@ -75,41 +73,41 @@ class CanBridgeNode(Node): super().__init__("can_bridge_node") # ── Parameters ──────────────────────────────────────────────────── - self.declare_parameter("can_interface", "slcan0") - 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("can_interface", "slcan0") + self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) + self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) + self.declare_parameter("speed_scale", 1000.0) + self.declare_parameter("steer_scale", -500.0) self.declare_parameter("command_timeout_s", 0.5) - self._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._cmd_timeout: float = self.get_parameter("command_timeout_s").value + self._iface = self.get_parameter("can_interface").value + self._left_vesc_id = self.get_parameter("left_vesc_can_id").value + self._right_vesc_id = self.get_parameter("right_vesc_can_id").value + self._speed_scale = self.get_parameter("speed_scale").value + self._steer_scale = self.get_parameter("steer_scale").value + self._cmd_timeout = self.get_parameter("command_timeout_s").value # ── State ───────────────────────────────────────────────────────── self._bus: Optional[can.BusABC] = None self._connected: bool = False self._last_cmd_time: float = time.monotonic() - self._lock = threading.Lock() # protects _bus / _connected + self._lock = threading.Lock() # ── Publishers ──────────────────────────────────────────────────── - self._pub_imu = self.create_publisher(Imu, "/can/imu", 10) - self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) - self._pub_vesc_left = self.create_publisher( - Float32MultiArray, "/can/vesc/left/state", 10 - ) - self._pub_vesc_right = self.create_publisher( - Float32MultiArray, "/can/vesc/right/state", 10 - ) - self._pub_status = self.create_publisher( - String, "/can/connection_status", 10 - ) + self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10) + self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10) + self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) + self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state", 10) + self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state", 10) + self._pub_status = self.create_publisher(String, "/can/connection_status", 10) # ── Subscriptions ───────────────────────────────────────────────── - self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) - self.create_subscription(Bool, "/estop", self._estop_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, "/saltybot/arm", self._arm_cb, 10) # ── Timers ──────────────────────────────────────────────────────── - self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) + self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb) # ── Open CAN ────────────────────────────────────────────────────── @@ -123,22 +121,18 @@ 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"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " + f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}" ) # ── Connection management ────────────────────────────────────────────── def _try_connect(self) -> None: - """Attempt to open the CAN interface; silently skip if already connected.""" with self._lock: if self._connected: return try: - bus = can.interface.Bus( - channel=self._iface, - bustype="socketcan", - ) - self._bus = bus + self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan") self._connected = True self.get_logger().info(f"CAN bus connected: {self._iface}") self._publish_status("connected") @@ -151,12 +145,10 @@ class CanBridgeNode(Node): self._publish_status("disconnected") def _reconnect_cb(self) -> None: - """Periodic timer: try to reconnect when disconnected.""" if not self._connected: self._try_connect() def _handle_can_error(self, exc: Exception, context: str) -> None: - """Mark bus as disconnected on any CAN error.""" self.get_logger().warning(f"CAN error in {context}: {exc}") with self._lock: if self._bus is not None: @@ -171,59 +163,51 @@ class CanBridgeNode(Node): # ── ROS callbacks ───────────────────────────────────────────────────── def _cmd_vel_cb(self, msg: Twist) -> None: - """Convert /cmd_vel Twist to VESC speed commands over CAN.""" + """Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN.""" self._last_cmd_time = time.monotonic() - if not self._connected: return - - # Differential drive decomposition — individual wheel speeds in m/s. - # The VESC nodes interpret linear velocity directly; angular is handled - # by the sign difference between left and right. - linear = msg.linear.x - angular = msg.angular.z - - # Differential drive decomposition (positive angular = CCW = left turn). - left_mps = linear - angular - right_mps = linear + angular - - 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") + speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale))) + steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale))) + self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel") def _estop_cb(self, msg: Bool) -> None: """Forward /estop to ESP32 BALANCE over CAN.""" if not self._connected: return - self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "estop") if msg.data: - self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode") + self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop") self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE") + else: + # Clear estop: send DISARM then re-ARM (let operator decide to re-arm) + self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "estop_clear") + + def _arm_cb(self, msg: Bool) -> None: + """Forward /saltybot/arm to ORIN_CMD_ARM.""" + if not self._connected: + return + self._send_can(ORIN_CMD_ARM, encode_arm_cmd(msg.data), "arm") + self.get_logger().info(f"ARM command: {'ARM' if msg.data else 'DISARM'}") # ── Watchdog ────────────────────────────────────────────────────────── def _watchdog_cb(self) -> None: - """If no /cmd_vel arrives within the timeout, send zero velocity.""" + """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive).""" if not self._connected: return - elapsed = time.monotonic() - self._last_cmd_time - if elapsed > self._cmd_timeout: - self._send_can(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") + if time.monotonic() - self._last_cmd_time > self._cmd_timeout: + self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog") # ── CAN send helper ─────────────────────────────────────────────────── - def _send_can(self, arb_id: int, data: bytes, context: str) -> None: - """Send a standard CAN frame; handle errors gracefully.""" + def _send_can(self, arb_id: int, data: bytes, context: str, + extended: bool = False) -> None: with self._lock: if not self._connected or self._bus is None: return bus = self._bus - - msg = can.Message( - arbitration_id=arb_id, - data=data, - is_extended_id=False, - ) + msg = can.Message(arbitration_id=arb_id, data=data, + is_extended_id=extended) try: bus.send(msg, timeout=0.05) except can.CanError as exc: @@ -232,59 +216,41 @@ class CanBridgeNode(Node): # ── Background CAN reader ───────────────────────────────────────────── def _reader_loop(self) -> None: - """ - Blocking CAN read loop executed in a daemon thread. - Dispatches incoming frames to the appropriate handler. - """ while rclpy.ok(): with self._lock: - connected = self._connected - bus = self._bus - + connected, bus = self._connected, self._bus if not connected or bus is None: time.sleep(0.1) continue - try: frame = bus.recv(timeout=0.5) except can.CanError as exc: self._handle_can_error(exc, "reader_loop recv") continue - if frame is None: - # Timeout — no frame within 0.5 s, loop again continue - self._dispatch_frame(frame) def _dispatch_frame(self, frame: can.Message) -> None: - """Route an incoming CAN frame to the correct publisher.""" arb_id = frame.arbitration_id - data = bytes(frame.data) - - # VESC 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 - + data = bytes(frame.data) + vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id + vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id try: - if arb_id == FC_STATUS: - self._handle_fc_status(data) - - elif arb_id == FC_VESC: - self._handle_fc_vesc(data) - - 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_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) - + if arb_id == ESP32_TELEM_ATTITUDE: + self._handle_attitude(data) + elif arb_id == ESP32_TELEM_BATTERY: + self._handle_battery(data) + elif arb_id == vesc_l: + t = decode_vesc_status1(self._left_vesc_id, data) + m = Float32MultiArray() + m.data = [t.erpm, t.duty, 0.0, t.current] + self._pub_vesc_left.publish(m) + elif arb_id == vesc_r: + t = decode_vesc_status1(self._right_vesc_id, data) + m = Float32MultiArray() + m.data = [t.erpm, t.duty, 0.0, t.current] + self._pub_vesc_right.publish(m) except Exception as exc: self.get_logger().warning( f"Error parsing CAN frame 0x{arb_id:03X}: {exc}" @@ -292,38 +258,35 @@ class CanBridgeNode(Node): # ── Frame handlers ──────────────────────────────────────────────────── - def _handle_fc_status(self, data: bytes) -> None: - """FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags.""" - telem = decode_fc_status(data) + _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"} - # 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) + def _handle_attitude(self, data: bytes) -> None: + """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude.""" + t = decode_attitude(data) + now = self.get_clock().now().to_msg() + payload = { + "pitch_deg": round(t.pitch_deg, 2), + "speed_mps": round(t.speed, 3), + "yaw_rate": round(t.yaw_rate, 3), + "state": t.state, + "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"), + "flags": t.flags, + "ts": f"{now.sec}.{now.nanosec:09d}", + } + msg = String() + msg.data = json.dumps(payload) + self._pub_attitude.publish(msg) + self._pub_balance.publish(msg) # keep /saltybot/balance_state alive - # 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) - - def _handle_fc_vesc(self, data: bytes) -> None: - """FC_VESC (0x401): left/right RPM and current aggregated by BALANCE.""" - telem = decode_fc_vesc(data) - - left_msg = Float32MultiArray() - left_msg.data = [telem.left_rpm, 0.0, 0.0, telem.left_cur] - self._pub_vesc_left.publish(left_msg) - - right_msg = Float32MultiArray() - right_msg.data = [telem.right_rpm, 0.0, 0.0, telem.right_cur] - self._pub_vesc_right.publish(right_msg) + def _handle_battery(self, data: bytes) -> None: + """BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery.""" + t = decode_battery(data) + msg = BatteryState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.voltage = t.vbat_mv / 1000.0 + msg.present = True + msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + self._pub_battery.publish(msg) # ── Status helper ───────────────────────────────────────────────────── @@ -335,11 +298,10 @@ class CanBridgeNode(Node): # ── Shutdown ────────────────────────────────────────────────────────── def destroy_node(self) -> None: - """Send zero velocity and shut down the CAN bus cleanly.""" if self._connected and self._bus is not None: try: - self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "shutdown") - self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown") + self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown") + self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown") except Exception: pass try: @@ -349,8 +311,6 @@ class CanBridgeNode(Node): super().destroy_node() -# --------------------------------------------------------------------------- - def main(args=None) -> None: rclpy.init(args=args) node = CanBridgeNode() -- 2.47.2