Compare commits
2 Commits
a1233dbd04
...
c958cf4474
| Author | SHA1 | Date | |
|---|---|---|---|
|
c958cf4474 | ||
|
|
bb35ddd56d |
@ -7,12 +7,7 @@ The robot can now be armed and operated autonomously from the Jetson without req
|
||||
|
||||
### Jetson Autonomous Arming
|
||||
- Command: `A\n` (single byte 'A' followed by newline)
|
||||
<<<<<<< HEAD
|
||||
- Sent via USB CDC to the ESP32 BALANCE firmware
|
||||
=======
|
||||
- Sent via USB Serial (CH343) to the ESP32-S3 firmware
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
- Robot arms after ARMING_HOLD_MS (~500ms) safety hold period
|
||||
- Sent via USB Serial (CH343) to the ESP32-S3 firmware- Robot arms after ARMING_HOLD_MS (~500ms) safety hold period
|
||||
- Works even when RC is not connected or not armed
|
||||
|
||||
### RC Arming (Optional Override)
|
||||
@ -46,12 +41,7 @@ The robot can now be armed and operated autonomously from the Jetson without req
|
||||
|
||||
## Command Protocol
|
||||
|
||||
<<<<<<< HEAD
|
||||
### From Jetson to ESP32 BALANCE (USB CDC)
|
||||
=======
|
||||
### From Jetson to ESP32-S3 (USB Serial (CH343))
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
```
|
||||
### From Jetson to ESP32-S3 (USB Serial (CH343))```
|
||||
A — Request arm (triggers safety hold, then motors enable)
|
||||
D — Request disarm (immediate motor stop)
|
||||
E — Emergency stop (immediate motor cutoff, latched)
|
||||
@ -60,12 +50,7 @@ H — Heartbeat (refresh timeout timer, every 500ms)
|
||||
C<spd>,<str> — Drive command: speed, steer (also refreshes heartbeat)
|
||||
```
|
||||
|
||||
<<<<<<< HEAD
|
||||
### From ESP32 BALANCE to Jetson (USB CDC)
|
||||
=======
|
||||
### From ESP32-S3 to Jetson (USB Serial (CH343))
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Motor commands are gated by `bal.state == BALANCE_ARMED`:
|
||||
### From ESP32-S3 to Jetson (USB Serial (CH343))Motor commands are gated by `bal.state == BALANCE_ARMED`:
|
||||
- When ARMED: Motor commands sent every 20ms (50 Hz)
|
||||
- When DISARMED: Zero sent every 20ms (prevents ESC timeout)
|
||||
|
||||
|
||||
21
CLAUDE.md
21
CLAUDE.md
@ -1,34 +1,13 @@
|
||||
# SaltyLab Firmware — Agent Playbook
|
||||
|
||||
## Project
|
||||
<<<<<<< HEAD
|
||||
**SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg).
|
||||
Two ESP32-S3 boards + Jetson Orin via CAN. Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
|
||||
|
||||
| Board | Role |
|
||||
|-------|------|
|
||||
| **ESP32-S3 BALANCE** | QMI8658 IMU, PID balance, CAN→VESC (L:68 / R:56), GC9A01 LCD (Waveshare Touch LCD 1.28) |
|
||||
| **ESP32-S3 IO** | TBS Crossfire RC, ELRS failover, BTS7960 motors, NFC/baro/ToF, WS2812 |
|
||||
| **Jetson Orin** | AI/SLAM, CANable2 USB→CAN, cmds 0x300–0x303, telemetry 0x400–0x401 |
|
||||
|
||||
> **Legacy:** `src/` and `include/` = archived STM32 HAL — do not extend. New firmware in `esp32/`.
|
||||
=======
|
||||
Self-balancing two-wheeled robot: ESP32-S3 ESP32-S3 BALANCE, hoverboard hub motors, Jetson Orin Nano Super for AI/SLAM.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
## Team
|
||||
| Agent | Role | Focus |
|
||||
|-------|------|-------|
|
||||
<<<<<<< HEAD
|
||||
| **sl-firmware** | Embedded Firmware Lead | ESP32-S3, ESP-IDF, QMI8658, CAN/UART protocol, BTS7960 |
|
||||
| **sl-controls** | Control Systems Engineer | PID tuning, IMU fusion, balance loop, safety |
|
||||
| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 |
|
||||
=======
|
||||
| **sl-firmware** | Embedded Firmware Lead | ESP-IDF, USB Serial (CH343) debugging, SPI/UART, PlatformIO, DFU bootloader |
|
||||
| **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
|
||||
| **sl-perception** | Perception / SLAM Engineer | Jetson Orin Nano Super, RealSense D435i, RPLIDAR, ROS2, Nav2 |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
## Status
|
||||
USB Serial (CH343) TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).
|
||||
|
||||
|
||||
36
TEAM.md
36
TEAM.md
@ -1,54 +1,29 @@
|
||||
# SaltyLab — Ideal Team
|
||||
|
||||
## Project
|
||||
<<<<<<< HEAD
|
||||
**SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg).
|
||||
Two ESP32-S3 boards (BALANCE + IO) + Jetson Orin. See `docs/SAUL-TEE-SYSTEM-REFERENCE.md`.
|
||||
|
||||
## Current Status
|
||||
- **Hardware:** ESP32-S3 BALANCE (Waveshare Touch LCD 1.28, CH343 USB) + ESP32-S3 IO (bare devkit, JTAG USB)
|
||||
- **Firmware:** ESP-IDF/PlatformIO target; legacy `src/` STM32 HAL archived
|
||||
- **Comms:** UART 460800 baud inter-board; CANable2 USB→CAN for Orin; CAN 500 kbps to VESCs (L:68 / R:56)
|
||||
=======
|
||||
Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hoverboard hub motors, and eventually a Jetson Orin Nano Super for AI/SLAM.
|
||||
|
||||
## Current Status
|
||||
- **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand
|
||||
- **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB Serial (CH343) bug
|
||||
- **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3 — see `legacy/stm32/USB_CDC_BUG.md` for historical context
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
- **Blocker:** USB Serial (CH343) TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB on ESP32-S3
|
||||
---
|
||||
|
||||
## Roles Needed
|
||||
|
||||
### 1. Embedded Firmware Engineer (Lead)
|
||||
**Must-have:**
|
||||
<<<<<<< HEAD
|
||||
- Deep ESP32 (Arduino/ESP-IDF) or STM32 HAL experience
|
||||
- USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts)
|
||||
- SPI + UART + USB coexistence on ESP32
|
||||
- PlatformIO or bare-metal ESP32 toolchain
|
||||
- DFU bootloader implementation
|
||||
=======
|
||||
- Deep ESP-IDF experience (ESP32-S3 specifically)
|
||||
- USB Serial (CH343) / UART debugging on ESP32-S3
|
||||
- SPI + UART + USB coexistence on ESP32-S3
|
||||
- ESP-IDF / Arduino-ESP32 toolchain
|
||||
- OTA firmware update implementation
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
**Nice-to-have:**
|
||||
- ESP32-S3 peripheral coexistence (SPI + UART + USB)
|
||||
- PID control loop tuning for balance robots
|
||||
- FOC motor control (hoverboard ESC protocol)
|
||||
|
||||
<<<<<<< HEAD
|
||||
**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged STM32 USB issues before — ESP32 firmware for the balance loop and I/O needs to be written from scratch.
|
||||
=======
|
||||
**Why:** The immediate blocker is a USB peripheral conflict on ESP32-S3. Need someone who's debugged ESP32-S3 USB Serial (CH343) issues before — this is not a software logic bug, it's a hardware peripheral interaction issue.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
### 2. Control Systems / Robotics Engineer
|
||||
**Must-have:**
|
||||
- PID tuning for inverted pendulum / self-balancing systems
|
||||
@ -83,12 +58,7 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
|
||||
## Hardware Reference
|
||||
| Component | Details |
|
||||
|-----------|---------|
|
||||
<<<<<<< HEAD
|
||||
| FC | ESP32 BALANCE (ESP32RET6, MPU6000) |
|
||||
=======
|
||||
| FC | ESP32-S3 BALANCE (ESP32-S3RET6, QMI8658) |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| Motors | 2x 8" pneumatic hoverboard hub motors |
|
||||
| FC | ESP32-S3 BALANCE (ESP32-S3RET6, QMI8658) || Motors | 2x 8" pneumatic hoverboard hub motors |
|
||||
| ESC | Hoverboard ESC (EFeru FOC firmware) |
|
||||
| Battery | 36V pack |
|
||||
| RC | BetaFPV ELRS 2.4GHz TX + RX |
|
||||
@ -100,4 +70,4 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
|
||||
## Repo
|
||||
- Gitea: https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
- Design doc: `projects/saltybot/SALTYLAB.md`
|
||||
- Bug doc: `legacy/stm32/USB_CDC_BUG.md` (archived — STM32 era)
|
||||
- Design doc: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
|
||||
|
||||
@ -56,16 +56,7 @@
|
||||
3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m.
|
||||
4. Insert battery pack; route Velcro straps through slots and cinch.
|
||||
|
||||
<<<<<<< HEAD
|
||||
### 7 MCU mount (ESP32 BALANCE + ESP32 IO)
|
||||
|
||||
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE retired. Two ESP32 boards replace it.
|
||||
> Board dimensions and hole patterns TBD — await spec from max before machining mount plate.
|
||||
|
||||
=======
|
||||
### 7 FC mount (ESP32-S3 BALANCE)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
1. Place silicone anti-vibration grommets onto nylon M3 standoffs.
|
||||
### 7 FC mount (ESP32-S3 BALANCE)1. Place silicone anti-vibration grommets onto nylon M3 standoffs.
|
||||
2. Lower ESP32 BALANCE board onto standoffs; secure with M3×6 BHCS. Snug only.
|
||||
3. Mount ESP32 IO board adjacent — exact placement TBD pending board dimensions.
|
||||
4. Orient USB connectors toward front of robot for cable access.
|
||||
|
||||
@ -41,12 +41,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
|
||||
| 3 | Dropout clamp — upper | 2 | 8mm 6061-T6 Al | 90×70mm blank | D-cut bore; `RENDER="clamp_upper_2d"` |
|
||||
| 4 | Stem flange ring | 2 | 6mm Al or acrylic | Ø82mm disc | One above + one below plate; `RENDER="stem_flange_2d"` |
|
||||
| 5 | Vertical stem tube | 1 | 38.1mm OD × 1.5mm wall 6061-T6 Al | 1050mm length | 1.5" EMT conduit is a drop-in alternative |
|
||||
<<<<<<< HEAD
|
||||
| 6 | MCU standoff M3×6mm nylon | 4 | Nylon | — | ESP32 BALANCE / IO board isolation (dimensions TBD) |
|
||||
=======
|
||||
| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE vibration isolation |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
|
||||
| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE vibration isolation || 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
|
||||
|
||||
### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B
|
||||
|
||||
@ -97,19 +92,10 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
|
||||
|
||||
| # | Part | Qty | Spec | Notes |
|
||||
|---|------|-----|------|-------|
|
||||
<<<<<<< HEAD
|
||||
| 13 | ESP32 BALANCE board | 1 | TBD — mount pattern TBD | PID balance loop; replaces ESP32 BALANCE |
|
||||
| 13b | ESP32 IO board | 1 | TBD — mount pattern TBD | Motor/sensor/comms I/O |
|
||||
| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | ESP32 board isolation |
|
||||
| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under ESP32 mount pads |
|
||||
| 16 | Jetson Orin module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
|
||||
=======
|
||||
| 13 | ESP32-S3 ESP32-S3 BALANCE FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front |
|
||||
| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation |
|
||||
| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads |
|
||||
| 16 | Jetson Orin Nano Super B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs |
|
||||
| 16 | Jetson Orin Nano Super B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern || 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs |
|
||||
|
||||
---
|
||||
|
||||
|
||||
@ -104,12 +104,7 @@ IP54-rated enclosures and sensor housings for all-weather outdoor robot operatio
|
||||
| Component | Thermal strategy | Max junction | Enclosure budget |
|
||||
|-----------|-----------------|-------------|-----------------|
|
||||
| Jetson Orin NX | Al pad → lid → fan forced convection | 95 °C Tj | Target ≤ 60 °C case |
|
||||
<<<<<<< HEAD
|
||||
| FC (ESP32 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
|
||||
=======
|
||||
| FC (ESP32-S3 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C |
|
||||
| FC (ESP32-S3 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK || ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C |
|
||||
| D435i | Passive; housing vent gap on rear cap | 45 °C surface | — |
|
||||
|
||||
Fan spec: 40 mm, 12 V, ≥10 CFM at 0.1" H₂O static pressure.
|
||||
|
||||
@ -4,24 +4,6 @@ You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read
|
||||
|
||||
## ⚠️ ARCHITECTURE — SAUL-TEE (finalised 2026-04-04)
|
||||
|
||||
<<<<<<< HEAD
|
||||
Full hardware spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` — **read it before writing firmware.**
|
||||
|
||||
| Board | Role |
|
||||
|-------|------|
|
||||
| **ESP32-S3 BALANCE** | Waveshare Touch LCD 1.28 (CH343 USB). QMI8658 IMU, PID loop, CAN→VESC L(68)/R(56), GC9A01 LCD |
|
||||
| **ESP32-S3 IO** | Bare devkit (JTAG USB). TBS Crossfire RC (UART0), ELRS failover (UART2), BTS7960 motors, NFC/baro/ToF, WS2812, buzzer/horn/headlight/fan |
|
||||
| **Jetson Orin** | CANable2 USB→CAN. Cmds on 0x300–0x303, telemetry on 0x400–0x401 |
|
||||
|
||||
```
|
||||
Jetson Orin ──CANable2──► CAN 500kbps ◄───────────────────────┐
|
||||
│ │
|
||||
ESP32-S3 BALANCE ←─UART 460800─► ESP32-S3 IO
|
||||
(QMI8658, PID loop) (BTS7960, RC, sensors)
|
||||
│ CAN 500kbps
|
||||
┌─────────┴──────────┐
|
||||
VESC Left (ID 68) VESC Right (ID 56)
|
||||
=======
|
||||
A hoverboard-based balancing robot with two compute layers:
|
||||
1. **ESP32-S3 BALANCE** — ESP32-S3 BALANCE (ESP32-S3RET6 + MPU6000 IMU). Runs a lean C balance loop at up to 8kHz. Talks UART to the hoverboard ESC. This is the safety-critical layer.
|
||||
2. **Jetson Orin Nano Super** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently.
|
||||
@ -33,12 +15,10 @@ Jetson (speed+steer via UART1) ←→ ELRS RC (UART3, kill switch)
|
||||
ESP32-S3 BALANCE (MPU6000 IMU, PID balance)
|
||||
│
|
||||
▼ UART2
|
||||
Hoverboard ESC (FOC) → 2× 8" hub motors
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
```
|
||||
Hoverboard ESC (FOC) → 2× 8" hub motors```
|
||||
|
||||
Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
|
||||
Legacy `src/` STM32 HAL code is **archived — do not extend.**
|
||||
Active firmware: `esp32/balance_fw/` (ESP32-S3 BALANCE) and `esp32/io_fw/` (ESP32-S3 IO).
|
||||
|
||||
## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT
|
||||
|
||||
@ -57,12 +37,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
|
||||
## Repository Layout
|
||||
|
||||
```
|
||||
<<<<<<< HEAD
|
||||
firmware/ # Legacy ESP32/STM32 HAL firmware (PlatformIO, archived)
|
||||
=======
|
||||
firmware/ # ESP-IDF firmware (PlatformIO)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
├── src/
|
||||
firmware/ # ESP-IDF firmware (PlatformIO)├── src/
|
||||
│ ├── main.c # Entry point, clock config, main loop
|
||||
│ ├── icm42688.c # QMI8658-P SPI driver (backup IMU — currently broken)
|
||||
│ ├── bmp280.c # Barometer driver (disabled)
|
||||
@ -108,25 +83,16 @@ PLATFORM.md # Hardware platform reference
|
||||
|
||||
## Hardware Quick Reference
|
||||
|
||||
<<<<<<< HEAD
|
||||
### ESP32 BALANCE Flight Controller
|
||||
|
||||
| Spec | Value |
|
||||
|------|-------|
|
||||
| MCU | ESP32RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
|
||||
=======
|
||||
### ESP32-S3 BALANCE Flight Controller
|
||||
|
||||
| Spec | Value |
|
||||
|------|-------|
|
||||
| MCU | ESP32-S3RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| Primary IMU | MPU6000 (WHO_AM_I = 0x68) |
|
||||
| MCU | ESP32-S3RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) || Primary IMU | MPU6000 (WHO_AM_I = 0x68) |
|
||||
| IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 |
|
||||
| IMU EXTI | PC4 (data ready interrupt) |
|
||||
| IMU Orientation | CW270 (Betaflight convention) |
|
||||
| Secondary IMU | QMI8658-P (on same SPI1, CS unknown — currently non-functional) |
|
||||
| Betaflight Target | DIAT-MAMBAF722_2022B |
|
||||
| Board Name | Waveshare ESP32-S3 Touch LCD 1.28 |
|
||||
| USB | OTG FS (PA11/PA12), enumerates as /dev/cu.usbmodemSALTY0011 |
|
||||
| VID/PID | 0x0483/0x5740 |
|
||||
| LEDs | PC15 (LED1), PC14 (LED2), active low |
|
||||
@ -194,12 +160,7 @@ PLATFORM.md # Hardware platform reference
|
||||
### Critical Lessons Learned (DON'T REPEAT THESE)
|
||||
|
||||
1. **SysTick_Handler with HAL_IncTick() is MANDATORY** — without it, HAL_Delay() and every HAL timeout hangs forever. This bricked us multiple times.
|
||||
<<<<<<< HEAD
|
||||
2. **DCache breaks SPI on ESP32** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.
|
||||
=======
|
||||
2. **DCache breaks SPI on ESP32-S3** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes.
|
||||
2. **DCache breaks SPI on ESP32-S3** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes.
|
||||
4. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
|
||||
5. **USB Serial (CH343) needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception.
|
||||
|
||||
@ -210,19 +171,14 @@ The firmware supports reboot-to-DFU via USB command:
|
||||
2. Firmware writes `0xDEADBEEF` to RTC backup register 0
|
||||
3. `NVIC_SystemReset()` — clean hardware reset
|
||||
4. On boot, `checkForBootloader()` (called after `HAL_Init()`) reads the magic
|
||||
<<<<<<< HEAD
|
||||
5. If magic found: clears it, remaps system memory, jumps to ESP32 BALANCE bootloader at `0x1FF00000`
|
||||
=======
|
||||
5. If magic found: clears it, remaps system memory, jumps to ESP32-S3 bootloader at `0x1FF00000`
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
6. Board appears as DFU device, ready for `dfu-util` flash
|
||||
5. If magic found: clears it, remaps system memory, jumps to ESP32-S3 bootloader at `0x1FF00000`6. Board appears as DFU device, ready for `dfu-util` flash
|
||||
|
||||
### Build & Flash
|
||||
|
||||
```bash
|
||||
cd firmware/
|
||||
python3 -m platformio run # Build
|
||||
dfu-util -a 0 -s 0x08000000:leave -D .pio/build/f722/firmware.bin # Flash
|
||||
esptool.py --port /dev/esp32-balance write_flash 0x0 firmware.bin # Flash
|
||||
```
|
||||
|
||||
Dev machine: mbpm4 (seb@192.168.87.40), PlatformIO project at `~/Projects/saltylab-firmware/`
|
||||
|
||||
@ -1,11 +1,6 @@
|
||||
# Face LCD Animation System (Issue #507)
|
||||
|
||||
<<<<<<< HEAD
|
||||
Implements expressive face animations on an ESP32 LCD display with 5 core emotions and smooth transitions.
|
||||
=======
|
||||
Implements expressive face animations on an ESP32-S3 LCD display with 5 core emotions and smooth transitions.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
## Features
|
||||
|
||||
### Emotions
|
||||
@ -86,12 +81,7 @@ STATUS → Echo current emotion + idle state
|
||||
- Colors: Monochrome (1-bit) or RGB565
|
||||
|
||||
### Microcontroller
|
||||
<<<<<<< HEAD
|
||||
- ESP32xx (ESP32 BALANCE)
|
||||
=======
|
||||
- ESP32-S3xx (ESP32-S3 BALANCE)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
- Available UART: USART3 (PB10=TX, PB11=RX)
|
||||
- ESP32-S3xx (ESP32-S3 BALANCE)- Available UART: USART3 (PB10=TX, PB11=RX)
|
||||
- Clock: 216 MHz
|
||||
|
||||
## Animation Timing
|
||||
|
||||
@ -102,11 +102,8 @@ balance loop, and drives the hoverboard ESC via UART. Jetson Orin Nano Super
|
||||
sends velocity commands over UART1. ELRS receiver on UART3 provides RC
|
||||
override and kill-switch capability.
|
||||
|
||||
The legacy STM32 firmware (Mamba F722S era) has been archived to
|
||||
=======
|
||||
The legacy STM32 firmware (STM32 era) has been archived to
|
||||
`legacy/stm32/` and is no longer built or deployed.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
## LED Subsystem (ESP32-C3)
|
||||
|
||||
|
||||
@ -1,26 +1,18 @@
|
||||
# SaltyLab / SAUL-TEE Wiring Reference
|
||||
|
||||
> ⚠️ **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.
|
||||
> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
|
||||
|
||||
---
|
||||
|
||||
## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
## System Overview
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ ORIN NANO SUPER │
|
||||
│ (Top Plate — 25W) │
|
||||
│ │
|
||||
<<<<<<< HEAD
|
||||
│ USB-A ──── CANable2 USB-CAN adapter (slcan0, 500 kbps) │
|
||||
│ USB-A ──── ESP32-S3 IO (/dev/esp32-io, 460800 baud) │
|
||||
=======
|
||||
│ USB-C ──── ESP32-S3 CDC (/dev/esp32-bridge, 921600 baud) │
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
│ USB-A1 ─── RealSense D435i (USB 3.1) │
|
||||
│ USB-C ──── ESP32-S3 CDC (/dev/esp32-bridge, 921600 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) │
|
||||
@ -38,14 +30,8 @@
|
||||
│ 500 kbps │
|
||||
▼ ▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
<<<<<<< HEAD
|
||||
│ ESP32-S3 BALANCE │
|
||||
│ (Waveshare Touch LCD 1.28, Middle Plate) │
|
||||
=======
|
||||
│ ESP32-S3 BALANCE (FC) │
|
||||
│ (Middle Plate — foam mounted) │
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
│ │
|
||||
│ (Middle Plate — foam mounted) ││ │
|
||||
│ 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 │
|
||||
@ -77,29 +63,16 @@
|
||||
|
||||
## Wire-by-Wire Connections
|
||||
|
||||
<<<<<<< HEAD
|
||||
### 1. Orin <-> ESP32-S3 BALANCE (Primary: CAN Bus via CANable2)
|
||||
=======
|
||||
### 1. Orin ↔ FC (Primary: USB Serial (CH343))
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
### 1. Orin ↔ ESP32-S3 BALANCE (Primary: USB Serial via CH343)
|
||||
| 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 |
|
||||
|
||||
<<<<<<< HEAD
|
||||
- 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)
|
||||
=======
|
||||
- Device: `/dev/ttyACM0` → symlink `/dev/esp32-bridge`
|
||||
- Baud: 921600, 8N1
|
||||
- Protocol: JSON telemetry (FC→Orin), ASCII commands (Orin→FC)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
### 2. Orin <-> ESP32-S3 BALANCE (Fallback: Hardware UART)
|
||||
|
||||
| Orin Pin | Signal | ESP32-S3 Pin | Notes |
|
||||
@ -164,47 +137,14 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
|
||||
| CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
|
||||
|
||||
|
||||
<<<<<<< HEAD
|
||||
## FC UART Summary (MAMBA F722S — OBSOLETE)
|
||||
|
||||
| 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).
|
||||
=======
|
||||
## FC UART Summary (ESP32-S3 BALANCE)
|
||||
|
||||
| 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 Serial (CH343) | USB-C | 921600 | Jetson primary | `/dev/esp32-bridge` |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
## ESP32-S3 BALANCE — UART Summary
|
||||
|
||||
| UART | GPIO Pins | Baud | Assignment | Notes |
|
||||
|------|-----------|------|------------|-------|
|
||||
| UART0 (CRSF primary) | IO44=RX, IO43=TX | 400000 | TBS Crossfire RC | via ESP32-S3 IO board |
|
||||
| UART1 (inter-board) | IO17=TX, IO18=RX | 460800 | ESP32-S3 IO ↔ BALANCE | binary `[0xAA][LEN][TYPE]` |
|
||||
| CAN (SN65HVD230) | IO43=TX, IO44=RX | 500 kbps | VESCs + Orin CANable2 | ISO 11898 |
|
||||
| USB Serial (CH343) | USB-C | 460800 | Orin primary | `/dev/balance-esp` |
|
||||
|
||||
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
|
||||
|
||||
@ -263,14 +203,7 @@ VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44).
|
||||
|
||||
| Device | Interface | Power Draw |
|
||||
|--------|-----------|------------|
|
||||
<<<<<<< HEAD
|
||||
| CANable2 USB-CAN | USB-A | ~0.5W |
|
||||
| ESP32-S3 BALANCE | USB-C | ~0.8W (WiFi off) |
|
||||
| ESP32-S3 IO | USB-C | ~0.5W |
|
||||
=======
|
||||
| ESP32-S3 FC (CDC) | USB-C | ~0.5W (data only, FC on 5V bus) |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| RealSense D435i | USB-A | ~1.5W (3.5W peak) |
|
||||
| ESP32-S3 BALANCE (CH343) | USB-C | ~0.5W (data only, BALANCE on 5V bus) || 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-A | ~0.5W |
|
||||
@ -294,25 +227,14 @@ Orin Nano Super delivers up to 25W --- USB peripherals are well within budget.
|
||||
└──────┬───────┘
|
||||
│ UART
|
||||
┌────────────▼────────────┐
|
||||
<<<<<<< HEAD
|
||||
│ ESP32-S3 BALANCE │
|
||||
│ (Waveshare LCD 1.28) │
|
||||
=======
|
||||
│ ESP32-S3 BALANCE │
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
│ │
|
||||
│ ESP32-S3 BALANCE │ │ │
|
||||
│ QMI8658 -> Balance PID │
|
||||
│ RC -> Mode Manager │
|
||||
│ Safety Monitor │
|
||||
│ │
|
||||
└──┬──────────┬───────────┘
|
||||
<<<<<<< HEAD
|
||||
CAN 500kbps─┘ └───── CAN bus / UART fallback
|
||||
=======
|
||||
USART2 ─────┘ └───── USB Serial (CH343) / USART6
|
||||
26400 baud 921600 baud
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
│ │
|
||||
26400 baud 921600 baud │ │
|
||||
┌────┴────────────┐ ▼
|
||||
│ CAN bus (500k) │ ┌───────────────────┐
|
||||
├─ VESC Left 56 │ │ Orin Nano Super │
|
||||
|
||||
@ -14,12 +14,7 @@ Self-balancing robot: Jetson Orin Nano Super dev environment for ROS2 Humble + S
|
||||
| Nav | Nav2 |
|
||||
| Depth camera | Intel RealSense D435i |
|
||||
| LiDAR | RPLIDAR A1M8 |
|
||||
<<<<<<< HEAD
|
||||
| MCU bridge | ESP32 (USB CDC @ 921600) |
|
||||
=======
|
||||
| MCU bridge | ESP32-S3 (USB Serial (CH343) @ 921600) |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
## Quick Start
|
||||
|
||||
```bash
|
||||
@ -46,12 +41,7 @@ bash scripts/build-and-run.sh shell
|
||||
```
|
||||
jetson/
|
||||
├── Dockerfile # L4T base + ROS2 Humble + SLAM packages
|
||||
<<<<<<< HEAD
|
||||
├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32 BALANCE)
|
||||
=======
|
||||
├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32-S3)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
├── README.md # This file
|
||||
├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32-S3)├── README.md # This file
|
||||
├── docs/
|
||||
│ ├── pinout.md # GPIO/I2C/UART pinout reference
|
||||
│ └── power-budget.md # Power budget analysis (10W envelope)
|
||||
|
||||
@ -34,12 +34,7 @@ Recovery behaviors are triggered when Nav2 encounters navigation failures (path
|
||||
|
||||
The emergency stop system (Issue #459, `saltybot_emergency` package) runs independently of Nav2 and takes absolute priority.
|
||||
|
||||
<<<<<<< HEAD
|
||||
Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32 BALANCE firmware.
|
||||
=======
|
||||
Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32-S3 firmware.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
## Behavior Tree Sequence
|
||||
|
||||
Recovery runs in a round-robin fashion with up to 6 retry cycles.
|
||||
|
||||
@ -12,12 +12,7 @@
|
||||
# /scan — RPLIDAR A1M8 (obstacle layer)
|
||||
# /camera/depth/color/points — RealSense D435i (voxel layer)
|
||||
#
|
||||
<<<<<<< HEAD
|
||||
# Output: /cmd_vel (Twist) — ESP32 bridge consumes this topic.
|
||||
=======
|
||||
# Output: /cmd_vel (Twist) — ESP32-S3 bridge consumes this topic.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
@ -97,12 +97,7 @@ services:
|
||||
rgb_camera.profile:=640x480x30
|
||||
"
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ── ESP32 bridge node (bidirectional serial<->ROS2) ────────────────────────
|
||||
=======
|
||||
# ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ────────────────────────
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
esp32-bridge:
|
||||
# ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ──────────────────────── esp32-bridge:
|
||||
image: saltybot/ros2-humble:jetson-orin
|
||||
build:
|
||||
context: .
|
||||
@ -212,14 +207,8 @@ services:
|
||||
"
|
||||
|
||||
|
||||
<<<<<<< HEAD
|
||||
# -- Remote e-stop bridge (MQTT over 4G -> ESP32 CDC) ----------------------
|
||||
# Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32 BALANCE.
|
||||
=======
|
||||
# -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ----------------------
|
||||
# Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
|
||||
# Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3. # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
|
||||
remote-estop:
|
||||
image: saltybot/ros2-humble:jetson-orin
|
||||
build:
|
||||
|
||||
@ -1,10 +1,5 @@
|
||||
# Jetson Orin Nano Super — GPIO / I2C / UART / CSI Pinout Reference
|
||||
<<<<<<< HEAD
|
||||
## Self-Balancing Robot: ESP32 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
|
||||
=======
|
||||
## Self-Balancing Robot: ESP32-S3 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Last updated: 2026-02-28
|
||||
JetPack version: 6.x (L4T R36.x / Ubuntu 22.04)
|
||||
|
||||
@ -47,50 +42,26 @@ i2cdetect -l
|
||||
|
||||
---
|
||||
|
||||
<<<<<<< HEAD
|
||||
## 1. ESP32 Bridge (USB CDC — Primary)
|
||||
|
||||
The ESP32 BALANCE acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**.
|
||||
=======
|
||||
## 1. ESP32-S3 Bridge (USB Serial (CH343) — Primary)
|
||||
|
||||
The ESP32-S3 acts as a real-time motor + IMU controller. Communication is via **USB Serial (CH343) serial**.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
### USB Serial (CH343) Connection
|
||||
| Connection | Detail |
|
||||
|-----------|--------|
|
||||
<<<<<<< HEAD
|
||||
| Interface | USB on ESP32 BALANCE board → USB-A on Jetson |
|
||||
| Device node | `/dev/ttyACM0` → symlink `/dev/esp32-bridge` (via udev) |
|
||||
| Baud rate | 921600 (configured in ESP32 BALANCE firmware) |
|
||||
=======
|
||||
| Interface | USB Micro-B on ESP32-S3 dev board → USB-A on Jetson |
|
||||
| Device node | `/dev/ttyACM0` → symlink `/dev/esp32-bridge` (via udev) |
|
||||
| Baud rate | 921600 (configured in ESP32-S3 firmware) |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) |
|
||||
| Baud rate | 921600 (configured in ESP32-S3 firmware) || Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) |
|
||||
| Power | Powered via robot 5V bus (data-only via USB) |
|
||||
|
||||
### Hardware UART (Fallback — 40-pin header)
|
||||
<<<<<<< HEAD
|
||||
| Jetson Pin | Signal | ESP32 Pin | Notes |
|
||||
=======
|
||||
| Jetson Pin | Signal | ESP32-S3 Pin | Notes |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|-----------|--------|-----------|-------|
|
||||
| Jetson Pin | Signal | ESP32-S3 Pin | Notes ||-----------|--------|-----------|-------|
|
||||
| Pin 8 (TXD0) | TX → | PA10 (UART1 RX) | Cross-connect TX→RX |
|
||||
| Pin 10 (RXD0) | RX ← | PA9 (UART1 TX) | Cross-connect RX→TX |
|
||||
| Pin 6 (GND) | GND | GND | Common ground **required** |
|
||||
|
||||
**Jetson device node:** `/dev/ttyTHS0`
|
||||
**Baud rate:** 921600, 8N1
|
||||
<<<<<<< HEAD
|
||||
**Voltage level:** 3.3V — both Jetson Orin and ESP32 are 3.3V GPIO
|
||||
=======
|
||||
**Voltage level:** 3.3V — both Jetson Orin and ESP32-S3 are 3.3V GPIO
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
```bash
|
||||
# Verify UART
|
||||
ls /dev/ttyTHS0
|
||||
@ -99,15 +70,6 @@ sudo usermod -aG dialout $USER
|
||||
picocom -b 921600 /dev/ttyTHS0
|
||||
```
|
||||
|
||||
<<<<<<< HEAD
|
||||
**ROS2 topics (ESP32 bridge node):**
|
||||
| ROS2 Topic | Direction | Content |
|
||||
|-----------|-----------|---------
|
||||
| `/saltybot/imu` | ESP32 BALANCE→Jetson | IMU data (accel, gyro) at 50Hz |
|
||||
| `/saltybot/balance_state` | ESP32 BALANCE→Jetson | Motor cmd, pitch, state |
|
||||
| `/cmd_vel` | Jetson→ESP32 BALANCE | Velocity commands → `C<spd>,<str>\n` |
|
||||
| `/saltybot/estop` | Jetson→ESP32 BALANCE | Emergency stop |
|
||||
=======
|
||||
**ROS2 topics (ESP32-S3 bridge node):**
|
||||
| ROS2 Topic | Direction | Content |
|
||||
|-----------|-----------|---------
|
||||
@ -115,8 +77,6 @@ picocom -b 921600 /dev/ttyTHS0
|
||||
| `/saltybot/balance_state` | ESP32-S3→Jetson | Motor cmd, pitch, state |
|
||||
| `/cmd_vel` | Jetson→ESP32-S3 | Velocity commands → `C<spd>,<str>\n` |
|
||||
| `/saltybot/estop` | Jetson→ESP32-S3 | Emergency stop |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
---
|
||||
|
||||
## 2. RealSense D435i (USB 3.1)
|
||||
@ -300,12 +260,7 @@ sudo mkdir -p /mnt/nvme
|
||||
|------|------|----------|
|
||||
| USB-A (top, blue) | USB 3.1 Gen 1 | RealSense D435i |
|
||||
| USB-A (bottom) | USB 2.0 | RPLIDAR (via USB-UART adapter) |
|
||||
<<<<<<< HEAD
|
||||
| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32 CDC or host flash |
|
||||
=======
|
||||
| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32-S3 CDC or host flash |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| Micro-USB | Debug/flash | JetPack flash only |
|
||||
| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32-S3 CDC or host flash || Micro-USB | Debug/flash | JetPack flash only |
|
||||
|
||||
---
|
||||
|
||||
@ -315,18 +270,10 @@ sudo mkdir -p /mnt/nvme
|
||||
|-------------|----------|---------|----------|
|
||||
| 3 | SDA1 | 3.3V | I2C data (i2c-7) |
|
||||
| 5 | SCL1 | 3.3V | I2C clock (i2c-7) |
|
||||
<<<<<<< HEAD
|
||||
| 8 | TXD0 | 3.3V | UART TX → ESP32 BALANCE (fallback) |
|
||||
| 10 | RXD0 | 3.3V | UART RX ← ESP32 BALANCE (fallback) |
|
||||
| USB-A ×2 | — | 5V | D435i, RPLIDAR |
|
||||
| USB-C | — | 5V | ESP32 CDC |
|
||||
=======
|
||||
| 8 | TXD0 | 3.3V | UART TX → ESP32-S3 (fallback) |
|
||||
| 10 | RXD0 | 3.3V | UART RX ← ESP32-S3 (fallback) |
|
||||
| USB-A ×2 | — | 5V | D435i, RPLIDAR |
|
||||
| USB-C | — | 5V | ESP32-S3 CDC |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left |
|
||||
| USB-C | — | 5V | ESP32-S3 CDC || CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left |
|
||||
| CSI-B (J8) | MIPI CSI-2 | — | Cameras rear + right |
|
||||
| M.2 Key M | PCIe Gen3 ×4 | — | NVMe SSD |
|
||||
|
||||
@ -343,12 +290,7 @@ Apply stable device names:
|
||||
KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
|
||||
SYMLINK+="rplidar", MODE="0666"
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ESP32 USB CDC (STMicroelectronics)
|
||||
=======
|
||||
# ESP32-S3 USB Serial (CH343) (STMicroelectronics)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
||||
# ESP32-S3 USB Serial (CH343) (STMicroelectronics)KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
||||
SYMLINK+="esp32-bridge", MODE="0666"
|
||||
|
||||
# Intel RealSense D435i
|
||||
|
||||
@ -56,12 +56,7 @@ sudo jtop
|
||||
|-----------|----------|------------|----------|-----------|-------|
|
||||
| RealSense D435i | 0.3 | 1.5 | 3.5 | USB 3.1 | Peak during boot/init |
|
||||
| RPLIDAR A1M8 | 0.4 | 2.6 | 3.0 | USB (UART adapter) | Motor spinning |
|
||||
<<<<<<< HEAD
|
||||
| ESP32 bridge | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V |
|
||||
=======
|
||||
| ESP32-S3 bridge | 0.0 | 0.0 | 0.0 | USB Serial (CH343) | Self-powered from robot 5V |
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
| 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active |
|
||||
| ESP32-S3 bridge | 0.0 | 0.0 | 0.0 | USB Serial (CH343) | Self-powered from robot 5V || 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active |
|
||||
| **Peripheral Subtotal** | **0.9** | **6.1** | **8.9** | | |
|
||||
|
||||
### Total System (from Jetson 5V barrel jack)
|
||||
@ -155,12 +150,7 @@ LiPo 4S (16.8V max)
|
||||
├─► DC-DC Buck → 5V 6A ──► Jetson Orin barrel jack (30W)
|
||||
│ (e.g., XL4016E1)
|
||||
│
|
||||
<<<<<<< HEAD
|
||||
├─► DC-DC Buck → 5V 3A ──► ESP32 + logic 5V rail
|
||||
=======
|
||||
├─► DC-DC Buck → 5V 3A ──► ESP32-S3 + logic 5V rail
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
│
|
||||
├─► DC-DC Buck → 5V 3A ──► ESP32-S3 + logic 5V rail │
|
||||
└─► Hoverboard ESC ──► Hub motors (48V loop)
|
||||
```
|
||||
|
||||
|
||||
@ -11,12 +11,7 @@ reconnect_delay: 2.0 # seconds between reconnect attempts on serial disconne
|
||||
# ── saltybot_cmd_node (bidirectional) only ─────────────────────────────────────
|
||||
|
||||
# Heartbeat: H\n sent every heartbeat_period seconds.
|
||||
<<<<<<< HEAD
|
||||
# ESP32 BALANCE reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
|
||||
=======
|
||||
# ESP32-S3 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
heartbeat_period: 0.2 # seconds (= 200ms)
|
||||
# ESP32-S3 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.heartbeat_period: 0.2 # seconds (= 200ms)
|
||||
|
||||
# Twist → ESC command scaling
|
||||
# speed = clamp(linear.x * speed_scale, -1000, 1000) [m/s → ESC units]
|
||||
|
||||
@ -1,10 +1,5 @@
|
||||
# cmd_vel_bridge_params.yaml
|
||||
<<<<<<< HEAD
|
||||
# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32 BALANCE autonomous drive.
|
||||
=======
|
||||
# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 autonomous drive.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
#
|
||||
# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 autonomous drive.#
|
||||
# Run with:
|
||||
# ros2 launch saltybot_bridge cmd_vel_bridge.launch.py
|
||||
# Or override individual params:
|
||||
@ -18,12 +13,7 @@ timeout: 0.05 # serial readline timeout (s)
|
||||
reconnect_delay: 2.0 # seconds between reconnect attempts
|
||||
|
||||
# ── Heartbeat ──────────────────────────────────────────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
# ESP32 BALANCE jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).
|
||||
=======
|
||||
# ESP32-S3 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# Keep heartbeat well below that threshold.
|
||||
# ESP32-S3 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).# Keep heartbeat well below that threshold.
|
||||
heartbeat_period: 0.2 # seconds (200ms)
|
||||
|
||||
# ── Velocity limits ────────────────────────────────────────────────────────────
|
||||
@ -58,9 +48,4 @@ ramp_rate: 500 # ESC units/second
|
||||
# ── Deadman switch ─────────────────────────────────────────────────────────────
|
||||
# If /cmd_vel is not received for this many seconds, target speed/steer are
|
||||
# zeroed immediately. The ramp then drives the robot to a stop.
|
||||
<<<<<<< HEAD
|
||||
# 500ms matches the ESP32 BALANCE jetson heartbeat timeout for consistency.
|
||||
=======
|
||||
# 500ms matches the ESP32-S3 jetson heartbeat timeout for consistency.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
cmd_vel_timeout: 0.5 # seconds
|
||||
# 500ms matches the ESP32-S3 jetson heartbeat timeout for consistency.cmd_vel_timeout: 0.5 # seconds
|
||||
|
||||
@ -1,21 +1,3 @@
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml
|
||||
# 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-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 (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
|
||||
=======
|
||||
# esp32_cmd_params.yaml — Configuration for esp32_cmd_node (Issue #119)
|
||||
# Binary-framed Jetson↔ESP32-S3 bridge at 921600 baud.
|
||||
|
||||
@ -46,4 +28,3 @@ watchdog_timeout: 0.5 # 500ms
|
||||
# Tune speed_scale to set the physical top speed.
|
||||
speed_scale: 1000.0
|
||||
steer_scale: -500.0
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/config/esp32_cmd_params.yaml
|
||||
|
||||
@ -6,12 +6,7 @@ Two deployment modes:
|
||||
1. Full bidirectional (recommended for Nav2):
|
||||
ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional
|
||||
Starts saltybot_cmd_node — owns serial port, handles both RX telemetry
|
||||
<<<<<<< HEAD
|
||||
and TX /cmd_vel → ESP32 BALANCE commands + heartbeat.
|
||||
=======
|
||||
and TX /cmd_vel → ESP32-S3 commands + heartbeat.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
2. RX-only (telemetry monitor, no drive commands):
|
||||
ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only
|
||||
Starts serial_bridge_node — telemetry RX only. Use when you want to
|
||||
@ -69,12 +64,7 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument("mode", default_value="bidirectional",
|
||||
description="bidirectional | rx_only"),
|
||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0",
|
||||
<<<<<<< HEAD
|
||||
description="ESP32 USB CDC device node"),
|
||||
=======
|
||||
description="ESP32-S3 USB CDC device node"),
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
||||
description="ESP32-S3 USB CDC device node"), DeclareLaunchArgument("baud_rate", default_value="921600"),
|
||||
DeclareLaunchArgument("speed_scale", default_value="1000.0",
|
||||
description="m/s → ESC units (linear.x scale)"),
|
||||
DeclareLaunchArgument("steer_scale", default_value="-500.0",
|
||||
|
||||
@ -1,19 +1,9 @@
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32 BALANCE autonomous drive bridge.
|
||||
=======
|
||||
cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32-S3 autonomous drive bridge.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Starts cmd_vel_bridge_node, which owns the serial port exclusively and provides:
|
||||
- /cmd_vel subscription with velocity limits + smooth ramp
|
||||
- Deadman switch (zero speed if /cmd_vel silent > cmd_vel_timeout)
|
||||
<<<<<<< HEAD
|
||||
- Mode gate (drives only when ESP32 BALANCE is in AUTONOMOUS mode, md=2)
|
||||
=======
|
||||
- Mode gate (drives only when ESP32-S3 is in AUTONOMOUS mode, md=2)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
- Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics
|
||||
- Mode gate (drives only when ESP32-S3 is in AUTONOMOUS mode, md=2) - Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics
|
||||
- /saltybot/cmd publisher (observability)
|
||||
|
||||
Do NOT run alongside serial_bridge_node or saltybot_cmd_node on the same port.
|
||||
@ -80,21 +70,11 @@ def generate_launch_description():
|
||||
description="Full path to cmd_vel_bridge_params.yaml (overrides inline args)"),
|
||||
DeclareLaunchArgument(
|
||||
"serial_port", default_value="/dev/ttyACM0",
|
||||
<<<<<<< HEAD
|
||||
description="ESP32 USB CDC device node"),
|
||||
=======
|
||||
description="ESP32-S3 USB CDC device node"),
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
DeclareLaunchArgument(
|
||||
description="ESP32-S3 USB CDC device node"), DeclareLaunchArgument(
|
||||
"baud_rate", default_value="921600"),
|
||||
DeclareLaunchArgument(
|
||||
"heartbeat_period",default_value="0.2",
|
||||
<<<<<<< HEAD
|
||||
description="Heartbeat interval (s); must be < ESP32 BALANCE HB timeout (0.5s)"),
|
||||
=======
|
||||
description="Heartbeat interval (s); must be < ESP32-S3 HB timeout (0.5s)"),
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
DeclareLaunchArgument(
|
||||
description="Heartbeat interval (s); must be < ESP32-S3 HB timeout (0.5s)"), DeclareLaunchArgument(
|
||||
"max_linear_vel", default_value="0.5",
|
||||
description="Hard speed cap before scaling (m/s)"),
|
||||
DeclareLaunchArgument(
|
||||
|
||||
@ -1,16 +1,3 @@
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
|
||||
"""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:
|
||||
ros2 launch saltybot_bridge stm32_cmd.launch.py
|
||||
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
|
||||
=======
|
||||
"""esp32_cmd.launch.py — Launch the binary-framed ESP32-S3 command node (Issue #119).
|
||||
|
||||
Usage:
|
||||
@ -21,9 +8,7 @@ Usage:
|
||||
ros2 launch saltybot_bridge esp32_cmd.launch.py serial_port:=/dev/ttyACM1
|
||||
|
||||
# Custom velocity scales:
|
||||
ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/launch/esp32_cmd.launch.py
|
||||
"""
|
||||
ros2 launch saltybot_bridge esp32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
@ -2,12 +2,7 @@
|
||||
uart_bridge.launch.py — FC↔Orin UART bridge (Issue #362)
|
||||
|
||||
Launches serial_bridge_node configured for Jetson Orin UART port.
|
||||
<<<<<<< HEAD
|
||||
Bridges Flight Controller (ESP32) telemetry from /dev/ttyTHS1 into ROS2.
|
||||
=======
|
||||
Bridges Flight Controller (ESP32-S3) telemetry from /dev/ttyTHS1 into ROS2.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Published topics (same as USB CDC bridge):
|
||||
/saltybot/imu sensor_msgs/Imu — pitch/roll/yaw as angular velocity
|
||||
/saltybot/balance_state std_msgs/String (JSON) — full PID diagnostics
|
||||
@ -24,12 +19,7 @@ Usage:
|
||||
|
||||
Prerequisites:
|
||||
- Flight Controller connected to /dev/ttyTHS1 @ 921600 baud
|
||||
<<<<<<< HEAD
|
||||
- ESP32 BALANCE firmware transmitting JSON telemetry frames (50 Hz)
|
||||
=======
|
||||
- ESP32-S3 firmware transmitting JSON telemetry frames (50 Hz)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
- ROS2 environment sourced (source install/setup.bash)
|
||||
- ESP32-S3 firmware transmitting JSON telemetry frames (50 Hz) - ROS2 environment sourced (source install/setup.bash)
|
||||
|
||||
Note:
|
||||
/dev/ttyTHS1 is the native UART1 on Jetson Orin. Verify connectivity:
|
||||
|
||||
@ -14,12 +14,7 @@ Alert levels (SoC thresholds):
|
||||
5% EMERGENCY — publish zero /cmd_vel, disarm, log + alert
|
||||
|
||||
SoC source priority:
|
||||
<<<<<<< HEAD
|
||||
1. soc_pct field from ESP32 BATTERY telemetry (fuel gauge or lookup on ESP32 BALANCE)
|
||||
=======
|
||||
1. soc_pct field from ESP32-S3 BATTERY telemetry (fuel gauge or lookup on ESP32-S3)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known
|
||||
1. soc_pct field from ESP32-S3 BATTERY telemetry (fuel gauge or lookup on ESP32-S3) 2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known
|
||||
|
||||
Parameters (config/battery_params.yaml):
|
||||
db_path /var/log/saltybot/battery.db
|
||||
@ -324,12 +319,7 @@ class BatteryNode(Node):
|
||||
self._speed_limit_pub.publish(msg)
|
||||
|
||||
def _execute_safe_stop(self) -> None:
|
||||
<<<<<<< HEAD
|
||||
"""Send zero /cmd_vel and disarm the ESP32 BALANCE."""
|
||||
=======
|
||||
"""Send zero /cmd_vel and disarm the ESP32-S3."""
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming")
|
||||
"""Send zero /cmd_vel and disarm the ESP32-S3.""" self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming")
|
||||
# Publish zero velocity
|
||||
zero_twist = Twist()
|
||||
self._cmd_vel_pub.publish(zero_twist)
|
||||
|
||||
@ -1,10 +1,5 @@
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32 BALANCE drive command bridge.
|
||||
=======
|
||||
cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32-S3 drive command bridge.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Extends the basic saltybot_cmd_node with four additions required for safe
|
||||
autonomous operation on a self-balancing robot:
|
||||
|
||||
@ -16,28 +11,16 @@ autonomous operation on a self-balancing robot:
|
||||
3. Deadman switch — if /cmd_vel is silent for cmd_vel_timeout seconds,
|
||||
zero targets immediately (Nav2 node crash / planner
|
||||
stall → robot coasts to stop rather than running away).
|
||||
<<<<<<< HEAD
|
||||
4. Mode gate — only issue non-zero drive commands when ESP32 BALANCE reports
|
||||
=======
|
||||
4. Mode gate — only issue non-zero drive commands when ESP32-S3 reports
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
md=2 (AUTONOMOUS). In any other mode (RC_MANUAL,
|
||||
4. Mode gate — only issue non-zero drive commands when ESP32-S3 reports md=2 (AUTONOMOUS). In any other mode (RC_MANUAL,
|
||||
RC_ASSISTED) Jetson cannot override the RC pilot.
|
||||
On mode re-entry current ramp state resets to 0 so
|
||||
acceleration is always smooth from rest.
|
||||
|
||||
Serial protocol (C<speed>,<steer>\\n / H\\n — same as saltybot_cmd_node):
|
||||
C<spd>,<str>\\n — drive command. speed/steer: -1000..+1000 integers.
|
||||
<<<<<<< HEAD
|
||||
H\\n — heartbeat. ESP32 BALANCE reverts steer to 0 after 500ms silence.
|
||||
|
||||
Telemetry (50 Hz from ESP32 BALANCE):
|
||||
=======
|
||||
H\\n — heartbeat. ESP32-S3 reverts steer to 0 after 500ms silence.
|
||||
|
||||
Telemetry (50 Hz from ESP32-S3):
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Same RX/publish pipeline as saltybot_cmd_node.
|
||||
Telemetry (50 Hz from ESP32-S3): Same RX/publish pipeline as saltybot_cmd_node.
|
||||
The "md" field (0=MANUAL,1=ASSISTED,2=AUTO) is parsed for the mode gate.
|
||||
|
||||
Topics published:
|
||||
@ -164,12 +147,7 @@ class CmdVelBridgeNode(Node):
|
||||
self._open_serial()
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
# Telemetry read at 100 Hz (ESP32 BALANCE sends at 50 Hz)
|
||||
=======
|
||||
# Telemetry read at 100 Hz (ESP32-S3 sends at 50 Hz)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self._read_timer = self.create_timer(0.01, self._read_cb)
|
||||
# Telemetry read at 100 Hz (ESP32-S3 sends at 50 Hz) self._read_timer = self.create_timer(0.01, self._read_cb)
|
||||
# Control loop at 50 Hz: ramp + deadman + mode gate + send
|
||||
self._control_timer = self.create_timer(1.0 / _CONTROL_HZ, self._control_cb)
|
||||
# Heartbeat TX
|
||||
@ -256,12 +234,7 @@ class CmdVelBridgeNode(Node):
|
||||
speed = self._current_speed
|
||||
steer = self._current_steer
|
||||
|
||||
<<<<<<< HEAD
|
||||
# Send to ESP32 BALANCE
|
||||
=======
|
||||
# Send to ESP32-S3
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
frame = f"C{speed},{steer}\n".encode("ascii")
|
||||
# Send to ESP32-S3 frame = f"C{speed},{steer}\n".encode("ascii")
|
||||
if not self._write(frame):
|
||||
self.get_logger().warn(
|
||||
"Cannot send cmd — serial not open",
|
||||
@ -278,12 +251,7 @@ class CmdVelBridgeNode(Node):
|
||||
# ── Heartbeat TX ──────────────────────────────────────────────────────────
|
||||
|
||||
def _heartbeat_cb(self):
|
||||
<<<<<<< HEAD
|
||||
"""H\\n keeps ESP32 BALANCE jetson_cmd heartbeat alive regardless of mode."""
|
||||
=======
|
||||
"""H\\n keeps ESP32-S3 jetson_cmd heartbeat alive regardless of mode."""
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self._write(b"H\n")
|
||||
"""H\\n keeps ESP32-S3 jetson_cmd heartbeat alive regardless of mode.""" self._write(b"H\n")
|
||||
|
||||
# ── Telemetry RX ──────────────────────────────────────────────────────────
|
||||
|
||||
@ -404,12 +372,7 @@ class CmdVelBridgeNode(Node):
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
=======
|
||||
status.hardware_id = "esp32s322"
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
status.message = f"{state_label} [{mode_label}]"
|
||||
status.hardware_id = "esp32s322" status.message = f"{state_label} [{mode_label}]"
|
||||
status.level = (
|
||||
DiagnosticStatus.OK if state == 1 else
|
||||
DiagnosticStatus.WARN if state == 0 else
|
||||
@ -436,20 +399,11 @@ class CmdVelBridgeNode(Node):
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"ESP32 BALANCE IMU fault: errno={errno}")
|
||||
=======
|
||||
status.hardware_id = "esp32s322"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"ESP32-S3 IMU fault: errno={errno}")
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self):
|
||||
|
||||
@ -1,31 +1,15 @@
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.
|
||||
=======
|
||||
"""esp32_cmd_node.py — Full bidirectional binary-framed ESP32-S3↔Jetson bridge.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
|
||||
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).
|
||||
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
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.
|
||||
=======
|
||||
TX commands (Jetson → ESP32-S3):
|
||||
SPEED_STEER — 50 Hz from /cmd_vel subscription
|
||||
HEARTBEAT — 200 ms timer (ESP32-S3 watchdog fires at 500 ms)
|
||||
ARM — via /saltybot/arm service
|
||||
SET_MODE — via /saltybot/set_mode service
|
||||
PID_UPDATE — via /saltybot/pid_update topic
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
|
||||
Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud
|
||||
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
RX from ESP32 IO:
|
||||
RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON)
|
||||
SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON)
|
||||
=======
|
||||
RX telemetry (ESP32-S3 → Jetson):
|
||||
IMU → /saltybot/imu (sensor_msgs/Imu)
|
||||
BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON)
|
||||
@ -33,20 +17,11 @@ RX telemetry (ESP32-S3 → Jetson):
|
||||
ARM_STATE → /saltybot/arm_state (std_msgs/String JSON)
|
||||
ERROR → /saltybot/error (std_msgs/String JSON)
|
||||
All frames → /diagnostics (diagnostic_msgs/DiagnosticArray)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
|
||||
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)
|
||||
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
Parameters (config/stm32_cmd_params.yaml):
|
||||
serial_port /dev/esp32-io
|
||||
baud_rate 460800
|
||||
reconnect_delay 2.0
|
||||
heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms)
|
||||
=======
|
||||
Parameters (config/esp32_cmd_params.yaml):
|
||||
serial_port /dev/ttyACM0
|
||||
baud_rate 921600
|
||||
@ -54,9 +29,7 @@ Parameters (config/esp32_cmd_params.yaml):
|
||||
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)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
"""
|
||||
steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention)"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@ -73,13 +46,7 @@ import serial
|
||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
from std_msgs.msg import String
|
||||
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
from .stm32_protocol import (
|
||||
BAUD_RATE,
|
||||
=======
|
||||
from .esp32_protocol import (
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
FrameParser,
|
||||
from .esp32_protocol import ( FrameParser,
|
||||
RcChannels,
|
||||
SensorData,
|
||||
encode_heartbeat,
|
||||
@ -88,13 +55,8 @@ from .esp32_protocol import (
|
||||
)
|
||||
|
||||
|
||||
class Stm32CmdNode(Node):
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
"""Orin ↔ ESP32-S3 IO auxiliary bridge node."""
|
||||
=======
|
||||
class Esp32CmdNode(Node):
|
||||
"""Binary-framed Jetson↔ESP32-S3 bridge node."""
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("esp32_cmd_node")
|
||||
|
||||
@ -138,13 +100,8 @@ class Stm32CmdNode(Node):
|
||||
self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
|
||||
|
||||
self.get_logger().info(
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
|
||||
=======
|
||||
f"esp32_cmd_node started — {port} @ {baud} baud | "
|
||||
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
)
|
||||
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms" )
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────
|
||||
|
||||
@ -245,9 +202,6 @@ class Stm32CmdNode(Node):
|
||||
type_code, _ = msg
|
||||
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
|
||||
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
# ── TX ────────────────────────────────────────────────────────────────
|
||||
=======
|
||||
elif isinstance(frame, ArmStateFrame):
|
||||
self._publish_arm_state(frame, now)
|
||||
|
||||
@ -358,8 +312,6 @@ class Stm32CmdNode(Node):
|
||||
"SPEED_STEER dropped — serial not open",
|
||||
throttle_duration_sec=2.0,
|
||||
)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
|
||||
def _heartbeat_cb(self) -> None:
|
||||
self._write(encode_heartbeat())
|
||||
|
||||
@ -399,14 +351,8 @@ class Stm32CmdNode(Node):
|
||||
diag = DiagnosticArray()
|
||||
diag.header.stamp = self.get_clock().now().to_msg()
|
||||
status = DiagnosticStatus()
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
status.name = "saltybot/esp32_io_bridge"
|
||||
status.hardware_id = "esp32-s3-io"
|
||||
=======
|
||||
status.name = "saltybot/esp32_cmd_node"
|
||||
status.hardware_id = "esp32s322"
|
||||
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
port_ok = self._ser is not None and self._ser.is_open
|
||||
status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
|
||||
status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
|
||||
@ -436,7 +382,7 @@ class Stm32CmdNode(Node):
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = Stm32CmdNode()
|
||||
node = Esp32CmdNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
|
||||
@ -1,16 +1,8 @@
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32 USB CDC
|
||||
|
||||
{"kill": true} -> writes 'E\n' to ESP32 BALANCE (ESTOP_REMOTE, immediate motor cutoff)
|
||||
{"kill": false} -> writes 'Z\n' to ESP32 BALANCE (clear latch, robot can re-arm)
|
||||
=======
|
||||
remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32-S3 USB CDC
|
||||
|
||||
{"kill": true} -> writes 'E\n' to ESP32-S3 (ESTOP_REMOTE, immediate motor cutoff)
|
||||
{"kill": false} -> writes 'Z\n' to ESP32-S3 (clear latch, robot can re-arm)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in
|
||||
AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT).
|
||||
|
||||
|
||||
@ -9,13 +9,13 @@ back to FC over CAN.
|
||||
CAN interface: SocketCAN (CANable USB adapter on vcan0 / can0)
|
||||
|
||||
FC → Orin (telemetry):
|
||||
0x400 FC_STATUS int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
|
||||
0x400 BALANCE_STATUS int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
|
||||
uint8 balance_state, uint8 flags (10 Hz)
|
||||
0x401 FC_VESC int16 left_rpm_x10, int16 right_rpm_x10,
|
||||
0x401 BALANCE_VESC int16 left_rpm_x10, int16 right_rpm_x10,
|
||||
int16 left_cur_x10, int16 right_cur_x10 (10 Hz)
|
||||
0x402 FC_IMU int16 pitch_x10, int16 roll_x10, int16 yaw_x10,
|
||||
0x402 BALANCE_IMU int16 pitch_x10, int16 roll_x10, int16 yaw_x10,
|
||||
uint8 cal_status, uint8 balance_state (50 Hz)
|
||||
0x403 FC_BARO int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz)
|
||||
0x403 BALANCE_BARO int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz)
|
||||
|
||||
Orin → FC (commands):
|
||||
0x300 HEARTBEAT uint32 sequence counter (5 Hz)
|
||||
@ -57,10 +57,10 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
|
||||
# ---- CAN frame IDs ------------------------------------------------
|
||||
|
||||
CAN_FC_STATUS = 0x400
|
||||
CAN_FC_VESC = 0x401
|
||||
CAN_FC_IMU = 0x402
|
||||
CAN_FC_BARO = 0x403
|
||||
CAN_BALANCE_STATUS = 0x400
|
||||
CAN_BALANCE_VESC = 0x401
|
||||
CAN_BALANCE_IMU = 0x402
|
||||
CAN_BALANCE_BARO = 0x403
|
||||
|
||||
CAN_HEARTBEAT = 0x300
|
||||
CAN_DRIVE = 0x301
|
||||
@ -216,11 +216,11 @@ class SaltybotCanNode(Node):
|
||||
|
||||
def _dispatch(self, can_id: int, data: bytes):
|
||||
now = self.get_clock().now().to_msg()
|
||||
if can_id == CAN_FC_IMU and len(data) >= 8:
|
||||
if can_id == CAN_BALANCE_IMU and len(data) >= 8:
|
||||
self._handle_fc_imu(data, now)
|
||||
elif can_id == CAN_FC_STATUS and len(data) >= 8:
|
||||
elif can_id == CAN_BALANCE_STATUS and len(data) >= 8:
|
||||
self._handle_fc_status(data)
|
||||
elif can_id == CAN_FC_BARO and len(data) >= 8:
|
||||
elif can_id == CAN_BALANCE_BARO and len(data) >= 8:
|
||||
self._handle_fc_baro(data, now)
|
||||
|
||||
# ── Frame handlers ───────────────────────────────────────────────
|
||||
@ -322,12 +322,7 @@ class SaltybotCanNode(Node):
|
||||
diag.header.stamp = stamp
|
||||
st = DiagnosticStatus()
|
||||
st.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
st.hardware_id = "esp32"
|
||||
=======
|
||||
st.hardware_id = "esp32s322"
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
st.message = state_label
|
||||
st.hardware_id = "esp32s322" st.message = state_label
|
||||
st.level = (DiagnosticStatus.OK if state == 1 else
|
||||
DiagnosticStatus.WARN if state == 0 else
|
||||
DiagnosticStatus.ERROR)
|
||||
|
||||
@ -1,26 +1,8 @@
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
saltybot_cmd_node — full bidirectional ESP32 BALANCE↔Jetson bridge
|
||||
=======
|
||||
saltybot_cmd_node — full bidirectional ESP32-S3↔Jetson bridge
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Combines telemetry RX (from serial_bridge_node) with drive command TX.
|
||||
saltybot_cmd_node — full bidirectional ESP32-S3↔Jetson bridgeCombines telemetry RX (from serial_bridge_node) with drive command TX.
|
||||
|
||||
Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node.
|
||||
|
||||
<<<<<<< HEAD
|
||||
RX path (50Hz from ESP32 BALANCE):
|
||||
JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics
|
||||
|
||||
TX path:
|
||||
/cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n → ESP32 BALANCE
|
||||
Heartbeat timer (200ms) → H\\n → ESP32 BALANCE
|
||||
|
||||
Protocol:
|
||||
H\\n — heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms.
|
||||
C<spd>,<str>\\n — drive command. speed/steer: -1000..+1000 integers.
|
||||
C command also refreshes ESP32 BALANCE heartbeat timer.
|
||||
=======
|
||||
RX path (50Hz from ESP32-S3):
|
||||
JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics
|
||||
|
||||
@ -32,8 +14,6 @@ Protocol:
|
||||
H\\n — heartbeat. ESP32-S3 reverts steer to 0 if gap > 500ms.
|
||||
C<spd>,<str>\\n — drive command. speed/steer: -1000..+1000 integers.
|
||||
C command also refreshes ESP32-S3 heartbeat timer.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Twist mapping (configurable via ROS2 params):
|
||||
speed = clamp(linear.x * speed_scale, -1000, 1000)
|
||||
steer = clamp(angular.z * steer_scale, -1000, 1000)
|
||||
@ -118,12 +98,7 @@ class SaltybotCmdNode(Node):
|
||||
self._open_serial()
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
# Telemetry read at 100Hz (ESP32 BALANCE sends at 50Hz)
|
||||
=======
|
||||
# Telemetry read at 100Hz (ESP32-S3 sends at 50Hz)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self._read_timer = self.create_timer(0.01, self._read_cb)
|
||||
# Telemetry read at 100Hz (ESP32-S3 sends at 50Hz) self._read_timer = self.create_timer(0.01, self._read_cb)
|
||||
# Heartbeat TX at configured period (default 200ms)
|
||||
self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb)
|
||||
|
||||
@ -288,12 +263,7 @@ class SaltybotCmdNode(Node):
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
=======
|
||||
status.hardware_id = "esp32s322"
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
status.message = state_label
|
||||
status.hardware_id = "esp32s322" status.message = state_label
|
||||
if state == 1:
|
||||
status.level = DiagnosticStatus.OK
|
||||
elif state == 0:
|
||||
@ -320,20 +290,11 @@ class SaltybotCmdNode(Node):
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"ESP32 BALANCE IMU fault: errno={errno}")
|
||||
=======
|
||||
status.hardware_id = "esp32s322"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"ESP32-S3 IMU fault: errno={errno}")
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
# ── TX — command send ─────────────────────────────────────────────────────
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist):
|
||||
@ -350,12 +311,7 @@ class SaltybotCmdNode(Node):
|
||||
)
|
||||
|
||||
def _heartbeat_cb(self):
|
||||
<<<<<<< HEAD
|
||||
"""Send H\\n heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms."""
|
||||
=======
|
||||
"""Send H\\n heartbeat. ESP32-S3 reverts steer to 0 if gap > 500ms."""
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self._write(b"H\n")
|
||||
"""Send H\\n heartbeat. ESP32-S3 reverts steer to 0 if gap > 500ms.""" self._write(b"H\n")
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
@ -1,11 +1,6 @@
|
||||
"""
|
||||
saltybot_bridge — serial_bridge_node
|
||||
<<<<<<< HEAD
|
||||
ESP32 USB CDC → ROS2 topic publisher
|
||||
=======
|
||||
ESP32-S3 USB CDC → ROS2 topic publisher
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Telemetry frame (50 Hz, newline-delimited JSON):
|
||||
{"p":<pitch×10>,"r":<roll×10>,"e":<err×10>,"ig":<integral×10>,
|
||||
"m":<motor_cmd -1000..1000>,"s":<state 0-2>,"y":<yaw×10>}
|
||||
@ -33,12 +28,7 @@ from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import String
|
||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
|
||||
<<<<<<< HEAD
|
||||
# Balance state labels matching ESP32 BALANCE balance_state_t enum
|
||||
=======
|
||||
# Balance state labels matching ESP32-S3 balance_state_t enum
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
||||
# Balance state labels matching ESP32-S3 balance_state_t enum_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
||||
|
||||
# Sensor frame_id published in Imu header
|
||||
IMU_FRAME_ID = "imu_link"
|
||||
@ -91,15 +81,10 @@ class SerialBridgeNode(Node):
|
||||
|
||||
# ── Open serial and start read timer ──────────────────────────────────
|
||||
self._open_serial()
|
||||
<<<<<<< HEAD
|
||||
# Poll at 100 Hz — ESP32 BALANCE sends at 50 Hz, so we never miss a frame
|
||||
=======
|
||||
# Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self._timer = self.create_timer(0.01, self._read_cb)
|
||||
# Poll at 100 Hz — ESP32-S3 sends at 50 Hz, so we never miss a frame self._timer = self.create_timer(0.01, self._read_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f"stm32_serial_bridge started — {port} @ {baud} baud"
|
||||
f"esp32_serial_bridge started — {port} @ {baud} baud"
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────────
|
||||
@ -129,12 +114,7 @@ class SerialBridgeNode(Node):
|
||||
|
||||
def write_serial(self, data: bytes) -> bool:
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
Send raw bytes to ESP32 BALANCE over the open serial port.
|
||||
=======
|
||||
Send raw bytes to ESP32-S3 over the open serial port.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Returns False if port is not open (caller should handle gracefully).
|
||||
Send raw bytes to ESP32-S3 over the open serial port. Returns False if port is not open (caller should handle gracefully).
|
||||
Note: for bidirectional use prefer saltybot_cmd_node which owns TX natively.
|
||||
"""
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
@ -222,12 +202,7 @@ class SerialBridgeNode(Node):
|
||||
"""
|
||||
Publish sensor_msgs/Imu.
|
||||
|
||||
<<<<<<< HEAD
|
||||
The ESP32 BALANCE IMU gives Euler angles (pitch/roll from accelerometer+gyro
|
||||
=======
|
||||
The ESP32-S3 IMU gives Euler angles (pitch/roll from accelerometer+gyro
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
fusion, yaw from gyro integration). We publish them as angular_velocity
|
||||
The ESP32-S3 IMU gives Euler angles (pitch/roll from accelerometer+gyro fusion, yaw from gyro integration). We publish them as angular_velocity
|
||||
for immediate use by slam_toolbox / robot_localization.
|
||||
|
||||
Note: orientation quaternion is left zeroed (covariance [-1,...]) until
|
||||
@ -284,12 +259,7 @@ class SerialBridgeNode(Node):
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
=======
|
||||
status.hardware_id = "esp32s322"
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
status.message = state_label
|
||||
status.hardware_id = "esp32s322" status.message = state_label
|
||||
|
||||
if state == 1: # ARMED
|
||||
status.level = DiagnosticStatus.OK
|
||||
@ -317,20 +287,11 @@ class SerialBridgeNode(Node):
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"ESP32 BALANCE reported IMU fault: errno={errno}")
|
||||
=======
|
||||
status.hardware_id = "esp32s322"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"ESP32-S3 reported IMU fault: errno={errno}")
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
def destroy_node(self):
|
||||
self._close_serial()
|
||||
super().destroy_node()
|
||||
|
||||
@ -29,12 +29,7 @@ setup(
|
||||
zip_safe=True,
|
||||
maintainer="sl-jetson",
|
||||
maintainer_email="sl-jetson@saltylab.local",
|
||||
<<<<<<< HEAD
|
||||
description="ESP32 USB CDC → ROS2 serial bridge for saltybot",
|
||||
=======
|
||||
description="ESP32-S3 USB CDC → ROS2 serial bridge for saltybot",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
license="MIT",
|
||||
description="ESP32-S3 USB CDC → ROS2 serial bridge for saltybot", license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
@ -45,14 +40,8 @@ setup(
|
||||
# Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate
|
||||
"cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main",
|
||||
"remote_estop_node = saltybot_bridge.remote_estop_node:main",
|
||||
<<<<<<< HEAD
|
||||
# Binary-framed ESP32 BALANCE command node (Issue #119)
|
||||
"stm32_cmd_node = saltybot_bridge.stm32_cmd_node:main",
|
||||
=======
|
||||
# Binary-framed ESP32-S3 command node (Issue #119)
|
||||
"esp32_cmd_node = saltybot_bridge.esp32_cmd_node:main",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# Battery management node (Issue #125)
|
||||
"esp32_cmd_node = saltybot_bridge.esp32_cmd_node:main", # Battery management node (Issue #125)
|
||||
"battery_node = saltybot_bridge.battery_node:main",
|
||||
# Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685)
|
||||
"saltybot_can_node = saltybot_bridge.saltybot_can_node:main",
|
||||
|
||||
@ -1,10 +1,5 @@
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
Unit tests for Jetson→ESP32 BALANCE command serialization logic.
|
||||
=======
|
||||
Unit tests for Jetson→ESP32-S3 command serialization logic.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Tests Twist→speed/steer conversion and frame formatting.
|
||||
Unit tests for Jetson→ESP32-S3 command serialization logic.Tests Twist→speed/steer conversion and frame formatting.
|
||||
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
|
||||
"""
|
||||
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
"""test_esp32_cmd_node.py — Unit tests for Stm32CmdNode with mock serial port.
|
||||
"""test_esp32_cmd_node.py — Unit tests for Esp32CmdNode with mock serial port.
|
||||
|
||||
Tests:
|
||||
- Serial open/close lifecycle
|
||||
|
||||
@ -1,10 +1,5 @@
|
||||
"""
|
||||
<<<<<<< HEAD
|
||||
Unit tests for ESP32 BALANCE telemetry parsing logic.
|
||||
=======
|
||||
Unit tests for ESP32-S3 telemetry parsing logic.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
||||
Unit tests for ESP32-S3 telemetry parsing logic.Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
||||
"""
|
||||
|
||||
import json
|
||||
|
||||
@ -19,12 +19,7 @@
|
||||
# inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
|
||||
# DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer)
|
||||
#
|
||||
<<<<<<< HEAD
|
||||
# Output: /cmd_vel (Twist) — ESP32 bridge consumes this topic.
|
||||
=======
|
||||
# Output: /cmd_vel (Twist) — ESP32-S3 bridge consumes this topic.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
|
||||
@ -2,12 +2,7 @@
|
||||
# Master configuration for full stack bringup
|
||||
|
||||
# ────────────────────────────────────────────────────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
# HARDWARE — ESP32 BALANCE Bridge & Motor Control
|
||||
=======
|
||||
# HARDWARE — ESP32-S3 Bridge & Motor Control
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# ────────────────────────────────────────────────────────────────────────────
|
||||
# HARDWARE — ESP32-S3 Bridge & Motor Control# ────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
saltybot_bridge_node:
|
||||
ros__parameters:
|
||||
|
||||
@ -39,12 +39,7 @@ Modes
|
||||
─ UWB driver (2-anchor DW3000, publishes /uwb/target)
|
||||
─ YOLOv8n person detection (TensorRT)
|
||||
─ Person follower with UWB+camera fusion
|
||||
<<<<<<< HEAD
|
||||
─ cmd_vel bridge → ESP32 BALANCE (deadman + ramp + AUTONOMOUS gate)
|
||||
=======
|
||||
─ cmd_vel bridge → ESP32-S3 (deadman + ramp + AUTONOMOUS gate)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
─ rosbridge WebSocket (port 9090)
|
||||
─ cmd_vel bridge → ESP32-S3 (deadman + ramp + AUTONOMOUS gate) ─ rosbridge WebSocket (port 9090)
|
||||
|
||||
outdoor
|
||||
─ RPLIDAR + RealSense D435i sensors (no SLAM)
|
||||
@ -61,14 +56,8 @@ Modes
|
||||
Launch sequence (wall-clock delays — conservative for cold start)
|
||||
─────────────────────────────────────────────────────────────────
|
||||
t= 0s robot_description (URDF + TF tree)
|
||||
<<<<<<< HEAD
|
||||
t= 0s ESP32 bridge (serial port owner — must be first)
|
||||
t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32 bridge up)
|
||||
=======
|
||||
t= 0s ESP32-S3 bridge (serial port owner — must be first)
|
||||
t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 bridge up)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
t= 2s sensors (RPLIDAR + RealSense)
|
||||
t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 bridge up) t= 2s sensors (RPLIDAR + RealSense)
|
||||
t= 4s UWB driver (independent serial device)
|
||||
t= 4s CSI cameras (optional, independent)
|
||||
t= 5s audio_pipeline (Jabra SPEAK 810: wake word + STT + TTS; Issue #503)
|
||||
@ -80,18 +69,10 @@ Launch sequence (wall-clock delays — conservative for cold start)
|
||||
|
||||
Safety wiring
|
||||
─────────────
|
||||
<<<<<<< HEAD
|
||||
• ESP32 bridge must be up before cmd_vel bridge sends any command.
|
||||
• cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
|
||||
• ESP32 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
|
||||
until ESP32 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel.
|
||||
=======
|
||||
• ESP32-S3 bridge must be up before cmd_vel bridge sends any command.
|
||||
• cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
|
||||
• ESP32-S3 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
|
||||
until ESP32-S3 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
• follow_enabled:=false disables person follower without stopping the node.
|
||||
until ESP32-S3 firmware is in AUTONOMOUS mode regardless of /cmd_vel. • follow_enabled:=false disables person follower without stopping the node.
|
||||
• To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}'
|
||||
|
||||
Topics published by this stack
|
||||
@ -107,12 +88,7 @@ Topics published by this stack
|
||||
/person/target PoseStamped (camera position, base_link)
|
||||
/person/detections Detection2DArray
|
||||
/cmd_vel Twist (from follower or Nav2)
|
||||
<<<<<<< HEAD
|
||||
/saltybot/cmd String (to ESP32 BALANCE)
|
||||
=======
|
||||
/saltybot/cmd String (to ESP32-S3)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
/saltybot/imu Imu
|
||||
/saltybot/cmd String (to ESP32-S3) /saltybot/imu Imu
|
||||
/saltybot/balance_state String
|
||||
"""
|
||||
|
||||
@ -229,12 +205,7 @@ def generate_launch_description():
|
||||
enable_bridge_arg = DeclareLaunchArgument(
|
||||
"enable_bridge",
|
||||
default_value="true",
|
||||
<<<<<<< HEAD
|
||||
description="Launch ESP32 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
|
||||
=======
|
||||
description="Launch ESP32-S3 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
)
|
||||
description="Launch ESP32-S3 serial bridge + cmd_vel bridge (disable for sim/rosbag)", )
|
||||
|
||||
enable_rosbridge_arg = DeclareLaunchArgument(
|
||||
"enable_rosbridge",
|
||||
@ -294,12 +265,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
esp32_port_arg = DeclareLaunchArgument(
|
||||
"esp32_port",
|
||||
default_value="/dev/esp32-bridge",
|
||||
<<<<<<< HEAD
|
||||
description="ESP32 USB CDC serial port",
|
||||
=======
|
||||
description="ESP32-S3 USB CDC serial port",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
)
|
||||
description="ESP32-S3 USB CDC serial port", )
|
||||
|
||||
# ── Shared substitution handles ───────────────────────────────────────────
|
||||
# Profile argument for parameter override (Issue #506)
|
||||
@ -318,12 +284,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||
)
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ── t=0s ESP32 bidirectional serial bridge ────────────────────────────────
|
||||
=======
|
||||
# ── t=0s ESP32-S3 bidirectional serial bridge ────────────────────────────────
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
esp32_bridge = GroupAction(
|
||||
# ── t=0s ESP32-S3 bidirectional serial bridge ──────────────────────────────── esp32_bridge = GroupAction(
|
||||
condition=IfCondition(LaunchConfiguration("enable_bridge")),
|
||||
actions=[
|
||||
IncludeLaunchDescription(
|
||||
@ -352,12 +313,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
],
|
||||
)
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ── t=2s cmd_vel safety bridge (depends on ESP32 bridge) ────────────────
|
||||
=======
|
||||
# ── t=2s cmd_vel safety bridge (depends on ESP32-S3 bridge) ────────────────
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
cmd_vel_bridge = TimerAction(
|
||||
# ── t=2s cmd_vel safety bridge (depends on ESP32-S3 bridge) ──────────────── cmd_vel_bridge = TimerAction(
|
||||
period=2.0,
|
||||
actions=[
|
||||
GroupAction(
|
||||
|
||||
@ -19,12 +19,7 @@ Usage
|
||||
|
||||
Startup sequence
|
||||
────────────────
|
||||
<<<<<<< HEAD
|
||||
GROUP A — Drivers t= 0 s ESP32 bridge, RealSense+RPLIDAR, motor daemon, IMU
|
||||
=======
|
||||
GROUP A — Drivers t= 0 s ESP32-S3 bridge, RealSense+RPLIDAR, motor daemon, IMU
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
|
||||
GROUP A — Drivers t= 0 s ESP32-S3 bridge, RealSense+RPLIDAR, motor daemon, IMU health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
|
||||
GROUP B — Perception t= 8 s UWB, person detection, object detection, depth costmap, gimbal
|
||||
health gate ───────────────────────────────────────────────── t=16 s (full/debug)
|
||||
GROUP C — Navigation t=16 s SLAM, Nav2, lidar avoidance, follower, docking
|
||||
@ -127,12 +122,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901
|
||||
esp32_port_arg = DeclareLaunchArgument(
|
||||
"esp32_port",
|
||||
default_value="/dev/esp32-bridge",
|
||||
<<<<<<< HEAD
|
||||
description="ESP32 UART bridge serial device",
|
||||
=======
|
||||
description="ESP32-S3 USART bridge serial device",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
)
|
||||
description="ESP32-S3 USART bridge serial device", )
|
||||
|
||||
uwb_port_a_arg = DeclareLaunchArgument(
|
||||
"uwb_port_a",
|
||||
@ -206,12 +196,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901
|
||||
|
||||
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||
# GROUP A — DRIVERS (t = 0 s, all profiles)
|
||||
<<<<<<< HEAD
|
||||
# Dependency order: ESP32 bridge first, then sensors, then motor daemon.
|
||||
=======
|
||||
# Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# Health gate: subsequent groups delayed until t_perception (8 s full/debug).
|
||||
# Dependency order: ESP32-S3 bridge first, then sensors, then motor daemon. # Health gate: subsequent groups delayed until t_perception (8 s full/debug).
|
||||
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||
|
||||
group_a_banner = LogInfo(
|
||||
@ -224,12 +209,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901
|
||||
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||
)
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ESP32 BALANCE bridge
|
||||
=======
|
||||
# ESP32-S3 bidirectional bridge (JLINK USART1)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
esp32_bridge = IncludeLaunchDescription(
|
||||
# ESP32-S3 bidirectional bridge (JLINK USART1) esp32_bridge = IncludeLaunchDescription(
|
||||
_launch("saltybot_bridge", "launch", "bridge.launch.py"),
|
||||
launch_arguments={
|
||||
"mode": "bidirectional",
|
||||
@ -248,12 +228,7 @@ def generate_launch_description() -> LaunchDescription: # noqa: C901
|
||||
],
|
||||
)
|
||||
|
||||
<<<<<<< HEAD
|
||||
# Motor daemon: /cmd_vel → ESP32 BALANCE DRIVE frames (depends on bridge at t=0)
|
||||
=======
|
||||
# Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
motor_daemon = TimerAction(
|
||||
# Motor daemon: /cmd_vel → ESP32-S3 DRIVE frames (depends on bridge at t=0) motor_daemon = TimerAction(
|
||||
period=2.5,
|
||||
actions=[
|
||||
LogInfo(msg="[saltybot_bringup] t=2.5s Starting motor daemon"),
|
||||
|
||||
@ -20,12 +20,7 @@ theta is kept in (−π, π] after every step.
|
||||
|
||||
Int32 rollover
|
||||
--------------
|
||||
<<<<<<< HEAD
|
||||
ESP32 BALANCE encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles
|
||||
=======
|
||||
ESP32-S3 encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handles
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
this by detecting jumps larger than half the int32 range and adjusting by the
|
||||
ESP32-S3 encoder counters are int32 and wrap at ±2^31. `unwrap_delta` handlesthis by detecting jumps larger than half the int32 range and adjusting by the
|
||||
full range:
|
||||
|
||||
if delta > 2^30 : delta -= 2^31
|
||||
|
||||
@ -69,12 +69,7 @@ class Profile:
|
||||
t_ui: float = 22.0 # Group D (nav2 needs ~4 s to load costmaps)
|
||||
|
||||
# ── Safety ────────────────────────────────────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
watchdog_timeout_s: float = 5.0 # max silence from ESP32 bridge (s)
|
||||
=======
|
||||
watchdog_timeout_s: float = 5.0 # max silence from ESP32-S3 bridge (s)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
cmd_vel_deadman_s: float = 0.5 # cmd_vel watchdog in bridge
|
||||
watchdog_timeout_s: float = 5.0 # max silence from ESP32-S3 bridge (s) cmd_vel_deadman_s: float = 0.5 # cmd_vel watchdog in bridge
|
||||
max_linear_vel: float = 0.5 # m/s cap passed to bridge + follower
|
||||
follow_distance_m: float = 1.5 # target follow distance (m)
|
||||
|
||||
@ -94,12 +89,7 @@ class Profile:
|
||||
# ── Profile factory ────────────────────────────────────────────────────────────
|
||||
|
||||
def _minimal() -> Profile:
|
||||
<<<<<<< HEAD
|
||||
"""Minimal: ESP32 bridge + sensors + motor daemon.
|
||||
=======
|
||||
"""Minimal: ESP32-S3 bridge + sensors + motor daemon.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Safe drive control only. No AI, no nav, no social.
|
||||
Boot time ~4 s. RAM ~400 MB.
|
||||
"""
|
||||
|
||||
@ -1,12 +1,7 @@
|
||||
"""
|
||||
wheel_odom_node.py — Differential drive wheel encoder odometry (Issue #184).
|
||||
|
||||
<<<<<<< HEAD
|
||||
Subscribes to raw encoder tick counts from the ESP32 bridge, integrates
|
||||
=======
|
||||
Subscribes to raw encoder tick counts from the ESP32-S3 bridge, integrates
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz.
|
||||
Subscribes to raw encoder tick counts from the ESP32-S3 bridge, integratesdifferential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz.
|
||||
Optionally broadcasts the odom → base_link TF transform.
|
||||
|
||||
Subscribes:
|
||||
|
||||
@ -61,12 +61,7 @@ kill %1
|
||||
|
||||
### Core System Components
|
||||
- Robot Description (URDF/TF tree)
|
||||
<<<<<<< HEAD
|
||||
- ESP32 Serial Bridge
|
||||
=======
|
||||
- ESP32-S3 Serial Bridge
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
- cmd_vel Bridge
|
||||
- ESP32-S3 Serial Bridge- cmd_vel Bridge
|
||||
- Rosbridge WebSocket
|
||||
|
||||
### Sensors
|
||||
@ -129,12 +124,7 @@ free -h
|
||||
|
||||
### cmd_vel bridge not responding
|
||||
```bash
|
||||
<<<<<<< HEAD
|
||||
# Verify ESP32 bridge is running first
|
||||
=======
|
||||
# Verify ESP32-S3 bridge is running first
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
ros2 node list | grep bridge
|
||||
# Verify ESP32-S3 bridge is running firstros2 node list | grep bridge
|
||||
|
||||
# Check serial port
|
||||
ls -l /dev/esp32-bridge
|
||||
|
||||
@ -3,5 +3,5 @@ can_bridge_node:
|
||||
can_interface: slcan0
|
||||
left_vesc_can_id: 56
|
||||
right_vesc_can_id: 68
|
||||
mamba_can_id: 1
|
||||
balance_can_id: 1
|
||||
command_timeout_s: 0.5
|
||||
|
||||
@ -6,13 +6,13 @@ and VESC telemetry.
|
||||
CAN message layout
|
||||
------------------
|
||||
Command frames (Orin → ESP32-S3 BALANCE / 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
|
||||
BALANCE_CMD_VELOCITY 0x100 8 bytes left_speed (f32, m/s) | right_speed (f32, m/s)
|
||||
BALANCE_CMD_MODE 0x101 1 byte mode (0=idle, 1=drive, 2=estop)
|
||||
BALANCE_CMD_ESTOP 0x102 1 byte 0x01 = stop
|
||||
|
||||
Telemetry frames (ESP32-S3 BALANCE → 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)
|
||||
BALANCE_TELEM_IMU 0x200 24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each)
|
||||
BALANCE_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)
|
||||
@ -30,12 +30,12 @@ from typing import Tuple
|
||||
# CAN message IDs
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
MAMBA_CMD_VELOCITY: int = 0x100
|
||||
MAMBA_CMD_MODE: int = 0x101
|
||||
MAMBA_CMD_ESTOP: int = 0x102
|
||||
BALANCE_CMD_VELOCITY: int = 0x100
|
||||
BALANCE_CMD_MODE: int = 0x101
|
||||
BALANCE_CMD_ESTOP: int = 0x102
|
||||
|
||||
MAMBA_TELEM_IMU: int = 0x200
|
||||
MAMBA_TELEM_BATTERY: int = 0x201
|
||||
BALANCE_TELEM_IMU: int = 0x200
|
||||
BALANCE_TELEM_BATTERY: int = 0x201
|
||||
|
||||
VESC_TELEM_STATE: int = 0x300
|
||||
ORIN_CAN_ID_PID_SET: int = 0x305
|
||||
@ -56,7 +56,7 @@ MODE_ESTOP: int = 2
|
||||
|
||||
@dataclass
|
||||
class ImuTelemetry:
|
||||
"""Decoded IMU telemetry from ESP32-S3 BALANCE (MAMBA_TELEM_IMU)."""
|
||||
"""Decoded IMU telemetry from ESP32-S3 BALANCE (BALANCE_TELEM_IMU)."""
|
||||
|
||||
accel_x: float = 0.0 # m/s²
|
||||
accel_y: float = 0.0
|
||||
@ -68,7 +68,7 @@ class ImuTelemetry:
|
||||
|
||||
@dataclass
|
||||
class BatteryTelemetry:
|
||||
"""Decoded battery telemetry from ESP32-S3 BALANCE (MAMBA_TELEM_BATTERY)."""
|
||||
"""Decoded battery telemetry from ESP32-S3 BALANCE (BALANCE_TELEM_BATTERY)."""
|
||||
|
||||
voltage: float = 0.0 # V
|
||||
current: float = 0.0 # A
|
||||
@ -106,7 +106,7 @@ _FMT_VESC = ">ffff" # 4 × float32
|
||||
|
||||
def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
|
||||
"""
|
||||
Encode a MAMBA_CMD_VELOCITY payload.
|
||||
Encode a BALANCE_CMD_VELOCITY payload.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
@ -122,7 +122,7 @@ def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
|
||||
|
||||
def encode_mode_cmd(mode: int) -> bytes:
|
||||
"""
|
||||
Encode a MAMBA_CMD_MODE payload.
|
||||
Encode a BALANCE_CMD_MODE payload.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
@ -139,7 +139,7 @@ def encode_mode_cmd(mode: int) -> bytes:
|
||||
|
||||
def encode_estop_cmd(stop: bool = True) -> bytes:
|
||||
"""
|
||||
Encode a MAMBA_CMD_ESTOP payload.
|
||||
Encode a BALANCE_CMD_ESTOP payload.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
@ -165,7 +165,7 @@ def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes:
|
||||
|
||||
def decode_imu_telem(data: bytes) -> ImuTelemetry:
|
||||
"""
|
||||
Decode a MAMBA_TELEM_IMU payload.
|
||||
Decode a BALANCE_TELEM_IMU payload.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
@ -188,7 +188,7 @@ def decode_imu_telem(data: bytes) -> ImuTelemetry:
|
||||
|
||||
def decode_battery_telem(data: bytes) -> BatteryTelemetry:
|
||||
"""
|
||||
Decode a MAMBA_TELEM_BATTERY payload.
|
||||
Decode a BALANCE_TELEM_BATTERY payload.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
@ -39,22 +39,22 @@ from sensor_msgs.msg import BatteryState
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
MAMBA_CMD_ESTOP,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_TELEM_BATTERY,
|
||||
MAMBA_TELEM_IMU,
|
||||
BALANCE_CMD_ESTOP,
|
||||
BALANCE_CMD_MODE,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_TELEM_BATTERY,
|
||||
BALANCE_TELEM_IMU,
|
||||
VESC_TELEM_STATE,
|
||||
ORIN_CAN_ID_FC_PID_ACK,
|
||||
ORIN_CAN_ID_PID_SET,
|
||||
MODE_DRIVE,
|
||||
MODE_IDLE,
|
||||
encode_drive_cmd,
|
||||
encode_arm_cmd,
|
||||
encode_velocity_cmd,
|
||||
encode_mode_cmd,
|
||||
encode_estop_cmd,
|
||||
decode_attitude,
|
||||
decode_battery,
|
||||
decode_vesc_status1,
|
||||
decode_imu_telem,
|
||||
decode_battery_telem,
|
||||
decode_vesc_state,
|
||||
)
|
||||
|
||||
# Reconnect attempt interval when CAN bus is lost
|
||||
@ -179,10 +179,10 @@ class CanBridgeNode(Node):
|
||||
right_mps = linear + angular
|
||||
|
||||
payload = encode_velocity_cmd(left_mps, right_mps)
|
||||
self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
|
||||
self._send_can(BALANCE_CMD_VELOCITY, payload, "cmd_vel")
|
||||
|
||||
# Keep ESP32-S3 BALANCE in DRIVE mode while receiving commands
|
||||
self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
|
||||
self._send_can(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
|
||||
|
||||
def _estop_cb(self, msg: Bool) -> None:
|
||||
"""Forward /estop to ESP32-S3 BALANCE over CAN."""
|
||||
@ -190,7 +190,7 @@ class CanBridgeNode(Node):
|
||||
return
|
||||
if msg.data:
|
||||
self._send_can(
|
||||
MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
|
||||
BALANCE_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
|
||||
)
|
||||
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32-S3 BALANCE")
|
||||
|
||||
@ -201,7 +201,8 @@ class CanBridgeNode(Node):
|
||||
if not self._connected:
|
||||
return
|
||||
if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
|
||||
self._send_can(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0), "watchdog")
|
||||
self._send_can(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog mode")
|
||||
|
||||
# ── CAN send helper ───────────────────────────────────────────────────
|
||||
|
||||
@ -236,25 +237,28 @@ class CanBridgeNode(Node):
|
||||
continue
|
||||
self._dispatch_frame(frame)
|
||||
|
||||
# VESC STATUS packet type = 9 (upper byte of extended arb_id)
|
||||
_VESC_STATUS_PKT: int = 9
|
||||
|
||||
def _dispatch_frame(self, frame: can.Message) -> None:
|
||||
arb_id = frame.arbitration_id
|
||||
data = bytes(frame.data)
|
||||
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||
vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||
vesc_l = (self._VESC_STATUS_PKT << 8) | self._left_vesc_id
|
||||
vesc_r = (self._VESC_STATUS_PKT << 8) | self._right_vesc_id
|
||||
try:
|
||||
if arb_id == ESP32_TELEM_ATTITUDE:
|
||||
self._handle_attitude(data)
|
||||
elif arb_id == ESP32_TELEM_BATTERY:
|
||||
if arb_id == BALANCE_TELEM_IMU:
|
||||
self._handle_imu(data)
|
||||
elif arb_id == BALANCE_TELEM_BATTERY:
|
||||
self._handle_battery(data)
|
||||
elif arb_id == vesc_l:
|
||||
t = decode_vesc_status1(self._left_vesc_id, data)
|
||||
t = decode_vesc_state(data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
m.data = [t.erpm, t.duty, t.voltage, t.current]
|
||||
self._pub_vesc_left.publish(m)
|
||||
elif arb_id == vesc_r:
|
||||
t = decode_vesc_status1(self._right_vesc_id, data)
|
||||
t = decode_vesc_state(data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
m.data = [t.erpm, t.duty, t.voltage, t.current]
|
||||
self._pub_vesc_right.publish(m)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
@ -263,20 +267,18 @@ class CanBridgeNode(Node):
|
||||
|
||||
# ── Frame handlers ────────────────────────────────────────────────────
|
||||
|
||||
_STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
|
||||
|
||||
def _handle_attitude(self, data: bytes) -> None:
|
||||
"""ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
|
||||
t = decode_attitude(data)
|
||||
def _handle_imu(self, data: bytes) -> None:
|
||||
"""BALANCE_TELEM_IMU (0x200): accel + gyro → /saltybot/attitude."""
|
||||
t = decode_imu_telem(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}",
|
||||
"accel_x": round(t.accel_x, 4),
|
||||
"accel_y": round(t.accel_y, 4),
|
||||
"accel_z": round(t.accel_z, 4),
|
||||
"gyro_x": round(t.gyro_x, 4),
|
||||
"gyro_y": round(t.gyro_y, 4),
|
||||
"gyro_z": round(t.gyro_z, 4),
|
||||
"ts": f"{now.sec}.{now.nanosec:09d}",
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload)
|
||||
@ -284,11 +286,12 @@ class CanBridgeNode(Node):
|
||||
self._pub_balance.publish(msg) # keep /saltybot/balance_state alive
|
||||
|
||||
def _handle_battery(self, data: bytes) -> None:
|
||||
"""BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
|
||||
t = decode_battery(data)
|
||||
"""BALANCE_TELEM_BATTERY (0x201): voltage + current → /can/battery."""
|
||||
t = decode_battery_telem(data)
|
||||
msg = BatteryState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.voltage = t.vbat_mv / 1000.0
|
||||
msg.voltage = t.voltage
|
||||
msg.current = t.current
|
||||
msg.present = True
|
||||
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
self._pub_battery.publish(msg)
|
||||
@ -305,8 +308,9 @@ class CanBridgeNode(Node):
|
||||
def destroy_node(self) -> None:
|
||||
if self._connected and self._bus is not None:
|
||||
try:
|
||||
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")
|
||||
# Send zero velocity and idle mode on shutdown
|
||||
self._send_can(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0), "shutdown")
|
||||
self._send_can(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown")
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
|
||||
@ -15,12 +15,7 @@ setup(
|
||||
zip_safe=True,
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
<<<<<<< HEAD
|
||||
description="CAN bus bridge for ESP32 IO motor controller and VESC telemetry",
|
||||
=======
|
||||
description="CAN bus bridge for ESP32-S3 BALANCE controller and VESC telemetry",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
license="MIT",
|
||||
description="CAN bus bridge for ESP32-S3 BALANCE controller and VESC telemetry", license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
|
||||
@ -12,11 +12,11 @@ import struct
|
||||
import unittest
|
||||
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
MAMBA_CMD_ESTOP,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_TELEM_BATTERY,
|
||||
MAMBA_TELEM_IMU,
|
||||
BALANCE_CMD_ESTOP,
|
||||
BALANCE_CMD_MODE,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_TELEM_BATTERY,
|
||||
BALANCE_TELEM_IMU,
|
||||
VESC_TELEM_STATE,
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
@ -37,13 +37,13 @@ class TestMessageIDs(unittest.TestCase):
|
||||
"""Verify the CAN message ID constants are correct."""
|
||||
|
||||
def test_command_ids(self):
|
||||
self.assertEqual(MAMBA_CMD_VELOCITY, 0x100)
|
||||
self.assertEqual(MAMBA_CMD_MODE, 0x101)
|
||||
self.assertEqual(MAMBA_CMD_ESTOP, 0x102)
|
||||
self.assertEqual(BALANCE_CMD_VELOCITY, 0x100)
|
||||
self.assertEqual(BALANCE_CMD_MODE, 0x101)
|
||||
self.assertEqual(BALANCE_CMD_ESTOP, 0x102)
|
||||
|
||||
def test_telemetry_ids(self):
|
||||
self.assertEqual(MAMBA_TELEM_IMU, 0x200)
|
||||
self.assertEqual(MAMBA_TELEM_BATTERY, 0x201)
|
||||
self.assertEqual(BALANCE_TELEM_IMU, 0x200)
|
||||
self.assertEqual(BALANCE_TELEM_BATTERY, 0x201)
|
||||
self.assertEqual(VESC_TELEM_STATE, 0x300)
|
||||
|
||||
|
||||
|
||||
@ -4,42 +4,39 @@ protocol_defs.py — CAN message ID constants and frame builders/parsers for the
|
||||
Orin↔ESP32-S3 BALANCE↔VESC integration test suite.
|
||||
|
||||
All IDs and payload formats are derived from:
|
||||
include/orin_can.h — Orin↔FC (ESP32-S3 BALANCE) protocol
|
||||
include/orin_can.h — Orin↔BALANCE (ESP32-S3 BALANCE) protocol
|
||||
include/vesc_can.h — VESC CAN protocol
|
||||
saltybot_can_bridge/balance_protocol.py — existing bridge constants
|
||||
|
||||
CAN IDs used in tests
|
||||
---------------------
|
||||
Orin → FC (ESP32-S3 BALANCE) commands (standard 11-bit, matching orin_can.h):
|
||||
Orin → BALANCE (ESP32-S3 BALANCE) commands (standard 11-bit, matching orin_can.h):
|
||||
ORIN_CMD_HEARTBEAT 0x300
|
||||
ORIN_CMD_DRIVE 0x301 int16 speed (−1000..+1000), int16 steer (−1000..+1000)
|
||||
ORIN_CMD_DRIVE 0x301 int16 speed (-1000..+1000), int16 steer (-1000..+1000)
|
||||
ORIN_CMD_MODE 0x302 uint8 mode byte
|
||||
ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR)
|
||||
|
||||
FC (ESP32-S3 BALANCE) → Orin telemetry (standard 11-bit, matching orin_can.h):
|
||||
FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t)
|
||||
FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t)
|
||||
FC_IMU 0x402 8 bytes
|
||||
FC_BARO 0x403 8 bytes
|
||||
BALANCE (ESP32-S3 BALANCE) -> Orin telemetry (standard 11-bit, matching orin_can.h):
|
||||
BALANCE_STATUS 0x400 8 bytes (see orin_can_balance_status_t)
|
||||
BALANCE_VESC 0x401 8 bytes (see orin_can_balance_vesc_t)
|
||||
BALANCE_IMU 0x402 8 bytes
|
||||
BALANCE_BARO 0x403 8 bytes
|
||||
|
||||
Mamba ↔ VESC internal commands (matching balance_protocol.py):
|
||||
=======
|
||||
ESP32-S3 BALANCE ↔ VESC internal commands (matching balance_protocol.py):
|
||||
>>>>>>> 9aed963 (fix: scrub remaining Mamba references in can_bridge and e2e test protocol files)
|
||||
MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian
|
||||
MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop)
|
||||
MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop
|
||||
ESP32-S3 BALANCE <-> VESC internal commands (matching balance_protocol.py):
|
||||
BALANCE_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian
|
||||
BALANCE_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop)
|
||||
BALANCE_CMD_ESTOP 0x102 1 byte 0x01=stop
|
||||
|
||||
VESC STATUS (extended 29-bit, matching vesc_can.h):
|
||||
arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id = (9 << 8) | node_id
|
||||
Payload: int32 RPM (BE), int16 current×10 (BE), int16 duty×1000 (BE)
|
||||
Payload: int32 RPM (BE), int16 current x10 (BE), int16 duty x1000 (BE)
|
||||
"""
|
||||
|
||||
import struct
|
||||
from typing import Tuple
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Orin → FC (ESP32-S3 BALANCE) command IDs (from orin_can.h)
|
||||
# Orin -> BALANCE (ESP32-S3 BALANCE) command IDs (from orin_can.h)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
ORIN_CMD_HEARTBEAT: int = 0x300
|
||||
@ -48,28 +45,25 @@ ORIN_CMD_MODE: int = 0x302
|
||||
ORIN_CMD_ESTOP: int = 0x303
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# FC (ESP32-S3 BALANCE) → Orin telemetry IDs (from orin_can.h)
|
||||
# BALANCE (ESP32-S3 BALANCE) -> Orin telemetry IDs (from orin_can.h)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
FC_STATUS: int = 0x400
|
||||
FC_VESC: int = 0x401
|
||||
FC_IMU: int = 0x402
|
||||
FC_BARO: int = 0x403
|
||||
BALANCE_STATUS: int = 0x400
|
||||
BALANCE_VESC: int = 0x401
|
||||
BALANCE_IMU: int = 0x402
|
||||
BALANCE_BARO: int = 0x403
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Mamba → VESC internal command IDs (from balance_protocol.py)
|
||||
=======
|
||||
# ESP32-S3 BALANCE → VESC internal command IDs (from balance_protocol.py)
|
||||
>>>>>>> 9aed963 (fix: scrub remaining Mamba references in can_bridge and e2e test protocol files)
|
||||
# ESP32-S3 BALANCE -> VESC internal command IDs (from balance_protocol.py)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
MAMBA_CMD_VELOCITY: int = 0x100
|
||||
MAMBA_CMD_MODE: int = 0x101
|
||||
MAMBA_CMD_ESTOP: int = 0x102
|
||||
BALANCE_CMD_VELOCITY: int = 0x100
|
||||
BALANCE_CMD_MODE: int = 0x101
|
||||
BALANCE_CMD_ESTOP: int = 0x102
|
||||
|
||||
MAMBA_TELEM_IMU: int = 0x200
|
||||
MAMBA_TELEM_BATTERY: int = 0x201
|
||||
VESC_TELEM_STATE: int = 0x300
|
||||
BALANCE_TELEM_IMU: int = 0x200
|
||||
BALANCE_TELEM_BATTERY: int = 0x201
|
||||
VESC_TELEM_STATE: int = 0x300
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Mode constants
|
||||
@ -111,7 +105,7 @@ def VESC_SET_RPM_ID(vesc_node_id: int) -> int:
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Frame builders — Orin → FC
|
||||
# Frame builders — Orin -> BALANCE
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def build_heartbeat(seq: int = 0) -> bytes:
|
||||
@ -125,8 +119,8 @@ def build_drive_cmd(speed: int, steer: int) -> bytes:
|
||||
|
||||
Parameters
|
||||
----------
|
||||
speed: int, −1000..+1000 (mapped directly to int16)
|
||||
steer: int, −1000..+1000
|
||||
speed: int, -1000..+1000 (mapped directly to int16)
|
||||
steer: int, -1000..+1000
|
||||
"""
|
||||
return struct.pack(">hh", int(speed), int(steer))
|
||||
|
||||
@ -137,20 +131,17 @@ def build_mode_cmd(mode: int) -> bytes:
|
||||
|
||||
|
||||
def build_estop_cmd(action: int = 1) -> bytes:
|
||||
"""Build an ORIN_CMD_ESTOP payload. action=1 → ESTOP, 0 → CLEAR."""
|
||||
"""Build an ORIN_CMD_ESTOP payload. action=1 -> ESTOP, 0 -> CLEAR."""
|
||||
return struct.pack(">B", action & 0xFF)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Frame builders — Mamba velocity commands (balance_protocol.py encoding)
|
||||
=======
|
||||
# Frame builders — ESP32-S3 BALANCE velocity commands (balance_protocol.py encoding)
|
||||
>>>>>>> 9aed963 (fix: scrub remaining Mamba references in can_bridge and e2e test protocol files)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
|
||||
"""
|
||||
Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
|
||||
Build a BALANCE_CMD_VELOCITY payload (8 bytes, 2 x float32 big-endian).
|
||||
|
||||
Matches encode_velocity_cmd() in balance_protocol.py.
|
||||
"""
|
||||
@ -158,10 +149,10 @@ def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Frame builders — FC → Orin telemetry
|
||||
# Frame builders — BALANCE -> Orin telemetry
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def build_fc_status(
|
||||
def build_balance_status(
|
||||
pitch_x10: int = 0,
|
||||
motor_cmd: int = 0,
|
||||
vbat_mv: int = 24000,
|
||||
@ -169,9 +160,9 @@ def build_fc_status(
|
||||
flags: int = 0,
|
||||
) -> bytes:
|
||||
"""
|
||||
Build an FC_STATUS (0x400) payload.
|
||||
Build a BALANCE_STATUS (0x400) payload.
|
||||
|
||||
Layout (orin_can_fc_status_t, 8 bytes, big-endian):
|
||||
Layout (orin_can_balance_status_t, 8 bytes, big-endian):
|
||||
int16 pitch_x10
|
||||
int16 motor_cmd
|
||||
uint16 vbat_mv
|
||||
@ -188,23 +179,23 @@ def build_fc_status(
|
||||
)
|
||||
|
||||
|
||||
def build_fc_vesc(
|
||||
def build_balance_vesc(
|
||||
left_rpm_x10: int = 0,
|
||||
right_rpm_x10: int = 0,
|
||||
left_current_x10: int = 0,
|
||||
right_current_x10: int = 0,
|
||||
) -> bytes:
|
||||
"""
|
||||
Build an FC_VESC (0x401) payload.
|
||||
Build a BALANCE_VESC (0x401) payload.
|
||||
|
||||
Layout (orin_can_fc_vesc_t, 8 bytes, big-endian):
|
||||
Layout (orin_can_balance_vesc_t, 8 bytes, big-endian):
|
||||
int16 left_rpm_x10
|
||||
int16 right_rpm_x10
|
||||
int16 left_current_x10
|
||||
int16 right_current_x10
|
||||
|
||||
RPM values are RPM / 10 (e.g. 3000 RPM → 300).
|
||||
Current values are A × 10 (e.g. 5.5 A → 55).
|
||||
RPM values are RPM / 10 (e.g. 3000 RPM -> 300).
|
||||
Current values are A x 10 (e.g. 5.5 A -> 55).
|
||||
"""
|
||||
return struct.pack(
|
||||
">hhhh",
|
||||
@ -225,8 +216,8 @@ def build_vesc_status(
|
||||
|
||||
Layout (from vesc_can.h / VESC FW 6.x, big-endian):
|
||||
int32 rpm
|
||||
int16 current × 10
|
||||
int16 duty × 1000
|
||||
int16 current x 10
|
||||
int16 duty x 1000
|
||||
Total: 8 bytes.
|
||||
"""
|
||||
return struct.pack(
|
||||
@ -241,9 +232,9 @@ def build_vesc_status(
|
||||
# Frame parsers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def parse_fc_status(data: bytes):
|
||||
def parse_balance_status(data: bytes):
|
||||
"""
|
||||
Parse an FC_STATUS (0x400) payload.
|
||||
Parse a BALANCE_STATUS (0x400) payload.
|
||||
|
||||
Returns
|
||||
-------
|
||||
@ -251,7 +242,7 @@ def parse_fc_status(data: bytes):
|
||||
estop_active (bool), armed (bool)
|
||||
"""
|
||||
if len(data) < 8:
|
||||
raise ValueError(f"FC_STATUS needs 8 bytes, got {len(data)}")
|
||||
raise ValueError(f"BALANCE_STATUS needs 8 bytes, got {len(data)}")
|
||||
pitch_x10, motor_cmd, vbat_mv, balance_state, flags = struct.unpack(
|
||||
">hhHBB", data[:8]
|
||||
)
|
||||
@ -266,9 +257,9 @@ def parse_fc_status(data: bytes):
|
||||
}
|
||||
|
||||
|
||||
def parse_fc_vesc(data: bytes):
|
||||
def parse_balance_vesc(data: bytes):
|
||||
"""
|
||||
Parse an FC_VESC (0x401) payload.
|
||||
Parse a BALANCE_VESC (0x401) payload.
|
||||
|
||||
Returns
|
||||
-------
|
||||
@ -276,7 +267,7 @@ def parse_fc_vesc(data: bytes):
|
||||
right_current_x10, left_rpm (float), right_rpm (float)
|
||||
"""
|
||||
if len(data) < 8:
|
||||
raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}")
|
||||
raise ValueError(f"BALANCE_VESC needs 8 bytes, got {len(data)}")
|
||||
left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack(
|
||||
">hhhh", data[:8]
|
||||
)
|
||||
@ -312,12 +303,12 @@ def parse_vesc_status(data: bytes):
|
||||
|
||||
def parse_velocity_cmd(data: bytes) -> Tuple[float, float]:
|
||||
"""
|
||||
Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
|
||||
Parse a BALANCE_CMD_VELOCITY payload (8 bytes, 2 x float32 big-endian).
|
||||
|
||||
Returns
|
||||
-------
|
||||
(left_mps, right_mps)
|
||||
"""
|
||||
if len(data) < 8:
|
||||
raise ValueError(f"MAMBA_CMD_VELOCITY needs 8 bytes, got {len(data)}")
|
||||
raise ValueError(f"BALANCE_CMD_VELOCITY needs 8 bytes, got {len(data)}")
|
||||
return struct.unpack(">ff", data[:8])
|
||||
|
||||
@ -4,7 +4,7 @@ test_drive_command.py — Integration tests for the drive command path.
|
||||
|
||||
Tests verify:
|
||||
DRIVE cmd → ESP32-S3 BALANCE receives velocity command frame → mock VESC status response
|
||||
→ FC_VESC broadcast contains correct RPMs.
|
||||
→ BALANCE_VESC broadcast contains correct RPMs.
|
||||
|
||||
All tests run without real hardware or a running ROS2 system.
|
||||
Run with: python -m pytest test/test_drive_command.py -v
|
||||
@ -14,19 +14,19 @@ import struct
|
||||
import pytest
|
||||
|
||||
from saltybot_can_e2e_test.protocol_defs import (
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_CMD_MODE,
|
||||
FC_VESC,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_CMD_MODE,
|
||||
BALANCE_VESC,
|
||||
MODE_DRIVE,
|
||||
MODE_IDLE,
|
||||
VESC_CAN_ID_LEFT,
|
||||
VESC_CAN_ID_RIGHT,
|
||||
VESC_STATUS_ID,
|
||||
build_velocity_cmd,
|
||||
build_fc_vesc,
|
||||
build_balance_vesc,
|
||||
build_vesc_status,
|
||||
parse_velocity_cmd,
|
||||
parse_fc_vesc,
|
||||
parse_balance_vesc,
|
||||
)
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
encode_velocity_cmd,
|
||||
@ -50,8 +50,8 @@ def _send_drive(bus, left_mps: float, right_mps: float) -> None:
|
||||
self.data = bytearray(data)
|
||||
self.is_extended_id = False
|
||||
|
||||
bus.send(_Msg(MAMBA_CMD_VELOCITY, payload))
|
||||
bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
|
||||
bus.send(_Msg(BALANCE_CMD_VELOCITY, payload))
|
||||
bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
@ -62,11 +62,11 @@ class TestDriveForward:
|
||||
def test_drive_forward_velocity_frame_sent(self, mock_can_bus):
|
||||
"""
|
||||
Inject DRIVE cmd (1.0 m/s, 1.0 m/s) → verify ESP32-S3 BALANCE receives
|
||||
a MAMBA_CMD_VELOCITY frame with correct payload.
|
||||
a BALANCE_CMD_VELOCITY frame with correct payload.
|
||||
"""
|
||||
_send_drive(mock_can_bus, 1.0, 1.0)
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 1, "Expected exactly one velocity command frame"
|
||||
|
||||
left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
|
||||
@ -77,26 +77,26 @@ class TestDriveForward:
|
||||
"""After a drive command, a MODE=drive frame must accompany it."""
|
||||
_send_drive(mock_can_bus, 1.0, 1.0)
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert len(mode_frames) >= 1, "Expected at least one MODE frame"
|
||||
assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE])
|
||||
|
||||
def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus):
|
||||
"""
|
||||
Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct.
|
||||
(In the real loop ESP32-S3 BALANCE computes RPM from m/s and broadcasts FC_VESC.)
|
||||
This test checks the FC_VESC frame format and parser.
|
||||
Simulate BALANCE_VESC broadcast arriving after drive cmd; verify parse is correct.
|
||||
(In the real loop ESP32-S3 BALANCE computes RPM from m/s and broadcasts BALANCE_VESC.)
|
||||
This test checks the BALANCE_VESC frame format and parser.
|
||||
"""
|
||||
# Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics)
|
||||
fc_payload = build_fc_vesc(
|
||||
fc_payload = build_balance_vesc(
|
||||
left_rpm_x10=300, right_rpm_x10=300,
|
||||
left_current_x10=50, right_current_x10=50,
|
||||
)
|
||||
mock_can_bus.inject(FC_VESC, fc_payload)
|
||||
mock_can_bus.inject(BALANCE_VESC, fc_payload)
|
||||
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
assert frame is not None, "FC_VESC frame not received"
|
||||
parsed = parse_fc_vesc(bytes(frame.data))
|
||||
assert frame is not None, "BALANCE_VESC frame not received"
|
||||
parsed = parse_balance_vesc(bytes(frame.data))
|
||||
assert parsed["left_rpm_x10"] == 300
|
||||
assert parsed["right_rpm_x10"] == 300
|
||||
assert abs(parsed["left_rpm"] - 3000.0) < 0.1
|
||||
@ -109,7 +109,7 @@ class TestDriveTurn:
|
||||
"""
|
||||
_send_drive(mock_can_bus, 0.5, -0.5)
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 1
|
||||
|
||||
left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
|
||||
@ -119,14 +119,14 @@ class TestDriveTurn:
|
||||
assert left > 0 and right < 0
|
||||
|
||||
def test_drive_turn_fc_vesc_differential(self, mock_can_bus):
|
||||
"""Simulated FC_VESC for a turn has opposite-sign RPMs."""
|
||||
fc_payload = build_fc_vesc(
|
||||
"""Simulated BALANCE_VESC for a turn has opposite-sign RPMs."""
|
||||
fc_payload = build_balance_vesc(
|
||||
left_rpm_x10=150, right_rpm_x10=-150,
|
||||
left_current_x10=30, right_current_x10=30,
|
||||
)
|
||||
mock_can_bus.inject(FC_VESC, fc_payload)
|
||||
mock_can_bus.inject(BALANCE_VESC, fc_payload)
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
parsed = parse_fc_vesc(bytes(frame.data))
|
||||
parsed = parse_balance_vesc(bytes(frame.data))
|
||||
assert parsed["left_rpm_x10"] > 0
|
||||
assert parsed["right_rpm_x10"] < 0
|
||||
|
||||
@ -142,7 +142,7 @@ class TestDriveZero:
|
||||
|
||||
_send_drive(mock_can_bus, 0.0, 0.0)
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 1
|
||||
left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
|
||||
assert abs(left) < 1e-5, "Left motor not stopped"
|
||||
@ -156,7 +156,7 @@ class TestDriveCmdTimeout:
|
||||
zero velocity is sent. We test the encoding directly (without timers).
|
||||
"""
|
||||
# The watchdog in CanBridgeNode calls encode_velocity_cmd(0.0, 0.0) and
|
||||
# sends it on MAMBA_CMD_VELOCITY. Replicate that here.
|
||||
# sends it on BALANCE_CMD_VELOCITY. Replicate that here.
|
||||
zero_payload = encode_velocity_cmd(0.0, 0.0)
|
||||
|
||||
class _Msg:
|
||||
@ -165,16 +165,16 @@ class TestDriveCmdTimeout:
|
||||
self.data = bytearray(data)
|
||||
self.is_extended_id = False
|
||||
|
||||
mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, zero_payload))
|
||||
mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
|
||||
mock_can_bus.send(_Msg(BALANCE_CMD_VELOCITY, zero_payload))
|
||||
mock_can_bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 1
|
||||
left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
|
||||
assert abs(left) < 1e-5
|
||||
assert abs(right) < 1e-5
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert len(mode_frames) == 1
|
||||
assert bytes(mode_frames[0].data) == bytes([MODE_IDLE])
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@ Covers:
|
||||
- ESTOP command halts motors immediately
|
||||
- ESTOP persists: DRIVE commands ignored while ESTOP is active
|
||||
- ESTOP clear restores normal drive operation
|
||||
- Firmware-side estop via FC_STATUS flags is detected correctly
|
||||
- Firmware-side estop via BALANCE_STATUS flags is detected correctly
|
||||
|
||||
No ROS2 or real CAN hardware required.
|
||||
Run with: python -m pytest test/test_estop.py -v
|
||||
@ -17,20 +17,20 @@ import pytest
|
||||
|
||||
from saltybot_can_e2e_test.can_mock import MockCANBus
|
||||
from saltybot_can_e2e_test.protocol_defs import (
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_ESTOP,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_CMD_MODE,
|
||||
BALANCE_CMD_ESTOP,
|
||||
ORIN_CMD_ESTOP,
|
||||
FC_STATUS,
|
||||
BALANCE_STATUS,
|
||||
MODE_IDLE,
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
build_estop_cmd,
|
||||
build_mode_cmd,
|
||||
build_velocity_cmd,
|
||||
build_fc_status,
|
||||
build_balance_status,
|
||||
parse_velocity_cmd,
|
||||
parse_fc_status,
|
||||
parse_balance_status,
|
||||
)
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
encode_velocity_cmd,
|
||||
@ -68,16 +68,16 @@ class EstopStateMachine:
|
||||
"""Send ESTOP and transition to estop mode."""
|
||||
self._estop_active = True
|
||||
self._mode = MODE_ESTOP
|
||||
self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
|
||||
self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
|
||||
self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(True)))
|
||||
|
||||
def clear_estop(self) -> None:
|
||||
"""Clear ESTOP and return to IDLE mode."""
|
||||
self._estop_active = False
|
||||
self._mode = MODE_IDLE
|
||||
self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False)))
|
||||
self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(False)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
|
||||
|
||||
def send_drive(self, left_mps: float, right_mps: float) -> None:
|
||||
"""Send velocity command only if ESTOP is not active."""
|
||||
@ -85,8 +85,8 @@ class EstopStateMachine:
|
||||
# Bridge silently drops commands while estopped
|
||||
return
|
||||
self._mode = MODE_DRIVE
|
||||
self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
|
||||
self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
|
||||
|
||||
@property
|
||||
def estop_active(self) -> bool:
|
||||
@ -105,7 +105,7 @@ class TestEstopHaltsMotors:
|
||||
sm = EstopStateMachine(mock_can_bus)
|
||||
sm.assert_estop()
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) >= 1, "No velocity frame after ESTOP"
|
||||
l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
|
||||
assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP"
|
||||
@ -116,17 +116,17 @@ class TestEstopHaltsMotors:
|
||||
sm = EstopStateMachine(mock_can_bus)
|
||||
sm.assert_estop()
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert any(
|
||||
bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames
|
||||
), "MODE=ESTOP not found in sent frames"
|
||||
|
||||
def test_estop_flag_byte_is_0x01(self, mock_can_bus):
|
||||
"""MAMBA_CMD_ESTOP payload must be 0x01 when asserting e-stop."""
|
||||
"""BALANCE_CMD_ESTOP payload must be 0x01 when asserting e-stop."""
|
||||
sm = EstopStateMachine(mock_can_bus)
|
||||
sm.assert_estop()
|
||||
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP)
|
||||
assert len(estop_frames) >= 1
|
||||
assert bytes(estop_frames[-1].data) == b"\x01", \
|
||||
f"ESTOP payload {estop_frames[-1].data!r} != 0x01"
|
||||
@ -143,7 +143,7 @@ class TestEstopPersists:
|
||||
|
||||
sm.send_drive(1.0, 1.0) # should be suppressed
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 0, \
|
||||
"Velocity command was forwarded while ESTOP is active"
|
||||
|
||||
@ -158,7 +158,7 @@ class TestEstopPersists:
|
||||
sm.send_drive(0.5, 0.5)
|
||||
|
||||
# No mode frames should have been emitted (drive was suppressed)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert all(
|
||||
bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames
|
||||
), "MODE=DRIVE was set despite active ESTOP"
|
||||
@ -174,19 +174,19 @@ class TestEstopClear:
|
||||
|
||||
sm.send_drive(0.8, 0.8)
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear"
|
||||
l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
|
||||
assert abs(l - 0.8) < 1e-4
|
||||
assert abs(r - 0.8) < 1e-4
|
||||
|
||||
def test_estop_clear_flag_byte_is_0x00(self, mock_can_bus):
|
||||
"""MAMBA_CMD_ESTOP payload must be 0x00 when clearing e-stop."""
|
||||
"""BALANCE_CMD_ESTOP payload must be 0x00 when clearing e-stop."""
|
||||
sm = EstopStateMachine(mock_can_bus)
|
||||
sm.assert_estop()
|
||||
sm.clear_estop()
|
||||
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP)
|
||||
assert len(estop_frames) >= 2
|
||||
# Last ESTOP frame should be the clear
|
||||
assert bytes(estop_frames[-1].data) == b"\x00", \
|
||||
@ -198,7 +198,7 @@ class TestEstopClear:
|
||||
sm.assert_estop()
|
||||
sm.clear_estop()
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
last_mode = bytes(mode_frames[-1].data)
|
||||
assert last_mode == bytes([MODE_IDLE]), \
|
||||
f"Mode after ESTOP clear is {last_mode!r}, expected MODE_IDLE"
|
||||
@ -207,55 +207,55 @@ class TestEstopClear:
|
||||
class TestFirmwareSideEstop:
|
||||
def test_fc_status_estop_flag_detected(self, mock_can_bus):
|
||||
"""
|
||||
Simulate firmware sending estop via FC_STATUS flags (bit0=estop_active).
|
||||
Simulate firmware sending estop via BALANCE_STATUS flags (bit0=estop_active).
|
||||
Verify the Orin bridge side correctly parses the flag.
|
||||
"""
|
||||
# Build FC_STATUS with estop_active bit set (flags=0x01)
|
||||
payload = build_fc_status(
|
||||
# Build BALANCE_STATUS with estop_active bit set (flags=0x01)
|
||||
payload = build_balance_status(
|
||||
pitch_x10=0,
|
||||
motor_cmd=0,
|
||||
vbat_mv=24000,
|
||||
balance_state=2, # TILT_FAULT
|
||||
flags=0x01, # bit0 = estop_active
|
||||
)
|
||||
mock_can_bus.inject(FC_STATUS, payload)
|
||||
mock_can_bus.inject(BALANCE_STATUS, payload)
|
||||
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
assert frame is not None, "FC_STATUS frame not received"
|
||||
parsed = parse_fc_status(bytes(frame.data))
|
||||
assert frame is not None, "BALANCE_STATUS frame not received"
|
||||
parsed = parse_balance_status(bytes(frame.data))
|
||||
assert parsed["estop_active"] is True, \
|
||||
"estop_active flag not set in FC_STATUS"
|
||||
"estop_active flag not set in BALANCE_STATUS"
|
||||
assert parsed["balance_state"] == 2
|
||||
|
||||
def test_fc_status_no_estop_flag(self, mock_can_bus):
|
||||
"""FC_STATUS with flags=0x00 must NOT set estop_active."""
|
||||
payload = build_fc_status(flags=0x00)
|
||||
mock_can_bus.inject(FC_STATUS, payload)
|
||||
"""BALANCE_STATUS with flags=0x00 must NOT set estop_active."""
|
||||
payload = build_balance_status(flags=0x00)
|
||||
mock_can_bus.inject(BALANCE_STATUS, payload)
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
parsed = parse_fc_status(bytes(frame.data))
|
||||
parsed = parse_balance_status(bytes(frame.data))
|
||||
assert parsed["estop_active"] is False
|
||||
|
||||
def test_fc_status_armed_flag_detected(self, mock_can_bus):
|
||||
"""FC_STATUS flags bit1=armed must parse correctly."""
|
||||
payload = build_fc_status(flags=0x02) # bit1 = armed
|
||||
mock_can_bus.inject(FC_STATUS, payload)
|
||||
"""BALANCE_STATUS flags bit1=armed must parse correctly."""
|
||||
payload = build_balance_status(flags=0x02) # bit1 = armed
|
||||
mock_can_bus.inject(BALANCE_STATUS, payload)
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
parsed = parse_fc_status(bytes(frame.data))
|
||||
parsed = parse_balance_status(bytes(frame.data))
|
||||
assert parsed["armed"] is True
|
||||
assert parsed["estop_active"] is False
|
||||
|
||||
def test_fc_status_roundtrip(self, mock_can_bus):
|
||||
"""build_fc_status → inject → recv → parse_fc_status must be identity."""
|
||||
payload = build_fc_status(
|
||||
"""build_balance_status → inject → recv → parse_balance_status must be identity."""
|
||||
payload = build_balance_status(
|
||||
pitch_x10=150,
|
||||
motor_cmd=-200,
|
||||
vbat_mv=23800,
|
||||
balance_state=1,
|
||||
flags=0x03,
|
||||
)
|
||||
mock_can_bus.inject(FC_STATUS, payload)
|
||||
mock_can_bus.inject(BALANCE_STATUS, payload)
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
parsed = parse_fc_status(bytes(frame.data))
|
||||
parsed = parse_balance_status(bytes(frame.data))
|
||||
assert parsed["pitch_x10"] == 150
|
||||
assert parsed["motor_cmd"] == -200
|
||||
assert parsed["vbat_mv"] == 23800
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests.
|
||||
test_fc_vesc_broadcast.py — BALANCE_VESC broadcast and VESC STATUS integration tests.
|
||||
|
||||
Covers:
|
||||
- VESC STATUS extended frame for left VESC (ID 56) triggers FC_VESC broadcast
|
||||
- Both left (56) and right (68) VESC STATUS combined in FC_VESC
|
||||
- FC_VESC broadcast rate (~10 Hz)
|
||||
- VESC STATUS extended frame for left VESC (ID 56) triggers BALANCE_VESC broadcast
|
||||
- Both left (56) and right (68) VESC STATUS combined in BALANCE_VESC
|
||||
- BALANCE_VESC broadcast rate (~10 Hz)
|
||||
- current_x10 scaling matches protocol spec
|
||||
|
||||
No ROS2 or real CAN hardware required.
|
||||
@ -19,15 +19,15 @@ import pytest
|
||||
|
||||
from saltybot_can_e2e_test.can_mock import MockCANBus
|
||||
from saltybot_can_e2e_test.protocol_defs import (
|
||||
FC_VESC,
|
||||
BALANCE_VESC,
|
||||
VESC_CAN_ID_LEFT,
|
||||
VESC_CAN_ID_RIGHT,
|
||||
VESC_STATUS_ID,
|
||||
VESC_SET_RPM_ID,
|
||||
VESC_TELEM_STATE,
|
||||
build_vesc_status,
|
||||
build_fc_vesc,
|
||||
parse_fc_vesc,
|
||||
build_balance_vesc,
|
||||
parse_balance_vesc,
|
||||
parse_vesc_status,
|
||||
)
|
||||
from saltybot_can_bridge.balance_protocol import (
|
||||
@ -44,8 +44,8 @@ class VescStatusAggregator:
|
||||
"""
|
||||
Simulates the firmware logic that:
|
||||
1. Receives VESC STATUS extended frames from left/right VESCs
|
||||
2. Builds an FC_VESC broadcast payload
|
||||
3. Injects the FC_VESC frame onto the mock bus
|
||||
2. Builds an BALANCE_VESC broadcast payload
|
||||
3. Injects the BALANCE_VESC frame onto the mock bus
|
||||
|
||||
This represents the ESP32-S3 BALANCE → Orin telemetry path.
|
||||
"""
|
||||
@ -62,7 +62,7 @@ class VescStatusAggregator:
|
||||
def process_vesc_status(self, arb_id: int, data: bytes) -> None:
|
||||
"""
|
||||
Process an incoming VESC STATUS frame (extended 29-bit ID).
|
||||
Updates internal state; broadcasts FC_VESC when at least one side is known.
|
||||
Updates internal state; broadcasts BALANCE_VESC when at least one side is known.
|
||||
"""
|
||||
node_id = arb_id & 0xFF
|
||||
parsed = parse_vesc_status(data)
|
||||
@ -77,17 +77,17 @@ class VescStatusAggregator:
|
||||
self._right_current_x10 = parsed["current_x10"]
|
||||
self._right_seen = True
|
||||
|
||||
# Broadcast FC_VESC whenever we receive any update
|
||||
# Broadcast BALANCE_VESC whenever we receive any update
|
||||
self._broadcast_fc_vesc()
|
||||
|
||||
def _broadcast_fc_vesc(self) -> None:
|
||||
payload = build_fc_vesc(
|
||||
payload = build_balance_vesc(
|
||||
left_rpm_x10=self._left_rpm_x10,
|
||||
right_rpm_x10=self._right_rpm_x10,
|
||||
left_current_x10=self._left_current_x10,
|
||||
right_current_x10=self._right_current_x10,
|
||||
)
|
||||
self._bus.inject(FC_VESC, payload)
|
||||
self._bus.inject(BALANCE_VESC, payload)
|
||||
|
||||
|
||||
def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int,
|
||||
@ -105,7 +105,7 @@ def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int,
|
||||
class TestVescStatusToFcVesc:
|
||||
def test_left_vesc_status_triggers_broadcast(self, mock_can_bus):
|
||||
"""
|
||||
Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains
|
||||
Inject VESC STATUS for left VESC (ID 56) → verify BALANCE_VESC contains
|
||||
the correct left RPM (rpm / 10).
|
||||
"""
|
||||
agg = VescStatusAggregator(mock_can_bus)
|
||||
@ -116,14 +116,14 @@ class TestVescStatusToFcVesc:
|
||||
agg.process_vesc_status(arb_id, payload)
|
||||
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
assert frame is not None, "No FC_VESC broadcast after left VESC STATUS"
|
||||
parsed = parse_fc_vesc(bytes(frame.data))
|
||||
assert frame is not None, "No BALANCE_VESC broadcast after left VESC STATUS"
|
||||
parsed = parse_balance_vesc(bytes(frame.data))
|
||||
assert parsed["left_rpm_x10"] == 300, \
|
||||
f"left_rpm_x10 {parsed['left_rpm_x10']} != 300"
|
||||
assert abs(parsed["left_rpm"] - 3000.0) < 1.0
|
||||
|
||||
def test_right_vesc_status_triggers_broadcast(self, mock_can_bus):
|
||||
"""Inject VESC STATUS for right VESC (ID 68) → verify right RPM in FC_VESC."""
|
||||
"""Inject VESC STATUS for right VESC (ID 68) → verify right RPM in BALANCE_VESC."""
|
||||
agg = VescStatusAggregator(mock_can_bus)
|
||||
|
||||
arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT)
|
||||
@ -132,7 +132,7 @@ class TestVescStatusToFcVesc:
|
||||
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
assert frame is not None
|
||||
parsed = parse_fc_vesc(bytes(frame.data))
|
||||
parsed = parse_balance_vesc(bytes(frame.data))
|
||||
assert parsed["right_rpm_x10"] == 200
|
||||
|
||||
def test_left_vesc_id_matches_constant(self):
|
||||
@ -150,7 +150,7 @@ class TestBothVescStatusCombined:
|
||||
def test_both_vesc_status_combined_in_fc_vesc(self, mock_can_bus):
|
||||
"""
|
||||
Inject both left (56) and right (68) VESC STATUS frames.
|
||||
Final FC_VESC must contain both RPMs.
|
||||
Final BALANCE_VESC must contain both RPMs.
|
||||
"""
|
||||
agg = VescStatusAggregator(mock_can_bus)
|
||||
|
||||
@ -165,7 +165,7 @@ class TestBothVescStatusCombined:
|
||||
build_vesc_status(rpm=-1500, current_x10=30),
|
||||
)
|
||||
|
||||
# Drain two FC_VESC frames (one per update), check the latest
|
||||
# Drain two BALANCE_VESC frames (one per update), check the latest
|
||||
frames = []
|
||||
while True:
|
||||
f = mock_can_bus.recv(timeout=0.05)
|
||||
@ -173,16 +173,16 @@ class TestBothVescStatusCombined:
|
||||
break
|
||||
frames.append(f)
|
||||
|
||||
assert len(frames) >= 2, "Expected at least 2 FC_VESC frames"
|
||||
assert len(frames) >= 2, "Expected at least 2 BALANCE_VESC frames"
|
||||
# Last frame must have both sides
|
||||
last = parse_fc_vesc(bytes(frames[-1].data))
|
||||
last = parse_balance_vesc(bytes(frames[-1].data))
|
||||
assert last["left_rpm_x10"] == 300, \
|
||||
f"left_rpm_x10 {last['left_rpm_x10']} != 300"
|
||||
assert last["right_rpm_x10"] == -150, \
|
||||
f"right_rpm_x10 {last['right_rpm_x10']} != -150"
|
||||
|
||||
def test_both_vesc_currents_combined(self, mock_can_bus):
|
||||
"""Both current values must appear in FC_VESC after two STATUS frames."""
|
||||
"""Both current values must appear in BALANCE_VESC after two STATUS frames."""
|
||||
agg = VescStatusAggregator(mock_can_bus)
|
||||
agg.process_vesc_status(
|
||||
VESC_STATUS_ID(VESC_CAN_ID_LEFT),
|
||||
@ -198,7 +198,7 @@ class TestBothVescStatusCombined:
|
||||
if f is None:
|
||||
break
|
||||
frames.append(f)
|
||||
last = parse_fc_vesc(bytes(frames[-1].data))
|
||||
last = parse_balance_vesc(bytes(frames[-1].data))
|
||||
assert last["left_current_x10"] == 55
|
||||
assert last["right_current_x10"] == 42
|
||||
|
||||
@ -206,11 +206,11 @@ class TestBothVescStatusCombined:
|
||||
class TestVescBroadcastRate:
|
||||
def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus):
|
||||
"""
|
||||
Simulate FC_VESC broadcasts at ~10 Hz and verify the rate.
|
||||
Simulate BALANCE_VESC broadcasts at ~10 Hz and verify the rate.
|
||||
We inject 12 frames over ~120 ms, then verify count and average interval.
|
||||
"""
|
||||
_FC_VESC_HZ = 10
|
||||
_interval = 1.0 / _FC_VESC_HZ
|
||||
_BALANCE_VESC_HZ = 10
|
||||
_interval = 1.0 / _BALANCE_VESC_HZ
|
||||
|
||||
timestamps = []
|
||||
stop_event = threading.Event()
|
||||
@ -219,8 +219,8 @@ class TestVescBroadcastRate:
|
||||
while not stop_event.is_set():
|
||||
t = time.monotonic()
|
||||
mock_can_bus.inject(
|
||||
FC_VESC,
|
||||
build_fc_vesc(100, -100, 30, 30),
|
||||
BALANCE_VESC,
|
||||
build_balance_vesc(100, -100, 30, 30),
|
||||
timestamp=t,
|
||||
)
|
||||
timestamps.append(t)
|
||||
@ -232,18 +232,18 @@ class TestVescBroadcastRate:
|
||||
stop_event.set()
|
||||
t.join(timeout=0.2)
|
||||
|
||||
assert len(timestamps) >= 1, "No FC_VESC broadcasts in 150 ms window"
|
||||
assert len(timestamps) >= 1, "No BALANCE_VESC broadcasts in 150 ms window"
|
||||
|
||||
if len(timestamps) >= 2:
|
||||
intervals = [timestamps[i+1] - timestamps[i] for i in range(len(timestamps)-1)]
|
||||
avg = sum(intervals) / len(intervals)
|
||||
# ±40 ms tolerance for OS scheduling
|
||||
assert 0.06 <= avg <= 0.14, \
|
||||
f"FC_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms"
|
||||
f"BALANCE_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms"
|
||||
|
||||
def test_fc_vesc_frame_is_8_bytes(self):
|
||||
"""FC_VESC payload must always be exactly 8 bytes."""
|
||||
payload = build_fc_vesc(300, -150, 55, 42)
|
||||
"""BALANCE_VESC payload must always be exactly 8 bytes."""
|
||||
payload = build_balance_vesc(300, -150, 55, 42)
|
||||
assert len(payload) == 8
|
||||
|
||||
|
||||
@ -267,16 +267,16 @@ class TestVescCurrentScaling:
|
||||
assert abs(parsed["current"] - (-3.0)) < 0.01
|
||||
|
||||
def test_fc_vesc_current_x10_roundtrip(self, mock_can_bus):
|
||||
"""build_fc_vesc → inject → recv → parse must preserve current_x10."""
|
||||
payload = build_fc_vesc(
|
||||
"""build_balance_vesc → inject → recv → parse must preserve current_x10."""
|
||||
payload = build_balance_vesc(
|
||||
left_rpm_x10=200,
|
||||
right_rpm_x10=200,
|
||||
left_current_x10=55,
|
||||
right_current_x10=42,
|
||||
)
|
||||
mock_can_bus.inject(FC_VESC, payload)
|
||||
mock_can_bus.inject(BALANCE_VESC, payload)
|
||||
frame = mock_can_bus.recv(timeout=0.1)
|
||||
parsed = parse_fc_vesc(bytes(frame.data))
|
||||
parsed = parse_balance_vesc(bytes(frame.data))
|
||||
assert parsed["left_current_x10"] == 55
|
||||
assert parsed["right_current_x10"] == 42
|
||||
|
||||
@ -306,7 +306,7 @@ class TestVescCurrentScaling:
|
||||
)
|
||||
frame = mock_can_bus.recv(timeout=0.05)
|
||||
assert frame is not None
|
||||
parsed = parse_fc_vesc(bytes(frame.data))
|
||||
parsed = parse_balance_vesc(bytes(frame.data))
|
||||
if vesc_id == VESC_CAN_ID_LEFT:
|
||||
assert parsed["left_rpm_x10"] == expected_rpm_x10, \
|
||||
f"left_rpm_x10={parsed['left_rpm_x10']} expected {expected_rpm_x10}"
|
||||
|
||||
@ -21,9 +21,9 @@ from saltybot_can_e2e_test.protocol_defs import (
|
||||
ORIN_CMD_HEARTBEAT,
|
||||
ORIN_CMD_ESTOP,
|
||||
ORIN_CMD_MODE,
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_ESTOP,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_CMD_MODE,
|
||||
BALANCE_CMD_ESTOP,
|
||||
MODE_IDLE,
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
@ -100,9 +100,9 @@ def _simulate_estop_on_timeout(bus: MockCANBus) -> None:
|
||||
self.data = bytearray(data)
|
||||
self.is_extended_id = False
|
||||
|
||||
bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
|
||||
bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
|
||||
bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
|
||||
bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
|
||||
bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
|
||||
bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(True)))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
@ -121,25 +121,25 @@ class TestHeartbeatLoss:
|
||||
# Simulate bridge detecting timeout and escalating
|
||||
_simulate_estop_on_timeout(mock_can_bus)
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) >= 1, "Zero velocity not sent after timeout"
|
||||
l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
|
||||
assert abs(l) < 1e-5, "Left not zero on timeout"
|
||||
assert abs(r) < 1e-5, "Right not zero on timeout"
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert any(
|
||||
bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames
|
||||
), "ESTOP mode not asserted on heartbeat timeout"
|
||||
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP)
|
||||
assert len(estop_frames) >= 1, "ESTOP command not sent"
|
||||
assert bytes(estop_frames[0].data) == b"\x01"
|
||||
|
||||
def test_heartbeat_loss_zero_velocity(self, mock_can_bus):
|
||||
"""Zero velocity frame must appear among sent frames after timeout."""
|
||||
_simulate_estop_on_timeout(mock_can_bus)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) >= 1
|
||||
for f in vel_frames:
|
||||
l, r = parse_velocity_cmd(bytes(f.data))
|
||||
@ -165,20 +165,20 @@ class TestHeartbeatRecovery:
|
||||
mock_can_bus.reset()
|
||||
|
||||
# Phase 2: recovery — clear estop, restore drive mode
|
||||
mock_can_bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False)))
|
||||
mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
|
||||
mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5)))
|
||||
mock_can_bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(False)))
|
||||
mock_can_bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
|
||||
mock_can_bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5)))
|
||||
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
|
||||
estop_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_ESTOP)
|
||||
assert any(bytes(f.data) == b"\x00" for f in estop_frames), \
|
||||
"ESTOP clear not sent on recovery"
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert any(
|
||||
bytes(f.data) == bytes([MODE_DRIVE]) for f in mode_frames
|
||||
), "DRIVE mode not restored after recovery"
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) >= 1
|
||||
l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
|
||||
assert abs(l - 0.5) < 1e-4
|
||||
|
||||
@ -17,9 +17,9 @@ import pytest
|
||||
|
||||
from saltybot_can_e2e_test.can_mock import MockCANBus
|
||||
from saltybot_can_e2e_test.protocol_defs import (
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_CMD_MODE,
|
||||
MAMBA_CMD_ESTOP,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_CMD_MODE,
|
||||
BALANCE_CMD_ESTOP,
|
||||
MODE_IDLE,
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
@ -64,12 +64,12 @@ class ModeStateMachine:
|
||||
|
||||
prev_mode = self._mode
|
||||
self._mode = mode
|
||||
self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(mode)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_MODE, encode_mode_cmd(mode)))
|
||||
|
||||
# Side-effects of entering ESTOP from DRIVE
|
||||
if mode == MODE_ESTOP and prev_mode == MODE_DRIVE:
|
||||
self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
|
||||
self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_ESTOP, encode_estop_cmd(True)))
|
||||
|
||||
return True
|
||||
|
||||
@ -79,7 +79,7 @@ class ModeStateMachine:
|
||||
"""
|
||||
if self._mode != MODE_DRIVE:
|
||||
return False
|
||||
self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
|
||||
self._bus.send(_Msg(BALANCE_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
|
||||
return True
|
||||
|
||||
@property
|
||||
@ -97,7 +97,7 @@ class TestIdleToDrive:
|
||||
sm = ModeStateMachine(mock_can_bus)
|
||||
sm.set_mode(MODE_DRIVE)
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert len(mode_frames) == 1
|
||||
assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE])
|
||||
|
||||
@ -108,7 +108,7 @@ class TestIdleToDrive:
|
||||
forwarded = sm.send_drive(1.0, 1.0)
|
||||
assert forwarded is False, "Drive cmd should be blocked in IDLE mode"
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 0
|
||||
|
||||
def test_drive_mode_allows_commands(self, mock_can_bus):
|
||||
@ -120,7 +120,7 @@ class TestIdleToDrive:
|
||||
forwarded = sm.send_drive(0.5, 0.5)
|
||||
assert forwarded is True
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 1
|
||||
l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
|
||||
assert abs(l - 0.5) < 1e-4
|
||||
@ -137,7 +137,7 @@ class TestDriveToEstop:
|
||||
|
||||
sm.set_mode(MODE_ESTOP)
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) >= 1, "No velocity frame on DRIVE→ESTOP transition"
|
||||
l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
|
||||
assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP"
|
||||
@ -149,7 +149,7 @@ class TestDriveToEstop:
|
||||
sm.set_mode(MODE_DRIVE)
|
||||
sm.set_mode(MODE_ESTOP)
|
||||
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
|
||||
mode_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_MODE)
|
||||
assert any(bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames)
|
||||
|
||||
def test_estop_blocks_subsequent_drive(self, mock_can_bus):
|
||||
@ -162,7 +162,7 @@ class TestDriveToEstop:
|
||||
forwarded = sm.send_drive(1.0, 1.0)
|
||||
assert forwarded is False
|
||||
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
|
||||
vel_frames = mock_can_bus.get_sent_frames_by_id(BALANCE_CMD_VELOCITY)
|
||||
assert len(vel_frames) == 0
|
||||
|
||||
|
||||
|
||||
@ -27,12 +27,7 @@ robot:
|
||||
stem_od: 0.0381 # m STEM_OD = 38.1mm
|
||||
stem_height: 1.050 # m nominal cut length
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ── FC / IMU (ESP32 BALANCE) ──────────────────────────────────────────────────
|
||||
=======
|
||||
# ── FC / IMU (ESP32-S3 BALANCE) ──────────────────────────────────────────────────
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105)
|
||||
# ── FC / IMU (ESP32-S3 BALANCE) ────────────────────────────────────────────────── # fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105)
|
||||
# z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm
|
||||
imu_x: 0.050 # m forward of base_link center
|
||||
imu_y: 0.000 # m
|
||||
|
||||
@ -5,12 +5,7 @@ Comprehensive hardware diagnostics and health monitoring for SaltyBot.
|
||||
## Features
|
||||
|
||||
### Startup Checks
|
||||
<<<<<<< HEAD
|
||||
- RPLIDAR, RealSense, VESC, Jabra mic, ESP32 BALANCE, servos
|
||||
=======
|
||||
- RPLIDAR, RealSense, VESC, Jabra mic, ESP32-S3, servos
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
- WiFi, GPS, disk space, RAM
|
||||
- RPLIDAR, RealSense, VESC, Jabra mic, ESP32-S3, servos- WiFi, GPS, disk space, RAM
|
||||
- Boot result TTS + face animation
|
||||
- JSON logging
|
||||
|
||||
|
||||
@ -138,12 +138,7 @@ class DiagnosticsNode(Node):
|
||||
self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {})
|
||||
|
||||
def _check_stm32(self):
|
||||
<<<<<<< HEAD
|
||||
self.hardware_checks["stm32"] = ("OK", "ESP32 bridge online", {})
|
||||
=======
|
||||
self.hardware_checks["stm32"] = ("OK", "ESP32-S3 bridge online", {})
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
def _check_servos(self):
|
||||
try:
|
||||
result = subprocess.run(["i2cdetect", "-y", "1"], capture_output=True, text=True, timeout=2)
|
||||
|
||||
@ -7,12 +7,7 @@
|
||||
# ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.2
|
||||
#
|
||||
# IMPORTANT: This node publishes raw /cmd_vel. The cmd_vel_bridge_node (PR #46)
|
||||
<<<<<<< HEAD
|
||||
# applies the ESC ramp, deadman switch, and ESP32 BALANCE AUTONOMOUS mode gate.
|
||||
=======
|
||||
# applies the ESC ramp, deadman switch, and ESP32-S3 AUTONOMOUS mode gate.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# Do not run this node without the cmd_vel bridge running on the same robot.
|
||||
# applies the ESC ramp, deadman switch, and ESP32-S3 AUTONOMOUS mode gate.# Do not run this node without the cmd_vel bridge running on the same robot.
|
||||
|
||||
# ── Follow geometry ────────────────────────────────────────────────────────────
|
||||
# The target distance to maintain behind the person (metres).
|
||||
@ -74,9 +69,4 @@ control_rate: 20.0 # Hz — lower than cmd_vel bridge (50Hz) by desig
|
||||
# ── Mode integration ──────────────────────────────────────────────────────────
|
||||
# Master enable for the follow controller. When false, node publishes zero cmd_vel.
|
||||
# Toggle at runtime: ros2 param set /person_follower follow_enabled false
|
||||
<<<<<<< HEAD
|
||||
# The cmd_vel bridge independently gates on ESP32 BALANCE AUTONOMOUS mode (md=2).
|
||||
=======
|
||||
# The cmd_vel bridge independently gates on ESP32-S3 AUTONOMOUS mode (md=2).
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
follow_enabled: true
|
||||
# The cmd_vel bridge independently gates on ESP32-S3 AUTONOMOUS mode (md=2).follow_enabled: true
|
||||
|
||||
@ -28,12 +28,7 @@ State machine
|
||||
|
||||
Safety wiring
|
||||
-------------
|
||||
<<<<<<< HEAD
|
||||
* cmd_vel bridge (PR #46) applies ramp + deadman + ESP32 BALANCE AUTONOMOUS mode gate --
|
||||
=======
|
||||
* cmd_vel bridge (PR #46) applies ramp + deadman + ESP32-S3 AUTONOMOUS mode gate --
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
this node publishes raw /cmd_vel, the bridge handles hardware safety.
|
||||
* cmd_vel bridge (PR #46) applies ramp + deadman + ESP32-S3 AUTONOMOUS mode gate -- this node publishes raw /cmd_vel, the bridge handles hardware safety.
|
||||
* follow_enabled param (default True) lets the operator disable the controller
|
||||
at runtime: ros2 param set /person_follower follow_enabled false
|
||||
* Obstacle override: if Nav2 local costmap shows a lethal cell in the forward
|
||||
|
||||
@ -1,11 +1,6 @@
|
||||
gimbal_node:
|
||||
ros__parameters:
|
||||
<<<<<<< HEAD
|
||||
# Serial port connecting to ESP32 BALANCE over JLINK protocol
|
||||
=======
|
||||
# Serial port connecting to ESP32-S3 over JLINK protocol
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
serial_port: "/dev/ttyTHS1"
|
||||
# Serial port connecting to ESP32-S3 over JLINK protocol serial_port: "/dev/ttyTHS1"
|
||||
baud_rate: 921600
|
||||
|
||||
# Soft angle limits (degrees, ± from center)
|
||||
|
||||
@ -14,12 +14,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/ttyTHS1",
|
||||
<<<<<<< HEAD
|
||||
description="JLINK serial port to ESP32 BALANCE",
|
||||
=======
|
||||
description="JLINK serial port to ESP32-S3",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
)
|
||||
description="JLINK serial port to ESP32-S3", )
|
||||
pan_limit_arg = DeclareLaunchArgument(
|
||||
"pan_limit_deg",
|
||||
default_value="150.0",
|
||||
|
||||
@ -1,12 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
"""gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548).
|
||||
|
||||
<<<<<<< HEAD
|
||||
Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32 BALANCE.
|
||||
=======
|
||||
Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32-S3.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
Implements smooth trapezoidal motion profiles with configurable axis limits.
|
||||
Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32-S3.Implements smooth trapezoidal motion profiles with configurable axis limits.
|
||||
|
||||
Subscribed topics:
|
||||
/saltybot/gimbal/cmd (geometry_msgs/Vector3) x=pan_deg, y=tilt_deg, z=speed_deg_s
|
||||
|
||||
@ -1,25 +1,13 @@
|
||||
"""jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548).
|
||||
|
||||
<<<<<<< HEAD
|
||||
Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32 side).
|
||||
|
||||
Command type (Jetson → ESP32 BALANCE):
|
||||
=======
|
||||
Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32-S3 side).
|
||||
|
||||
Command type (Jetson → ESP32-S3):
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
0x0B GIMBAL_POS — int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes)
|
||||
Command type (Jetson → ESP32-S3): 0x0B GIMBAL_POS — int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes)
|
||||
pan_x10 = pan_deg * 10 (±1500 for ±150°)
|
||||
tilt_x10 = tilt_deg * 10 (±450 for ±45°)
|
||||
speed = servo speed register 0–4095 (0 = max)
|
||||
|
||||
<<<<<<< HEAD
|
||||
Telemetry type (ESP32 BALANCE → Jetson):
|
||||
=======
|
||||
Telemetry type (ESP32-S3 → Jetson):
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
0x84 GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
|
||||
Telemetry type (ESP32-S3 → Jetson): 0x84 GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
|
||||
uint16 pan_speed_raw + uint16 tilt_speed_raw +
|
||||
uint8 torque_en + uint8 rx_err_pct (10 bytes)
|
||||
|
||||
@ -41,14 +29,8 @@ ETX = 0x03
|
||||
|
||||
# ── Command / telemetry type codes ─────────────────────────────────────────────
|
||||
|
||||
<<<<<<< HEAD
|
||||
CMD_GIMBAL_POS = 0x0B # Jetson → ESP32 BALANCE: set pan/tilt target
|
||||
TLM_GIMBAL_STATE = 0x84 # ESP32 BALANCE → Jetson: measured state
|
||||
=======
|
||||
CMD_GIMBAL_POS = 0x0B # Jetson → ESP32-S3: set pan/tilt target
|
||||
TLM_GIMBAL_STATE = 0x84 # ESP32-S3 → Jetson: measured state
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
# Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
|
||||
# Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))
|
||||
_MAX_SPEED_DEGS = 360.0 # degrees/sec at speed_reg=0
|
||||
|
||||
@ -5,12 +5,7 @@
|
||||
#
|
||||
# Topic wiring:
|
||||
# /rc/joy → mode_switch_node (CRSF channels)
|
||||
<<<<<<< HEAD
|
||||
# /saltybot/balance_state → mode_switch_node (ESP32 BALANCE state)
|
||||
=======
|
||||
# /saltybot/balance_state → mode_switch_node (ESP32-S3 state)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix)
|
||||
# /saltybot/balance_state → mode_switch_node (ESP32-S3 state)# /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix)
|
||||
# /saltybot/control_mode ← mode_switch_node (JSON mode + alpha)
|
||||
# /saltybot/led_pattern ← mode_switch_node (LED name)
|
||||
#
|
||||
|
||||
@ -13,12 +13,7 @@ Topic graph
|
||||
|
||||
In RC mode (blend_alpha ≈ 0) the node publishes Twist(0,0) so the bridge
|
||||
receives zeros — this is harmless because the bridge's mode gate already
|
||||
<<<<<<< HEAD
|
||||
prevents autonomous commands when the ESP32 BALANCE is in RC_MANUAL.
|
||||
=======
|
||||
prevents autonomous commands when the ESP32-S3 is in RC_MANUAL.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
The bridge's existing ESC ramp handles hardware-level smoothing;
|
||||
the blend_alpha here provides the higher-level cmd_vel policy ramp.
|
||||
|
||||
|
||||
@ -6,16 +6,9 @@ state machine can be exercised in unit tests without a ROS2 runtime.
|
||||
|
||||
Mode vocabulary
|
||||
---------------
|
||||
<<<<<<< HEAD
|
||||
"RC" — ESP32 BALANCE executing RC pilot commands; Jetson cmd_vel blocked.
|
||||
"RAMP_TO_AUTO" — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
|
||||
"AUTO" — ESP32 BALANCE executing Jetson cmd_vel; RC sticks idle.
|
||||
=======
|
||||
"RC" — ESP32-S3 executing RC pilot commands; Jetson cmd_vel blocked.
|
||||
"RAMP_TO_AUTO" — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
|
||||
"AUTO" — ESP32-S3 executing Jetson cmd_vel; RC sticks idle.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
"RAMP_TO_RC" — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
|
||||
"AUTO" — ESP32-S3 executing Jetson cmd_vel; RC sticks idle. "RAMP_TO_RC" — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
|
||||
|
||||
Blend alpha
|
||||
-----------
|
||||
|
||||
@ -9,12 +9,7 @@ Inputs
|
||||
axes[stick_axes...] Roll/Pitch/Throttle/Yaw — override detection
|
||||
|
||||
/saltybot/balance_state (std_msgs/String JSON)
|
||||
<<<<<<< HEAD
|
||||
Parsed for RC link health (field "rc_link") and ESP32 BALANCE mode.
|
||||
=======
|
||||
Parsed for RC link health (field "rc_link") and ESP32-S3 mode.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
<slam_fix_topic> (geometry_msgs/PoseWithCovarianceStamped)
|
||||
Any message received within slam_fix_timeout_s → SLAM fix valid.
|
||||
|
||||
|
||||
@ -1,14 +1,8 @@
|
||||
vesc_can_odometry:
|
||||
ros__parameters:
|
||||
# ── CAN motor IDs (used for CAN addressing) ───────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
left_can_id: 56 # left motor VESC CAN ID (ESP32 BALANCE)
|
||||
right_can_id: 68 # right motor VESC CAN ID (ESP32 BALANCE)
|
||||
=======
|
||||
left_can_id: 56 # left motor VESC CAN ID (ESP32-S3 BALANCE)
|
||||
right_can_id: 68 # right motor VESC CAN ID (ESP32-S3 BALANCE)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
# ── State topic names (must match VESC telemetry publisher) ──────────────
|
||||
left_state_topic: /vesc/left/state
|
||||
right_state_topic: /vesc/right/state
|
||||
|
||||
@ -12,12 +12,7 @@
|
||||
# Hardware:
|
||||
# IMU: RealSense D435i BMI055 → /imu/data
|
||||
# GPS: SIM7600X cellular → /gps/fix (±2.5 m CEP)
|
||||
<<<<<<< HEAD
|
||||
# Odom: ESP32 BALANCE wheel encoders → /odom
|
||||
=======
|
||||
# Odom: ESP32-S3 wheel encoders → /odom
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# RTK: ZED-F9P (optional) → /gps/fix (±2 cm CEP when use_rtk: true)
|
||||
# Odom: ESP32-S3 wheel encoders → /odom# RTK: ZED-F9P (optional) → /gps/fix (±2 cm CEP when use_rtk: true)
|
||||
|
||||
# ── Local EKF: fuses wheel odometry + IMU in odom frame ──────────────────────
|
||||
ekf_filter_node_odom:
|
||||
|
||||
@ -71,12 +71,7 @@ class ParameterServer(Node):
|
||||
defs = {
|
||||
'hardware': {
|
||||
'serial_port': ParamInfo('serial_port', '/dev/esp32-bridge', 'string',
|
||||
<<<<<<< HEAD
|
||||
'hardware', description='ESP32 bridge serial port'),
|
||||
=======
|
||||
'hardware', description='ESP32-S3 bridge serial port'),
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware',
|
||||
'hardware', description='ESP32-S3 bridge serial port'), 'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware',
|
||||
min_val=9600, max_val=3000000,
|
||||
description='Serial baud rate'),
|
||||
'timeout': ParamInfo('timeout', 0.05, 'float', 'hardware',
|
||||
|
||||
@ -370,12 +370,7 @@ class PIDAutotuneNode(Node):
|
||||
ser.write(frame_set)
|
||||
time.sleep(0.05) # allow FC to process PID_SET
|
||||
ser.write(frame_save)
|
||||
<<<<<<< HEAD
|
||||
# Flash erase takes ~1s on ESP32; wait for it
|
||||
=======
|
||||
# Flash erase takes ~1s on ESP32-S3; wait for it
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
time.sleep(1.5)
|
||||
# Flash erase takes ~1s on ESP32-S3; wait for it time.sleep(1.5)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Sent PID_SET+PID_SAVE to FC via {self._jlink_port}: "
|
||||
|
||||
@ -9,12 +9,7 @@
|
||||
#
|
||||
# GPS source: SIM7600X → /gps/fix (NavSatFix, ±2.5m CEP) — PR #65
|
||||
# Heading: D435i IMU → /imu/data, converted yaw → route waypoint heading_deg
|
||||
<<<<<<< HEAD
|
||||
# Odometry: ESP32 BALANCE wheel encoders → /odom
|
||||
=======
|
||||
# Odometry: ESP32-S3 wheel encoders → /odom
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# UWB: /uwb/target (follow-me reference, logged for context)
|
||||
# Odometry: ESP32-S3 wheel encoders → /odom# UWB: /uwb/target (follow-me reference, logged for context)
|
||||
|
||||
route_recorder:
|
||||
ros__parameters:
|
||||
|
||||
@ -10,12 +10,7 @@ Depends on:
|
||||
saltybot-nav2 container (Nav2 action server /navigate_through_poses)
|
||||
saltybot_cellular (/gps/fix from SIM7600X GPS — PR #65)
|
||||
saltybot_uwb (/uwb/target — PR #66, used for context during recording)
|
||||
<<<<<<< HEAD
|
||||
ESP32 bridge (/odom from wheel encoders)
|
||||
=======
|
||||
ESP32-S3 bridge (/odom from wheel encoders)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
D435i (/imu/data for heading)
|
||||
ESP32-S3 bridge (/odom from wheel encoders) D435i (/imu/data for heading)
|
||||
|
||||
Usage — record a route:
|
||||
# Set name, start recording, ride with Tee, stop and save:
|
||||
|
||||
@ -5,12 +5,7 @@ Hardware
|
||||
────────
|
||||
SaltyRover: 4-wheel ground robot with individual brushless ESCs.
|
||||
ESCs controlled via PWM (servo-style 1000–2000 µs pulses).
|
||||
<<<<<<< HEAD
|
||||
Communication: USB CDC serial to ESP32 BALANCE or Raspberry Pi Pico GPIO PWM bridge.
|
||||
=======
|
||||
Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
ESC channel assignments (configurable):
|
||||
CH1 = left-front
|
||||
CH2 = left-rear
|
||||
|
||||
@ -39,12 +39,6 @@ safety_zone:
|
||||
# ── cmd_vel topics ───────────────────────────────────────────────────────
|
||||
# Safety zone node intercepts cmd_vel from upstream, overrides to zero on estop.
|
||||
# Typical chain:
|
||||
<<<<<<< HEAD
|
||||
# cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → ESP32 BALANCE
|
||||
cmd_vel_input_topic: /cmd_vel_input # upstream velocity (remap as needed)
|
||||
cmd_vel_output_topic: /cmd_vel # downstream (to ESP32 bridge)
|
||||
=======
|
||||
# cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → ESP32-S3
|
||||
cmd_vel_input_topic: /cmd_vel_input # upstream velocity (remap as needed)
|
||||
cmd_vel_output_topic: /cmd_vel # downstream (to ESP32-S3 bridge)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
@ -143,14 +143,7 @@ class SocialEnrollmentNode(Node):
|
||||
self.create_timer(0.5, self._enrollment_timeout_check)
|
||||
|
||||
self.get_logger().info(
|
||||
<<<<<<< HEAD
|
||||
f'Social enrollment node initialized. '
|
||||
f'Queue: {self.queue_dir}, '
|
||||
f'Speakers: {self.speaker_embeddings_path}'
|
||||
=======
|
||||
f'Social enrollment node initialized. Queue: {self.queue_dir}'
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
)
|
||||
f'Social enrollment node initialized. Queue: {self.queue_dir}' )
|
||||
|
||||
def _on_orchestrator_state(self, msg: String) -> None:
|
||||
"""Handle orchestrator state transitions."""
|
||||
@ -170,12 +163,6 @@ class SocialEnrollmentNode(Node):
|
||||
context=context,
|
||||
timestamp=time.time()
|
||||
)
|
||||
<<<<<<< HEAD
|
||||
self._face_embedding_timestamp = 0.0
|
||||
self._voice_embedding_timestamp = 0.0
|
||||
self._image_timestamp = 0.0
|
||||
=======
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
|
||||
self.get_logger().info(
|
||||
f'Enrollment triggered: {name} (ID: {person_id})'
|
||||
@ -193,24 +180,13 @@ class SocialEnrollmentNode(Node):
|
||||
if self._enrollment_request is None:
|
||||
return
|
||||
|
||||
<<<<<<< HEAD
|
||||
# Take first detected face embedding
|
||||
=======
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
face_emb = msg.embeddings[0]
|
||||
emb_array = np.frombuffer(face_emb.embedding, dtype=np.float32)
|
||||
|
||||
if len(emb_array) == self.face_emb_dim:
|
||||
self._latest_face_embedding = emb_array.copy()
|
||||
self._face_embedding_timestamp = time.time()
|
||||
<<<<<<< HEAD
|
||||
self.get_logger().debug(
|
||||
f'Face embedding captured: {face_emb.track_id}'
|
||||
)
|
||||
=======
|
||||
self.get_logger().debug(f'Face embedding captured')
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
|
||||
def _on_speaker_embedding(self, msg: String) -> None:
|
||||
"""Capture voice speaker embedding from ECAPA-TDNN."""
|
||||
try:
|
||||
@ -226,14 +202,7 @@ class SocialEnrollmentNode(Node):
|
||||
if len(emb_array) == self.voice_emb_dim:
|
||||
self._latest_voice_embedding = emb_array.copy()
|
||||
self._voice_embedding_timestamp = time.time()
|
||||
<<<<<<< HEAD
|
||||
self.get_logger().debug(
|
||||
f'Voice embedding captured: {len(emb_array)} dims'
|
||||
)
|
||||
=======
|
||||
self.get_logger().debug(f'Voice embedding captured')
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
|
||||
except json.JSONDecodeError as e:
|
||||
self.get_logger().error(f'Invalid speaker embedding JSON: {e}')
|
||||
|
||||
@ -244,10 +213,6 @@ class SocialEnrollmentNode(Node):
|
||||
if self._enrollment_request is None:
|
||||
return
|
||||
|
||||
<<<<<<< HEAD
|
||||
# Store latest image
|
||||
=======
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
self._latest_image = msg
|
||||
self._image_timestamp = time.time()
|
||||
|
||||
@ -261,22 +226,12 @@ class SocialEnrollmentNode(Node):
|
||||
return
|
||||
|
||||
now = time.time()
|
||||
<<<<<<< HEAD
|
||||
timeout = 10.0 # 10 seconds to collect embeddings
|
||||
=======
|
||||
timeout = 10.0
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
|
||||
# Check if all data collected
|
||||
has_face = self._latest_face_embedding is not None and \
|
||||
(now - self._face_embedding_timestamp < 5.0)
|
||||
has_voice = self._latest_voice_embedding is not None and \
|
||||
(now - self._voice_embedding_timestamp < 5.0)
|
||||
<<<<<<< HEAD
|
||||
has_image = self._latest_image is not None and \
|
||||
(now - self._image_timestamp < 5.0)
|
||||
=======
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
|
||||
# If we have face + voice, proceed with enrollment
|
||||
if has_face and has_voice:
|
||||
@ -303,18 +258,7 @@ class SocialEnrollmentNode(Node):
|
||||
'context': request.context,
|
||||
'timestamp': request.timestamp,
|
||||
'datetime': datetime.fromtimestamp(request.timestamp).isoformat(),
|
||||
<<<<<<< HEAD
|
||||
'face_embedding_shape': list(self._latest_face_embedding.shape)
|
||||
if self._latest_face_embedding is not None else None,
|
||||
'voice_embedding_shape': list(self._latest_voice_embedding.shape)
|
||||
if self._latest_voice_embedding is not None else None,
|
||||
}
|
||||
|
||||
# Save queue JSON
|
||||
=======
|
||||
}
|
||||
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
queue_file = self.queue_dir / f"enrollment_{request.person_id}_{int(request.timestamp)}.json"
|
||||
with open(queue_file, 'w') as f:
|
||||
json.dump(enroll_data, f, indent=2)
|
||||
@ -418,10 +362,6 @@ class SocialEnrollmentNode(Node):
|
||||
)
|
||||
return
|
||||
|
||||
<<<<<<< HEAD
|
||||
# Call EnrollPerson service
|
||||
=======
|
||||
>>>>>>> origin/sl-firmware/issue-400-encounter-enrollment
|
||||
req = EnrollPerson.Request()
|
||||
req.name = request.name
|
||||
req.mode = 'face'
|
||||
|
||||
@ -10,12 +10,7 @@
|
||||
# ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0
|
||||
#
|
||||
# Data flow:
|
||||
<<<<<<< HEAD
|
||||
# person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE
|
||||
=======
|
||||
# person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
# ── Controller ─────────────────────────────────────────────────────────────────
|
||||
control_rate: 50.0 # Hz — 50ms tick, same as cmd_vel_bridge
|
||||
input_topic: /cmd_vel_raw # Upstream cmd_vel source
|
||||
@ -87,20 +82,11 @@ ride:
|
||||
target_vel_max: 15.0 # m/s — cap; EUC max ~30 km/h = 8.3 m/s typical
|
||||
|
||||
# ── Notes ─────────────────────────────────────────────────────────────────────
|
||||
<<<<<<< HEAD
|
||||
# 1. To enable ride profile, the Jetson → ESP32 BALANCE cmd_vel_bridge must also be
|
||||
# reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150
|
||||
# at ride speed (slower ramp = smoother balance).
|
||||
#
|
||||
# 2. The ESP32 BALANCE balance PID gains likely need retuning for ride speed. Expect
|
||||
=======
|
||||
# 1. To enable ride profile, the Jetson → ESP32-S3 cmd_vel_bridge must also be
|
||||
# reconfigured: max_linear_vel=8.0, ramp_rate=500 → consider ramp_rate=150
|
||||
# at ride speed (slower ramp = smoother balance).
|
||||
#
|
||||
# 2. The ESP32-S3 balance PID gains likely need retuning for ride speed. Expect
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s.
|
||||
# 2. The ESP32-S3 balance PID gains likely need retuning for ride speed. Expect# increased sensitivity to pitch angle errors at 8 m/s vs 0.5 m/s.
|
||||
#
|
||||
# 3. Test sequence recommendation:
|
||||
# - Validate walk profile on flat indoor surface first
|
||||
|
||||
@ -10,12 +10,7 @@ cmd_vel_bridge with matching limits:
|
||||
ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=8.0
|
||||
|
||||
Prerequisite node pipeline:
|
||||
<<<<<<< HEAD
|
||||
person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32 BALANCE
|
||||
=======
|
||||
person_follower → /cmd_vel_raw → [speed_controller] → /cmd_vel → cmd_vel_bridge → ESP32-S3
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
Usage:
|
||||
# Defaults (walk profile initially, adapts via UWB + GPS):
|
||||
ros2 launch saltybot_speed_controller outdoor_speed.launch.py
|
||||
|
||||
@ -5,12 +5,7 @@ Hardware
|
||||
────────
|
||||
SaltyTank: tracked robot with left/right independent brushless ESCs.
|
||||
ESCs controlled via PWM (servo-style 1000–2000 µs pulses).
|
||||
<<<<<<< HEAD
|
||||
Communication: USB CDC serial to ESP32 BALANCE or Raspberry Pi Pico GPIO PWM bridge.
|
||||
=======
|
||||
Communication: USB CDC serial to ESP32-S3 or Raspberry Pi Pico GPIO PWM bridge.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
ESC channel assignments (configurable):
|
||||
CH1 = left-front (or left-track in 2WD/tracked mode)
|
||||
CH2 = left-rear (mirrored in 2WD/tracked mode)
|
||||
|
||||
@ -298,12 +298,7 @@ class TestBatteryMonitoring(unittest.TestCase):
|
||||
rclpy.spin_once(self.node, timeout_sec=0.1)
|
||||
|
||||
def test_01_battery_topic_advertised(self):
|
||||
<<<<<<< HEAD
|
||||
"""Battery topic must be advertised (from ESP32 bridge)."""
|
||||
=======
|
||||
"""Battery topic must be advertised (from ESP32-S3 bridge)."""
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
self._spin(5.0)
|
||||
"""Battery topic must be advertised (from ESP32-S3 bridge).""" self._spin(5.0)
|
||||
all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
|
||||
|
||||
battery_topics = [
|
||||
@ -331,12 +326,7 @@ class TestBatteryMonitoring(unittest.TestCase):
|
||||
self.node.destroy_subscription(sub)
|
||||
|
||||
if not received:
|
||||
<<<<<<< HEAD
|
||||
pytest.skip("Battery data not publishing (ESP32 bridge may be disabled in test mode)")
|
||||
=======
|
||||
pytest.skip("Battery data not publishing (ESP32-S3 bridge may be disabled in test mode)")
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
|
||||
class TestDockingServices(unittest.TestCase):
|
||||
"""Verify autonomous docking services are available."""
|
||||
|
||||
@ -1,10 +1,5 @@
|
||||
# VESC CAN Telemetry Node — SaltyBot dual FSESC 6.7 Pro (FW 6.6)
|
||||
<<<<<<< HEAD
|
||||
# SocketCAN interface: can0 (SN65HVD230 transceiver on ESP32 BALANCE CAN2)
|
||||
=======
|
||||
# SocketCAN interface: can0 (SN65HVD230 transceiver on ESP32-S3 BALANCE CAN2)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
vesc_telemetry:
|
||||
ros__parameters:
|
||||
# SocketCAN interface name
|
||||
|
||||
@ -1,4 +0,0 @@
|
||||
# Legacy STM32 Firmware (Archived 2026-04-04)
|
||||
This directory contains the archived STM32F7 (Mamba F722S) firmware.
|
||||
Hardware retired 2026-04-04. Replaced by ESP32-S3 BALANCE + ESP32-S3 IO.
|
||||
See docs/SAUL-TEE-SYSTEM-REFERENCE.md for current architecture.
|
||||
@ -1,149 +0,0 @@
|
||||
# USB CDC TX Bug — Investigation & Resolution
|
||||
|
||||
**Issue #524** | Investigated 2026-03-06 | **RESOLVED** (PR #10)
|
||||
|
||||
---
|
||||
|
||||
## Problem
|
||||
|
||||
Balance firmware produced no USB CDC output. Minimal "hello" test firmware worked fine.
|
||||
|
||||
- USB enumerated correctly in both cases (port appeared as `/dev/cu.usbmodemSALTY0011`)
|
||||
- DFU reboot via RTC backup register worked (Betaflight-proven pattern)
|
||||
- Balance firmware: port opened, no data ever arrived
|
||||
|
||||
---
|
||||
|
||||
## Root Causes Found (Two Independent Bugs)
|
||||
|
||||
### Bug 1 (Primary): DCache Coherency — USB Buffers Were Cached
|
||||
|
||||
**The Cortex-M7 has a split Harvard cache (ICache + DCache). The USB OTG FS
|
||||
peripheral's internal DMA engine reads directly from physical SRAM. The CPU
|
||||
writes through the DCache. If the cache line was not flushed before the USB
|
||||
FIFO loader fired, the peripheral read stale/zero bytes from SRAM.**
|
||||
|
||||
This is the classic Cortex-M7 DMA coherency trap. The test firmware worked
|
||||
because it ran before DCache was enabled or because the tiny buffer happened to
|
||||
be flushed by the time the FIFO loaded. The balance firmware with DCache enabled
|
||||
throughout never flushed the TX buffer, so USB TX always transferred zeros or
|
||||
nothing.
|
||||
|
||||
**Fix applied** (`lib/USB_CDC/src/usbd_conf.c`, `lib/USB_CDC/src/usbd_cdc_if.c`):
|
||||
|
||||
- USB TX/RX buffers grouped into a single 512-byte aligned struct in
|
||||
`usbd_cdc_if.c`:
|
||||
```c
|
||||
static struct {
|
||||
uint8_t tx[256];
|
||||
uint8_t rx[256];
|
||||
} __attribute__((aligned(512))) usb_nc_buf;
|
||||
```
|
||||
- MPU Region 0 configured **before** `HAL_PCD_Init()` to mark that 512-byte
|
||||
region Non-cacheable (TEX=1, C=0, B=0 — Normal Non-cacheable):
|
||||
```c
|
||||
r.TypeExtField = MPU_TEX_LEVEL1;
|
||||
r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
|
||||
r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
|
||||
```
|
||||
- `SCB_EnableDCache()` left enabled in `main.c` — DCache stays on globally for
|
||||
performance; only the USB buffers are excluded via MPU.
|
||||
- `CDC_Transmit()` always copies caller data into `UserTxBuffer` before calling
|
||||
`USBD_CDC_TransmitPacket()`, so the USB hardware always reads from the
|
||||
non-cacheable region regardless of where the caller's buffer lives.
|
||||
|
||||
### Bug 2 (Secondary): IWDG Started Before Long Peripheral Inits
|
||||
|
||||
`mpu6000_init()` + `mpu6000_calibrate()` block for ~510ms (gyro bias
|
||||
integration). If IWDG had been started with a 50ms timeout before these calls,
|
||||
the watchdog would have fired during calibration and reset the MCU in a hard
|
||||
loop — USB would never enumerate cleanly.
|
||||
|
||||
**Fix applied** (`src/main.c`, `src/safety.c`):
|
||||
|
||||
- `safety_init()` (which calls `watchdog_init(2000)`) is deferred to **after**
|
||||
all peripheral inits, after IMU calibration, after USB enumeration delay:
|
||||
```c
|
||||
/* USB CDC, status, IMU, hoverboard, balance, motors, CRSF, audio,
|
||||
* buzzer, LEDs, power, servo, ultrasonic, mode manager, battery,
|
||||
* I2C sensors — ALL init first */
|
||||
safety_init(); /* IWDG starts HERE — 2s timeout */
|
||||
```
|
||||
- IWDG timeout extended to 2000ms (from 50ms) to accommodate worst-case main
|
||||
loop delays (BNO055 I2C reads at ~3ms each, audio/buzzer blocking patterns).
|
||||
|
||||
---
|
||||
|
||||
## Investigation: What Was Ruled Out
|
||||
|
||||
### DMA Channel Conflicts
|
||||
- USB OTG FS does **not** use DMA (`hpcd.Init.dma_enable = 0`); it uses the
|
||||
internal FIFO with CPU-driven transfers. No DMA channel conflict possible.
|
||||
- SPI1 (IMU/MPU6000): DMA2 Stream 0/3
|
||||
- USART2 (hoverboard ESC): DMA1 Stream 5/6
|
||||
- UART4 (CRSF/ELRS): DMA1 Stream 2/4
|
||||
- No overlapping DMA streams between any peripheral.
|
||||
|
||||
### USB Interrupt Priority Starvation
|
||||
- `OTG_FS_IRQn` configured at NVIC priority 6 (`HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0)`).
|
||||
- No other ISR in the codebase uses a priority ≤6 that could starve USB.
|
||||
- SysTick runs at default priority 15 (lowest). Not a factor.
|
||||
|
||||
### GPIO Pin Conflicts
|
||||
- USB OTG FS: PA11 (DM), PA12 (DP) — AF10
|
||||
- SPI1 (IMU): PA4 (NSS), PA5 (SCK), PA6 (MISO), PA7 (MOSI) — no overlap
|
||||
- USART2 (hoverboard): PA2 (TX), PA3 (RX) — no overlap
|
||||
- LEDs: PC14, PC15 — no overlap
|
||||
- Buzzer: PB2 — no overlap
|
||||
- No GPIO conflicts with USB OTG FS pins.
|
||||
|
||||
### Clock Tree
|
||||
- USB requires a 48 MHz clock. `SystemClock_Config()` routes 48 MHz from PLLSAI
|
||||
(`RCC_CLK48SOURCE_PLLSAIP`, PLLSAIN=384, PLLSAIP=DIV8 → 384/8=48 MHz). ✓
|
||||
- PLLSAI is independent of PLL1 (system clock) and PLLSAI.PLLSAIQ (I2S).
|
||||
No clock tree contention.
|
||||
|
||||
### TxState Stuck-Busy
|
||||
- `CDC_Init()` resets `hcdc->TxState = 0` on every host (re)connect. ✓
|
||||
- `CDC_Transmit()` includes a busy-count recovery (force-clears TxState after
|
||||
100 consecutive BUSY returns). ✓
|
||||
- Not a contributing factor once the DCache issue is fixed.
|
||||
|
||||
---
|
||||
|
||||
## Hardware Reference
|
||||
|
||||
| Signal | Pin | Peripheral |
|
||||
|--------|-----|------------|
|
||||
| USB D- | PA11 | OTG_FS AF10 |
|
||||
| USB D+ | PA12 | OTG_FS AF10 |
|
||||
| IMU SCK | PA5 | SPI1 |
|
||||
| IMU MISO | PA6 | SPI1 |
|
||||
| IMU MOSI | PA7 | SPI1 |
|
||||
| IMU CS | PA4 | GPIO |
|
||||
| ESC TX | PA2 | USART2 |
|
||||
| ESC RX | PA3 | USART2 |
|
||||
| LED1 | PC14 | GPIO |
|
||||
| LED2 | PC15 | GPIO |
|
||||
| Buzzer | PB2 | GPIO/TIM4_CH3 |
|
||||
|
||||
MCU: ESP32RET6 (ESP32 BALANCE FC, Betaflight target DIAT-MAMBAF722_2022B)
|
||||
|
||||
---
|
||||
|
||||
## Files Changed (PR #10)
|
||||
|
||||
- `lib/USB_CDC/src/usbd_cdc_if.c` — 512-byte aligned non-cacheable buffer struct, `CDC_Transmit` copy-to-fixed-buffer
|
||||
- `lib/USB_CDC/src/usbd_conf.c` — `USB_NC_MPU_Config()` MPU region before `HAL_PCD_Init()`
|
||||
- `src/main.c` — `safety_init()` deferred after all peripheral init; DCache stays enabled with comment
|
||||
- `src/safety.c` / `src/watchdog.c` — IWDG timeout 2000ms; `watchdog_was_reset_by_watchdog()` for reset detection logging
|
||||
|
||||
---
|
||||
|
||||
## Lessons Learned
|
||||
|
||||
1. **Cortex-M7 + DMA + DCache = always configure MPU non-cacheable regions for DMA buffers.** The cache is not write-through to SRAM; the DMA engine sees physical SRAM, not the cache. The MPU is the correct fix (not `SCB_CleanDCache_by_Addr` before every TX, which is fragile).
|
||||
|
||||
2. **IWDG must start after all slow blocking inits.** IMU calibration can take 500ms+. The IWDG cannot be paused once started. Defer `safety_init()` until the main loop is ready to kick the watchdog every cycle.
|
||||
|
||||
3. **USB enumeration success does not prove data flow.** The host handshake and port appearance can succeed even when TX buffers are incoherent. Test with actual data transfer, not just enumeration.
|
||||
@ -1,106 +0,0 @@
|
||||
#ifndef AUDIO_H
|
||||
#define AUDIO_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* audio.h — I2S audio output driver (Issue #143)
|
||||
*
|
||||
* Hardware: SPI3 repurposed as I2S3 master TX (blackbox flash not used
|
||||
* on balance bot). Supports MAX98357A (I2S class-D amp) and PCM5102A
|
||||
* (I2S DAC + external amp) — both use standard Philips I2S.
|
||||
*
|
||||
* Pin assignment (SPI3 / I2S3, defined in config.h):
|
||||
* PC10 I2S3_CK (BCLK) AF6
|
||||
* PA15 I2S3_WS (LRCLK) AF6
|
||||
* PB5 I2S3_SD (DIN) AF6
|
||||
* PC5 AUDIO_MUTE (GPIO) active-high = enabled; low = muted/shutdown
|
||||
*
|
||||
* PLLI2S: N=192, R=2 → 96 MHz I2S clock → 22058 Hz (< 0.04% from 22050)
|
||||
* DMA1 Stream7 Channel0 (SPI3_TX), circular, double-buffer ping-pong.
|
||||
*
|
||||
* Mixer priority (highest to lowest):
|
||||
* 1. PCM audio chunks from Jetson (via JLINK_CMD_AUDIO, written to FIFO)
|
||||
* 2. Notification tones (queued by audio_play_tone)
|
||||
* 3. Silence
|
||||
*
|
||||
* Volume applies to all sources via integer sample scaling (0–100).
|
||||
*/
|
||||
|
||||
/* Maximum int16_t samples per JLINK_CMD_AUDIO frame (252-byte payload / 2) */
|
||||
#define AUDIO_CHUNK_MAX_SAMPLES 126u
|
||||
|
||||
/* Pre-defined notification tones */
|
||||
typedef enum {
|
||||
AUDIO_TONE_BEEP_SHORT = 0, /* 880 Hz, 100 ms — acknowledge / UI feedback */
|
||||
AUDIO_TONE_BEEP_LONG = 1, /* 880 Hz, 500 ms — generic warning */
|
||||
AUDIO_TONE_STARTUP = 2, /* C5→E5→G5 arpeggio (3 × 120 ms) */
|
||||
AUDIO_TONE_ARM = 3, /* 880 Hz→1047 Hz two-beep ascending */
|
||||
AUDIO_TONE_DISARM = 4, /* 880 Hz→659 Hz two-beep descending */
|
||||
AUDIO_TONE_FAULT = 5, /* 200 Hz buzz, 500 ms — tilt/safety fault */
|
||||
AUDIO_TONE_COUNT
|
||||
} AudioTone;
|
||||
|
||||
/*
|
||||
* audio_init()
|
||||
*
|
||||
* Configure PLLI2S, GPIO, DMA1 Stream7, and SPI3/I2S3.
|
||||
* Pre-fills DMA buffer with silence, starts circular DMA TX, then
|
||||
* unmutes the amp. Call once before safety_init().
|
||||
*/
|
||||
void audio_init(void);
|
||||
|
||||
/*
|
||||
* audio_mute(mute)
|
||||
*
|
||||
* Drive AUDIO_MUTE_PIN: false = hardware-muted (SD/XSMT low),
|
||||
* true = active (amp enabled). Does NOT stop DMA; allows instant
|
||||
* un-mute without DMA restart clicks.
|
||||
*/
|
||||
void audio_mute(bool active);
|
||||
|
||||
/*
|
||||
* audio_set_volume(vol)
|
||||
*
|
||||
* Software volume 0–100. Applied in ISR fill path via integer scaling.
|
||||
* 0 = silence, 100 = full scale (±16384 for square wave, passthrough for PCM).
|
||||
*/
|
||||
void audio_set_volume(uint8_t vol);
|
||||
|
||||
/*
|
||||
* audio_play_tone(tone)
|
||||
*
|
||||
* Queue a pre-defined notification tone. The tone plays after any tones
|
||||
* already in the queue. Returns false if the tone queue is full (depth 4).
|
||||
* Tones are pre-empted by incoming PCM audio from the Jetson.
|
||||
*/
|
||||
bool audio_play_tone(AudioTone tone);
|
||||
|
||||
/*
|
||||
* audio_write_pcm(samples, n)
|
||||
*
|
||||
* Write mono 16-bit 22050 Hz PCM samples into the Jetson PCM FIFO.
|
||||
* Called from jlink_process() dispatch on JLINK_CMD_AUDIO (main-loop context).
|
||||
* Returns the number of samples actually accepted (0 if FIFO is full).
|
||||
*/
|
||||
uint16_t audio_write_pcm(const int16_t *samples, uint16_t n);
|
||||
|
||||
/*
|
||||
* audio_tick(now_ms)
|
||||
*
|
||||
* Advance the tone sequencer state machine. Must be called every 1 ms
|
||||
* from the main loop. Manages step transitions and gap timing; updates
|
||||
* the volatile active-tone parameters read by the ISR fill path.
|
||||
*/
|
||||
void audio_tick(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* audio_is_playing()
|
||||
*
|
||||
* Returns true if the DMA is running (always true after audio_init()
|
||||
* unless the amp is hardware-muted or the I2S peripheral has an error).
|
||||
*/
|
||||
bool audio_is_playing(void);
|
||||
|
||||
#endif /* AUDIO_H */
|
||||
@ -1,53 +0,0 @@
|
||||
#ifndef BALANCE_H
|
||||
#define BALANCE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "mpu6000.h"
|
||||
#include "slope_estimator.h"
|
||||
|
||||
/*
|
||||
* SaltyLab Balance Controller
|
||||
*
|
||||
* Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter)
|
||||
* PID controller → motor speed command
|
||||
* Safety: tilt cutoff, arming, watchdog
|
||||
*/
|
||||
|
||||
typedef enum {
|
||||
BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */
|
||||
BALANCE_ARMED = 1, /* Active balancing */
|
||||
BALANCE_TILT_FAULT = 2, /* Tilt exceeded limit, motors killed */
|
||||
BALANCE_PARKED = 3, /* PID frozen, motors off — quick re-arm via button (Issue #682) */
|
||||
} balance_state_t;
|
||||
|
||||
typedef struct {
|
||||
/* State */
|
||||
balance_state_t state;
|
||||
float pitch_deg; /* Current pitch angle (degrees) */
|
||||
float pitch_rate; /* Gyro pitch rate (deg/s) */
|
||||
|
||||
/* PID internals */
|
||||
float integral;
|
||||
float prev_error;
|
||||
int16_t motor_cmd; /* Output to ESC: -1000..+1000 */
|
||||
|
||||
/* Tuning */
|
||||
float kp, ki, kd;
|
||||
float setpoint; /* Target pitch angle (degrees) — tune for COG offset */
|
||||
|
||||
/* Safety */
|
||||
float max_tilt; /* Cutoff angle (degrees) */
|
||||
int16_t max_speed; /* Speed limit */
|
||||
|
||||
/* Slope compensation (Issue #600) */
|
||||
slope_estimator_t slope;
|
||||
} balance_t;
|
||||
|
||||
void balance_init(balance_t *b);
|
||||
void balance_update(balance_t *b, const IMUData *imu, float dt);
|
||||
void balance_arm(balance_t *b);
|
||||
void balance_disarm(balance_t *b);
|
||||
void balance_park(balance_t *b); /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */
|
||||
void balance_unpark(balance_t *b); /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */
|
||||
|
||||
#endif
|
||||
@ -1,66 +0,0 @@
|
||||
#ifndef BARO_H
|
||||
#define BARO_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* baro — BME280/BMP280 barometric pressure & ambient temperature module
|
||||
* (Issue #672).
|
||||
*
|
||||
* Reads pressure and temperature from the BME280 at BARO_READ_HZ (1 Hz),
|
||||
* computes pressure altitude using the ISA barometric formula, and publishes
|
||||
* JLINK_TLM_BARO (0x8D) telemetry to the Orin at BARO_TLM_HZ (1 Hz).
|
||||
*
|
||||
* Runs entirely on the Mamba F722S — no Orin dependency.
|
||||
* Altitude is exposed via baro_get_alt_cm() for use by slope compensation
|
||||
* in the balance PID (Issue #672 requirement).
|
||||
*
|
||||
* Usage:
|
||||
* 1. Call i2c1_init() then bmp280_init() and pass the chip_id result.
|
||||
* 2. Call baro_tick(now_ms) every ms from the main loop.
|
||||
* 3. Call baro_get_alt_cm() to read the latest altitude.
|
||||
*/
|
||||
|
||||
/* ---- Configuration ---- */
|
||||
#define BARO_READ_HZ 1u /* sensor poll rate (Hz) */
|
||||
#define BARO_TLM_HZ 1u /* JLink telemetry rate (Hz) */
|
||||
|
||||
/* ---- Data ---- */
|
||||
typedef struct {
|
||||
int32_t pressure_pa; /* barometric pressure (Pa) */
|
||||
int16_t temp_x10; /* ambient temperature (°C × 10; e.g. 235 = 23.5 °C) */
|
||||
int32_t alt_cm; /* pressure altitude above ISA sea level (cm) */
|
||||
int16_t humidity_pct_x10; /* %RH × 10 (BME280 only); -1 if BMP280/absent */
|
||||
bool valid; /* true once at least one reading has been obtained */
|
||||
} baro_data_t;
|
||||
|
||||
/* ---- API ---- */
|
||||
|
||||
/*
|
||||
* baro_init(chip_id) — register chip type from bmp280_init() result.
|
||||
* chip_id : 0x58 = BMP280, 0x60 = BME280, 0 = absent/not found.
|
||||
* Call after i2c1_init() and bmp280_init(); no-op if chip_id == 0.
|
||||
*/
|
||||
void baro_init(int chip_id);
|
||||
|
||||
/*
|
||||
* baro_tick(now_ms) — rate-limited sensor read + JLink telemetry publish.
|
||||
* Call every ms from the main loop. No-op if chip absent.
|
||||
* Reads at BARO_READ_HZ; sends JLINK_TLM_BARO at BARO_TLM_HZ.
|
||||
*/
|
||||
void baro_tick(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* baro_get(out) — copy latest baro data into *out.
|
||||
* Returns true on success; false if no valid reading yet.
|
||||
*/
|
||||
bool baro_get(baro_data_t *out);
|
||||
|
||||
/*
|
||||
* baro_get_alt_cm() — latest pressure altitude (cm above ISA sea level).
|
||||
* Returns 0 if no valid reading. Used by slope compensation in balance PID.
|
||||
*/
|
||||
int32_t baro_get_alt_cm(void);
|
||||
|
||||
#endif /* BARO_H */
|
||||
@ -1,49 +0,0 @@
|
||||
#ifndef BATTERY_H
|
||||
#define BATTERY_H
|
||||
|
||||
/*
|
||||
* battery.h — Vbat ADC reading for CRSF telemetry (Issue #103)
|
||||
*
|
||||
* Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722).
|
||||
* Voltage divider: 10 kΩ / 1 kΩ → 11:1 ratio.
|
||||
* Resolution: 12-bit (0–4095), Vref = 3.3 V.
|
||||
*
|
||||
* Filtered output in millivolts. Reading is averaged over
|
||||
* BATTERY_SAMPLES conversions (software oversampling) to reduce noise.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* Initialise ADC3 for single-channel Vbat reading on PC1. */
|
||||
void battery_init(void);
|
||||
|
||||
/*
|
||||
* battery_read_mv() — blocking single-shot read; returns Vbat in mV.
|
||||
* Takes ~1 µs (12-bit conversion at 36 MHz APB2 / 8 prescaler = 4.5 MHz ADC clk).
|
||||
* Returns 0 if ADC not initialised or conversion times out.
|
||||
*/
|
||||
uint32_t battery_read_mv(void);
|
||||
|
||||
/*
|
||||
* battery_estimate_pct() — coarse SoC estimate from Vbat (mV).
|
||||
* Works for 3S LiPo (10.5–12.6 V) and 4S (14.0–16.8 V).
|
||||
* Detection is automatic based on voltage.
|
||||
* Returns 0–100, or 255 if voltage is out of range.
|
||||
*/
|
||||
uint8_t battery_estimate_pct(uint32_t voltage_mv);
|
||||
|
||||
/*
|
||||
* battery_accumulate_coulombs() — periodically integrate battery current.
|
||||
* Call every 10-20 ms (50-100 Hz) from main loop to accumulate coulombs.
|
||||
* Reads motor currents from INA219 sensors.
|
||||
*/
|
||||
void battery_accumulate_coulombs(void);
|
||||
|
||||
/*
|
||||
* battery_get_soc_coulomb() — get coulomb-based SoC estimate.
|
||||
* Returns 0–100 (percent), or 255 if coulomb counter not yet valid.
|
||||
* Preferred over voltage-based when valid.
|
||||
*/
|
||||
uint8_t battery_get_soc_coulomb(void);
|
||||
|
||||
#endif /* BATTERY_H */
|
||||
@ -1,143 +0,0 @@
|
||||
/*
|
||||
* battery_adc.h — DMA-based battery voltage/current ADC driver (Issue #533)
|
||||
*
|
||||
* Hardware:
|
||||
* ADC3 channel IN11 (PC1) — Vbat through 10kΩ/1kΩ divider (11:1 ratio)
|
||||
* ADC3 channel IN13 (PC3) — Ibat via shunt amplifier (ADC_IBAT_SCALE=115)
|
||||
* DMA2 Stream0 Channel2 — ADC3 → memory circular (8-word buffer)
|
||||
* USART1 (jlink) — telemetry to Jetson via JLINK_TLM_BATTERY (0x82)
|
||||
*
|
||||
* HOW IT WORKS:
|
||||
* 1. ADC3 runs in continuous scan mode, alternating IN11 (Vbat) and IN13 (Ibat)
|
||||
* at APB2/8 clock (≈ 13.5 MHz ADC clock on STM32F7 @ 216 MHz).
|
||||
* 480-cycle sampling per channel → ~35 µs per scan pair, ~28 kHz scan rate.
|
||||
*
|
||||
* 2. DMA2_Stream0 (circular) fills an 8-word buffer: 4 Vbat samples followed
|
||||
* by 4 Ibat samples per DMA half-complete cycle. Interleaved layout:
|
||||
* [vbat0, ibat0, vbat1, ibat1, vbat2, ibat2, vbat3, ibat3]
|
||||
*
|
||||
* 3. battery_adc_tick() (call from main loop, 10–100 Hz) averages the 4 Vbat
|
||||
* and 4 Ibat raw values (4× hardware oversampling), then feeds a 1st-order
|
||||
* IIR low-pass filter:
|
||||
* filtered += (raw - filtered) >> BATTERY_ADC_LPF_SHIFT
|
||||
* With LPF_SHIFT=3 (α = 1/8) and 100 Hz tick rate, cutoff ≈ 4 Hz.
|
||||
*
|
||||
* 4. Calibration scales and offsets the filtered output:
|
||||
* vbat_mv = filtered_raw * (VBAT_AREF_MV * VBAT_SCALE_NUM) / 4096
|
||||
* + cal.vbat_offset_mv
|
||||
* ibat_ma = filtered_raw * ADC_IBAT_SCALE_MA_PER_COUNT / 1000
|
||||
* + cal.ibat_offset_ma
|
||||
* User calibration adjusts cal.vbat_offset_mv to null out divider tolerance.
|
||||
*
|
||||
* 5. battery_adc_publish() sends JLINK_TLM_BATTERY (0x82) to Jetson at 1 Hz.
|
||||
*
|
||||
* 6. battery_adc_check_pm() monitors for low voltage. If Vbat drops below
|
||||
* BATTERY_ADC_LOW_MV for BATTERY_ADC_LOW_HOLD_MS, calls
|
||||
* power_mgmt_notify_battery(vbat_mv) which requests sleep (Issue #467).
|
||||
*
|
||||
* Interrupt safety:
|
||||
* s_dma_buf is written by DMA hardware; battery_adc_tick() reads it with a
|
||||
* brief __disable_irq() snapshot to prevent torn reads of the 16-bit words.
|
||||
* All other state is private to the main-loop call path.
|
||||
*/
|
||||
|
||||
#ifndef BATTERY_ADC_H
|
||||
#define BATTERY_ADC_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* ---- Low-pass filter ---- */
|
||||
/* IIR shift: α = 1/8 → cutoff ≈ 4 Hz at 100 Hz tick rate */
|
||||
#define BATTERY_ADC_LPF_SHIFT 3u
|
||||
|
||||
/* ---- Low-voltage thresholds (mV) ---- */
|
||||
/* 3S LiPo: 9.0 V cell floor ×3 = 9900 mV full, 9000 mV absolute minimum */
|
||||
#define BATTERY_ADC_LOW_MV 10200u /* ≈ 15% SoC — warn / throttle */
|
||||
#define BATTERY_ADC_CRITICAL_MV 9600u /* ≈ 5% SoC — request sleep (#467) */
|
||||
#define BATTERY_ADC_LOW_HOLD_MS 5000u /* must stay below this long to act */
|
||||
|
||||
/* 4S LiPo equivalents (auto-detected when Vbat ≥ 13 V at boot) */
|
||||
#define BATTERY_ADC_LOW_MV_4S 13600u
|
||||
#define BATTERY_ADC_CRITICAL_MV_4S 12800u
|
||||
|
||||
/* ---- Telemetry rate ---- */
|
||||
#define BATTERY_ADC_PUBLISH_HZ 1u /* JLINK_TLM_BATTERY TX rate */
|
||||
|
||||
/* ---- Calibration struct ---- */
|
||||
typedef struct {
|
||||
int16_t vbat_offset_mv; /* additive offset after scale (mV, ±500 clamp) */
|
||||
int16_t ibat_offset_ma; /* additive offset for current (mA, ±200 clamp) */
|
||||
uint16_t vbat_scale_num; /* divider numerator override; 0 = use VBAT_SCALE_NUM */
|
||||
uint16_t vbat_scale_den; /* divider denominator override; 0 = use 1 */
|
||||
} battery_adc_cal_t;
|
||||
|
||||
/* ---- API ---- */
|
||||
|
||||
/*
|
||||
* battery_adc_init() — configure ADC3 continuous-scan + DMA2_Stream0.
|
||||
* Must be called after __HAL_RCC_ADC3_CLK_ENABLE / GPIO clock enables.
|
||||
* Call once during system init, before battery_adc_tick().
|
||||
*/
|
||||
void battery_adc_init(void);
|
||||
|
||||
/*
|
||||
* battery_adc_tick(now_ms) — average DMA buffer, apply IIR LPF, update state.
|
||||
* Call from main loop at 10–100 Hz. Non-blocking (<5 µs).
|
||||
*/
|
||||
void battery_adc_tick(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* battery_adc_get_voltage_mv() — calibrated, LPF-filtered Vbat in mV.
|
||||
* Returns 0 if ADC not initialised.
|
||||
*/
|
||||
uint32_t battery_adc_get_voltage_mv(void);
|
||||
|
||||
/*
|
||||
* battery_adc_get_current_ma() — calibrated, LPF-filtered Ibat in mA.
|
||||
* Positive = discharging (load current). Returns 0 if not initialised.
|
||||
*/
|
||||
int32_t battery_adc_get_current_ma(void);
|
||||
|
||||
/*
|
||||
* battery_adc_get_raw_voltage_mv() — unfiltered last-tick average (mV).
|
||||
* Useful for calibration; use filtered version for control logic.
|
||||
*/
|
||||
uint32_t battery_adc_get_raw_voltage_mv(void);
|
||||
|
||||
/*
|
||||
* battery_adc_calibrate(cal) — store calibration constants.
|
||||
* Applies immediately to subsequent battery_adc_tick() calls.
|
||||
* Pass NULL to reset to defaults (0 offset, default scale).
|
||||
*/
|
||||
void battery_adc_calibrate(const battery_adc_cal_t *cal);
|
||||
|
||||
/*
|
||||
* battery_adc_get_calibration(out_cal) — read back current calibration.
|
||||
*/
|
||||
void battery_adc_get_calibration(battery_adc_cal_t *out_cal);
|
||||
|
||||
/*
|
||||
* battery_adc_publish(now_ms) — send JLINK_TLM_BATTERY (0x82) frame.
|
||||
* Rate-limited to BATTERY_ADC_PUBLISH_HZ; safe to call every main loop tick.
|
||||
*/
|
||||
void battery_adc_publish(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* battery_adc_check_pm(now_ms) — evaluate low-voltage thresholds.
|
||||
* Calls power_mgmt_notify_battery() on sustained critical voltage.
|
||||
* Call from main loop after battery_adc_tick().
|
||||
*/
|
||||
void battery_adc_check_pm(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* battery_adc_is_low() — true if Vbat below BATTERY_ADC_LOW_MV (warn level).
|
||||
*/
|
||||
bool battery_adc_is_low(void);
|
||||
|
||||
/*
|
||||
* battery_adc_is_critical() — true if Vbat below BATTERY_ADC_CRITICAL_MV.
|
||||
*/
|
||||
bool battery_adc_is_critical(void);
|
||||
|
||||
#endif /* BATTERY_ADC_H */
|
||||
@ -1,28 +0,0 @@
|
||||
#ifndef BMP280_H
|
||||
#define BMP280_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* BMP280 / BME280 barometer driver.
|
||||
*
|
||||
* Probes I2C1 at 0x76 then 0x77.
|
||||
* Returns chip_id (0x58=BMP280, 0x60=BME280) on success, negative if not found.
|
||||
* Requires i2c1_init() to have been called first.
|
||||
*
|
||||
* All I2C operations use 100ms timeouts — init will not hang on missing hardware.
|
||||
*/
|
||||
int bmp280_init(void);
|
||||
void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10);
|
||||
|
||||
/*
|
||||
* BME280-only humidity readout. Call AFTER bmp280_read() (uses cached t_fine).
|
||||
* Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH).
|
||||
* Returns -1 if chip is BMP280 (no humidity) or not initialised.
|
||||
*/
|
||||
int16_t bmp280_read_humidity(void);
|
||||
|
||||
/* Convert pressure (Pa) to altitude above sea level (cm), ISA p0=101325 Pa. */
|
||||
int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa);
|
||||
|
||||
#endif /* BMP280_H */
|
||||
@ -1,99 +0,0 @@
|
||||
#ifndef BNO055_H
|
||||
#define BNO055_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "mpu6000.h" /* IMUData */
|
||||
|
||||
/*
|
||||
* BNO055 NDOF IMU driver over I2C1 (shared bus — PB8=SCL, PB9=SDA).
|
||||
*
|
||||
* Issue #135: auto-detected alongside MPU6000. Acts as:
|
||||
* PRIMARY — when MPU6000 init fails (seamless fallback)
|
||||
* AUGMENT — when both present; BNO055 provides better NDOF-fused yaw
|
||||
*
|
||||
* I2C addresses probed: 0x28 (ADR=0, default) then 0x29 (ADR=1).
|
||||
* Chip-ID register 0x00 must read 0xA0.
|
||||
*
|
||||
* Operating mode: NDOF (0x0C) — 9DOF fusion with magnetometer.
|
||||
* Falls back to IMUPLUS (0x08, no mag) if mag calibration stalls.
|
||||
*
|
||||
* Calibration offsets are saved to/restored from STM32 RTC backup
|
||||
* registers (BKP0R–BKP6R = 28 bytes), identified by a magic word.
|
||||
* If valid offsets are present, bno055_is_ready() returns true
|
||||
* immediately after init. Otherwise, waits for gyro+accel cal ≥ 2.
|
||||
*
|
||||
* Temperature compensation is handled internally by the BNO055 silicon
|
||||
* (it compensates all three sensors continuously). bno055_temperature()
|
||||
* exposes the onboard thermometer reading for telemetry.
|
||||
*
|
||||
* Loop-rate note: BNO055 reads over I2C1 @100kHz take ~3ms, so the
|
||||
* main balance loop drops from ~1kHz (MPU6000/SPI) to ~250Hz when
|
||||
* BNO055 is active. 250Hz is sufficient for stable self-balancing.
|
||||
* PID gain tuning may be required when switching IMU sources.
|
||||
*/
|
||||
|
||||
/* ---- Calibration status nibble masks (CALIB_STAT reg 0x35) ---- */
|
||||
#define BNO055_CAL_SYS_MASK 0xC0u /* bits [7:6] — overall system */
|
||||
#define BNO055_CAL_GYR_MASK 0x30u /* bits [5:4] — gyroscope */
|
||||
#define BNO055_CAL_ACC_MASK 0x0Cu /* bits [3:2] — accelerometer */
|
||||
#define BNO055_CAL_MAG_MASK 0x03u /* bits [1:0] — magnetometer */
|
||||
/* Each field: 0=uncalibrated, 3=fully calibrated */
|
||||
|
||||
/*
|
||||
* bno055_init() — probe I2C1 for BNO055, reset, enter NDOF mode,
|
||||
* restore saved calibration offsets if present.
|
||||
* Requires i2c1_init() already called.
|
||||
* Returns 0 on success, -1 if not found.
|
||||
* Blocks ~750ms (POR + mode-switch settle).
|
||||
* Call BEFORE safety_init() (IWDG not yet running).
|
||||
*/
|
||||
int bno055_init(void);
|
||||
|
||||
/*
|
||||
* bno055_read(data) — fill IMUData from BNO055 NDOF fusion output.
|
||||
* Uses Euler angles for pitch/roll/yaw and gyro registers for pitch_rate.
|
||||
* Triggers one I2C burst read (~3ms at 100kHz).
|
||||
* Call from main loop balance gate (not every loop iteration).
|
||||
*/
|
||||
void bno055_read(IMUData *data);
|
||||
|
||||
/*
|
||||
* bno055_is_ready() — true when BNO055 is suitable for balance arming.
|
||||
* True immediately if offsets were restored from backup RAM.
|
||||
* Otherwise true once gyro calibration ≥ 2 and accel ≥ 2.
|
||||
*/
|
||||
bool bno055_is_ready(void);
|
||||
|
||||
/*
|
||||
* bno055_calib_status() — raw CALIB_STAT byte.
|
||||
* Use BNO055_CAL_*_MASK to extract individual sensor calibration levels.
|
||||
* Returned value is updated lazily on each bno055_read() call.
|
||||
*/
|
||||
uint8_t bno055_calib_status(void);
|
||||
|
||||
/*
|
||||
* bno055_temperature() — onboard temperature in °C (gyro source).
|
||||
* Updated once per second (every ~250 calls to bno055_read()).
|
||||
* Range: -40..+85°C. Use for telemetry reporting only.
|
||||
*/
|
||||
int8_t bno055_temperature(void);
|
||||
|
||||
/*
|
||||
* bno055_save_offsets() — write current calibration offsets to
|
||||
* STM32 RTC backup registers BKP0R–BKP6R (22 bytes + magic).
|
||||
* Call once after sys+acc+gyr calibration all reach level 3.
|
||||
* Returns true if successful, false if BNO055 not present.
|
||||
* Temporarily switches to CONFIGMODE — do NOT call while armed.
|
||||
*/
|
||||
bool bno055_save_offsets(void);
|
||||
|
||||
/*
|
||||
* bno055_restore_offsets() — read offsets from RTC backup registers
|
||||
* and write them to BNO055 hardware (in CONFIGMODE).
|
||||
* Called automatically by bno055_init().
|
||||
* Returns true if valid offsets found and applied.
|
||||
*/
|
||||
bool bno055_restore_offsets(void);
|
||||
|
||||
#endif /* BNO055_H */
|
||||
@ -1,146 +0,0 @@
|
||||
#ifndef BUZZER_H
|
||||
#define BUZZER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* buzzer.h — Piezo buzzer melody driver (Issue #253)
|
||||
*
|
||||
* STM32F722 driver for piezo buzzer on PA8 using TIM1 PWM.
|
||||
* Plays predefined melodies and tones with non-blocking queue.
|
||||
*
|
||||
* Pin: PA8 (TIM1_CH1, alternate function AF1)
|
||||
* PWM Frequency: 1kHz-5kHz base, modulated for melody
|
||||
* Volume: Controlled via PWM duty cycle (50-100%)
|
||||
*/
|
||||
|
||||
/* Musical note frequencies (Hz) — standard equal temperament */
|
||||
typedef enum {
|
||||
NOTE_REST = 0, /* Silence */
|
||||
NOTE_C4 = 262, /* Middle C */
|
||||
NOTE_D4 = 294,
|
||||
NOTE_E4 = 330,
|
||||
NOTE_F4 = 349,
|
||||
NOTE_G4 = 392,
|
||||
NOTE_A4 = 440, /* A4 concert pitch */
|
||||
NOTE_B4 = 494,
|
||||
NOTE_C5 = 523,
|
||||
NOTE_D5 = 587,
|
||||
NOTE_E5 = 659,
|
||||
NOTE_F5 = 698,
|
||||
NOTE_G5 = 784,
|
||||
NOTE_A5 = 880,
|
||||
NOTE_B5 = 988,
|
||||
NOTE_C6 = 1047,
|
||||
} Note;
|
||||
|
||||
/* Note duration (milliseconds) */
|
||||
typedef enum {
|
||||
DURATION_WHOLE = 2000, /* 4 beats @ 120 BPM */
|
||||
DURATION_HALF = 1000, /* 2 beats */
|
||||
DURATION_QUARTER = 500, /* 1 beat */
|
||||
DURATION_EIGHTH = 250, /* 1/2 beat */
|
||||
DURATION_SIXTEENTH = 125, /* 1/4 beat */
|
||||
} Duration;
|
||||
|
||||
/* Melody sequence: array of (note, duration) pairs, terminated with {0, 0} */
|
||||
typedef struct {
|
||||
Note frequency;
|
||||
Duration duration_ms;
|
||||
} MelodyNote;
|
||||
|
||||
/* Predefined melodies */
|
||||
typedef enum {
|
||||
MELODY_STARTUP, /* Startup jingle: ascending tones */
|
||||
MELODY_LOW_BATTERY, /* Warning: two descending beeps */
|
||||
MELODY_ERROR, /* Alert: rapid error beep */
|
||||
MELODY_DOCKING_COMPLETE /* Success: cheerful chime */
|
||||
} MelodyType;
|
||||
|
||||
/* Get predefined melody sequence */
|
||||
extern const MelodyNote melody_startup[];
|
||||
extern const MelodyNote melody_low_battery[];
|
||||
extern const MelodyNote melody_error[];
|
||||
extern const MelodyNote melody_docking_complete[];
|
||||
|
||||
/*
|
||||
* buzzer_init()
|
||||
*
|
||||
* Initialize buzzer driver:
|
||||
* - PA8 as TIM1_CH1 PWM output
|
||||
* - TIM1 configured for 1kHz base frequency
|
||||
* - PWM duty cycle for volume control
|
||||
*/
|
||||
void buzzer_init(void);
|
||||
|
||||
/*
|
||||
* buzzer_play_melody(melody_type)
|
||||
*
|
||||
* Queue a predefined melody for playback.
|
||||
* Non-blocking: returns immediately, melody plays asynchronously.
|
||||
* Multiple calls queue melodies in sequence.
|
||||
*
|
||||
* Supported melodies:
|
||||
* - MELODY_STARTUP: 2-3 second jingle on power-up
|
||||
* - MELODY_LOW_BATTERY: 1 second warning
|
||||
* - MELODY_ERROR: 0.5 second alert beep
|
||||
* - MELODY_DOCKING_COMPLETE: 1-1.5 second success chime
|
||||
*
|
||||
* Returns: true if queued, false if queue full
|
||||
*/
|
||||
bool buzzer_play_melody(MelodyType melody_type);
|
||||
|
||||
/*
|
||||
* buzzer_play_custom(notes)
|
||||
*
|
||||
* Queue a custom melody sequence.
|
||||
* Notes array must be terminated with {NOTE_REST, 0}.
|
||||
* Useful for error codes or custom notifications.
|
||||
*
|
||||
* Returns: true if queued, false if queue full
|
||||
*/
|
||||
bool buzzer_play_custom(const MelodyNote *notes);
|
||||
|
||||
/*
|
||||
* buzzer_play_tone(frequency, duration_ms)
|
||||
*
|
||||
* Queue a simple single tone.
|
||||
* Useful for beeps and alerts.
|
||||
*
|
||||
* Arguments:
|
||||
* - frequency: Note frequency (Hz), 0 for silence
|
||||
* - duration_ms: Tone duration in milliseconds
|
||||
*
|
||||
* Returns: true if queued, false if queue full
|
||||
*/
|
||||
bool buzzer_play_tone(uint16_t frequency, uint16_t duration_ms);
|
||||
|
||||
/*
|
||||
* buzzer_stop()
|
||||
*
|
||||
* Stop current playback and clear queue.
|
||||
* Buzzer returns to silence immediately.
|
||||
*/
|
||||
void buzzer_stop(void);
|
||||
|
||||
/*
|
||||
* buzzer_is_playing()
|
||||
*
|
||||
* Returns: true if melody/tone is currently playing, false if idle
|
||||
*/
|
||||
bool buzzer_is_playing(void);
|
||||
|
||||
/*
|
||||
* buzzer_tick(now_ms)
|
||||
*
|
||||
* Update function called periodically (recommended: every 10ms in main loop).
|
||||
* Manages melody timing and PWM frequency transitions.
|
||||
* Must be called regularly for non-blocking operation.
|
||||
*
|
||||
* Arguments:
|
||||
* - now_ms: current time in milliseconds (from HAL_GetTick() or similar)
|
||||
*/
|
||||
void buzzer_tick(uint32_t now_ms);
|
||||
|
||||
#endif /* BUZZER_H */
|
||||
@ -1,54 +0,0 @@
|
||||
#ifndef CAN_DRIVER_H
|
||||
#define CAN_DRIVER_H
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#define CAN_NUM_MOTORS 2u
|
||||
#define CAN_NODE_LEFT 0u
|
||||
#define CAN_NODE_RIGHT 1u
|
||||
#define CAN_ID_VEL_CMD_BASE 0x100u
|
||||
#define CAN_ID_ENABLE_CMD_BASE 0x110u
|
||||
#define CAN_ID_FEEDBACK_BASE 0x200u
|
||||
#define CAN_FILTER_STDID 0x200u
|
||||
#define CAN_FILTER_MASK 0x7E0u
|
||||
#define CAN_PRESCALER 6u
|
||||
#define CAN_TX_RATE_HZ 100u
|
||||
#define CAN_NODE_TIMEOUT_MS 100u
|
||||
#define CAN_WDOG_RESTART_MS 200u
|
||||
typedef struct { int16_t velocity_rpm; int16_t torque_x100; } can_cmd_t;
|
||||
typedef struct {
|
||||
int16_t velocity_rpm; int16_t current_ma; int16_t position_x100;
|
||||
int8_t temperature_c; uint8_t fault; uint32_t last_rx_ms;
|
||||
} can_feedback_t;
|
||||
typedef struct {
|
||||
uint32_t tx_count; uint32_t rx_count; uint16_t err_count;
|
||||
uint8_t bus_off; uint8_t _pad;
|
||||
} can_stats_t;
|
||||
typedef enum {
|
||||
CAN_ERR_NOMINAL = 0u, CAN_ERR_WARNING = 1u,
|
||||
CAN_ERR_ERROR_PASSIVE = 2u, CAN_ERR_BUS_OFF = 3u,
|
||||
} can_error_state_t;
|
||||
typedef struct {
|
||||
uint32_t restart_count; uint32_t busoff_count;
|
||||
uint16_t errpassive_count; uint16_t errwarn_count;
|
||||
can_error_state_t error_state; uint8_t tec; uint8_t rec; uint8_t busoff_pending;
|
||||
uint32_t busoff_ms;
|
||||
} can_wdog_t;
|
||||
void can_driver_init(void);
|
||||
void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd);
|
||||
void can_driver_send_enable(uint8_t node_id, bool enable);
|
||||
bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out);
|
||||
bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms);
|
||||
void can_driver_get_stats(can_stats_t *out);
|
||||
void can_driver_process(void);
|
||||
can_error_state_t can_driver_watchdog_tick(uint32_t now_ms);
|
||||
void can_driver_get_wdog(can_wdog_t *out);
|
||||
#ifdef TEST_HOST
|
||||
void can_driver_inject_esr(uint32_t esr_val);
|
||||
#endif
|
||||
typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len);
|
||||
typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len);
|
||||
void can_driver_set_ext_cb(can_ext_frame_cb_t cb);
|
||||
void can_driver_set_std_cb(can_std_frame_cb_t cb);
|
||||
void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len);
|
||||
void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len);
|
||||
#endif /* CAN_DRIVER_H */
|
||||
@ -1,308 +0,0 @@
|
||||
#ifndef CONFIG_H
|
||||
#define CONFIG_H
|
||||
|
||||
// ============================================
|
||||
// SaltyLab Balance Bot — MAMBA F722S FC
|
||||
// Pin assignments from Betaflight: DIAT-MAMBAF722_2022B
|
||||
// ============================================
|
||||
|
||||
// --- IMU: MPU6000 (SPI1) ---
|
||||
// SPI1: PA5=SCK, PA6=MISO, PA7=MOSI
|
||||
// WHO_AM_I = 0x68
|
||||
#define MPU_SPI SPI1
|
||||
#define MPU_CS_PORT GPIOA
|
||||
#define MPU_CS_PIN GPIO_PIN_4 // GYRO_CS 1
|
||||
#define MPU_EXTI_PORT GPIOC
|
||||
#define MPU_EXTI_PIN GPIO_PIN_4 // GYRO_EXTI 1 (data ready IRQ)
|
||||
#define GYRO_ALIGN CW270 // gyro_1_sensor_align = CW270
|
||||
|
||||
// --- Barometer: BMP280 or DPS310 (I2C1) ---
|
||||
#define BARO_I2C I2C1
|
||||
#define BARO_SCL_PORT GPIOB
|
||||
#define BARO_SCL_PIN GPIO_PIN_8 // I2C_SCL 1
|
||||
#define BARO_SDA_PORT GPIOB
|
||||
#define BARO_SDA_PIN GPIO_PIN_9 // I2C_SDA 1
|
||||
// Magnetometer also on I2C1 (external, header only)
|
||||
|
||||
// --- LEDs ---
|
||||
#define LED1_PORT GPIOC
|
||||
#define LED1_PIN GPIO_PIN_15 // LED 1 (active low)
|
||||
#define LED2_PORT GPIOC
|
||||
#define LED2_PIN GPIO_PIN_14 // LED 2 (active low)
|
||||
|
||||
// --- Buzzer ---
|
||||
#define BEEPER_PORT GPIOB
|
||||
#define BEEPER_PIN GPIO_PIN_2 // BEEPER 1
|
||||
#define BEEPER_INVERTED 1 // beeper_inversion = ON
|
||||
// beeper_od = OFF (push-pull)
|
||||
|
||||
// --- Battery Monitoring (ADC3) ---
|
||||
#define ADC_VBAT_PORT GPIOC
|
||||
#define ADC_VBAT_PIN GPIO_PIN_1 // ADC_BATT 1
|
||||
#define ADC_CURR_PORT GPIOC
|
||||
#define ADC_CURR_PIN GPIO_PIN_3 // ADC_CURR 1
|
||||
#define ADC_IBAT_SCALE 115 // ibata_scale
|
||||
|
||||
// --- LED Strip (WS2812 NeoPixel, Issue #193) ---
|
||||
// TIM3_CH1 PWM on PB4 for 8-LED ring status indicator
|
||||
#define LED_STRIP_TIM TIM3
|
||||
#define LED_STRIP_CHANNEL TIM_CHANNEL_1
|
||||
#define LED_STRIP_PORT GPIOB
|
||||
#define LED_STRIP_PIN GPIO_PIN_4 // LED_STRIP 1 (TIM3_CH1)
|
||||
#define LED_STRIP_AF GPIO_AF2_TIM3 // Alternate function
|
||||
#define LED_STRIP_NUM_LEDS 8u // 8-LED ring
|
||||
#define LED_STRIP_FREQ_HZ 800000u // 800 kHz PWM for NeoPixel (1.25 µs per bit)
|
||||
|
||||
// --- Servo Pan-Tilt (Issue #206) ---
|
||||
// TIM4_CH1 (PB6) for pan servo, TIM4_CH2 (PB7) for tilt servo
|
||||
#define SERVO_TIM TIM4
|
||||
#define SERVO_PAN_PORT GPIOB
|
||||
#define SERVO_PAN_PIN GPIO_PIN_6 // TIM4_CH1
|
||||
#define SERVO_PAN_CHANNEL TIM_CHANNEL_1
|
||||
#define SERVO_TILT_PORT GPIOB
|
||||
#define SERVO_TILT_PIN GPIO_PIN_7 // TIM4_CH2
|
||||
#define SERVO_TILT_CHANNEL TIM_CHANNEL_2
|
||||
#define SERVO_AF GPIO_AF2_TIM4 // Alternate function
|
||||
#define SERVO_FREQ_HZ 50u // 50 Hz (20ms period, standard servo)
|
||||
#define SERVO_MIN_US 500u // 500µs = 0°
|
||||
#define SERVO_MAX_US 2500u // 2500µs = 180°
|
||||
#define SERVO_CENTER_US 1500u // 1500µs = 90°
|
||||
|
||||
// --- OSD: MAX7456 (SPI2) ---
|
||||
#define OSD_SPI SPI2
|
||||
#define OSD_CS_PORT GPIOB
|
||||
#define OSD_CS_PIN GPIO_PIN_12 // OSD_CS 1
|
||||
// SPI2: PB13=SCK, PB14=MISO, PB15=MOSI
|
||||
|
||||
// --- Blackbox Flash: M25P16 (SPI3) ---
|
||||
#define FLASH_SPI SPI3
|
||||
#define FLASH_CS_PORT GPIOA
|
||||
#define FLASH_CS_PIN GPIO_PIN_15 // FLASH_CS 1
|
||||
// SPI3: PC10=SCK, PC11=MISO, PB5=MOSI
|
||||
|
||||
// --- Motor Outputs (PWM/DShot) ---
|
||||
#define MOTOR1_PORT GPIOC
|
||||
#define MOTOR1_PIN GPIO_PIN_8 // TIM8_CH3
|
||||
#define MOTOR2_PORT GPIOC
|
||||
#define MOTOR2_PIN GPIO_PIN_9 // TIM8_CH4
|
||||
#define MOTOR3_PORT GPIOA
|
||||
#define MOTOR3_PIN GPIO_PIN_8 // TIM1_CH1
|
||||
#define MOTOR4_PORT GPIOA
|
||||
#define MOTOR4_PIN GPIO_PIN_9 // TIM1_CH2
|
||||
#define MOTOR5_PORT GPIOB
|
||||
#define MOTOR5_PIN GPIO_PIN_0 // TIM3_CH3
|
||||
#define MOTOR6_PORT GPIOB
|
||||
#define MOTOR6_PIN GPIO_PIN_1 // TIM3_CH4
|
||||
#define MOTOR7_PORT GPIOA
|
||||
#define MOTOR7_PIN GPIO_PIN_10 // TIM1_CH3
|
||||
#define MOTOR8_PORT GPIOB
|
||||
#define MOTOR8_PIN GPIO_PIN_4 // TIM3_CH1
|
||||
|
||||
// --- UARTs ---
|
||||
// USART1: PB6=TX, PB7=RX (serial 0, SmartAudio/VTX)
|
||||
#define UART1_TX_PORT GPIOB
|
||||
#define UART1_TX_PIN GPIO_PIN_6
|
||||
#define UART1_RX_PORT GPIOB
|
||||
#define UART1_RX_PIN GPIO_PIN_7
|
||||
|
||||
// USART2: PA2=TX, PA3=RX (serial 1)
|
||||
#define UART2_TX_PORT GPIOA
|
||||
#define UART2_TX_PIN GPIO_PIN_2
|
||||
#define UART2_RX_PORT GPIOA
|
||||
#define UART2_RX_PIN GPIO_PIN_3
|
||||
|
||||
// USART3: PB10=TX, PB11=RX (serial 2, SBUS RX default)
|
||||
#define UART3_TX_PORT GPIOB
|
||||
#define UART3_TX_PIN GPIO_PIN_10
|
||||
#define UART3_RX_PORT GPIOB
|
||||
#define UART3_RX_PIN GPIO_PIN_11
|
||||
|
||||
// UART4: PA0=TX, PA1=RX (serial 3)
|
||||
#define UART4_TX_PORT GPIOA
|
||||
#define UART4_TX_PIN GPIO_PIN_0
|
||||
#define UART4_RX_PORT GPIOA
|
||||
#define UART4_RX_PIN GPIO_PIN_1
|
||||
|
||||
// UART5: PC12=TX, PD2=RX (serial 4)
|
||||
#define UART5_TX_PORT GPIOC
|
||||
#define UART5_TX_PIN GPIO_PIN_12
|
||||
#define UART5_RX_PORT GPIOD
|
||||
#define UART5_RX_PIN GPIO_PIN_2
|
||||
|
||||
// USART6: PC6=TX, PC7=RX (serial 5)
|
||||
#define UART6_TX_PORT GPIOC
|
||||
#define UART6_TX_PIN GPIO_PIN_6
|
||||
#define UART6_RX_PORT GPIOC
|
||||
#define UART6_RX_PIN GPIO_PIN_7
|
||||
|
||||
// --- PINIO (switchable outputs, e.g. VTX power) ---
|
||||
#define PINIO1_PORT GPIOC
|
||||
#define PINIO1_PIN GPIO_PIN_2 // pinio_config = 129 (USER1)
|
||||
#define PINIO2_PORT GPIOC
|
||||
#define PINIO2_PIN GPIO_PIN_0 // pinio_config = 129 (USER2)
|
||||
|
||||
// --- JLink: Jetson Serial Binary Protocol (USART1, Issue #120) ---
|
||||
#define JLINK_BAUD 921600 /* USART1 baud rate */
|
||||
#define JLINK_HB_TIMEOUT_MS 1000 /* Jetson heartbeat timeout (ms) */
|
||||
#define JLINK_TLM_HZ 50 /* STATUS telemetry TX rate (Hz) */
|
||||
|
||||
// --- Firmware Version ---
|
||||
#define FW_MAJOR 1
|
||||
#define FW_MINOR 0
|
||||
#define FW_PATCH 0
|
||||
|
||||
// --- SaltyLab Assignments ---
|
||||
// Hoverboard ESC: USART2 (PA2=TX, PA3=RX) or USART3
|
||||
// ELRS Receiver: UART4 (PA0=TX, PA1=RX) — CRSF 420000 baud
|
||||
// Jetson (JLink binary protocol, Issue #120): USART1 (PB6=TX, PB7=RX) @ 921600
|
||||
// USART6 (PC6=TX, PC7=RX): legacy Jetson CDC path — reserved for VESC (Issue #383)
|
||||
// Debug: UART5 (PC12=TX, PD2=RX)
|
||||
|
||||
// --- ESC Backend Selection (Issue #388) ---
|
||||
// Pluggable ESC abstraction layer — supports multiple backends:
|
||||
// HOVERBOARD: EFeru FOC (USART2 @ 115200) — current default
|
||||
// VESC: FSESC 4.20 Plus (USART6 @ 921600, balance mode) — future
|
||||
#define ESC_BACKEND HOVERBOARD /* HOVERBOARD or VESC */
|
||||
|
||||
// --- CRSF / ExpressLRS ---
|
||||
// CH1[0]=steer CH2[1]=throttle CH5[4]=arm CH6[5]=mode
|
||||
#define CRSF_ARM_THRESHOLD 1750 /* CH5 raw value; > threshold = armed */
|
||||
#define CRSF_STEER_MAX 400 /* CH1 range: -400..+400 motor counts */
|
||||
#define CRSF_FAILSAFE_MS 500 /* Disarm after this ms without a frame (Issue #103) */
|
||||
|
||||
// --- Battery ADC (ADC3, PC1 = ADC123_IN11) ---
|
||||
/* Mamba F722: 10kΩ + 1kΩ voltage divider → 11:1 ratio */
|
||||
#define VBAT_SCALE_NUM 11 /* Numerator of divider ratio */
|
||||
#define VBAT_AREF_MV 3300 /* ADC reference in mV */
|
||||
#define VBAT_ADC_BITS 12 /* 12-bit ADC → 4096 counts */
|
||||
/* Filtered Vbat in mV: (raw * 3300 * 11) / 4096, updated at 10Hz */
|
||||
|
||||
// --- CRSF Telemetry TX (uplink: FC → ELRS module → pilot handset) ---
|
||||
#define CRSF_TELEMETRY_HZ 1 /* Telemetry TX rate (Hz) */
|
||||
|
||||
// --- PID Tuning ---
|
||||
#define PID_KP 35.0f
|
||||
#define PID_KI 1.0f
|
||||
#define PID_KD 1.0f
|
||||
#define PID_INTEGRAL_MAX 500.0f
|
||||
#define PID_LOOP_HZ 1000
|
||||
|
||||
// --- Safety ---
|
||||
#define MAX_TILT_DEG 25.0f
|
||||
#define RC_TIMEOUT_MS 500
|
||||
#define ARMING_HOLD_MS 3000
|
||||
#define MAX_SPEED_LIMIT 100
|
||||
#define WATCHDOG_TIMEOUT_MS 50
|
||||
|
||||
// --- Motor Driver ---
|
||||
#define MOTOR_CMD_MAX 1000 /* ESC range: -1000..+1000 */
|
||||
#define MOTOR_STEER_RAMP_RATE 20 /* counts/ms — steer ramp only */
|
||||
|
||||
// --- IMU Calibration ---
|
||||
#define GYRO_CAL_SAMPLES 1000 /* gyro bias samples (~1s at 1ms/sample) */
|
||||
|
||||
// --- RC / Mode Manager ---
|
||||
/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */
|
||||
#define CRSF_CH_STEER 0 /* CH1 — right stick horizontal (steer) */
|
||||
#define CRSF_CH_SPEED 1 /* CH2 — right stick vertical (throttle) */
|
||||
#define CRSF_CH_ARM 4 /* CH5 — arm switch (2-pos) */
|
||||
#define CRSF_CH_MODE 5 /* CH6 — mode switch (3-pos) */
|
||||
/* Deadband around CRSF center (992) in raw counts (~2% of range) */
|
||||
#define CRSF_DEADBAND 30
|
||||
/* CH6 mode thresholds (raw CRSF counts) */
|
||||
#define CRSF_MODE_LOW_THRESH 600 /* <= → RC_MANUAL */
|
||||
#define CRSF_MODE_HIGH_THRESH 1200 /* >= → AUTONOMOUS */
|
||||
/* Max speed bias RC can add to balance PID output (counts, same scale as ESC) */
|
||||
#define MOTOR_RC_SPEED_MAX 300
|
||||
/* Full blend transition time: MANUAL→AUTO takes this many ms */
|
||||
#define MODE_BLEND_MS 500
|
||||
|
||||
// --- Power Management (STOP mode, Issue #178) ---
|
||||
#define PM_IDLE_TIMEOUT_MS 30000u // 30s no activity → PM_SLEEP_PENDING
|
||||
#define PM_FADE_MS 3000u // LED fade-out duration before STOP entry
|
||||
#define PM_LED_PERIOD_MS 2000u // sleep-pending triangle-wave period (ms)
|
||||
// Estimated per-subsystem currents (mA) — used for JLINK_TLM_POWER telemetry
|
||||
#define PM_CURRENT_BASE_MA 30 // SPI1(IMU)+UART4(CRSF)+USART1(JLink)+core
|
||||
#define PM_CURRENT_AUDIO_MA 8 // I2S3 + amplifier quiescent
|
||||
#define PM_CURRENT_OSD_MA 5 // SPI2 OSD (MAX7456)
|
||||
#define PM_CURRENT_DEBUG_MA 1 // UART5 + USART6
|
||||
#define PM_CURRENT_STOP_MA 1 // MCU in STOP mode (< 1 mA)
|
||||
#define PM_TLM_HZ 1 // JLINK_TLM_POWER transmit rate (Hz)
|
||||
|
||||
// --- Audio Amplifier (I2S3, Issue #143) ---
|
||||
// SPI3 repurposed as I2S3; blackbox flash unused on balance bot
|
||||
#define AUDIO_BCLK_PORT GPIOC
|
||||
#define AUDIO_BCLK_PIN GPIO_PIN_10 // I2S3_CK (PC10, AF6)
|
||||
#define AUDIO_LRCK_PORT GPIOA
|
||||
#define AUDIO_LRCK_PIN GPIO_PIN_15 // I2S3_WS (PA15, AF6)
|
||||
#define AUDIO_DOUT_PORT GPIOB
|
||||
#define AUDIO_DOUT_PIN GPIO_PIN_5 // I2S3_SD (PB5, AF6)
|
||||
#define AUDIO_MUTE_PORT GPIOC
|
||||
#define AUDIO_MUTE_PIN GPIO_PIN_5 // active-high = amp enabled
|
||||
// PLLI2S: N=192, R=2 → I2S clock=96 MHz → FS≈22058 Hz (< 0.04% error)
|
||||
#define AUDIO_SAMPLE_RATE 22050u // nominal sample rate (Hz)
|
||||
#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz
|
||||
#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100
|
||||
|
||||
// --- Gimbal Servo Bus (ST3215, USART3 half-duplex, Issue #547) ---
|
||||
// Half-duplex single-wire on PB10 (USART3_TX, AF7) at 1 Mbps.
|
||||
// USART3 is available: not assigned to any active subsystem.
|
||||
#define SERVO_BUS_UART USART3
|
||||
#define SERVO_BUS_PORT GPIOB
|
||||
#define SERVO_BUS_PIN GPIO_PIN_10 // USART3_TX, AF7
|
||||
#define SERVO_BUS_BAUD 1000000u // 1 Mbps (ST3215 default)
|
||||
#define GIMBAL_PAN_ID 1u // ST3215 servo ID for pan
|
||||
#define GIMBAL_TILT_ID 2u // ST3215 servo ID for tilt
|
||||
#define GIMBAL_TLM_HZ 50u // position feedback rate (Hz)
|
||||
#define GIMBAL_PAN_LIMIT_DEG 180.0f // pan soft limit (deg each side)
|
||||
#define GIMBAL_TILT_LIMIT_DEG 90.0f // tilt soft limit (deg each side)
|
||||
|
||||
// --- CAN Bus Driver (Issue #597, remapped Issue #676) ---
|
||||
// CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) — SCL/SDA pads on Mamba F722S MK2
|
||||
// I2C1 freed: BME280 moved to I2C2 (PB10/PB11); PB8/PB9 repurposed for CAN1
|
||||
#define CAN_RPM_SCALE 10 // motor_cmd to RPM: 1 cmd count = 10 RPM
|
||||
#define CAN_TLM_HZ 1u // JLINK_TLM_CAN_STATS transmit rate (Hz)
|
||||
|
||||
|
||||
// --- LVC: Low Voltage Cutoff (Issue #613) ---
|
||||
// 3-stage undervoltage protection; voltages in mV
|
||||
#define LVC_WARNING_MV 21000u // 21.0 V -- buzzer alert, full power
|
||||
#define LVC_CRITICAL_MV 19800u // 19.8 V -- 50% motor power reduction
|
||||
#define LVC_CUTOFF_MV 18600u // 18.6 V -- motors disabled, latch until reboot
|
||||
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
|
||||
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz)
|
||||
|
||||
|
||||
// --- UART Command Protocol (Issue #629) ---
|
||||
// Jetson-STM32 binary command protocol on UART5 (PC12/PD2)
|
||||
// NOTE: Spec requested USART1 @ 115200; USART1 is occupied by JLink @ 921600.
|
||||
#define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol
|
||||
#define UART_PROT_HB_TIMEOUT_MS 500u // heartbeat timeout: Jetson considered lost after 500 ms
|
||||
|
||||
// --- Encoder Odometry (Issue #632) ---
|
||||
// Left encoder: TIM2 (32-bit), CH1=PA15 (AF1), CH2=PB3 (AF1)
|
||||
// Right encoder: TIM3 (16-bit), CH1=PC6 (AF2), CH2=PC7 (AF2)
|
||||
// Encoder mode 3: count on both A and B edges (x4 resolution)
|
||||
#define ENC_LEFT_TIM TIM2
|
||||
#define ENC_LEFT_CH1_PORT GPIOA
|
||||
#define ENC_LEFT_CH1_PIN GPIO_PIN_15 // TIM2_CH1, AF1
|
||||
#define ENC_LEFT_CH2_PORT GPIOB
|
||||
#define ENC_LEFT_CH2_PIN GPIO_PIN_3 // TIM2_CH2, AF1
|
||||
#define ENC_LEFT_AF GPIO_AF1_TIM2
|
||||
#define ENC_RIGHT_TIM TIM3
|
||||
#define ENC_RIGHT_CH1_PORT GPIOC
|
||||
#define ENC_RIGHT_CH1_PIN GPIO_PIN_6 // TIM3_CH1, AF2
|
||||
#define ENC_RIGHT_CH2_PORT GPIOC
|
||||
#define ENC_RIGHT_CH2_PIN GPIO_PIN_7 // TIM3_CH2, AF2
|
||||
#define ENC_RIGHT_AF GPIO_AF2_TIM3
|
||||
|
||||
// --- Hardware Button (Issue #682) ---
|
||||
// Active-low push button on PC2 (internal pull-up)
|
||||
#define BTN_PORT GPIOC
|
||||
#define BTN_PIN GPIO_PIN_2
|
||||
#define BTN_DEBOUNCE_MS 20u // ms debounce window
|
||||
#define BTN_LONG_MIN_MS 1500u // ms threshold: LONG press
|
||||
#define BTN_COMMIT_MS 500u // ms quiet after lone SHORT -> PARK event
|
||||
#define BTN_SEQ_TIMEOUT_MS 3000u // ms: sequence window; expired buffer abandoned
|
||||
|
||||
#endif // CONFIG_H
|
||||
@ -1,45 +0,0 @@
|
||||
#ifndef COULOMB_COUNTER_H
|
||||
#define COULOMB_COUNTER_H
|
||||
|
||||
/*
|
||||
* coulomb_counter.h — Battery coulomb counter for SoC estimation (Issue #325)
|
||||
*
|
||||
* Integrates battery current over time to track Ah consumed and remaining.
|
||||
* Provides accurate SoC independent of load, with fallback to voltage.
|
||||
*
|
||||
* Usage:
|
||||
* 1. Call coulomb_counter_init(capacity_mah) at startup
|
||||
* 2. Call coulomb_counter_accumulate(current_ma) at 50–100 Hz
|
||||
* 3. Call coulomb_counter_get_soc_pct() to get current SoC
|
||||
* 4. Call coulomb_counter_reset() on charge complete
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* Initialize coulomb counter with battery capacity (mAh). */
|
||||
void coulomb_counter_init(uint16_t capacity_mah);
|
||||
|
||||
/*
|
||||
* Accumulate coulomb from current reading + elapsed time.
|
||||
* Call this at regular intervals (e.g., 50–100 Hz from telemetry loop).
|
||||
* current_ma: battery current in milliamps (positive = discharge)
|
||||
*/
|
||||
void coulomb_counter_accumulate(int16_t current_ma);
|
||||
|
||||
/* Get current SoC as percentage (0–100, 255 = error). */
|
||||
uint8_t coulomb_counter_get_soc_pct(void);
|
||||
|
||||
/* Get consumed mAh (total charge removed from battery). */
|
||||
uint16_t coulomb_counter_get_consumed_mah(void);
|
||||
|
||||
/* Get remaining capacity in mAh. */
|
||||
uint16_t coulomb_counter_get_remaining_mah(void);
|
||||
|
||||
/* Reset accumulated coulombs (e.g., on charge complete). */
|
||||
void coulomb_counter_reset(void);
|
||||
|
||||
/* Check if coulomb counter is active (initialized and has measurements). */
|
||||
bool coulomb_counter_is_valid(void);
|
||||
|
||||
#endif /* COULOMB_COUNTER_H */
|
||||
@ -1,69 +0,0 @@
|
||||
#ifndef CRSF_H
|
||||
#define CRSF_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* CRSF/ExpressLRS RC receiver state.
|
||||
*
|
||||
* Updated from ISR context on every valid frame.
|
||||
* Read from main loop — values are naturally atomic (8/16-bit on Cortex-M).
|
||||
* last_rx_ms == 0 means no frame received yet (USB-only mode).
|
||||
*/
|
||||
typedef struct {
|
||||
uint16_t channels[16]; /* Raw CRSF values, 172 (988µs) – 1811 (2012µs) */
|
||||
uint32_t last_rx_ms; /* HAL_GetTick() at last valid RC frame */
|
||||
bool armed; /* CH5 arm switch: true when channels[4] > CRSF_ARM_THRESHOLD */
|
||||
|
||||
/* Link statistics (from 0x14 frames, optional) */
|
||||
int8_t rssi_dbm; /* Uplink RSSI in dBm (negative, e.g. -85) */
|
||||
uint8_t link_quality; /* Uplink link quality 0–100 % */
|
||||
int8_t snr; /* Uplink SNR in dB */
|
||||
} CRSFState;
|
||||
|
||||
/*
|
||||
* crsf_init() — configure UART4 (PA0=TX, PA1=RX) at 420000 baud with
|
||||
* DMA1 circular RX and IDLE interrupt. Call once before safety_init().
|
||||
*/
|
||||
void crsf_init(void);
|
||||
|
||||
/*
|
||||
* crsf_parse_byte() — feed one byte into the frame parser.
|
||||
* Called automatically from DMA/IDLE ISR. Available for unit tests.
|
||||
*/
|
||||
void crsf_parse_byte(uint8_t byte);
|
||||
|
||||
/*
|
||||
* crsf_to_range() — map raw CRSF value (172–1811) linearly to [min, max].
|
||||
* Clamps at boundaries. Midpoint 992 → (min+max)/2.
|
||||
*/
|
||||
int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max);
|
||||
|
||||
/*
|
||||
* crsf_send_battery() — transmit CRSF battery-sensor telemetry frame (type 0x08)
|
||||
* back to the ELRS TX module over UART4 TX. Call at CRSF_TELEMETRY_HZ (1 Hz).
|
||||
*
|
||||
* voltage_mv : battery voltage in millivolts (e.g. 12600 for 3S full)
|
||||
* capacity_mah : remaining battery capacity in mAh (Issue #325, coulomb counter)
|
||||
* remaining_pct: state-of-charge 0–100 % (255 = unknown)
|
||||
*
|
||||
* Frame: [0xC8][12][0x08][v16_hi][v16_lo][c16_hi][c16_lo][cap24×3][rem][CRC]
|
||||
* voltage unit: 100 mV (12600 mV → 126)
|
||||
* capacity unit: mAh (3-byte big-endian, max 16.7M mAh)
|
||||
*/
|
||||
void crsf_send_battery(uint32_t voltage_mv, uint32_t capacity_mah,
|
||||
uint8_t remaining_pct);
|
||||
|
||||
/*
|
||||
* crsf_send_flight_mode() — transmit CRSF flight-mode frame (type 0x21)
|
||||
* for display on the pilot's handset OSD.
|
||||
*
|
||||
* armed: true → "ARMED\0"
|
||||
* false → "DISARM\0"
|
||||
*/
|
||||
void crsf_send_flight_mode(bool armed);
|
||||
|
||||
extern volatile CRSFState crsf_state;
|
||||
|
||||
#endif /* CRSF_H */
|
||||
@ -1,151 +0,0 @@
|
||||
#ifndef ENCODER_ODOM_H
|
||||
#define ENCODER_ODOM_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* encoder_odom — quadrature encoder reading and differential-drive odometry
|
||||
* (Issue #632).
|
||||
*
|
||||
* HARDWARE:
|
||||
* Left encoder : TIM2 (32-bit) in encoder mode 3
|
||||
* CH1 = PA15 (AF1), CH2 = PB3 (AF1)
|
||||
* Right encoder : TIM3 (16-bit) in encoder mode 3
|
||||
* CH1 = PC6 (AF2), CH2 = PC7 (AF2)
|
||||
*
|
||||
* Both channels count on every edge (×4 resolution).
|
||||
* TIM2 ARR = 0xFFFFFFFF (32-bit, never overflows in practice).
|
||||
* TIM3 ARR = 0xFFFF (16-bit, delta decoded via int16_t subtraction).
|
||||
*
|
||||
* ODOMETRY MODEL (differential drive):
|
||||
*
|
||||
* meters_per_tick = (π × wheel_diam_mm × 1e-3) / ticks_per_rev
|
||||
*
|
||||
* d_left = Δticks_left × meters_per_tick
|
||||
* d_right = Δticks_right × meters_per_tick
|
||||
*
|
||||
* d_center = (d_left + d_right) / 2
|
||||
* dθ = (d_right - d_left) / wheel_base_mm × 1e-3 (radians)
|
||||
*
|
||||
* x += d_center × cos(θ)
|
||||
* y += d_center × sin(θ)
|
||||
* θ += dθ
|
||||
*
|
||||
* For small dt this is the standard Euler-forward integration; suitable for
|
||||
* the 50 Hz odometry tick rate.
|
||||
*
|
||||
* RPM:
|
||||
* rpm = Δticks × 60.0 / (ticks_per_rev × dt_s)
|
||||
*
|
||||
* FLASH CONFIG (ENC_FLASH_ADDR in sector 7):
|
||||
* Stores ticks_per_rev, wheel_diam_mm, wheel_base_mm validated by magic.
|
||||
* Falls back to compile-time defaults on magic mismatch.
|
||||
* Sector 7 is shared with PID flash; saving encoder config must be
|
||||
* coordinated with pid_flash_save_all() to avoid mutual erasure.
|
||||
*
|
||||
* TELEMETRY:
|
||||
* JLINK_TLM_ODOM (0x8C) published at ENC_TLM_HZ (50 Hz):
|
||||
* jlink_tlm_odom_t { int16 rpm_left, int16 rpm_right,
|
||||
* int32 x_mm, int32 y_mm,
|
||||
* int16 theta_cdeg, int16 speed_mmps }
|
||||
* 16 bytes, 22-byte frame.
|
||||
*/
|
||||
|
||||
/* ---- Default hardware parameters (override in flash config) ---- */
|
||||
/* Hoverboard 6.5" wheels with typical geared-motor encoder: */
|
||||
#define ENC_TICKS_PER_REV_DEFAULT 1320u /* 33 CPR × 40:1 gear = 1320 ticks/rev */
|
||||
#define ENC_WHEEL_DIAM_MM_DEFAULT 165u /* 6.5" ≈ 165 mm diameter */
|
||||
#define ENC_WHEEL_BASE_MM_DEFAULT 540u /* ~540 mm axle-to-axle separation */
|
||||
|
||||
/* ---- Flash config ---- */
|
||||
/* Stored in sector 7 immediately before the PID schedule area (0x0807FF40).
|
||||
* 64-byte block: magic(4) + config(12) + pad(48). */
|
||||
#define ENC_FLASH_ADDR 0x0807FF00UL
|
||||
#define ENC_FLASH_MAGIC 0x534C4503UL /* 'SLE\x03' — encoder config v3 */
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t magic; /* ENC_FLASH_MAGIC when valid */
|
||||
uint32_t ticks_per_rev; /* encoder ticks per full wheel revolution */
|
||||
uint16_t wheel_diam_mm; /* wheel outer diameter (mm) */
|
||||
uint16_t wheel_base_mm; /* lateral wheel separation centre-to-centre (mm) */
|
||||
uint8_t _pad[48]; /* reserved — total 64 bytes */
|
||||
} enc_flash_config_t;
|
||||
|
||||
/* ---- Runtime configuration ---- */
|
||||
typedef struct {
|
||||
uint32_t ticks_per_rev;
|
||||
uint16_t wheel_diam_mm;
|
||||
uint16_t wheel_base_mm;
|
||||
} enc_config_t;
|
||||
|
||||
/* ---- Runtime state ---- */
|
||||
typedef struct {
|
||||
/* Encoder counters (last sampled) */
|
||||
uint32_t cnt_left; /* last TIM2->CNT */
|
||||
uint16_t cnt_right; /* last TIM3->CNT */
|
||||
|
||||
/* Wheel speeds */
|
||||
int16_t rpm_left; /* left wheel RPM (signed; + = forward) */
|
||||
int16_t rpm_right; /* right wheel RPM (signed) */
|
||||
int16_t speed_mmps; /* linear speed of centre point (mm/s) */
|
||||
|
||||
/* Pose (relative to last reset) */
|
||||
float x_mm; /* forward displacement (mm) */
|
||||
float y_mm; /* lateral displacement (mm, + = left) */
|
||||
float theta_rad; /* heading (radians, + = CCW from start) */
|
||||
|
||||
/* Internal */
|
||||
float meters_per_tick; /* pre-computed from config */
|
||||
float wheel_base_m; /* wheel_base_mm / 1000.0 */
|
||||
uint32_t last_tick_ms; /* HAL_GetTick() at last encoder_odom_tick() */
|
||||
uint32_t last_tlm_ms; /* HAL_GetTick() at last TLM transmission */
|
||||
|
||||
enc_config_t cfg; /* active hardware parameters */
|
||||
} encoder_odom_t;
|
||||
|
||||
/* ---- Configuration ---- */
|
||||
#define ENC_TLM_HZ 50u /* JLINK_TLM_ODOM transmit rate (Hz) */
|
||||
|
||||
/* ---- API ---- */
|
||||
|
||||
/*
|
||||
* encoder_odom_init(eo) — configure TIM2/TIM3 in encoder mode, load flash
|
||||
* config (falling back to defaults), reset pose.
|
||||
* Call once during system init.
|
||||
*/
|
||||
void encoder_odom_init(encoder_odom_t *eo);
|
||||
|
||||
/*
|
||||
* encoder_odom_tick(eo, now_ms) — sample encoder counters, update RPM and
|
||||
* integrate odometry. Call from main loop at any rate ≥ 10 Hz (50 Hz ideal).
|
||||
*/
|
||||
void encoder_odom_tick(encoder_odom_t *eo, uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* encoder_odom_reset_pose(eo) — zero x/y/theta without resetting counters or
|
||||
* config. Call whenever odometry reference frame should be re-anchored.
|
||||
*/
|
||||
void encoder_odom_reset_pose(encoder_odom_t *eo);
|
||||
|
||||
/*
|
||||
* encoder_odom_save_config(cfg) — write enc_flash_config_t to ENC_FLASH_ADDR.
|
||||
* WARNING: erases sector 7 — must NOT be called while armed and must be
|
||||
* coordinated with PID flash saves (both records are in sector 7).
|
||||
* Returns true on success.
|
||||
*/
|
||||
bool encoder_odom_save_config(const enc_config_t *cfg);
|
||||
|
||||
/*
|
||||
* encoder_odom_load_config(cfg) — load config from flash.
|
||||
* Returns true if flash magic valid; false = defaults applied to *cfg.
|
||||
*/
|
||||
bool encoder_odom_load_config(enc_config_t *cfg);
|
||||
|
||||
/*
|
||||
* encoder_odom_send_tlm(eo, now_ms) — transmit JLINK_TLM_ODOM (0x8C) frame.
|
||||
* Rate-limited to ENC_TLM_HZ; safe to call every tick.
|
||||
*/
|
||||
void encoder_odom_send_tlm(const encoder_odom_t *eo, uint32_t now_ms);
|
||||
|
||||
#endif /* ENCODER_ODOM_H */
|
||||
@ -1,79 +0,0 @@
|
||||
#ifndef ESC_BACKEND_H
|
||||
#define ESC_BACKEND_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* ESC Backend Abstraction Layer
|
||||
*
|
||||
* Provides a pluggable interface for different ESC implementations:
|
||||
* - Hoverboard (EFeru FOC firmware, UART @ 115200)
|
||||
* - VESC (via UART @ 921600, with balance mode) — future
|
||||
*
|
||||
* Allows motor_driver.c to remain ESC-agnostic. Backend selection
|
||||
* via ESC_BACKEND compile-time define in config.h.
|
||||
*
|
||||
* Issue #388: ESC abstraction layer
|
||||
* Blocks Issue #383: VESC integration
|
||||
*/
|
||||
|
||||
/* Telemetry snapshot from ESC (polled on-demand) */
|
||||
typedef struct {
|
||||
int16_t speed; /* Motor speed (PWM duty or RPM, backend-dependent) */
|
||||
int16_t steer; /* Steering position (0 = centered) */
|
||||
uint16_t voltage_mv; /* Battery voltage in millivolts */
|
||||
int16_t current_ma; /* Motor current in milliamps (signed: discharge/charge) */
|
||||
int16_t temperature_c; /* ESC temperature in °C */
|
||||
uint16_t fault; /* Fault code (backend-specific) */
|
||||
} esc_telemetry_t;
|
||||
|
||||
/* Virtual function table for ESC backends */
|
||||
typedef struct {
|
||||
/* Initialize ESC hardware and UART (called once at startup) */
|
||||
void (*init)(void);
|
||||
|
||||
/* Send motor command to ESC (called at ~50Hz from motor_driver_update)
|
||||
* speed: -1000..+1000 (forward/reverse)
|
||||
* steer: -1000..+1000 (left/right)
|
||||
*/
|
||||
void (*send)(int16_t speed, int16_t steer);
|
||||
|
||||
/* Emergency stop: send zero and disable output
|
||||
* (called from safety or mode manager)
|
||||
*/
|
||||
void (*estop)(void);
|
||||
|
||||
/* Query current ESC state
|
||||
* Returns latest telemetry snapshot (may be cached/stale on some backends).
|
||||
* Safe to call from any context (non-blocking).
|
||||
*/
|
||||
void (*get_telemetry)(esc_telemetry_t *out);
|
||||
|
||||
/* Optional: resume from estop (not all backends use this) */
|
||||
void (*resume)(void);
|
||||
} esc_backend_t;
|
||||
|
||||
/*
|
||||
* Register a backend implementation at runtime.
|
||||
* Typically called during init sequence before motor_driver_init().
|
||||
*/
|
||||
void esc_backend_register(const esc_backend_t *backend);
|
||||
|
||||
/*
|
||||
* Get the currently active backend.
|
||||
* Returns pointer to vtable; nullptr if no backend registered.
|
||||
*/
|
||||
const esc_backend_t *esc_backend_get(void);
|
||||
|
||||
/*
|
||||
* High-level convenience wrappers (match motor_driver.c interface).
|
||||
* These call through the active backend if registered.
|
||||
*/
|
||||
void esc_init(void);
|
||||
void esc_send(int16_t speed, int16_t steer);
|
||||
void esc_estop(void);
|
||||
void esc_resume(void);
|
||||
void esc_get_telemetry(esc_telemetry_t *out);
|
||||
|
||||
#endif /* ESC_BACKEND_H */
|
||||
@ -1,111 +0,0 @@
|
||||
/*
|
||||
* face_animation.h — Face Emotion Renderer for LCD Display
|
||||
*
|
||||
* Renders expressive face animations for 5 core emotions:
|
||||
* - HAPPY: upturned eyes, curved smile
|
||||
* - SAD: downturned eyes, frown
|
||||
* - CURIOUS: raised eyebrows, wide eyes, slight tilt
|
||||
* - ANGRY: downturned brows, narrowed eyes, clenched mouth
|
||||
* - SLEEPING: closed eyes, relaxed mouth, gentle sway (optional)
|
||||
*
|
||||
* HOW IT WORKS:
|
||||
* - State machine with smooth transitions (easing over N frames)
|
||||
* - Idle behavior: periodic blinking (duration configurable)
|
||||
* - Each emotion has parameterized eye/mouth shapes (position, angle, curvature)
|
||||
* - Transitions interpolate between emotion parameter sets
|
||||
* - render() draws current state to LCD framebuffer via face_lcd_*() API
|
||||
* - tick() advances frame counter, handles transitions, triggers blink
|
||||
*
|
||||
* ANIMATION SPECS:
|
||||
* - Frame rate: 30 Hz (via systick)
|
||||
* - Transition time: 0.5–1.0s (15–30 frames)
|
||||
* - Blink duration: 100–150 ms (3–5 frames)
|
||||
* - Blink interval: 4–6 seconds (120–180 frames at 30Hz)
|
||||
*
|
||||
* API:
|
||||
* - face_animation_init() — Initialize state machine
|
||||
* - face_animation_set_emotion(emotion) — Request state change (with smooth transition)
|
||||
* - face_animation_tick() — Advance animation by 1 frame (call at 30Hz from systick)
|
||||
* - face_animation_render() — Draw current face to LCD framebuffer
|
||||
*/
|
||||
|
||||
#ifndef FACE_ANIMATION_H
|
||||
#define FACE_ANIMATION_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* === Emotion Types === */
|
||||
typedef enum {
|
||||
FACE_HAPPY = 0,
|
||||
FACE_SAD = 1,
|
||||
FACE_CURIOUS = 2,
|
||||
FACE_ANGRY = 3,
|
||||
FACE_SLEEPING = 4,
|
||||
FACE_NEUTRAL = 5, /* Default state */
|
||||
} face_emotion_t;
|
||||
|
||||
/* === Animation Parameters (per emotion) === */
|
||||
typedef struct {
|
||||
int16_t eye_x; /* Eye horizontal offset from center (pixels) */
|
||||
int16_t eye_y; /* Eye vertical offset from center (pixels) */
|
||||
int16_t eye_open_y; /* Eye open height (pixels) */
|
||||
int16_t eye_close_y; /* Eye close height (pixels, 0=fully closed) */
|
||||
int16_t brow_angle; /* Eyebrow angle (-30..+30 degrees, tilt) */
|
||||
int16_t brow_y_offset; /* Eyebrow vertical offset (pixels) */
|
||||
int16_t mouth_x; /* Mouth horizontal offset (pixels) */
|
||||
int16_t mouth_y; /* Mouth vertical offset (pixels) */
|
||||
int16_t mouth_width; /* Mouth width (pixels) */
|
||||
int16_t mouth_curve; /* Curvature: >0=smile, <0=frown, 0=neutral */
|
||||
uint8_t blink_interval_ms; /* Idle blink interval (seconds, in 30Hz ticks) */
|
||||
} face_params_t;
|
||||
|
||||
/* === Public API === */
|
||||
|
||||
/**
|
||||
* Initialize face animation system.
|
||||
* Sets initial emotion to NEUTRAL, clears blink timer.
|
||||
*/
|
||||
void face_animation_init(void);
|
||||
|
||||
/**
|
||||
* Request a state change to a new emotion.
|
||||
* Triggers smooth transition (easing) over TRANSITION_FRAMES.
|
||||
*/
|
||||
void face_animation_set_emotion(face_emotion_t emotion);
|
||||
|
||||
/**
|
||||
* Advance animation by one frame.
|
||||
* Called by systick ISR at 30 Hz.
|
||||
* Handles:
|
||||
* - Transition interpolation
|
||||
* - Blink timing and rendering
|
||||
* - Idle animations (sway, subtle movements)
|
||||
*/
|
||||
void face_animation_tick(void);
|
||||
|
||||
/**
|
||||
* Render current face state to LCD framebuffer.
|
||||
* Draws eyes, brows, mouth, and optional idle animations.
|
||||
* Should be called after face_animation_tick().
|
||||
*/
|
||||
void face_animation_render(void);
|
||||
|
||||
/**
|
||||
* Get current emotion (transition-aware).
|
||||
* Returns the target emotion, or current if transition in progress.
|
||||
*/
|
||||
face_emotion_t face_animation_get_emotion(void);
|
||||
|
||||
/**
|
||||
* Trigger a blink immediately (for special events).
|
||||
* Overrides idle blink timer.
|
||||
*/
|
||||
void face_animation_blink_now(void);
|
||||
|
||||
/**
|
||||
* Check if animation is idle (no active transition).
|
||||
*/
|
||||
bool face_animation_is_idle(void);
|
||||
|
||||
#endif // FACE_ANIMATION_H
|
||||
@ -1,116 +0,0 @@
|
||||
/*
|
||||
* face_lcd.h — STM32 LCD Display Driver for Face Animations
|
||||
*
|
||||
* Low-level abstraction for driving a small LCD/OLED display via SPI or I2C.
|
||||
* Supports pixel/line drawing primitives and full framebuffer operations.
|
||||
*
|
||||
* HOW IT WORKS:
|
||||
* - Initializes display (SPI/I2C, resolution, rotation)
|
||||
* - Provides framebuffer (in RAM or on-device)
|
||||
* - Exposes primitives: draw_pixel, draw_line, draw_circle, fill_rect
|
||||
* - Implements vsync-driven 30Hz refresh from systick
|
||||
* - Non-blocking DMA transfers for rapid display updates
|
||||
*
|
||||
* HARDWARE ASSUMPTIONS:
|
||||
* - SPI2 or I2C (configurable via #define LCD_INTERFACE)
|
||||
* - Typical sizes: 128×64, 240×135, 320×240
|
||||
* - Pixel depth: 1-bit (monochrome) or 16-bit (RGB565)
|
||||
* - Controller: SSD1306, ILI9341, ST7789, etc.
|
||||
*
|
||||
* API:
|
||||
* - face_lcd_init(width, height, bpp) — Initialize display
|
||||
* - face_lcd_clear() — Clear framebuffer
|
||||
* - face_lcd_pixel(x, y, color) — Set pixel
|
||||
* - face_lcd_line(x0, y0, x1, y1, color) — Draw line (Bresenham)
|
||||
* - face_lcd_circle(cx, cy, r, color) — Draw circle
|
||||
* - face_lcd_fill_rect(x, y, w, h, color) — Filled rectangle
|
||||
* - face_lcd_flush() — Push framebuffer to display (async via DMA)
|
||||
* - face_lcd_is_busy() — Check if transfer in progress
|
||||
* - face_lcd_tick() — Called by systick ISR for 30Hz vsync
|
||||
*/
|
||||
|
||||
#ifndef FACE_LCD_H
|
||||
#define FACE_LCD_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* === Configuration === */
|
||||
#define LCD_INTERFACE SPI /* SPI or I2C */
|
||||
#define LCD_WIDTH 128 /* pixels */
|
||||
#define LCD_HEIGHT 64 /* pixels */
|
||||
#define LCD_BPP 1 /* bits per pixel (1=mono, 16=RGB565) */
|
||||
#define LCD_REFRESH_HZ 30 /* target refresh rate */
|
||||
|
||||
#if LCD_BPP == 1
|
||||
typedef uint8_t lcd_color_t;
|
||||
#define LCD_BLACK 0x00
|
||||
#define LCD_WHITE 0x01
|
||||
#define LCD_FBSIZE (LCD_WIDTH * LCD_HEIGHT / 8) /* 1024 bytes */
|
||||
#else /* RGB565 */
|
||||
typedef uint16_t lcd_color_t;
|
||||
#define LCD_BLACK 0x0000
|
||||
#define LCD_WHITE 0xFFFF
|
||||
#define LCD_FBSIZE (LCD_WIDTH * LCD_HEIGHT * 2) /* 16384 bytes */
|
||||
#endif
|
||||
|
||||
/* === Public API === */
|
||||
|
||||
/**
|
||||
* Initialize LCD display and framebuffer.
|
||||
* Called once at startup.
|
||||
*/
|
||||
void face_lcd_init(void);
|
||||
|
||||
/**
|
||||
* Clear entire framebuffer to black.
|
||||
*/
|
||||
void face_lcd_clear(void);
|
||||
|
||||
/**
|
||||
* Set a single pixel in the framebuffer.
|
||||
* (Does NOT push to display immediately.)
|
||||
*/
|
||||
void face_lcd_pixel(uint16_t x, uint16_t y, lcd_color_t color);
|
||||
|
||||
/**
|
||||
* Draw a line from (x0,y0) to (x1,y1) using Bresenham algorithm.
|
||||
*/
|
||||
void face_lcd_line(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1,
|
||||
lcd_color_t color);
|
||||
|
||||
/**
|
||||
* Draw a circle with center (cx, cy) and radius r.
|
||||
*/
|
||||
void face_lcd_circle(uint16_t cx, uint16_t cy, uint16_t r, lcd_color_t color);
|
||||
|
||||
/**
|
||||
* Fill a rectangle at (x, y) with width w and height h.
|
||||
*/
|
||||
void face_lcd_fill_rect(uint16_t x, uint16_t y, uint16_t w, uint16_t h,
|
||||
lcd_color_t color);
|
||||
|
||||
/**
|
||||
* Push framebuffer to display (async via DMA if available).
|
||||
* Returns immediately; transfer happens in background.
|
||||
*/
|
||||
void face_lcd_flush(void);
|
||||
|
||||
/**
|
||||
* Check if a display transfer is currently in progress.
|
||||
* Returns true if DMA/SPI is busy, false if idle.
|
||||
*/
|
||||
bool face_lcd_is_busy(void);
|
||||
|
||||
/**
|
||||
* Called by systick ISR (~30Hz) to drive vsync and maintain refresh.
|
||||
* Updates frame counter and triggers flush if a new frame is needed.
|
||||
*/
|
||||
void face_lcd_tick(void);
|
||||
|
||||
/**
|
||||
* Get framebuffer address (for direct access if needed).
|
||||
*/
|
||||
uint8_t *face_lcd_get_fb(void);
|
||||
|
||||
#endif // FACE_LCD_H
|
||||
@ -1,76 +0,0 @@
|
||||
/*
|
||||
* face_uart.h — UART Command Interface for Face Animations
|
||||
*
|
||||
* Receives emotion commands from Jetson Orin via UART (USART3 by default).
|
||||
* Parses simple text commands and updates face animation state.
|
||||
*
|
||||
* PROTOCOL:
|
||||
* Text-based commands (newline-terminated):
|
||||
* HAPPY — Set emotion to happy
|
||||
* SAD — Set emotion to sad
|
||||
* CURIOUS — Set emotion to curious
|
||||
* ANGRY — Set emotion to angry
|
||||
* SLEEP — Set emotion to sleeping
|
||||
* NEUTRAL — Set emotion to neutral
|
||||
* BLINK — Trigger immediate blink
|
||||
* STATUS — Echo current emotion + animation state
|
||||
*
|
||||
* Example:
|
||||
* > HAPPY\n
|
||||
* < OK: HAPPY\n
|
||||
*
|
||||
* INTERFACE:
|
||||
* - UART3 (PB10=TX, PB11=RX) at 115200 baud
|
||||
* - RX ISR pushes bytes into ring buffer
|
||||
* - face_uart_process() checks for complete commands (polling)
|
||||
* - Case-insensitive command parsing
|
||||
* - Echoes command results to TX for debugging
|
||||
*
|
||||
* API:
|
||||
* - face_uart_init() — Configure UART3 @ 115200
|
||||
* - face_uart_process() — Parse and execute commands (call from main loop)
|
||||
* - face_uart_rx_isr() — Called by UART3 RX interrupt
|
||||
* - face_uart_send() — Send response string (used internally)
|
||||
*/
|
||||
|
||||
#ifndef FACE_UART_H
|
||||
#define FACE_UART_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* === Configuration === */
|
||||
#define FACE_UART_INSTANCE USART3 /* USART3 (PB10=TX, PB11=RX) */
|
||||
#define FACE_UART_BAUD 115200 /* 115200 baud */
|
||||
#define FACE_UART_RX_BUF_SZ 128 /* RX ring buffer size */
|
||||
|
||||
/* === Public API === */
|
||||
|
||||
/**
|
||||
* Initialize UART for face commands.
|
||||
* Configures USART3 @ 115200, enables RX interrupt.
|
||||
*/
|
||||
void face_uart_init(void);
|
||||
|
||||
/**
|
||||
* Process any pending RX data and execute commands.
|
||||
* Should be called periodically from main loop (or low-priority task).
|
||||
* Returns immediately if no complete command available.
|
||||
*/
|
||||
void face_uart_process(void);
|
||||
|
||||
/**
|
||||
* UART3 RX interrupt handler.
|
||||
* Called by HAL when a byte is received.
|
||||
* Pushes byte into ring buffer.
|
||||
*/
|
||||
void face_uart_rx_isr(uint8_t byte);
|
||||
|
||||
/**
|
||||
* Send a response string to UART3 TX.
|
||||
* Used for echoing status/ack messages.
|
||||
* Non-blocking (pushes to TX queue).
|
||||
*/
|
||||
void face_uart_send(const char *str);
|
||||
|
||||
#endif // FACE_UART_H
|
||||
@ -1,162 +0,0 @@
|
||||
#ifndef FAN_H
|
||||
#define FAN_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* fan.h — Cooling fan PWM speed controller (Issue #263)
|
||||
*
|
||||
* STM32F722 driver for brushless cooling fan on PA9 using TIM1_CH2 PWM.
|
||||
* Temperature-based speed curve with smooth ramp transitions.
|
||||
*
|
||||
* Pin: PA9 (TIM1_CH2, alternate function AF1)
|
||||
* PWM Frequency: 25 kHz (suitable for brushless DC fan)
|
||||
* Speed Range: 0-100% duty cycle
|
||||
*
|
||||
* Temperature Curve:
|
||||
* - Below 40°C: Fan off (0%)
|
||||
* - 40-50°C: Linear ramp from 0% to 30%
|
||||
* - 50-70°C: Linear ramp from 30% to 100%
|
||||
* - Above 70°C: Fan at maximum (100%)
|
||||
*/
|
||||
|
||||
/* Fan speed state */
|
||||
typedef enum {
|
||||
FAN_OFF, /* Motor disabled (0% duty) */
|
||||
FAN_LOW, /* Low speed (5-30%) */
|
||||
FAN_MEDIUM, /* Medium speed (31-60%) */
|
||||
FAN_HIGH, /* High speed (61-99%) */
|
||||
FAN_FULL /* Maximum speed (100%) */
|
||||
} FanState;
|
||||
|
||||
/*
|
||||
* fan_init()
|
||||
*
|
||||
* Initialize fan controller:
|
||||
* - PA9 as TIM1_CH2 PWM output
|
||||
* - TIM1 configured for 25 kHz frequency
|
||||
* - PWM duty cycle control (0-100%)
|
||||
* - Ramp rate limiter for smooth transitions
|
||||
*/
|
||||
void fan_init(void);
|
||||
|
||||
/*
|
||||
* fan_set_speed(percentage)
|
||||
*
|
||||
* Set fan speed directly (bypasses temperature control).
|
||||
* Used for manual testing or emergency cooling.
|
||||
*
|
||||
* Arguments:
|
||||
* - percentage: 0-100% duty cycle
|
||||
*
|
||||
* Returns: true if set successfully, false if invalid value
|
||||
*/
|
||||
bool fan_set_speed(uint8_t percentage);
|
||||
|
||||
/*
|
||||
* fan_get_speed()
|
||||
*
|
||||
* Get current fan speed setting.
|
||||
*
|
||||
* Returns: Current speed 0-100%
|
||||
*/
|
||||
uint8_t fan_get_speed(void);
|
||||
|
||||
/*
|
||||
* fan_set_target_speed(percentage)
|
||||
*
|
||||
* Set target speed with smooth ramping.
|
||||
* Speed transitions over time according to ramp rate.
|
||||
*
|
||||
* Arguments:
|
||||
* - percentage: Target speed 0-100%
|
||||
*
|
||||
* Returns: true if set successfully
|
||||
*/
|
||||
bool fan_set_target_speed(uint8_t percentage);
|
||||
|
||||
/*
|
||||
* fan_update_temperature(temp_celsius)
|
||||
*
|
||||
* Update temperature reading and apply speed curve.
|
||||
* Calculates target speed based on temperature curve.
|
||||
* Speed transition is smoothed via ramp limiter.
|
||||
*
|
||||
* Temperature Curve:
|
||||
* - temp < 40°C: 0% (off)
|
||||
* - 40°C ≤ temp < 50°C: 0% + (temp - 40) * 3% per °C = linear to 30%
|
||||
* - 50°C ≤ temp < 70°C: 30% + (temp - 50) * 3.5% per °C = linear to 100%
|
||||
* - temp ≥ 70°C: 100% (full)
|
||||
*
|
||||
* Arguments:
|
||||
* - temp_celsius: Temperature in degrees Celsius (int16_t for negative values)
|
||||
*/
|
||||
void fan_update_temperature(int16_t temp_celsius);
|
||||
|
||||
/*
|
||||
* fan_get_temperature()
|
||||
*
|
||||
* Get last recorded temperature.
|
||||
*
|
||||
* Returns: Temperature in °C (or 0 if not yet set)
|
||||
*/
|
||||
int16_t fan_get_temperature(void);
|
||||
|
||||
/*
|
||||
* fan_get_state()
|
||||
*
|
||||
* Get current fan operational state.
|
||||
*
|
||||
* Returns: FAN_OFF, FAN_LOW, FAN_MEDIUM, FAN_HIGH, or FAN_FULL
|
||||
*/
|
||||
FanState fan_get_state(void);
|
||||
|
||||
/*
|
||||
* fan_set_ramp_rate(percentage_per_ms)
|
||||
*
|
||||
* Configure speed ramp rate for smooth transitions.
|
||||
* Default: 5% per 100ms = 0.05% per ms.
|
||||
* Higher values = faster transitions.
|
||||
*
|
||||
* Arguments:
|
||||
* - percentage_per_ms: Speed change per millisecond (e.g., 1 = 1% per ms)
|
||||
*
|
||||
* Typical ranges:
|
||||
* - 0.01 = very slow (100% change in 10 seconds)
|
||||
* - 0.05 = slow (100% change in 2 seconds)
|
||||
* - 0.1 = medium (100% change in 1 second)
|
||||
* - 1.0 = fast (100% change in 100ms)
|
||||
*/
|
||||
void fan_set_ramp_rate(float percentage_per_ms);
|
||||
|
||||
/*
|
||||
* fan_is_ramping()
|
||||
*
|
||||
* Check if speed is currently transitioning.
|
||||
*
|
||||
* Returns: true if speed is ramping toward target, false if at target
|
||||
*/
|
||||
bool fan_is_ramping(void);
|
||||
|
||||
/*
|
||||
* fan_tick(now_ms)
|
||||
*
|
||||
* Update function called periodically (recommended: every 10-100ms).
|
||||
* Processes speed ramp transitions.
|
||||
* Must be called regularly for smooth ramping operation.
|
||||
*
|
||||
* Arguments:
|
||||
* - now_ms: current time in milliseconds (from HAL_GetTick() or similar)
|
||||
*/
|
||||
void fan_tick(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* fan_disable()
|
||||
*
|
||||
* Disable fan immediately (set to 0% duty).
|
||||
* Useful for shutdown or emergency stop.
|
||||
*/
|
||||
void fan_disable(void);
|
||||
|
||||
#endif /* FAN_H */
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user