Compare commits
1 Commits
main
...
sl-mechani
| Author | SHA1 | Date | |
|---|---|---|---|
| a2c554c232 |
@ -1,162 +0,0 @@
|
||||
# .gitea/workflows/ota-release.yml
|
||||
# Gitea Actions — ESP32 OTA firmware build & release (bd-9kod)
|
||||
#
|
||||
# Triggers on signed release tags:
|
||||
# esp32-balance/vX.Y.Z → builds esp32s3/balance/ (ESP32-S3 Balance board)
|
||||
# esp32-io/vX.Y.Z → builds esp32s3-io/ (ESP32-S3 IO board)
|
||||
#
|
||||
# Uses the official espressif/idf Docker image for reproducible builds.
|
||||
# Attaches <app>_<version>.bin + <app>_<version>.sha256 to the Gitea release.
|
||||
# The ESP32 Balance OTA system fetches the .bin from the release asset URL.
|
||||
|
||||
name: OTA release — build & attach firmware
|
||||
|
||||
on:
|
||||
push:
|
||||
tags:
|
||||
- "esp32-balance/v*"
|
||||
- "esp32-io/v*"
|
||||
|
||||
permissions:
|
||||
contents: write
|
||||
|
||||
jobs:
|
||||
build-and-release:
|
||||
name: Build ${{ github.ref_name }}
|
||||
runs-on: ubuntu-latest
|
||||
container:
|
||||
image: espressif/idf:v5.2.2
|
||||
options: --user root
|
||||
|
||||
steps:
|
||||
# ── 1. Checkout ───────────────────────────────────────────────────────────
|
||||
- name: Checkout
|
||||
uses: actions/checkout@v4
|
||||
|
||||
# ── 2. Resolve build target from tag ─────────────────────────────────────
|
||||
# Tag format: esp32-balance/v1.2.3 or esp32-io/v1.2.3
|
||||
- name: Resolve project from tag
|
||||
id: proj
|
||||
shell: bash
|
||||
run: |
|
||||
TAG="${GITHUB_REF_NAME}"
|
||||
case "$TAG" in
|
||||
esp32-balance/*)
|
||||
DIR="esp32s3/balance"
|
||||
APP="esp32s3_balance"
|
||||
;;
|
||||
esp32-io/*)
|
||||
DIR="esp32s3-io"
|
||||
APP="esp32s3_io"
|
||||
;;
|
||||
*)
|
||||
echo "::error::Unrecognised tag prefix: ${TAG}"
|
||||
exit 1
|
||||
;;
|
||||
esac
|
||||
VERSION="${TAG#*/}"
|
||||
echo "dir=${DIR}" >> "$GITHUB_OUTPUT"
|
||||
echo "app=${APP}" >> "$GITHUB_OUTPUT"
|
||||
echo "version=${VERSION}" >> "$GITHUB_OUTPUT"
|
||||
echo "tag=${TAG}" >> "$GITHUB_OUTPUT"
|
||||
echo "Build: ${APP} ${VERSION} from ${DIR}"
|
||||
|
||||
# ── 3. Build with ESP-IDF ─────────────────────────────────────────────────
|
||||
- name: Build firmware (idf.py build)
|
||||
shell: bash
|
||||
run: |
|
||||
. "${IDF_PATH}/export.sh"
|
||||
cd "${{ steps.proj.outputs.dir }}"
|
||||
idf.py build
|
||||
|
||||
# ── 4. Collect binary & generate checksum ────────────────────────────────
|
||||
- name: Collect artifacts
|
||||
id: art
|
||||
shell: bash
|
||||
run: |
|
||||
APP="${{ steps.proj.outputs.app }}"
|
||||
VER="${{ steps.proj.outputs.version }}"
|
||||
BIN_SRC="${{ steps.proj.outputs.dir }}/build/${APP}.bin"
|
||||
BIN_OUT="${APP}_${VER}.bin"
|
||||
SHA_OUT="${APP}_${VER}.sha256"
|
||||
|
||||
cp "$BIN_SRC" "$BIN_OUT"
|
||||
sha256sum "$BIN_OUT" > "$SHA_OUT"
|
||||
|
||||
echo "bin=${BIN_OUT}" >> "$GITHUB_OUTPUT"
|
||||
echo "sha=${SHA_OUT}" >> "$GITHUB_OUTPUT"
|
||||
|
||||
echo "Binary: ${BIN_OUT} ($(wc -c < "$BIN_OUT") bytes)"
|
||||
echo "Checksum: $(cat "$SHA_OUT")"
|
||||
|
||||
# ── 5. Archive artifacts in CI workspace ─────────────────────────────────
|
||||
- name: Upload build artifacts
|
||||
uses: actions/upload-artifact@v4
|
||||
with:
|
||||
name: firmware-${{ steps.proj.outputs.app }}-${{ steps.proj.outputs.version }}
|
||||
path: |
|
||||
${{ steps.art.outputs.bin }}
|
||||
${{ steps.art.outputs.sha }}
|
||||
|
||||
# ── 6. Create Gitea release (if needed) & upload assets ──────────────────
|
||||
# Uses GITHUB_TOKEN (auto-provided, contents:write from permissions block).
|
||||
# URL-encodes the tag to handle the slash in esp32-balance/vX.Y.Z.
|
||||
- name: Publish assets to Gitea release
|
||||
shell: bash
|
||||
env:
|
||||
GITEA_URL: https://gitea.vayrette.com
|
||||
TOKEN: ${{ secrets.GITHUB_TOKEN }}
|
||||
REPO: ${{ github.repository }}
|
||||
TAG: ${{ steps.proj.outputs.tag }}
|
||||
BIN: ${{ steps.art.outputs.bin }}
|
||||
SHA: ${{ steps.art.outputs.sha }}
|
||||
run: |
|
||||
API="${GITEA_URL}/api/v1/repos/${REPO}"
|
||||
|
||||
# URL-encode the tag (slash in esp32-balance/vX.Y.Z must be escaped)
|
||||
TAG_ENC=$(python3 -c "
|
||||
import urllib.parse, sys
|
||||
print(urllib.parse.quote(sys.argv[1], safe=''))
|
||||
" "$TAG")
|
||||
|
||||
# Try to fetch an existing release for this tag
|
||||
RELEASE=$(curl -sf \
|
||||
-H "Authorization: token ${TOKEN}" \
|
||||
"${API}/releases/tags/${TAG_ENC}") || true
|
||||
|
||||
# If no release yet, create it
|
||||
if [ -z "$RELEASE" ]; then
|
||||
echo "Creating release for tag: ${TAG}"
|
||||
RELEASE=$(curl -sf \
|
||||
-X POST \
|
||||
-H "Authorization: token ${TOKEN}" \
|
||||
-H "Content-Type: application/json" \
|
||||
-d "$(python3 -c "
|
||||
import json, sys
|
||||
print(json.dumps({
|
||||
'tag_name': sys.argv[1],
|
||||
'name': sys.argv[1],
|
||||
'draft': False,
|
||||
'prerelease': False,
|
||||
}))
|
||||
" "$TAG")" \
|
||||
"${API}/releases")
|
||||
fi
|
||||
|
||||
RELEASE_ID=$(echo "$RELEASE" | python3 -c "
|
||||
import sys, json; print(json.load(sys.stdin)['id'])
|
||||
")
|
||||
echo "Release ID: ${RELEASE_ID}"
|
||||
|
||||
# Upload binary and checksum
|
||||
for FILE in "$BIN" "$SHA"; do
|
||||
FNAME=$(basename "$FILE")
|
||||
echo "Uploading: ${FNAME}"
|
||||
curl -sf \
|
||||
-X POST \
|
||||
-H "Authorization: token ${TOKEN}" \
|
||||
-F "attachment=@${FILE}" \
|
||||
"${API}/releases/${RELEASE_ID}/assets?name=${FNAME}"
|
||||
done
|
||||
|
||||
echo "Published: ${BIN} + ${SHA} → release ${TAG}"
|
||||
@ -7,11 +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)
|
||||
- Sent via inter-board UART to the ESP32-S3 BALANCE firmware
|
||||
- Robot arms after ARMING_HOLD_MS (~500ms) safety hold period
|
||||
- Works even when RC is not connected or not armed
|
||||
|
||||
@ -46,11 +42,11 @@ 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)
|
||||
### Inter-board UART Protocol
|
||||
Communication uses 460800 baud UART with binary framing:
|
||||
`[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
|
||||
|
||||
### From Jetson to ESP32-S3 BALANCE (inter-board UART)
|
||||
```
|
||||
A — Request arm (triggers safety hold, then motors enable)
|
||||
D — Request disarm (immediate motor stop)
|
||||
@ -60,11 +56,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)
|
||||
### From ESP32-S3 BALANCE to Jetson (inter-board UART)
|
||||
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)
|
||||
@ -145,4 +137,4 @@ When RC is disconnected:
|
||||
|
||||
## References
|
||||
- Issue #512: Remove ELRS arm requirement
|
||||
- Files: `/src/main.c` (arming logic), `/lib/USB_CDC/src/usbd_cdc_if.c` (CDC commands)
|
||||
- Files: `esp32/balance/src/main.cpp` (arming logic), inter-board UART protocol (460800 baud, `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`)
|
||||
|
||||
31
CLAUDE.md
31
CLAUDE.md
@ -1,36 +1,17 @@
|
||||
# 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)
|
||||
SAUL-TEE 4-wheel wagon robot: ESP32-S3 BALANCE (PID/CAN), ESP32-S3 IO (RC/sensors), Jetson Orin Nano Super (ROS2/SLAM).
|
||||
|
||||
## 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-firmware** | Embedded Firmware Lead | ESP32-S3 firmware (Arduino/IDF), PlatformIO, CAN bus, inter-board UART protocol |
|
||||
| **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)
|
||||
| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 |
|
||||
|
||||
## Status
|
||||
USB Serial (CH343) TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).
|
||||
USB CDC TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).
|
||||
|
||||
## Repo Structure
|
||||
- `projects/saltybot/SALTYLAB.md` — Design doc
|
||||
@ -48,11 +29,11 @@ USB Serial (CH343) TX bug resolved (PR #10 — DCache MPU non-cacheable region +
|
||||
| `saltyrover-dev` | Integration — rover variant |
|
||||
| `saltytank` | Stable — tracked tank variant |
|
||||
| `saltytank-dev` | Integration — tank variant |
|
||||
| `main` | Shared code only (IMU drivers, USB Serial (CH343), balance core, safety) |
|
||||
| `main` | Shared code only (IMU drivers, USB CDC, balance core, safety) |
|
||||
|
||||
### Rules
|
||||
- Agents branch FROM `<variant>-dev` and PR back TO `<variant>-dev`
|
||||
- Shared/infrastructure code (IMU drivers, USB Serial (CH343), balance core, safety) goes in `main`
|
||||
- Shared/infrastructure code (IMU drivers, USB CDC, balance core, safety) goes in `main`
|
||||
- Variant-specific code (motor topology, kinematics, config) goes in variant branches
|
||||
- Stable branches get promoted from `-dev` after review and hardware testing
|
||||
- **Current SaltyLab team** works against `saltylab-dev`
|
||||
|
||||
65
TEAM.md
65
TEAM.md
@ -1,22 +1,12 @@
|
||||
# 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`.
|
||||
SAUL-TEE 4-wheel wagon robot using ESP32-S3 BALANCE (PID/CAN master) and ESP32-S3 IO (RC/sensors), with Jetson Orin Nano Super for AI/SLAM.
|
||||
|
||||
## 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)
|
||||
- **Hardware:** Assembled — ESP32-S3 BALANCE + IO, VESCs, IMU, battery, RC all on hand
|
||||
- **Firmware:** Balance PID + VESC CAN protocol written, ESP32-S3 inter-board UART protocol active
|
||||
- **Status:** See current bead list for active issues
|
||||
|
||||
---
|
||||
|
||||
@ -24,30 +14,18 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
|
||||
|
||||
### 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)
|
||||
- ESP32-S3 firmware (Arduino / ESP-IDF framework)
|
||||
- PlatformIO toolchain
|
||||
- CAN bus protocol and VESC CAN integration
|
||||
- Inter-board UART protocol (460800 baud, binary framed)
|
||||
- Safety system design (tilt cutoff, watchdog, arming sequences)
|
||||
|
||||
**Nice-to-have:**
|
||||
- ESP32-S3 peripheral coexistence (SPI + UART + USB)
|
||||
- VESC firmware / VESC Tool experience
|
||||
- PID control loop tuning for balance robots
|
||||
- FOC motor control (hoverboard ESC protocol)
|
||||
- ELRS/CRSF RC 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)
|
||||
**Why:** Core firmware runs on ESP32-S3 BALANCE (PID/CAN master) and ESP32-S3 IO (RC/sensors). Need expertise in ESP32-S3 firmware and CAN bus integration with VESC motor controllers.
|
||||
|
||||
### 2. Control Systems / Robotics Engineer
|
||||
**Must-have:**
|
||||
@ -57,7 +35,7 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
|
||||
- Safety system design (tilt cutoff, watchdog, arming sequences)
|
||||
|
||||
**Nice-to-have:**
|
||||
- Hoverboard hub motor experience
|
||||
- VESC motor controller experience
|
||||
- ELRS/CRSF RC protocol
|
||||
- ROS2 integration
|
||||
|
||||
@ -65,7 +43,7 @@ Self-balancing two-wheeled robot using a drone ESP32-S3 BALANCE (ESP32-S3), hove
|
||||
|
||||
### 3. Perception / SLAM Engineer (Phase 2)
|
||||
**Must-have:**
|
||||
- Jetson Orin Nano Super / NVIDIA Jetson platform
|
||||
- Jetson Orin Nano Super / NVIDIA Jetson platform (JetPack 6)
|
||||
- Intel RealSense D435i depth camera
|
||||
- RPLIDAR integration
|
||||
- SLAM (ORB-SLAM3, RTAB-Map, or similar)
|
||||
@ -83,13 +61,12 @@ 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 |
|
||||
| ESC | Hoverboard ESC (EFeru FOC firmware) |
|
||||
| BALANCE MCU | ESP32-S3 BALANCE (Waveshare Touch LCD 1.28, QMI8658 IMU) |
|
||||
| IO MCU | ESP32-S3 IO (RC/sensors/LEDs board) |
|
||||
| Motors | 2x 8" pneumatic hub motors |
|
||||
| ESC Left | VESC left (CAN ID 68) |
|
||||
| ESC Right | VESC right (CAN ID 56) |
|
||||
| CAN Bridge | CANable 2.0 (Jetson USB → can0, 500 kbps) |
|
||||
| Battery | 36V pack |
|
||||
| RC | BetaFPV ELRS 2.4GHz TX + RX |
|
||||
| AI Brain | Jetson Orin Nano Super + Noctua fan |
|
||||
@ -100,4 +77,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)
|
||||
- Archived bug doc: `USB_CDC_BUG.md` (legacy STM32 era)
|
||||
|
||||
@ -14,11 +14,12 @@ motor_axle_flat = 10; // Flat-to-flat if D-shaft
|
||||
motor_body_dia = 200; // ~8 inches
|
||||
motor_bolt_circle = 0; // Axle-only mount (clamp style)
|
||||
|
||||
// --- Drone FC (30.5mm standard) ---
|
||||
fc_hole_spacing = 25.5; // GEP-F722 AIO v2 (not standard 30.5!)
|
||||
fc_hole_dia = 3.2; // M3 clearance
|
||||
fc_board_size = 36; // Typical FC PCB
|
||||
fc_standoff_h = 5; // Rubber standoff height
|
||||
// --- ESP32-S3 BALANCE board (Waveshare Touch LCD 1.28) ---
|
||||
// ⚠ Confirm hole positions before printing — verify in esp32/balance/src/config.h
|
||||
mcu_bal_board_w = 40.0; // Waveshare Touch LCD 1.28 PCB approx width (TBD caliper)
|
||||
mcu_bal_board_d = 40.0; // Waveshare Touch LCD 1.28 PCB approx depth (TBD caliper)
|
||||
mcu_bal_hole_dia = 3.2; // M3 clearance
|
||||
mcu_standoff_h = 5; // Standoff height
|
||||
|
||||
// --- Jetson Orin Nano Super ---
|
||||
jetson_w = 100;
|
||||
|
||||
@ -10,7 +10,7 @@
|
||||
├─ bumper_bracket(front=+1) ──────────────────────┐
|
||||
│ │
|
||||
┌───────┴──────────── Main Deck (640×220×6mm Al) ─────────┴───────┐
|
||||
│ ← Jetson mount plate (rear/+X) FC mount (front/−X) → │
|
||||
│ ← Jetson mount plate (rear/+X) MCU mount (front/−X) → │
|
||||
│ [Battery tray hanging below centre] │
|
||||
└───┬──────────────────────────────────────────────────────────┬───┘
|
||||
│ │
|
||||
@ -56,24 +56,21 @@
|
||||
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)
|
||||
### 7 MCU mount (ESP32-S3 BALANCE + ESP32-S3 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.
|
||||
> ⚠ Board hole patterns TBD — measure Waveshare Touch LCD 1.28 PCB with calipers and
|
||||
> update `FC_PITCH` / `FC_MOUNT_SPACING` in all scad files before machining the mount plate.
|
||||
> Reference: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`.
|
||||
|
||||
=======
|
||||
### 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.
|
||||
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.
|
||||
1. Place silicone anti-vibration grommets onto M3 nylon standoffs.
|
||||
2. Lower ESP32-S3 BALANCE board onto standoffs; secure M3×6 BHCS — snug only.
|
||||
3. Mount ESP32-S3 IO board adjacent — exact layout TBD pending board dimensions.
|
||||
4. Orient USB-C connectors toward accessible side for field programming/debug.
|
||||
|
||||
### 8 Jetson Orin Nano Super mount plate
|
||||
### 8 Jetson Nano mount plate
|
||||
1. Press or thread M3 nylon standoffs (8mm) into plate holes.
|
||||
2. Bolt plate to deck: 4× M3×10 SHCS at deck corners.
|
||||
3. Set Jetson Orin Nano Super B01 carrier onto plate standoffs; fasten M3×6 BHCS.
|
||||
3. Set Jetson Nano B01 carrier onto plate standoffs; fasten M3×6 BHCS.
|
||||
|
||||
### 9 Bumper brackets
|
||||
1. Slide 22mm EMT conduit through saddle clamp openings.
|
||||
@ -95,8 +92,8 @@
|
||||
| Wheelbase (axle C/L to C/L) | 600 mm | ±1 mm |
|
||||
| Motor fork slot width | 24 mm | +0.5 / 0 |
|
||||
| Motor fork dropout depth | 60 mm | ±0.5 mm |
|
||||
| ESP32 BALANCE hole pattern | TBD — await spec from max | ±0.2 mm |
|
||||
| ESP32 IO hole pattern | TBD — await spec from max | ±0.2 mm |
|
||||
| ESP32-S3 BALANCE hole pattern | TBD — caliper Waveshare board | ±0.2 mm |
|
||||
| ESP32-S3 IO hole pattern | TBD — caliper bare board | ±0.2 mm |
|
||||
| Jetson hole pattern | 58 × 58 mm | ±0.2 mm |
|
||||
| Battery tray inner | 185 × 72 × 52 mm | +2 / 0 mm |
|
||||
|
||||
|
||||
@ -41,11 +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)
|
||||
| 6 | MCU standoff M3×6mm nylon | 4 | Nylon | — | ESP32-S3 BALANCE / IO vibration isolation |
|
||||
| 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
|
||||
|
||||
### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B
|
||||
@ -73,8 +69,8 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
|
||||
| 9 | Motor fork bracket (L) | 1 | 8mm 6061 aluminium | **Update fork slot to Ø16.51mm before cutting** |
|
||||
| 10 | Motor fork bracket (R) | 1 | 8mm 6061 aluminium | Mirror of item 9 |
|
||||
| 11 | Battery tray | 1 | 3mm PETG FDM or 3mm aluminium fold | `chassis_frame.scad` — `battery_tray()` module |
|
||||
| 12 | FC mount plate / standoffs | 1 set | PETG or nylon FDM | Includes 4× M3 nylon standoffs, 6mm height |
|
||||
| 13 | Jetson Orin Nano Super mount plate | 1 | 4mm 5052 aluminium or 4mm PETG FDM | B01 58×58mm hole pattern |
|
||||
| 12 | MCU mount plate / standoffs | 1 set | PETG or nylon FDM | Includes 4× M3 nylon standoffs, 6mm height — hole pattern TBD |
|
||||
| 13 | Jetson Nano mount plate | 1 | 4mm 5052 aluminium or 4mm PETG FDM | B01 58×58mm hole pattern |
|
||||
| 14 | Front bumper bracket | 1 | 5mm PETG FDM | Saddle clamps for 22mm EMT conduit |
|
||||
| 15 | Rear bumper bracket | 1 | 5mm PETG FDM | Mirror of item 14 |
|
||||
|
||||
@ -92,23 +88,16 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
|
||||
|
||||
## Electronics Mounts
|
||||
|
||||
> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE (ESP32) is retired.
|
||||
> Replaced by **ESP32 BALANCE** + **ESP32 IO**. Board dimensions and hole patterns TBD — await spec from max.
|
||||
> ⚠ MCU board dimensions TBD — caliper Waveshare Touch LCD 1.28 and bare ESP32-S3 IO board
|
||||
> before machining mount holes. See `docs/SAUL-TEE-SYSTEM-REFERENCE.md`.
|
||||
|
||||
| # | 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 |
|
||||
| 13 | ESP32-S3 BALANCE (Waveshare Touch LCD 1.28) | 1 | ~40×40mm PCB, hole pattern TBD | PID loop + CAN; USB-C toward accessible side |
|
||||
| 13b | ESP32-S3 IO (bare board) | 1 | TBD PCB size, hole pattern TBD | RC / motor / sensor 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)
|
||||
| 16 | Jetson Orin Nano Super | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
|
||||
| 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs |
|
||||
|
||||
---
|
||||
@ -159,8 +148,8 @@ Slide entire carousel up/down the stem with M6 collar bolts loosened. Tighten at
|
||||
| 26 | M6×60 SHCS | 4 | ISO 4762, SS | Collar clamping bolts |
|
||||
| 27 | M6 hex nut | 4 | ISO 4032, SS | Captured in collar pockets |
|
||||
| 28 | M6×12 set screw | 2 | ISO 4026, SS cup-point | Stem height lock (1 per collar half) |
|
||||
| 29 | M3×10 SHCS | 12 | ISO 4762, SS | ESP32 mount + miscellaneous |
|
||||
| 30 | M3×6 BHCS | 4 | ISO 4762, SS | ESP32 board bolts (qty TBD pending board spec) |
|
||||
| 29 | M3×10 SHCS | 12 | ISO 4762, SS | FC mount + miscellaneous |
|
||||
| 30 | M3×6 BHCS | 4 | ISO 4762, SS | FC board bolts |
|
||||
| 31 | Axle lock nut (match axle tip thread) | 4 | Flanged, confirm thread | 2 per motor |
|
||||
| 32 | Flat washer M5 | 32 | SS | |
|
||||
| 33 | Flat washer M4 | 32 | SS | |
|
||||
|
||||
@ -8,9 +8,9 @@
|
||||
// Requirements:
|
||||
// - 600mm wheelbase
|
||||
// - 2x hoverboard hub motors (170mm OD)
|
||||
// - ESP32-S3 ESP32-S3 BALANCE FC mount (30.5x30.5mm pattern)
|
||||
// - ESP32-S3 BALANCE + IO board mounts (TBD hole pattern — see SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
// - Battery tray (24V 4Ah — ~180x70x50mm pack)
|
||||
// - Jetson Orin Nano Super B01 mount plate (100x80mm, M3 holes)
|
||||
// - Jetson Nano B01 mount plate (100x80mm, M3 holes)
|
||||
// - Front/rear bumper brackets
|
||||
// =============================================================================
|
||||
|
||||
@ -37,8 +37,9 @@ MOTOR_FORK_H = 80; // mm, total height of motor fork bracket
|
||||
MOTOR_FORK_T = 8; // mm, fork plate thickness
|
||||
AXLE_HEIGHT = 310; // mm, axle CL above ground (motor radius + clearance)
|
||||
|
||||
// ── FC mount (ESP32-S3 BALANCE — 30.5 × 30.5 mm M3 pattern) ──────────────────────
|
||||
FC_MOUNT_SPACING = 30.5; // mm, hole pattern pitch
|
||||
// ── MCU mount (ESP32-S3 BALANCE / IO boards) ─────────────────────────────────
|
||||
// ⚠ Hole pattern TBD — update before machining. See docs/SAUL-TEE-SYSTEM-REFERENCE.md
|
||||
FC_MOUNT_SPACING = 0; // TBD — set to actual ESP32-S3 board hole spacing
|
||||
FC_MOUNT_HOLE_D = 3.2; // mm, M3 clearance
|
||||
FC_STANDOFF_H = 6; // mm, standoff height
|
||||
FC_PAD_T = 3; // mm, mounting pad thickness
|
||||
@ -52,7 +53,7 @@ BATT_FLOOR = 4; // mm, tray floor thickness
|
||||
BATT_STRAP_W = 20; // mm, Velcro strap slot width
|
||||
BATT_STRAP_T = 2; // mm, strap slot depth
|
||||
|
||||
// ── Jetson Orin Nano Super B01 mount plate ──────────────────────────────────────────────
|
||||
// ── Jetson Nano B01 mount plate ──────────────────────────────────────────────
|
||||
// B01 carrier board hole pattern: 58 x 58 mm M3 (inner) + corner pass-throughs
|
||||
JETSON_HOLE_PITCH = 58; // mm, M3 mounting hole pattern
|
||||
JETSON_HOLE_D = 3.2; // mm
|
||||
@ -210,7 +211,7 @@ module battery_tray() {
|
||||
|
||||
// ─── FC mount holes helper ────────────────────────────────────────────────────
|
||||
module fc_mount_holes(z_offset=0, depth=10) {
|
||||
// ESP32-S3 BALANCE: 30.5×30.5 mm M3 pattern, centred at origin
|
||||
// ESP32-S3 board M3 pattern, centred at origin — spacing TBD, update FC_MOUNT_SPACING
|
||||
for (x = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
|
||||
for (y = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
|
||||
translate([x, y, z_offset])
|
||||
@ -247,7 +248,7 @@ module fc_mount_plate() {
|
||||
}
|
||||
}
|
||||
|
||||
// ─── Jetson Orin Nano Super B01 mount plate ─────────────────────────────────────────────
|
||||
// ─── Jetson Nano B01 mount plate ─────────────────────────────────────────────
|
||||
// Positioned rear of deck, elevated on standoffs
|
||||
module jetson_mount_plate() {
|
||||
jet_x = 60; // offset toward rear
|
||||
|
||||
@ -104,11 +104,8 @@ 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)
|
||||
| ESP32-S3 BALANCE | Passive; mounted on standoffs | 105 °C Tj | <70 °C ambient OK |
|
||||
| ESP32-S3 IO | Passive; mounted on standoffs | 105 °C Tj | <70 °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 | — |
|
||||
|
||||
|
||||
@ -65,10 +65,11 @@ CLAMP_ALIGN_D = 4.1; // Ø4 pin
|
||||
// D-cut bore clearance
|
||||
DCUT_CL = 0.3;
|
||||
|
||||
// FC mount — ESP32-S3 BALANCE 30.5 × 30.5 mm M3
|
||||
FC_PITCH = 30.5;
|
||||
// MCU mount — ESP32-S3 BALANCE board (Waveshare Touch LCD 1.28)
|
||||
// ⚠ FC_PITCH TBD — update before machining. See docs/SAUL-TEE-SYSTEM-REFERENCE.md
|
||||
FC_PITCH = 0.0; // TBD — ESP32-S3 board hole spacing not yet confirmed
|
||||
FC_HOLE_D = 3.2;
|
||||
// FC is offset toward front of plate (away from stem)
|
||||
// Board is offset toward front of plate (away from stem)
|
||||
FC_X_OFFSET = -40.0; // mm from plate centre (negative = front/motor side)
|
||||
|
||||
// =============================================================================
|
||||
@ -202,7 +203,7 @@ module base_plate() {
|
||||
translate([STEM_FLANGE_BC/2, 0, -1])
|
||||
cylinder(d=M5, h=PLATE_THICK + 2);
|
||||
|
||||
// ── FC mount (ESP32-S3 BALANCE 30.5 × 30.5 M3) ────────────────────────
|
||||
// ── MCU mount (ESP32-S3 BALANCE — hole spacing TBD) ──────────────
|
||||
for (x = [FC_X_OFFSET - FC_PITCH/2, FC_X_OFFSET + FC_PITCH/2])
|
||||
for (y = [-FC_PITCH/2, FC_PITCH/2])
|
||||
translate([x, y, -1])
|
||||
|
||||
@ -10,8 +10,8 @@
|
||||
// • RPLIDAR A1M8 tower — integrated on lid top
|
||||
// • Ventilation slots — all 4 walls + lid
|
||||
//
|
||||
// Shared mounting patterns (swappable with SaltyLab):
|
||||
// FC : 30.5 × 30.5 mm M3 (ESP32-S3 BALANCE / Pixhawk)
|
||||
// Shared mounting patterns:
|
||||
// MCU : TBD mm M3 (ESP32-S3 BALANCE / IO — see docs/SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
// Jetson: 58 × 49 mm M3 (Orin NX / Nano Devkit carrier)
|
||||
//
|
||||
// Coordinate: bay centred at origin; Z=0 = deck top face.
|
||||
|
||||
@ -16,8 +16,8 @@
|
||||
// • D435i front bracket arm
|
||||
// • Weight target: <2 kg frame (excl. motors/electronics)
|
||||
//
|
||||
// Shared SaltyLab patterns (swappable electronics):
|
||||
// FC : 30.5 × 30.5 mm M3 (ESP32-S3 BALANCE / Pixhawk)
|
||||
// Shared patterns (swappable electronics):
|
||||
// MCU : TBD mm M3 (ESP32-S3 BALANCE / IO — see docs/SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
// Jetson: 58 × 49 mm M3 (Orin NX / Nano carrier board)
|
||||
// Stem : Ø25 mm bore (sensor head unchanged)
|
||||
//
|
||||
@ -87,9 +87,9 @@ STEM_COLLAR_OD = 50.0;
|
||||
STEM_COLLAR_H = 20.0; // raised boss height above deck top
|
||||
STEM_FLANGE_BC = 40.0; // 4× M4 bolt circle for stem adapter
|
||||
|
||||
// ── FC mount — ESP32-S3 BALANCE / Pixhawk (30.5 × 30.5 mm M3) ────────────────────
|
||||
// Shared with SaltyLab — swappable electronics
|
||||
FC_PITCH = 30.5;
|
||||
// ── MCU mount — ESP32-S3 BALANCE / IO boards ─────────────────────────────────
|
||||
// ⚠ FC_PITCH TBD — update before machining. See docs/SAUL-TEE-SYSTEM-REFERENCE.md
|
||||
FC_PITCH = 0.0; // TBD — ESP32-S3 board hole spacing
|
||||
FC_HOLE_D = 3.2;
|
||||
FC_POS_Y = ROVER_L/2 - 65.0; // near front edge
|
||||
|
||||
|
||||
383
docs/AGENTS.md
383
docs/AGENTS.md
@ -1,323 +1,184 @@
|
||||
# AGENTS.md — SaltyLab Agent Onboarding
|
||||
# AGENTS.md — SAUL-TEE Agent Onboarding
|
||||
|
||||
You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read this entire file before touching anything.
|
||||
You're working on **SAUL-TEE**, a 4-wheel wagon robot. Read this entire file before touching anything.
|
||||
|
||||
## ⚠️ ARCHITECTURE — SAUL-TEE (finalised 2026-04-04)
|
||||
**Full system reference:** `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
|
||||
|
||||
<<<<<<< HEAD
|
||||
Full hardware spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` — **read it before writing firmware.**
|
||||
## Project Overview
|
||||
|
||||
| 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 |
|
||||
A 4-wheel wagon robot (870×510×550 mm, 23 kg) with three compute layers:
|
||||
|
||||
1. **ESP32-S3 BALANCE** (Waveshare Touch LCD 1.28) — QMI8658 IMU, PID drive / stability loop, CAN bus master for VESCs. Safety-critical layer. Firmware in `esp32/balance/`.
|
||||
2. **ESP32-S3 IO** (bare board) — RC input (TBS Crossfire + ELRS failover), BTS7960 motor drivers, I2C sensors (NFC/baro/ToF), WS2812 LEDs, accessories. Firmware in `esp32/io/`.
|
||||
3. **Jetson Orin Nano Super** — AI brain: ROS2, SLAM, Nav2, perception. Sends high-level velocity commands over CAN (0x300–0x303). Receives telemetry on CAN (0x400–0x401).
|
||||
|
||||
```
|
||||
Jetson Orin ──CANable2──► CAN 500kbps ◄───────────────────────┐
|
||||
Orin (CAN 0x300-0x303) ←→ TBS Crossfire / ELRS (CRSF @ 420000)
|
||||
│ │
|
||||
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.
|
||||
|
||||
▼ CAN 500kbps │ inter-board UART 460800
|
||||
ESP32-S3 BALANCE ───────────────── ESP32-S3 IO
|
||||
QMI8658 IMU BTS7960 × 4 motor drivers
|
||||
PID loop NFC / baro / ToF (I2C)
|
||||
SN65HVD230 CAN WS2812 LEDs
|
||||
│ Horn / headlight / fan / buzzer
|
||||
▼ CAN 500kbps
|
||||
VESC left (ID 68) VESC right (ID 56)
|
||||
│ │
|
||||
Hub motors FL/RL Hub motors FR/RR
|
||||
```
|
||||
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)
|
||||
```
|
||||
|
||||
Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
|
||||
Legacy `src/` STM32 HAL code is **archived — do not extend.**
|
||||
|
||||
## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT
|
||||
|
||||
This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, and launch the frame. Every firmware change must preserve these invariants:
|
||||
This is not a toy. 4× hub motors + 36V × high-current VESCs can crush fingers, break toes, and throw the 23 kg frame. Every firmware change must preserve these invariants:
|
||||
|
||||
1. **Motors NEVER spin on power-on.** Requires deliberate arming: hold button 3s while upright.
|
||||
2. **Tilt cutoff at ±25°** — motors to zero, require manual re-arm. No retry, no recovery.
|
||||
3. **Hardware watchdog (50ms)** — if firmware hangs, motors cut.
|
||||
4. **RC kill switch** — dedicated ELRS channel, checked every loop iteration. Always overrides.
|
||||
5. **Jetson UART timeout (200ms)** — if Jetson disconnects, motors cut.
|
||||
1. **Motors NEVER spin on power-on.** Requires deliberate arming: deliberate ARM command.
|
||||
2. **RC kill switch** — dedicated ELRS/Crossfire channel, checked every loop iteration. Always overrides.
|
||||
3. **CAN watchdog** — if no Orin heartbeat for 500 ms, drop to RC-only mode.
|
||||
4. **ESTOP CAN frame** — 0x303 with magic byte 0xE5 cuts all motors instantly.
|
||||
5. **Inter-board heartbeat** — if IO board misses BALANCE heartbeat for 200 ms, IO disables all BTS7960 enables.
|
||||
6. **Speed hard cap** — firmware limit, start at 10%. Increase only after proven stable.
|
||||
7. **Never test untethered** until PID is stable for 5+ minutes on a tether.
|
||||
7. **Never test without RC transmitter in hand.**
|
||||
|
||||
**If you break any of these, you are removed from the project.**
|
||||
|
||||
## 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/
|
||||
│ ├── main.c # Entry point, clock config, main loop
|
||||
│ ├── icm42688.c # QMI8658-P SPI driver (backup IMU — currently broken)
|
||||
│ ├── bmp280.c # Barometer driver (disabled)
|
||||
│ └── status.c # LED + buzzer status patterns
|
||||
├── include/
|
||||
│ ├── config.h # Pin definitions, constants
|
||||
│ ├── icm42688.h
|
||||
│ ├── mpu6000.h # MPU6000 driver header (primary IMU)
|
||||
│ ├── hoverboard.h # Hoverboard ESC UART protocol
|
||||
│ ├── crsf.h # ELRS CRSF protocol
|
||||
│ ├── bmp280.h
|
||||
│ └── status.h
|
||||
├── lib/USB_CDC/ # USB Serial (CH343) stack (serial over USB)
|
||||
│ ├── src/ # CDC implementation, USB descriptors, PCD config
|
||||
│ └── include/
|
||||
└── platformio.ini # Build config
|
||||
esp32/
|
||||
├── balance/ — ESP32-S3 BALANCE firmware (PlatformIO)
|
||||
│ ├── src/
|
||||
│ │ ├── main.cpp
|
||||
│ │ ├── config.h ← GPIO assignments — update here first
|
||||
│ │ ├── imu_qmi8658.cpp/.h
|
||||
│ │ ├── can_vesc.cpp/.h
|
||||
│ │ └── protocol.cpp/.h
|
||||
│ └── platformio.ini
|
||||
├── io/ — ESP32-S3 IO firmware (PlatformIO)
|
||||
│ ├── src/
|
||||
│ │ ├── main.cpp
|
||||
│ │ ├── config.h ← GPIO assignments — update here first
|
||||
│ │ ├── rc_crsf.cpp/.h
|
||||
│ │ ├── motor_bts7960.cpp/.h
|
||||
│ │ └── protocol.cpp/.h
|
||||
│ └── platformio.ini
|
||||
└── shared/
|
||||
└── protocol.h ← inter-board frame types — authoritative
|
||||
|
||||
cad/ # OpenSCAD parametric parts (16 files)
|
||||
├── dimensions.scad # ALL measurements live here — single source of truth
|
||||
├── assembly.scad # Full robot assembly visualization
|
||||
├── motor_mount_plate.scad
|
||||
├── battery_shelf.scad
|
||||
├── fc_mount.scad # Vibration-isolated FC mount
|
||||
├── jetson_shelf.scad
|
||||
├── esc_mount.scad
|
||||
├── sensor_tower_top.scad
|
||||
├── lidar_standoff.scad
|
||||
├── realsense_bracket.scad
|
||||
├── bumper.scad # TPU bumpers (front + rear)
|
||||
├── handle.scad
|
||||
├── kill_switch_mount.scad
|
||||
├── tether_anchor.scad
|
||||
├── led_diffuser_ring.scad
|
||||
└── esp32c3_mount.scad
|
||||
src/ — LEGACY STM32 code (ARCHIVED — do not touch)
|
||||
include/ — LEGACY STM32 headers (ARCHIVED — do not touch)
|
||||
|
||||
ui/ # Web UI (Three.js + WebSerial)
|
||||
└── index.html # 3D board visualization, real-time IMU data
|
||||
chassis/ — OpenSCAD mechanical parts
|
||||
├── ASSEMBLY.md — assembly instructions
|
||||
├── BOM.md — bill of materials
|
||||
└── *.scad — parametric parts
|
||||
|
||||
SALTYLAB.md # Master design doc — architecture, wiring, build phases
|
||||
SALTYLAB-DETAILED.md # Power budget, weight budget, detailed schematics
|
||||
PLATFORM.md # Hardware platform reference
|
||||
docs/
|
||||
├── SAUL-TEE-SYSTEM-REFERENCE.md ← MASTER REFERENCE — read this
|
||||
├── AGENTS.md — this file
|
||||
├── wiring-diagram.md — wiring reference (see SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
└── SALTYLAB.md — legacy design doc (historical)
|
||||
```
|
||||
|
||||
## Hardware Quick Reference
|
||||
|
||||
<<<<<<< HEAD
|
||||
### ESP32 BALANCE Flight Controller
|
||||
### ESP32-S3 BALANCE (Waveshare Touch LCD 1.28)
|
||||
|
||||
| Spec | Value |
|
||||
|------|-------|
|
||||
| MCU | ESP32RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
|
||||
=======
|
||||
### ESP32-S3 BALANCE Flight Controller
|
||||
| MCU | ESP32-S3, dual-core 240 MHz, 8MB flash, 8MB PSRAM |
|
||||
| USB | CH343G USB-UART bridge (UART0 / GPIO43 TX, GPIO44 RX) |
|
||||
| Display | 1.28" round GC9A01 240×240 (SPI, onboard) |
|
||||
| IMU | QMI8658 6-axis (I2C-0 SDA=GPIO6, SCL=GPIO7, INT=GPIO3) |
|
||||
| CAN | SN65HVD230 external transceiver (GPIO TBD — see `esp32/balance/src/config.h`) |
|
||||
| Inter-board UART | UART1 (GPIO TBD) ↔ ESP32-IO @ 460800 baud |
|
||||
|
||||
| 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) |
|
||||
| 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 |
|
||||
| USB | OTG FS (PA11/PA12), enumerates as /dev/cu.usbmodemSALTY0011 |
|
||||
| VID/PID | 0x0483/0x5740 |
|
||||
| LEDs | PC15 (LED1), PC14 (LED2), active low |
|
||||
| Buzzer | PB2 (inverted push-pull) |
|
||||
| Battery ADC | PC1=VBAT, PC3=CURR (ADC3) |
|
||||
| DFU | Hold yellow BOOT button + plug USB (or send 'R' over CDC) |
|
||||
### ESP32-S3 IO (bare board)
|
||||
|
||||
### UART Assignments
|
||||
| Peripheral | Interface | GPIO |
|
||||
|------------|-----------|------|
|
||||
| TBS Crossfire RX | UART0 CRSF @ 420000 | GPIO43 TX / GPIO44 RX |
|
||||
| ELRS failover RX | UART2 CRSF @ 420000 | TBD |
|
||||
| BTS7960 FL/FR/RL/RR | PWM + GPIO | TBD — see config.h |
|
||||
| I2C bus (NFC/baro/ToF) | I2C | TBD |
|
||||
| WS2812B LEDs | RMT GPIO | TBD |
|
||||
| Horn / headlight / fan / buzzer | GPIO/PWM | TBD |
|
||||
| Inter-board UART | UART1 @ 460800 | TBD |
|
||||
|
||||
| UART | Pins | Connected To | Baud |
|
||||
|------|------|-------------|------|
|
||||
| USART1 | PA9/PA10 | Jetson Orin Nano Super | 115200 |
|
||||
| USART2 | PA2/PA3 | Hoverboard ESC | 115200 |
|
||||
| USART3 | PB10/PB11 | ELRS Receiver | 420000 (CRSF) |
|
||||
| UART4 | — | Spare | — |
|
||||
| UART5 | — | Spare | — |
|
||||
> All TBD GPIO assignments are confirmed in `esp32/io/src/config.h`.
|
||||
|
||||
### Motor/ESC
|
||||
### CAN Bus
|
||||
|
||||
- 2× 8" pneumatic hub motors (36V, hoverboard type)
|
||||
- Hoverboard ESC with FOC firmware
|
||||
- UART protocol: `{0xABCD, int16 speed, int16 steer, uint16 checksum}` at 115200
|
||||
- Speed range: -1000 to +1000
|
||||
| Node | CAN ID | Notes |
|
||||
|------|--------|-------|
|
||||
| VESC left motor | 68 (0x44) | FSESC 6.7 Pro Mini Dual |
|
||||
| VESC right motor | 56 (0x38) | FSESC 6.7 Pro Mini Dual |
|
||||
| Orin → robot cmds | 0x300–0x303 | drive / arm / PID / ESTOP |
|
||||
| BALANCE → Orin telemetry | 0x400–0x401 | attitude + battery + faults |
|
||||
|
||||
### Physical Dimensions (from `cad/dimensions.scad`)
|
||||
### Physical Dimensions
|
||||
|
||||
| Part | Key Measurement |
|
||||
|------|----------------|
|
||||
| FC mounting holes | 25.5mm spacing (NOT standard 30.5mm!) |
|
||||
| FC board size | ~36mm square |
|
||||
| Hub motor body | Ø200mm (~8") |
|
||||
| Motor axle | Ø12mm, 45mm long |
|
||||
| Jetson Orin Nano Super | 100×80×29mm, M2.5 holes at 86×58mm |
|
||||
| RealSense D435i | 90×25×25mm, 1/4-20 tripod mount |
|
||||
| RPLIDAR A1 | Ø70×41mm, 4× M2.5 on Ø67mm circle |
|
||||
| Kill switch hole | Ø22mm panel mount |
|
||||
| Battery pack | ~180×80×40mm |
|
||||
| Hoverboard ESC | ~80×50×15mm |
|
||||
| 2020 extrusion | 20mm square, M5 center bore |
|
||||
| Frame width | ~350mm (axle to axle) |
|
||||
| Frame height | ~500-550mm total |
|
||||
| Target weight | <8kg (current estimate: 7.4kg) |
|
||||
| Parameter | Value |
|
||||
|-----------|-------|
|
||||
| Robot (SAUL-TEE) | 870 × 510 × 550 mm, 23 kg |
|
||||
| Hub motor axle base OD | Ø16.11 mm (caliper-verified) |
|
||||
| Hub motor axle D-cut OD | Ø15.95 mm, 13.00 mm flat chord |
|
||||
| Bearing seat collar | Ø37.8 mm |
|
||||
| Tire | 10 × 2.125" pneumatic (Ø254 mm) |
|
||||
| ESP32-S3 BALANCE PCB | ~40×40 mm (TBD — caliper before machining) |
|
||||
| Orin carrier hole pattern | 58 × 49 mm M3 |
|
||||
|
||||
### 3D Printed Parts (16 files in `cad/`)
|
||||
## Inter-Board Protocol
|
||||
|
||||
| Part | Material | Infill |
|
||||
|------|----------|--------|
|
||||
| motor_mount_plate (350×150×6mm) | PETG | 80% |
|
||||
| battery_shelf | PETG | 60% |
|
||||
| esc_mount | PETG | 40% |
|
||||
| jetson_shelf | PETG | 40% |
|
||||
| sensor_tower_top | ASA | 80% |
|
||||
| lidar_standoff (Ø80×80mm) | ASA | 40% |
|
||||
| realsense_bracket | PETG | 60% |
|
||||
| fc_mount (vibration isolated) | TPU+PETG | — |
|
||||
| bumper front + rear (350×50×30mm) | TPU | 30% |
|
||||
| handle | PETG | 80% |
|
||||
| kill_switch_mount | PETG | 80% |
|
||||
| tether_anchor | PETG | 100% |
|
||||
| led_diffuser_ring (Ø120×15mm) | Clear PETG | 30% |
|
||||
| esp32c3_mount | PETG | 40% |
|
||||
**UART @ 460800 baud, 8N1.** Frame: `[0xAA][LEN][TYPE][PAYLOAD…][CRC8]`
|
||||
|
||||
## Firmware Architecture
|
||||
CRC polynomial: CRC-8/MAXIM (poly 0x31, init 0x00, RefIn/RefOut true).
|
||||
|
||||
### Critical Lessons Learned (DON'T REPEAT THESE)
|
||||
Authoritative message type definitions: `esp32/shared/protocol.h`
|
||||
|
||||
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.
|
||||
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.
|
||||
|
||||
### DFU Reboot (Betaflight Method)
|
||||
|
||||
The firmware supports reboot-to-DFU via USB command:
|
||||
1. Send `R` byte over USB Serial (CH343)
|
||||
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
|
||||
|
||||
### Build & 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
|
||||
# ESP32-S3 BALANCE
|
||||
cd esp32/balance
|
||||
pio run -t upload # Upload via USB (CH343 serial)
|
||||
|
||||
# ESP32-S3 IO
|
||||
cd esp32/io
|
||||
pio run -t upload # Upload via USB (JTAG/CDC)
|
||||
```
|
||||
|
||||
Dev machine: mbpm4 (seb@192.168.87.40), PlatformIO project at `~/Projects/saltylab-firmware/`
|
||||
## Critical Lessons Learned
|
||||
|
||||
### Clock Configuration
|
||||
1. **`-(int)0 == 0`** — checking `if (-result)` doesn't detect a zero error result. Use explicit error codes.
|
||||
2. **NEVER auto-run untested firmware on boot** — we bricked hardware doing this. Test manually first.
|
||||
3. **One variable at a time** — never change PID gains and speed limit in the same test session.
|
||||
4. **QMI8658 data ready** — poll INT pin (GPIO3) or use interrupt; don't poll status register in a tight loop.
|
||||
5. **CAN bus termination** — 120 Ω at each physical end of the bus. Missing termination = unreliable comms.
|
||||
|
||||
```
|
||||
HSE 8MHz → PLL (M=8, N=432, P=2, Q=9) → SYSCLK 216MHz
|
||||
PLLSAI (N=384, P=8) → CLK48 48MHz (USB)
|
||||
APB1 = HCLK/4 = 54MHz
|
||||
APB2 = HCLK/2 = 108MHz
|
||||
Fallback: HSI 16MHz if HSE fails (PLL M=16)
|
||||
```
|
||||
|
||||
## Current Status & Known Issues
|
||||
|
||||
### Working
|
||||
- USB Serial (CH343) serial streaming (50Hz JSON: `{"ax":...,"ay":...,"az":...,"gx":...,"gy":...,"gz":...}`)
|
||||
- Clock config with HSE + HSI fallback
|
||||
- Reboot-to-DFU via USB 'R' command
|
||||
- LED status patterns (status.c)
|
||||
- Web UI with WebSerial + Three.js 3D visualization
|
||||
|
||||
### Broken / In Progress
|
||||
- **QMI8658-P SPI reads return all zeros** — was the original IMU target, but SPI communication completely non-functional despite correct pin config. May be dead silicon. Switched to MPU6000 as primary.
|
||||
- **MPU6000 driver** — header exists but implementation needs completion
|
||||
- **PID balance loop** — not yet implemented
|
||||
- **Hoverboard ESC UART** — protocol defined, driver not written
|
||||
- **ELRS CRSF receiver** — protocol defined, driver not written
|
||||
- **Barometer (BMP280)** — I2C init hangs, disabled
|
||||
|
||||
### TODO (Priority Order)
|
||||
1. Get MPU6000 streaming accel+gyro data
|
||||
2. Implement complementary filter (pitch angle)
|
||||
3. Write hoverboard ESC UART driver
|
||||
4. Write PID balance loop with safety checks
|
||||
5. Wire ELRS receiver, implement CRSF parser
|
||||
6. Bench test (ESC disconnected, verify PID output)
|
||||
7. First tethered balance test at 10% speed
|
||||
8. Jetson UART integration
|
||||
9. LED subsystem (ESP32-C3)
|
||||
|
||||
## Communication Protocols
|
||||
|
||||
### Jetson → FC (UART1, 50Hz)
|
||||
```c
|
||||
struct { uint8_t header=0xAA; int16_t speed; int16_t steer; uint8_t mode; uint8_t checksum; };
|
||||
// mode: 0=idle, 1=balance, 2=follow, 3=RC
|
||||
```
|
||||
|
||||
### FC → Hoverboard ESC (UART2, loop rate)
|
||||
```c
|
||||
struct { uint16_t start=0xABCD; int16_t speed; int16_t steer; uint16_t checksum; };
|
||||
// speed/steer: -1000 to +1000
|
||||
```
|
||||
|
||||
### FC → Jetson Telemetry (UART1 TX, 50Hz)
|
||||
```
|
||||
T:12.3,P:45,L:100,R:-80,S:3\n
|
||||
// T=tilt°, P=PID output, L/R=motor commands, S=state (0-3)
|
||||
```
|
||||
|
||||
### FC → USB Serial (CH343) (50Hz JSON)
|
||||
```json
|
||||
{"ax":123,"ay":-456,"az":16384,"gx":10,"gy":-5,"gz":3,"t":250,"p":0,"bt":0}
|
||||
// Raw IMU values (int16), t=temp×10, p=pressure, bt=baro temp
|
||||
```
|
||||
|
||||
## LED Subsystem (ESP32-C3)
|
||||
|
||||
ESP32-C3 eavesdrops on FC→Jetson telemetry (listen-only tap on UART1 TX). No extra FC UART needed.
|
||||
## LED States (WS2812B on ESP32-IO)
|
||||
|
||||
| State | Pattern | Color |
|
||||
|-------|---------|-------|
|
||||
| Disarmed | Slow breathe | White |
|
||||
| Arming | Fast blink | Yellow |
|
||||
| Armed idle | Solid | Green |
|
||||
| Armed | Solid | Green |
|
||||
| Turning | Sweep direction | Orange |
|
||||
| Braking | Flash rear | Red |
|
||||
| Fault | Triple flash | Red |
|
||||
| Fault / ESTOP | Triple flash | Red |
|
||||
| RC lost | Alternating flash | Red/Blue |
|
||||
|
||||
## Printing (Bambu Lab)
|
||||
|
||||
- **X1C** (192.168.87.190) — for structural PETG/ASA parts
|
||||
- **A1** (192.168.86.161) — for TPU bumpers and prototypes
|
||||
- LAN access codes and MQTT details in main workspace MEMORY.md
|
||||
- **X1C** (192.168.87.190) — structural PETG/ASA parts
|
||||
- **A1** (192.168.86.161) — TPU bumpers, prototypes
|
||||
- STL export from OpenSCAD, slice in Bambu Studio
|
||||
|
||||
## Rules for Agents
|
||||
|
||||
1. **Read SALTYLAB.md fully** before making any design decisions
|
||||
2. **Never remove safety checks** from firmware — add more if needed
|
||||
3. **All measurements go in `cad/dimensions.scad`** — single source of truth
|
||||
4. **Test firmware on bench before any motor test** — ESC disconnected, verify outputs on serial
|
||||
5. **One variable at a time** — don't change PID and speed limit in the same test
|
||||
6. **Document what you change** — update this file if you add pins, change protocols, or discover hardware quirks
|
||||
7. **Ask before wiring changes** — wrong connections can fry the FC ($50+ board)
|
||||
1. **Read `docs/SAUL-TEE-SYSTEM-REFERENCE.md`** fully before any design or firmware decision
|
||||
2. **Never remove safety checks** — add more if needed
|
||||
3. **All mechanical measurements go in `cad/dimensions.scad`** — single source of truth
|
||||
4. **Test firmware on bench first** — VESCs/BTS7960 disconnected, verify outputs on serial
|
||||
5. **GPIO assignments live in `config.h`** — change there, not scattered in source
|
||||
6. **Document hardware quirks here** — if you find a gotcha, add a "Critical Lesson Learned"
|
||||
7. **Ask before wiring changes** — wrong connections can fry ESP32-S3 boards
|
||||
|
||||
@ -1,10 +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)
|
||||
Implements expressive face animations on the ESP32-S3 BALANCE board LCD display (Waveshare Touch LCD 1.28, GC9A01 1.28" round 240×240) with 5 core emotions and smooth transitions.
|
||||
|
||||
## Features
|
||||
|
||||
@ -86,13 +82,9 @@ 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)
|
||||
- Clock: 216 MHz
|
||||
- ESP32-S3 BALANCE (Waveshare Touch LCD 1.28)
|
||||
- Receives emotion commands from Orin via CAN (0x300 mode byte) or inter-board UART
|
||||
- Clock: 240 MHz
|
||||
|
||||
## Animation Timing
|
||||
|
||||
|
||||
241
docs/SALTYLAB.md
241
docs/SALTYLAB.md
@ -1,6 +1,6 @@
|
||||
# SAUL-TEE — Self-Balancing Wagon Robot 🔬
|
||||
# SaltyLab — Self-Balancing Indoor Bot 🔬
|
||||
|
||||
Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
|
||||
Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
|
||||
|
||||
## ⚠️ SAFETY — TOP PRIORITY
|
||||
|
||||
@ -11,7 +11,7 @@ Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REF
|
||||
2. **Software tilt cutoff** — if pitch exceeds ±25° (not 30°), motors go to zero immediately. No retry, no recovery. Requires manual re-arm.
|
||||
3. **Startup arming sequence** — motors NEVER spin on power-on. Requires deliberate arming: hold button for 3 seconds while robot is upright and stable.
|
||||
4. **Watchdog timeout** — if FC firmware hangs or crashes, hardware watchdog resets to safe state (motors off) within 50ms.
|
||||
5. **Current limiting** — hoverboard ESC max current set conservatively. Start low, increase gradually.
|
||||
5. **Current limiting** — VESC max current set conservatively. Start low, increase gradually.
|
||||
6. **Tether during development** — ceiling rope/strap during ALL balance testing. No free-standing tests until PID is proven stable for 5+ minutes tethered.
|
||||
7. **Speed limiting** — firmware hard cap on max speed. Start at 10% throttle, increase in 10% increments only after stable testing.
|
||||
8. **Remote kill** — Jetson can send emergency stop via UART. If Jetson disconnects (UART timeout >200ms), FC cuts motors automatically.
|
||||
@ -31,8 +31,9 @@ Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REF
|
||||
| Part | Status |
|
||||
|------|--------|
|
||||
| 2x 8" pneumatic hub motors (36 PSI) | ✅ Have |
|
||||
| 1x hoverboard ESC (FOC firmware) | ✅ Have |
|
||||
| 1x Drone FC (ESP32-S3 + QMI8658) | ✅ Have — balance brain |
|
||||
| 2x VESC FSESC 6.7 Pro Mini Dual (left ID 68, right ID 56) | ✅ Have |
|
||||
| 1x ESP32-S3 BALANCE (Waveshare Touch LCD 1.28) | ⬜ Need — PID loop + CAN master |
|
||||
| 1x ESP32-S3 IO (bare board) | ⬜ Need — RC / motor / sensor I/O |
|
||||
| 1x Jetson Orin Nano Super + Noctua fan | ✅ Have |
|
||||
| 1x RealSense D435i | ✅ Have |
|
||||
| 1x RPLIDAR A1M8 | ✅ Have |
|
||||
@ -47,21 +48,24 @@ Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REF
|
||||
| 1x Arming button (momentary, with LED) | ⬜ Need |
|
||||
| 1x Ceiling tether strap + carabiner | ⬜ Need |
|
||||
| 1x BetaFPV ELRS 2.4GHz 1W TX module | ✅ Have — RC control + kill switch |
|
||||
| 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART |
|
||||
| 1x ELRS receiver (matching) | ✅ Have — failover RC on ESP32-IO UART2 |
|
||||
|
||||
### ESP32-S3 BALANCE Board Details — Waveshare ESP32-S3 Touch LCD 1.28
|
||||
- **MCU:** ESP32-S3RET6 (Xtensa LX7 dual-core, 240MHz, 8MB Flash, 512KB SRAM)
|
||||
- **IMU:** QMI8658 (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
|
||||
- **Display:** 1.28" round LCD (GC9A01 driver, 240x240)
|
||||
- **DFU mode:** Hold BOOT button while plugging USB
|
||||
- **Firmware:** Custom balance firmware (ESP-IDF / Arduino-ESP32)
|
||||
- **USB:** USB Serial via CH343 chip
|
||||
- **UART assignments:**
|
||||
- UART0 → USB Serial (CH343) → debug/flash
|
||||
- UART1 → Jetson Orin Nano Super
|
||||
- UART2 → Hoverboard ESC
|
||||
- UART3 → ELRS receiver
|
||||
- UART4/5 → spare
|
||||
### ESP32-S3 BALANCE (Waveshare Touch LCD 1.28)
|
||||
- **MCU:** ESP32-S3, dual-core 240 MHz, 8MB flash, 8MB PSRAM
|
||||
- **Display:** 1.28" round GC9A01 240×240 LCD (face animations)
|
||||
- **IMU:** QMI8658 6-axis (I2C-0 SDA=GPIO6, SCL=GPIO7) — onboard
|
||||
- **CAN:** SN65HVD230 external transceiver → 500 kbps CAN to VESCs
|
||||
- **USB:** CH343G bridge (UART0 GPIO43/44) — programming + debug
|
||||
- **Firmware:** `esp32/balance/` (PlatformIO, Arduino framework)
|
||||
- **Role:** PID / stability loop, VESC CAN master, inter-board UART to IO board
|
||||
|
||||
### ESP32-S3 IO (bare board)
|
||||
- **USB:** Built-in JTAG/USB-CDC — programming + debug
|
||||
- **RC:** TBS Crossfire on UART0 (GPIO43/44), ELRS failover on UART2
|
||||
- **Drive:** 4× BTS7960 H-bridge drivers for hub motors (GPIO TBD)
|
||||
- **Sensors:** NFC, barometer, ToF distance (shared I2C, GPIO TBD)
|
||||
- **Outputs:** WS2812B LEDs (RMT), horn, headlight, fan, buzzer
|
||||
- **Firmware:** `esp32/io/` (PlatformIO, Arduino framework)
|
||||
|
||||
## Architecture
|
||||
|
||||
@ -73,61 +77,102 @@ Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REF
|
||||
│ RealSense │ ← Forward-facing depth+RGB
|
||||
│ D435i │
|
||||
├──────────────┤
|
||||
│ Jetson Orin Nano Super │ ← AI brain: navigation, person tracking
|
||||
│ │ Sends velocity commands via UART
|
||||
│ Jetson Orin │ ← AI brain: ROS2, SLAM, Nav2
|
||||
│ Nano Super │ Sends CAN cmds 0x300–0x303
|
||||
├──────────────┤
|
||||
│ Drone FC │ ← Balance brain: IMU + PID @ 8kHz
|
||||
│ F745+MPU6000 │ Custom firmware, UART out to ESC
|
||||
│ ESP32-S3 │ ← Balance brain: QMI8658 IMU + PID
|
||||
│ BALANCE │ CAN master to VESCs (SN65HVD230)
|
||||
├──────────────┤
|
||||
│ ESP32-S3 IO │ ← RC (CRSF/ELRS), sensors, LEDs
|
||||
├──────────────┤
|
||||
│ Battery 36V │
|
||||
│ + DC-DCs │
|
||||
├──────┬───────┤
|
||||
┌─────┤ ESC (FOC) ├─────┐
|
||||
│ │ Hoverboard │ │
|
||||
┌─────┤ VESC Left ├─────┐
|
||||
│ │ (ID 68) │ │
|
||||
│ │ VESC Right │ │
|
||||
│ │ (ID 56) │ │
|
||||
│ └──────────────┘ │
|
||||
┌──┴──┐ ┌──┴──┐
|
||||
│ 8" │ │ 8" │
|
||||
│ LEFT│ │RIGHT│
|
||||
│ Hub │ │ Hub │
|
||||
│motor│ │motor│
|
||||
└─────┘ └─────┘
|
||||
```
|
||||
|
||||
## Self-Balancing Control — ESP32-S3 BALANCE Board
|
||||
|
||||
> For full system architecture, firmware details, and protocol specs, see
|
||||
> **docs/SAUL-TEE-SYSTEM-REFERENCE.md**
|
||||
|
||||
The balance controller runs on the Waveshare ESP32-S3 Touch LCD 1.28 board
|
||||
(ESP32-S3 BALANCE). It reads the onboard QMI8658 IMU at 8kHz, runs a PID
|
||||
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)
|
||||
## Self-Balancing Control — Custom Firmware on ESP32-S3 BALANCE
|
||||
|
||||
### Architecture
|
||||
The ESP32-C3 eavesdrops on the FC→Jetson telemetry UART line (listen-only, one wire).
|
||||
No extra UART needed on the FC — zero firmware change.
|
||||
```
|
||||
Jetson Orin (CAN 0x300–0x303)
|
||||
│ - Drive cmd: speed + steer
|
||||
│ - Arm/disarm, PID tune, ESTOP
|
||||
▼
|
||||
ESP32-S3 BALANCE (PlatformIO / Arduino)
|
||||
│ - Reads QMI8658 IMU @ I2C (GPIO6/7)
|
||||
│ - Runs PID balance loop
|
||||
│ - Mixes balance correction + Orin velocity cmd
|
||||
│ - Sends VESC CAN commands (SN65HVD230, 500 kbps)
|
||||
│ - Inter-board UART @ 460800 → ESP32-S3 IO
|
||||
▼
|
||||
VESC Left (CAN ID 68) VESC Right (CAN ID 56)
|
||||
│ │
|
||||
FL + RL hub motors FR + RR hub motors
|
||||
```
|
||||
|
||||
### Wiring
|
||||
|
||||
```
|
||||
FC UART1 TX ──┬──→ Jetson RX
|
||||
└──→ ESP32-C3 RX (listen-only, same wire)
|
||||
Jetson Orin CANable 2.0
|
||||
──────────── ──────────
|
||||
USB-A ──→ USB-B
|
||||
CANH ──→ CAN bus CANH
|
||||
CANL ──→ CAN bus CANL
|
||||
|
||||
ESP32-S3 BALANCE SN65HVD230 transceiver
|
||||
──────────────── ──────────────────────
|
||||
CAN TX (GPIO TBD) ──→ D pin
|
||||
CAN RX (GPIO TBD) ←── R pin
|
||||
CANH ──→ CAN bus CANH
|
||||
CANL ──→ CAN bus CANL
|
||||
|
||||
ESP32-S3 BALANCE ESP32-S3 IO
|
||||
──────────────── ───────────
|
||||
UART1 TX (TBD) ──→ UART1 RX (TBD)
|
||||
UART1 RX (TBD) ←── UART1 TX (TBD)
|
||||
GND ──→ GND
|
||||
|
||||
TBS Crossfire RX ESP32-S3 IO
|
||||
──────────────── ───────────
|
||||
TX ──→ GPIO44 (UART0 RX)
|
||||
RX ←── GPIO43 (UART0 TX)
|
||||
GND ──→ GND
|
||||
5V ←── 5V
|
||||
```
|
||||
|
||||
### PID Tuning
|
||||
| Param | Starting Value | Notes |
|
||||
|-------|---------------|-------|
|
||||
| Kp | 30-50 | Main balance response |
|
||||
| Ki | 0.5-2 | Drift correction |
|
||||
| Kd | 0.5-2 | Damping oscillation |
|
||||
| Loop rate | 1 kHz | QMI8658 data-ready driven (GPIO3 INT) |
|
||||
| Max tilt | ±25° | Beyond this = cut motors, require re-arm |
|
||||
| CAN_WATCHDOG_MS | 500 | Drop to RC-only if Orin CAN heartbeat lost |
|
||||
| max_speed_limit | 10% | Start at 10%, increase after stable testing |
|
||||
| SPEED_TO_ANGLE_FACTOR | 0.01-0.05 | How much lean per speed unit |
|
||||
|
||||
## LED Subsystem (ESP32-S3 IO)
|
||||
|
||||
### Architecture
|
||||
WS2812B LEDs are driven directly by the ESP32-S3 IO board via its RMT peripheral.
|
||||
The IO board receives robot state over inter-board UART from ESP32-S3 BALANCE.
|
||||
|
||||
```
|
||||
ESP32-S3 BALANCE ──UART 460800──→ ESP32-S3 IO
|
||||
│
|
||||
└──→ WS2812B strip (via RMT peripheral)
|
||||
└──RMT──→ WS2812B strip
|
||||
```
|
||||
|
||||
### Telemetry Format (already sent by FC at 50Hz)
|
||||
```
|
||||
T:12.3,P:45,L:100,R:-80,S:3\n
|
||||
^-- State byte: 0=disarmed, 1=arming, 2=armed, 3=fault
|
||||
```
|
||||
ESP32-C3 parses the `S:` field and `L:/R:` for turn detection.
|
||||
|
||||
### LED Patterns
|
||||
| State | Pattern | Color |
|
||||
|-------|---------|-------|
|
||||
@ -140,25 +185,18 @@ ESP32-C3 parses the `S:` field and `L:/R:` for turn detection.
|
||||
| Fault | Triple flash | Red |
|
||||
| RC signal lost | Alternating flash | Red/Blue |
|
||||
|
||||
### Turn/Brake Detection (on ESP32-C3)
|
||||
```
|
||||
if (L - R > threshold) → turning right
|
||||
if (R - L > threshold) → turning left
|
||||
if (L < -threshold && R < -threshold) → braking
|
||||
```
|
||||
|
||||
### Wiring
|
||||
```
|
||||
FC UART1 TX pin ──→ ESP32-C3 GPIO RX (e.g. GPIO20)
|
||||
ESP32-C3 GPIO8 ──→ WS2812B data in
|
||||
ESC 5V BEC ──→ ESP32-C3 5V + WS2812B 5V
|
||||
ESP32-S3 IO RMT GPIO (TBD) ──→ WS2812B data in
|
||||
5V bus ──→ WS2812B 5V + ESP32-S3 IO VCC
|
||||
GND ──→ Common ground
|
||||
```
|
||||
|
||||
### Dev Tools
|
||||
- **Flashing:** ESP32-S3CubeProgrammer via USB (DFU mode) or SWD
|
||||
- **IDE:** PlatformIO + ESP-IDF, or ESP32-S3CubeIDE
|
||||
- **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
|
||||
- **Flashing BALANCE:** `pio run -t upload` in `esp32/balance/` via CH343G USB
|
||||
- **Flashing IO:** `pio run -t upload` in `esp32/io/` via JTAG/USB-CDC
|
||||
- **IDE:** PlatformIO + Arduino framework (ESP32)
|
||||
- **Debug:** USB serial monitor (`pio device monitor`), logic analyzer on UART/CAN
|
||||
|
||||
## Physical Design
|
||||
|
||||
@ -173,7 +211,7 @@ GND ──→ Common ground
|
||||
├───────────┤ ├─────────────────┤
|
||||
│ Jetson │ ~300mm │ [Jetson] │
|
||||
├───────────┤ ├─────────────────┤
|
||||
│ Drone FC │ ~200mm │ [Drone FC] │
|
||||
│ ESP32-S3 │ ~200mm │ [BALANCE] │
|
||||
├───────────┤ ├─────────────────┤
|
||||
│ Battery │ ~100mm │ [Battery] │
|
||||
│ + ESC │ LOW! │ [ESC+DCDC] │
|
||||
@ -213,7 +251,8 @@ GND ──→ Common ground
|
||||
| Sensor tower top | 120×120×10 | ASA 80% | 1 |
|
||||
| LIDAR standoff | Ø80×80 | ASA 40% | 1 |
|
||||
| RealSense bracket | 100×50×40 | PETG 60% | 1 |
|
||||
| FC mount (vibration isolated) | 30×30×15 | TPU+PETG | 1 |
|
||||
| MCU mount — ESP32-S3 BALANCE | TBD×TBD×15 | TPU+PETG | 1 |
|
||||
| MCU mount — ESP32-S3 IO | TBD×TBD×15 | PETG | 1 |
|
||||
| Bumper front | 350×50×30 | TPU 30% | 1 |
|
||||
| Bumper rear | 350×50×30 | TPU 30% | 1 |
|
||||
| Handle (for carrying) | 150×30×30 | PETG 80% | 1 |
|
||||
@ -225,14 +264,14 @@ GND ──→ Common ground
|
||||
## Software Stack
|
||||
|
||||
### Jetson Orin Nano Super
|
||||
- **OS:** JetPack 4.6.1 (Ubuntu 18.04)
|
||||
- **ROS2 Humble** (or Foxy) for:
|
||||
- **OS:** JetPack 6.x (Ubuntu 22.04)
|
||||
- **ROS2 Humble** for:
|
||||
- `nav2` — navigation stack
|
||||
- `slam_toolbox` — 2D SLAM from LIDAR
|
||||
- `realsense-ros` — depth camera
|
||||
- `rplidar_ros` — LIDAR driver
|
||||
- **Person following:** SSD-MobileNet-v2 via TensorRT (~20 FPS)
|
||||
- **Balance commands:** ROS topic → UART bridge to drone FC
|
||||
- **Person following:** SSD-MobileNet-v2 via TensorRT (~30+ FPS)
|
||||
- **Balance commands:** ROS topic → CAN bus → ESP32-S3 BALANCE (CANable 2.0, can0, 500 kbps)
|
||||
|
||||
### Modes
|
||||
1. **Idle** — self-balancing in place, waiting for command
|
||||
@ -251,33 +290,34 @@ GND ──→ Common ground
|
||||
- [ ] Install hardware kill switch inline with 36V battery (NC — press to kill)
|
||||
- [ ] Set up ceiling tether point above test area (rated for >15kg)
|
||||
- [ ] Clear test area: 3m radius, no loose items, shoes on
|
||||
- [ ] Set up PlatformIO project for ESP32-S3 (ESP-IDF)
|
||||
- [ ] Write QMI8658 SPI driver (read gyro+accel, complementary filter)
|
||||
- [ ] Set up PlatformIO projects for ESP32-S3 BALANCE + IO (`esp32/balance/`, `esp32/io/`)
|
||||
- [ ] Confirm QMI8658 I2C comms on GPIO6/7 (INT on GPIO3); verify IMU data on serial
|
||||
- [ ] Write PID balance loop with ALL safety checks:
|
||||
- ±25° tilt cutoff → disarm, require manual re-arm
|
||||
- Watchdog timer (50ms hardware WDT)
|
||||
- Speed limit at 10% (max_speed_limit = 100)
|
||||
- Arming sequence (3s hold while upright)
|
||||
- [ ] Write hoverboard ESC UART output (speed+steer protocol)
|
||||
- [ ] Flash firmware via USB DFU (boot0 jumper on FC)
|
||||
- [ ] Write ELRS CRSF receiver driver (UART3, parse channels + arm switch)
|
||||
- [ ] Bind ELRS TX ↔ RX, verify channel data on serial monitor
|
||||
- [ ] Map radio: CH1=steer, CH2=speed, CH5=arm/disarm switch
|
||||
- [ ] **Bench test first** — FC powered but ESC disconnected, verify IMU reads + PID output + RC channels on serial monitor
|
||||
- [ ] Wire FC UART2 → hoverboard ESC UART
|
||||
- [ ] Build minimal frame: motor plate + battery + ESC + FC
|
||||
- [ ] Power FC from ESC 5V BEC
|
||||
- CAN watchdog 500 ms (drop to RC-only if Orin silent)
|
||||
- Speed limit at 10%
|
||||
- Arming sequence (deliberate ARM command required on power-on)
|
||||
- [ ] Write VESC CAN commands (SN65HVD230 transceiver, 500 kbps, IDs 68/56)
|
||||
- [ ] Flash BALANCE via CH343G USB: `cd esp32/balance && pio run -t upload`
|
||||
- [ ] Write TBS Crossfire CRSF driver on IO board (UART0 GPIO43/44, 420000 baud)
|
||||
- [ ] Bind TBS TX ↔ RX, verify channel data on IO board serial monitor
|
||||
- [ ] Map radio: CH1=steer, CH2=speed, CH5=arm/disarm, CH6=mode
|
||||
- [ ] Flash IO via JTAG/USB-CDC: `cd esp32/io && pio run -t upload`
|
||||
- [ ] **Bench test first** — BALANCE powered but VESCs disconnected; verify IMU + PID output + RC channels on serial; no motors spin
|
||||
- [ ] Wire BALANCE CAN TX/RX → SN65HVD230 → CAN bus → VESCs
|
||||
- [ ] Build minimal frame: motor plate + battery + VESCs + ESP32-S3 boards
|
||||
- [ ] Power ESP32s from 5V DC-DC
|
||||
- [ ] **First balance test — TETHERED, kill switch in hand, 10% speed limit**
|
||||
- [ ] Tune PID at 10% speed until stable tethered for 5+ minutes
|
||||
- [ ] Gradually increase speed limit (10% increments, 5 min stable each)
|
||||
|
||||
### Phase 2: Brain (Week 2)
|
||||
- [ ] Mount Jetson + power (DC-DC 5V)
|
||||
- [ ] Mount Jetson Orin Nano Super + power (DC-DC 5V via USB-C PD)
|
||||
- [ ] Set up JetPack + ROS2
|
||||
- [ ] Add Jetson UART RX to FC firmware (receive speed+steer commands)
|
||||
- [ ] Wire Jetson UART1 → FC UART1
|
||||
- [ ] Python serial bridge: send speed+steer, read telemetry
|
||||
- [ ] Test: keyboard teleoperation while balancing
|
||||
- [ ] Bring up CANable 2.0 on Orin: `ip link set can0 up type can bitrate 500000`
|
||||
- [ ] Send drive CAN frames (0x300) from Orin → BALANCE firmware receives + acts
|
||||
- [ ] ROS2 node: subscribe to `/cmd_vel`, publish CAN drive frames
|
||||
- [ ] Test: keyboard teleoperation via ROS2 while balancing
|
||||
|
||||
### Phase 3: Senses (Week 3)
|
||||
- [ ] Mount RealSense + RPLIDAR
|
||||
@ -287,10 +327,9 @@ GND ──→ Common ground
|
||||
|
||||
### Phase 4: Polish (Week 4)
|
||||
- [ ] Print proper enclosures, bumpers, diffuser ring
|
||||
- [ ] Wire ESP32-C3 to FC telemetry TX line (listen-only tap)
|
||||
- [ ] Flash ESP32-C3: parse telemetry, drive WS2812B via RMT
|
||||
- [ ] Implement WS2812B LED patterns in ESP32-S3 IO firmware (RMT, state from inter-board UART)
|
||||
- [ ] Mount LED strip around frame with diffuser
|
||||
- [ ] Test all LED patterns: disarmed/arming/armed/turning/fault
|
||||
- [ ] Speaker for audio feedback
|
||||
- [ ] WiFi status dashboard (ESP32-C3 can serve this too)
|
||||
- [ ] Emergency stop button
|
||||
- [ ] Test all LED patterns: disarmed/arming/armed/turning/fault/RC-lost
|
||||
- [ ] Speaker / buzzer audio feedback (IO board GPIO)
|
||||
- [ ] WiFi status dashboard (serve from Orin or IO board AP)
|
||||
- [ ] Emergency stop button wired to IO board GPIO → ESTOP CAN frame 0x303
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
<html>
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<title>GEPRC GEP-F722-45A AIO — Board Layout (Legacy / Archived)</title>
|
||||
<title>ESP32-S3 BALANCE — Waveshare Touch LCD 1.28 Pinout</title>
|
||||
<style>
|
||||
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||||
body { background: #1a1a2e; color: #eee; font-family: 'Courier New', monospace; display: flex; flex-direction: column; align-items: center; padding: 20px; }
|
||||
@ -10,274 +10,219 @@ h1 { color: #e94560; margin-bottom: 5px; font-size: 1.4em; }
|
||||
.subtitle { color: #888; margin-bottom: 20px; font-size: 0.85em; }
|
||||
.container { display: flex; gap: 30px; align-items: flex-start; flex-wrap: wrap; justify-content: center; }
|
||||
.board-wrap { position: relative; }
|
||||
.board { width: 400px; height: 340px; background: #1a472a; border: 3px solid #333; border-radius: 8px; position: relative; box-shadow: 0 0 20px rgba(0,0,0,0.5); }
|
||||
.board::before { content: 'GEPRC GEP-F722-45A AIO'; position: absolute; top: 8px; left: 50%; transform: translateX(-50%); color: #fff3; font-size: 10px; letter-spacing: 2px; }
|
||||
.board { width: 400px; height: 380px; background: #1a472a; border: 3px solid #333; border-radius: 50%; position: relative; box-shadow: 0 0 20px rgba(0,0,0,0.5); display: flex; align-items: center; justify-content: center; }
|
||||
.board::before { content: 'Waveshare Touch LCD 1.28'; position: absolute; top: 30px; left: 50%; transform: translateX(-50%); color: #fff3; font-size: 9px; letter-spacing: 1px; white-space: nowrap; }
|
||||
|
||||
/* Mounting holes */
|
||||
.mount { width: 10px; height: 10px; background: #111; border: 2px solid #555; border-radius: 50%; position: absolute; }
|
||||
.mount.tl { top: 15px; left: 15px; }
|
||||
.mount.tr { top: 15px; right: 15px; }
|
||||
.mount.bl { bottom: 15px; left: 15px; }
|
||||
.mount.br { bottom: 15px; right: 15px; }
|
||||
/* LCD circle */
|
||||
.lcd { width: 180px; height: 180px; background: #111; border: 3px solid #444; border-radius: 50%; position: absolute; display: flex; align-items: center; justify-content: center; font-size: 9px; color: #64B5F6; text-align: center; line-height: 1.6; }
|
||||
.lcd-inner { text-align: center; }
|
||||
|
||||
/* MCU */
|
||||
.mcu { width: 80px; height: 80px; background: #222; border: 1px solid #555; position: absolute; top: 50%; left: 50%; transform: translate(-50%, -50%); display: flex; align-items: center; justify-content: center; font-size: 9px; color: #aaa; text-align: center; line-height: 1.3; }
|
||||
.mcu .dot { width: 5px; height: 5px; background: #666; border-radius: 50%; position: absolute; top: 4px; left: 4px; }
|
||||
|
||||
/* IMU */
|
||||
.imu { width: 32px; height: 32px; background: #333; border: 1px solid #e94560; position: absolute; top: 85px; left: 60px; display: flex; align-items: center; justify-content: center; font-size: 7px; color: #e94560; }
|
||||
.imu::after { content: 'CW90°'; position: absolute; bottom: -14px; color: #e94560; font-size: 8px; white-space: nowrap; }
|
||||
|
||||
/* Arrow showing CW90 rotation */
|
||||
.rotation-arrow { position: absolute; top: 72px; left: 55px; color: #e94560; font-size: 18px; }
|
||||
|
||||
/* Pads */
|
||||
.pad { position: absolute; display: flex; align-items: center; gap: 4px; font-size: 10px; cursor: pointer; }
|
||||
.pad .dot { width: 12px; height: 12px; border-radius: 50%; border: 2px solid; display: flex; align-items: center; justify-content: center; font-size: 7px; font-weight: bold; }
|
||||
.pad:hover .label { color: #fff; }
|
||||
.pad .label { transition: color 0.2s; }
|
||||
.pad .sublabel { font-size: 8px; color: #888; }
|
||||
|
||||
/* UART colors */
|
||||
.uart1 .dot { background: #2196F3; border-color: #64B5F6; }
|
||||
.uart2 .dot { background: #FF9800; border-color: #FFB74D; }
|
||||
.uart3 .dot { background: #9C27B0; border-color: #CE93D8; }
|
||||
.uart4 .dot { background: #4CAF50; border-color: #81C784; }
|
||||
.uart5 .dot { background: #F44336; border-color: #EF9A9A; }
|
||||
/* MCU chip */
|
||||
.mcu-label { position: absolute; top: 95px; left: 50%; transform: translateX(-50%); font-size: 8px; color: #aaa; text-align: center; white-space: nowrap; }
|
||||
|
||||
/* Component dots */
|
||||
.comp { position: absolute; font-size: 9px; display: flex; align-items: center; gap: 4px; }
|
||||
.comp .icon { width: 10px; height: 10px; border-radius: 2px; }
|
||||
|
||||
/* LED */
|
||||
.led-blue { position: absolute; width: 8px; height: 8px; background: #2196F3; border-radius: 50%; box-shadow: 0 0 8px #2196F3; top: 45px; right: 50px; }
|
||||
.led-label { position: absolute; top: 36px; right: 30px; font-size: 8px; color: #64B5F6; }
|
||||
|
||||
/* Boot button */
|
||||
.boot-btn { position: absolute; width: 16px; height: 10px; background: #b8860b; border: 1px solid #daa520; border-radius: 2px; bottom: 45px; right: 40px; }
|
||||
.boot-label { position: absolute; bottom: 32px; right: 30px; font-size: 8px; color: #daa520; }
|
||||
.comp { position: absolute; font-size: 9px; color: #ccc; }
|
||||
|
||||
/* USB */
|
||||
.usb { position: absolute; width: 30px; height: 14px; background: #444; border: 2px solid #777; border-radius: 3px; bottom: -3px; left: 50%; transform: translateX(-50%); }
|
||||
.usb-label { position: absolute; bottom: 14px; left: 50%; transform: translateX(-50%); font-size: 8px; color: #999; }
|
||||
.usb { position: absolute; width: 36px; height: 14px; background: #444; border: 2px solid #777; border-radius: 3px; bottom: 25px; left: 50%; transform: translateX(-50%); }
|
||||
.usb-label { position: absolute; bottom: 44px; left: 50%; transform: translateX(-50%); font-size: 8px; color: #999; white-space: nowrap; }
|
||||
|
||||
/* Connector pads along edges */
|
||||
/* Bottom row: T1 R1 T3 R3 */
|
||||
.pad-t1 { bottom: 20px; left: 40px; }
|
||||
.pad-r1 { bottom: 20px; left: 80px; }
|
||||
.pad-t3 { bottom: 20px; left: 140px; }
|
||||
.pad-r3 { bottom: 20px; left: 180px; }
|
||||
/* Pin rows */
|
||||
.pin-row { position: absolute; display: flex; flex-direction: column; gap: 4px; }
|
||||
.pin { display: flex; align-items: center; gap: 4px; font-size: 10px; }
|
||||
.pin .dot { width: 10px; height: 10px; border-radius: 50%; border: 2px solid; flex-shrink: 0; }
|
||||
.pin .name { min-width: 70px; }
|
||||
.pin .sublabel { font-size: 8px; color: #888; }
|
||||
|
||||
/* Right side: T2 R2 */
|
||||
.pad-t2 { right: 20px; top: 80px; flex-direction: row-reverse; }
|
||||
.pad-r2 { right: 20px; top: 110px; flex-direction: row-reverse; }
|
||||
/* Left side pins */
|
||||
.pins-left { left: 10px; top: 60px; }
|
||||
.pins-left .pin { flex-direction: row; }
|
||||
|
||||
/* Top row: T4 R4 T5 R5 */
|
||||
.pad-t4 { top: 30px; left: 40px; }
|
||||
.pad-r4 { top: 30px; left: 80px; }
|
||||
.pad-t5 { top: 30px; right: 100px; flex-direction: row-reverse; }
|
||||
.pad-r5 { top: 30px; right: 55px; flex-direction: row-reverse; }
|
||||
/* Right side pins */
|
||||
.pins-right { right: 10px; top: 60px; }
|
||||
.pins-right .pin { flex-direction: row-reverse; text-align: right; }
|
||||
|
||||
/* ESC pads (motor outputs - not used) */
|
||||
.esc-pads { position: absolute; left: 20px; top: 140px; }
|
||||
.esc-pads .esc-label { font-size: 8px; color: #555; }
|
||||
/* Colors by function */
|
||||
.imu .dot { background: #e94560; border-color: #ff6b81; }
|
||||
.can .dot { background: #FF9800; border-color: #FFB74D; }
|
||||
.uart .dot { background: #2196F3; border-color: #64B5F6; }
|
||||
.spi .dot { background: #9C27B0; border-color: #CE93D8; }
|
||||
.pwr .dot { background: #4CAF50; border-color: #81C784; }
|
||||
.io .dot { background: #607D8B; border-color: #90A4AE; }
|
||||
|
||||
/* Legend */
|
||||
.legend { background: #16213e; padding: 15px 20px; border-radius: 8px; min-width: 280px; }
|
||||
.legend { background: #16213e; padding: 15px 20px; border-radius: 8px; min-width: 290px; }
|
||||
.legend h2 { color: #e94560; font-size: 1.1em; margin-bottom: 10px; border-bottom: 1px solid #333; padding-bottom: 5px; }
|
||||
.legend-item { display: flex; align-items: center; gap: 8px; margin: 6px 0; font-size: 12px; }
|
||||
.legend-item .swatch { width: 14px; height: 14px; border-radius: 50%; flex-shrink: 0; }
|
||||
.legend-item .arrow { color: #888; font-size: 10px; }
|
||||
.legend-section { margin-top: 12px; padding-top: 8px; border-top: 1px solid #333; }
|
||||
.legend-section h3 { font-size: 0.9em; color: #888; margin-bottom: 6px; }
|
||||
|
||||
/* Orientation guide */
|
||||
.orient { margin-top: 20px; background: #16213e; padding: 15px 20px; border-radius: 8px; width: 100%; max-width: 710px; }
|
||||
.orient h2 { color: #4CAF50; font-size: 1.1em; margin-bottom: 10px; }
|
||||
.orient-grid { display: grid; grid-template-columns: 1fr 1fr; gap: 10px; }
|
||||
.orient-item { font-size: 12px; padding: 6px 10px; background: #1a1a2e; border-radius: 4px; }
|
||||
.orient-item .dir { color: #4CAF50; font-weight: bold; }
|
||||
|
||||
/* Axis overlay */
|
||||
.axis { position: absolute; }
|
||||
.axis-x { top: 50%; right: -60px; color: #F44336; font-size: 12px; font-weight: bold; }
|
||||
.axis-y { bottom: -30px; left: 50%; transform: translateX(-50%); color: #4CAF50; font-size: 12px; font-weight: bold; }
|
||||
.axis-arrow-x { position: absolute; top: 50%; right: -45px; transform: translateY(-50%); width: 30px; height: 2px; background: #F44336; }
|
||||
.axis-arrow-x::after { content: '▶'; position: absolute; right: -12px; top: -8px; color: #F44336; }
|
||||
.axis-arrow-y { position: absolute; bottom: -20px; left: 50%; transform: translateX(-50%); width: 2px; height: 20px; background: #4CAF50; }
|
||||
.axis-arrow-y::after { content: '▼'; position: absolute; bottom: -14px; left: -5px; color: #4CAF50; }
|
||||
|
||||
.note { margin-top: 15px; color: #888; font-size: 11px; text-align: center; max-width: 710px; }
|
||||
.note em { color: #e94560; font-style: normal; }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<<<<<<< HEAD
|
||||
<h1>🤖 GEPRC GEP-F722-45A AIO — SaltyLab Pinout (Legacy / Archived)</h1>
|
||||
<p class="subtitle">ESP32RET6 + ICM-42688-P | Betaflight target: GEPR-GEPRC_F722_AIO</p>
|
||||
=======
|
||||
<h1>🤖 GEPRC GEP-F722-45A AIO — SaltyLab Pinout</h1>
|
||||
<p class="subtitle">ESP32-S3RET6 + ICM-42688-P | Betaflight target: GEPR-GEPRC_F722_AIO</p>
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
<h1>🤖 ESP32-S3 BALANCE — Waveshare Touch LCD 1.28</h1>
|
||||
<p class="subtitle">ESP32-S3 240 MHz | QMI8658 IMU | GC9A01 1.28″ LCD | CH343G USB-UART | SN65HVD230 CAN</p>
|
||||
|
||||
<div class="container">
|
||||
<div class="board-wrap">
|
||||
<div class="board">
|
||||
<!-- Mounting holes -->
|
||||
<div class="mount tl"></div>
|
||||
<div class="mount tr"></div>
|
||||
<div class="mount bl"></div>
|
||||
<div class="mount br"></div>
|
||||
<!-- LCD circle -->
|
||||
<div class="lcd">
|
||||
<div class="lcd-inner">GC9A01<br>1.28″ round<br>240×240<br>SPI</div>
|
||||
</div>
|
||||
<div class="mcu-label">ESP32-S3<br>240 MHz / 8MB</div>
|
||||
|
||||
<!-- MCU -->
|
||||
<<<<<<< HEAD
|
||||
<div class="mcu"><div class="dot"></div>ESP32<br>(legacy:<br>F722RET6)</div>
|
||||
=======
|
||||
<div class="mcu"><div class="dot"></div>ESP32-S3<br>F722RET6<br>216MHz</div>
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
|
||||
<!-- IMU -->
|
||||
<div class="imu">ICM<br>42688</div>
|
||||
<div class="rotation-arrow">↻</div>
|
||||
|
||||
<!-- LED -->
|
||||
<div class="led-blue"></div>
|
||||
<div class="led-label">LED PC4</div>
|
||||
|
||||
<!-- Boot button -->
|
||||
<div class="boot-btn"></div>
|
||||
<div class="boot-label">BOOT 🟡</div>
|
||||
|
||||
<!-- USB -->
|
||||
<!-- USB CH343G -->
|
||||
<div class="usb-label">USB-A CH343G (UART0 debug/flash)</div>
|
||||
<div class="usb"></div>
|
||||
<div class="usb-label">USB-C (DFU)</div>
|
||||
|
||||
<!-- UART Pads - Bottom -->
|
||||
<div class="pad pad-t1 uart1">
|
||||
<div class="dot">T</div>
|
||||
<span class="label">T1<br><span class="sublabel">PA9</span></span>
|
||||
<!-- Left-side pins (onboard fixed) -->
|
||||
<div class="pin-row pins-left">
|
||||
<div class="pin imu">
|
||||
<div class="dot"></div>
|
||||
<span class="name">GPIO6</span>
|
||||
<span class="sublabel">IMU SDA (I2C-0)</span>
|
||||
</div>
|
||||
<div class="pad pad-r1 uart1">
|
||||
<div class="dot">R</div>
|
||||
<span class="label">R1<br><span class="sublabel">PA10</span></span>
|
||||
<div class="pin imu">
|
||||
<div class="dot"></div>
|
||||
<span class="name">GPIO7</span>
|
||||
<span class="sublabel">IMU SCL (I2C-0)</span>
|
||||
</div>
|
||||
<div class="pad pad-t3 uart3">
|
||||
<div class="dot">T</div>
|
||||
<span class="label">T3<br><span class="sublabel">PB10</span></span>
|
||||
<div class="pin imu">
|
||||
<div class="dot"></div>
|
||||
<span class="name">GPIO3</span>
|
||||
<span class="sublabel">QMI8658 INT</span>
|
||||
</div>
|
||||
<div class="pin uart">
|
||||
<div class="dot"></div>
|
||||
<span class="name">GPIO43</span>
|
||||
<span class="sublabel">UART0 TX (CH343G)</span>
|
||||
</div>
|
||||
<div class="pin uart">
|
||||
<div class="dot"></div>
|
||||
<span class="name">GPIO44</span>
|
||||
<span class="sublabel">UART0 RX (CH343G)</span>
|
||||
</div>
|
||||
<div class="pin uart">
|
||||
<div class="dot"></div>
|
||||
<span class="name">UART1 TX</span>
|
||||
<span class="sublabel">Inter-board → IO (TBD)</span>
|
||||
</div>
|
||||
<div class="pin uart">
|
||||
<div class="dot"></div>
|
||||
<span class="name">UART1 RX</span>
|
||||
<span class="sublabel">Inter-board ← IO (TBD)</span>
|
||||
</div>
|
||||
<div class="pad pad-r3 uart3">
|
||||
<div class="dot">R</div>
|
||||
<span class="label">R3<br><span class="sublabel">PB11</span></span>
|
||||
</div>
|
||||
|
||||
<!-- UART Pads - Right -->
|
||||
<div class="pad pad-t2 uart2">
|
||||
<span class="label">T2<br><span class="sublabel">PA2</span></span>
|
||||
<div class="dot">T</div>
|
||||
<!-- Right-side pins (external) -->
|
||||
<div class="pin-row pins-right">
|
||||
<div class="pin can">
|
||||
<div class="dot"></div>
|
||||
<span class="name">CAN TX</span>
|
||||
<span class="sublabel">→ SN65HVD230 D (TBD)</span>
|
||||
</div>
|
||||
<div class="pad pad-r2 uart2">
|
||||
<span class="label">R2<br><span class="sublabel">PA3</span></span>
|
||||
<div class="dot">R</div>
|
||||
<div class="pin can">
|
||||
<div class="dot"></div>
|
||||
<span class="name">CAN RX</span>
|
||||
<span class="sublabel">← SN65HVD230 R (TBD)</span>
|
||||
</div>
|
||||
|
||||
<!-- UART Pads - Top -->
|
||||
<div class="pad pad-t4 uart4">
|
||||
<div class="dot">T</div>
|
||||
<span class="label">T4<br><span class="sublabel">PC10</span></span>
|
||||
<div class="pin spi">
|
||||
<div class="dot"></div>
|
||||
<span class="name">LCD SPI</span>
|
||||
<span class="sublabel">GC9A01 (onboard)</span>
|
||||
</div>
|
||||
<div class="pad pad-r4 uart4">
|
||||
<div class="dot">R</div>
|
||||
<span class="label">R4<br><span class="sublabel">PC11</span></span>
|
||||
<div class="pin pwr">
|
||||
<div class="dot"></div>
|
||||
<span class="name">5V</span>
|
||||
<span class="sublabel">USB / ext 5V in</span>
|
||||
</div>
|
||||
<div class="pad pad-t5 uart5">
|
||||
<span class="label">T5<br><span class="sublabel">PC12</span></span>
|
||||
<div class="dot">T</div>
|
||||
<div class="pin pwr">
|
||||
<div class="dot"></div>
|
||||
<span class="name">3.3V</span>
|
||||
<span class="sublabel">LDO out</span>
|
||||
</div>
|
||||
<div class="pad pad-r5 uart5">
|
||||
<span class="label">R5<br><span class="sublabel">PD2</span></span>
|
||||
<div class="dot">R</div>
|
||||
<div class="pin pwr">
|
||||
<div class="dot"></div>
|
||||
<span class="name">GND</span>
|
||||
<span class="sublabel">Common ground</span>
|
||||
</div>
|
||||
|
||||
<!-- ESC motor pads label -->
|
||||
<div class="esc-pads">
|
||||
<div class="esc-label">M1-M4 (unused)<br>PC6-PC9</div>
|
||||
</div>
|
||||
|
||||
<!-- Board axes -->
|
||||
<div class="axis-arrow-x"></div>
|
||||
<div class="axis axis-x">X →<br><span style="font-size:9px;color:#888">board right</span></div>
|
||||
<div class="axis-arrow-y"></div>
|
||||
<div class="axis axis-y">Y ↓ (board forward = tilt axis)</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="legend">
|
||||
<h2>🔌 UART Assignments</h2>
|
||||
<h2>📌 Pin Assignments</h2>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#2196F3"></div>
|
||||
<span><b>USART1</b> T1/R1 → Jetson Orin Nano Super</span>
|
||||
<div class="swatch" style="background:#e94560"></div>
|
||||
<span><b>IMU (QMI8658)</b> — I2C-0 SDA=GPIO6, SCL=GPIO7, INT=GPIO3</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#FF9800"></div>
|
||||
<span><b>USART2</b> T2 → Hoverboard ESC (TX only)</span>
|
||||
<span><b>CAN (SN65HVD230)</b> — TX/RX TBD; confirm in <code>esp32/balance/src/config.h</code></span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#2196F3"></div>
|
||||
<span><b>UART0</b> GPIO43/44 — CH343G USB bridge (debug + flash)</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#2196F3"></div>
|
||||
<span><b>UART1</b> TBD — Inter-board @ 460800 baud → ESP32-S3 IO</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#9C27B0"></div>
|
||||
<span><b>I2C2</b> T3/R3 → Baro/Mag (reserved)</span>
|
||||
<span><b>LCD SPI</b> — GC9A01 1.28″ round 240×240 (onboard, fixed pins)</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#4CAF50"></div>
|
||||
<span><b>UART4</b> T4/R4 → ELRS RX (CRSF)</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<div class="swatch" style="background:#F44336"></div>
|
||||
<span><b>UART5</b> T5/R5 → Debug/spare</span>
|
||||
<span><b>Power</b> — 5V USB input; 3.3V LDO for logic + sensors</span>
|
||||
</div>
|
||||
|
||||
<div class="legend-section">
|
||||
<h3>📡 SPI Bus</h3>
|
||||
<h3>🔌 CAN Bus Topology</h3>
|
||||
<div class="legend-item">
|
||||
<span>SPI1: PA5/PA6/PA7 → IMU (CS: <em style="color:#e94560">PA15</em>)</span>
|
||||
<span>Orin → CANable 2.0 → <b>CANH/CANL</b> (500 kbps)</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<span>SPI2: PB13-15 → OSD MAX7456</span>
|
||||
<span>BALANCE: SN65HVD230 on CAN bus</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<span>SPI3: PB3-5 → Flash W25Q128</span>
|
||||
<span>VESC Left: ID <b>0x44</b> (68) | VESC Right: ID <b>0x38</b> (56)</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<span>120 Ω termination at each bus end</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="legend-section">
|
||||
<h3>⚡ Other</h3>
|
||||
<h3>📡 Inter-Board Protocol</h3>
|
||||
<div class="legend-item">
|
||||
<span>🔵 LED: PC4 | 📢 Beeper: PC15</span>
|
||||
<span>UART @ 460800 baud, 8N1</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<span>🔋 VBAT: PC2 | ⚡ Current: PC1</span>
|
||||
<span>Frame: <code>[0xAA][LEN][TYPE][PAYLOAD][CRC8]</code></span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<span>💡 LED Strip: PA1 (WS2812)</span>
|
||||
</div>
|
||||
<div class="legend-item">
|
||||
<span>📍 EXTI (IMU data-ready): PA8</span>
|
||||
</div>
|
||||
</div>
|
||||
<span>Types: see <code>esp32/shared/protocol.h</code></span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="orient">
|
||||
<h2>🧭 IMU Orientation (CW90° from chip to board)</h2>
|
||||
<div class="orient-grid">
|
||||
<div class="orient-item"><span class="dir">Board Forward</span> (tilt for balance) = Chip's +Y axis</div>
|
||||
<div class="orient-item"><span class="dir">Board Right</span> = Chip's -X axis</div>
|
||||
<div class="orient-item"><span class="dir">Board Pitch Rate</span> = -Gyro X (raw)</div>
|
||||
<div class="orient-item"><span class="dir">Board Accel Forward</span> = Accel Y (raw)</div>
|
||||
<div class="legend-section">
|
||||
<h3>⚡ Safety</h3>
|
||||
<div class="legend-item"><span>Motors NEVER spin on power-on — ARM required</span></div>
|
||||
<div class="legend-item"><span>RC kill switch checked every loop</span></div>
|
||||
<div class="legend-item"><span>CAN watchdog: 500 ms → RC-only mode</span></div>
|
||||
<div class="legend-item"><span>ESTOP: CAN 0x303 + 0xE5 → all motors off</span></div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<p class="note">
|
||||
⚠️ Pad positions are <em>approximate</em> — check the physical board silkscreen for exact locations.
|
||||
The CW90 rotation is handled in firmware (mpu6000.c). USB-C at bottom edge for DFU flashing.
|
||||
⚠️ CAN TX/RX GPIO assignments are <em>TBD</em> — confirm in <code>esp32/balance/src/config.h</code> before wiring.
|
||||
All inter-board UART GPIO also TBD. LCD and IMU pins are fixed by Waveshare hardware.
|
||||
</p>
|
||||
|
||||
</body>
|
||||
|
||||
@ -1,212 +1,174 @@
|
||||
# SaltyLab / SAUL-TEE Wiring Reference
|
||||
# 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.
|
||||
**Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
|
||||
This document is a quick-access wiring summary. For pin assignments, CAN frame formats,
|
||||
RC channel mapping, and serial commands, see the full reference doc.
|
||||
|
||||
---
|
||||
|
||||
## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md)
|
||||
## System Block Diagram
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ ORIN NANO SUPER │
|
||||
│ (Top Plate — 25W) │
|
||||
┌──────────────────────────────────────────────────────────┐
|
||||
│ JETSON 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-A2 ─── RPLIDAR A1M8 (via CP2102 adapter, 115200) │
|
||||
│ USB-C* ─── SIM7600A 4G/LTE modem (ttyUSB0-2, AT cmds + PPP) │
|
||||
│ USB ─────── Leap Motion Controller (hand/gesture tracking) │
|
||||
│ CSI-A ──── ArduCam adapter → 2x IMX219 (front + left) │
|
||||
│ CSI-B ──── ArduCam adapter → 2x IMX219 (rear + right) │
|
||||
│ M.2 ───── 1TB NVMe SSD │
|
||||
│ 40-pin ─── ReSpeaker 2-Mic HAT (I2S + I2C, WM8960 codec) │
|
||||
│ Pin 8 ──┐ │
|
||||
│ Pin 10 ─┤ UART fallback to ESP32-S3 BALANCE (ttyTHS0, 460800) │
|
||||
│ Pin 6 ──┘ GND │
|
||||
│ USB-A ──── CANable 2.0 USB↔CAN (can0, 500 kbps) │
|
||||
│ USB-A ──── RealSense D435i (USB 3.1) │
|
||||
│ USB-A ──── RPLIDAR A1M8 (CP2102, 115200) │
|
||||
│ USB-C ──── SIM7600A 4G/LTE modem │
|
||||
│ CSI-A ─── 2× IMX219 cameras (front + left) │
|
||||
│ CSI-B ─── 2× IMX219 cameras (rear + right) │
|
||||
│ 40-pin ── ReSpeaker 2-Mic HAT │
|
||||
└──────────────────────┬───────────────────────────────────┘
|
||||
│ USB-A → CANable 2.0
|
||||
│ can0, 500 kbps
|
||||
┌────────────────────────────────┴──────────────────────────────────┐
|
||||
│ CAN BUS (CANH / CANL / GND) │
|
||||
│ 120 Ω ─┤ ├─ 120 Ω │
|
||||
└───────────┬──────────────────────────────────────────┬────────────┘
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
│ USB-A (CANable2) │ UART fallback (3 wires)
|
||||
│ SocketCAN slcan0 │ 460800 baud, 3.3V
|
||||
│ 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)
|
||||
│ │
|
||||
│ CAN bus ──── CANable2 → Orin (primary link, ISO 11898) │
|
||||
│ UART0 ──── Orin UART fallback (460800 baud, 3.3V) │
|
||||
│ UART1 ──── VESC Left (CAN ID 56) via UART/CAN bridge │
|
||||
│ UART2 ──── VESC Right (CAN ID 68) via UART/CAN bridge │
|
||||
│ I2C ──── QMI8658 IMU (onboard, 6-DOF accel+gyro) │
|
||||
│ SPI ──── GC9A01 LCD (onboard, 240x240 round display) │
|
||||
│ GPIO ──── WS2812B LED strip │
|
||||
│ GPIO ──── Buzzer │
|
||||
│ ADC ──── Battery voltage divider │
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
│ CAN bus (ISO 11898) │ UART (460800 baud)
|
||||
│ 500 kbps │
|
||||
▼ ▼
|
||||
┌────────────────────────┐ ┌──────────────────────────┐
|
||||
│ VESC Left (ID 56) │ │ VESC Right (ID 68) │
|
||||
│ (Bottom Plate) │ │ (Bottom Plate) │
|
||||
│ │ │ │
|
||||
│ BLDC hub motor │ │ BLDC hub motor │
|
||||
│ CAN 500 kbps │ │ CAN 500 kbps │
|
||||
│ FOC current control │ │ FOC current control │
|
||||
│ VESC Status 1 (0x900) │ │ VESC Status 1 (0x910) │
|
||||
│ │ │ │
|
||||
└────────────────────────┘ └──────────────────────────┘
|
||||
│ │
|
||||
LEFT MOTOR RIGHT MOTOR
|
||||
┌───────────┴────────────┐ ┌─────────────┴──────────┐
|
||||
│ ESP32-S3 BALANCE │ │ VESC left (ID 68) │
|
||||
│ Waveshare Touch LCD │ │ VESC right (ID 56) │
|
||||
│ 1.28 — CH343 USB │ │ FSESC 6.7 Pro Mini │
|
||||
│ │ │ Dual │
|
||||
│ QMI8658 IMU (I2C) │ └──────┬─────────────────┘
|
||||
│ SN65HVD230 (CAN) │ │ Phase wires
|
||||
│ │ ┌────────┴─────────────┐
|
||||
│ UART ──────────────┐ │ │ Hub motors (4×) │
|
||||
└────────────────────────┘ │ FL / FR / RL / RR │
|
||||
↕ 460800 baud binary │ └──────────────────────┘
|
||||
inter-board proto │
|
||||
┌───────────────────────┘
|
||||
│ ESP32-S3 IO (bare board)
|
||||
│ JTAG USB
|
||||
│
|
||||
│ UART0 ── TBS Crossfire RX (CRSF @ 420000)
|
||||
│ UART2 ── ELRS receiver (CRSF failover @ 420000)
|
||||
│ PWM ──── 4× BTS7960 H-bridge motor drivers
|
||||
│ I2C ──── NFC + Barometer + ToF (shared bus)
|
||||
│ RMT ──── WS2812B LED strip
|
||||
│ GPIO ─── Horn / Headlight / Fan / Buzzer
|
||||
└──────────────────────────────────────────────
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 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)
|
||||
|
||||
| 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 |
|
||||
|----------|--------|--------------|-------|
|
||||
| Pin 8 | TXD0 | GPIO17 (UART0 RX) | Orin TX -> BALANCE RX |
|
||||
| Pin 10 | RXD0 | GPIO18 (UART0 TX) | Orin RX <- BALANCE TX |
|
||||
| Pin 6 | GND | GND | Common ground |
|
||||
|
||||
- Jetson device: `/dev/ttyTHS0`
|
||||
- Baud: 460800, 8N1
|
||||
- Voltage: 3.3V both sides (no level shifter needed)
|
||||
- Cross-connect: Orin TX -> BALANCE RX, Orin RX <- BALANCE TX
|
||||
|
||||
### 3. Orin <-> ESP32-S3 IO (USB Serial)
|
||||
### 1. Orin ↔ CAN Bus (via CANable 2.0)
|
||||
|
||||
| From | To | Notes |
|
||||
|------|----|-------|
|
||||
| Orin USB-A | ESP32-S3 IO USB-C | USB cable, /dev/esp32-io |
|
||||
| Orin USB-A | CANable 2.0 USB | `/dev/canable0` → `can0` |
|
||||
| CANable CANH | CAN bus CANH | Twisted pair |
|
||||
| CANable CANL | CAN bus CANL | Twisted pair |
|
||||
| CANable GND | CAN GND | Common |
|
||||
|
||||
- Device node: `/dev/esp32-io` (udev symlink)
|
||||
- Baud: 460800, 8N1
|
||||
- Protocol: Binary frames `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`
|
||||
- Use: IO expansion, GPIO control, sensor polling
|
||||
Setup: `ip link set can0 up type can bitrate 500000`
|
||||
|
||||
### 4. ESP32-S3 BALANCE <-> VESC Motors (CAN Bus)
|
||||
### 2. ESP32-S3 BALANCE ↔ CAN Bus
|
||||
|
||||
| BALANCE Pin | Signal | VESC Pin | Notes |
|
||||
|-------------|--------|----------|-------|
|
||||
| GPIO21 | CAN-H | CAN-H | ISO 11898 differential pair |
|
||||
| GPIO22 | CAN-L | CAN-L | ISO 11898 differential pair |
|
||||
| GND | GND | GND | Common ground |
|
||||
| Signal | GPIO | CAN bus |
|
||||
|--------|------|---------|
|
||||
| CAN TX | TBD | → SN65HVD230 D pin |
|
||||
| CAN RX | TBD | ← SN65HVD230 R pin |
|
||||
|
||||
- Baud: 500 kbps CAN
|
||||
- VESC Left: CAN ID 56, VESC Right: CAN ID 68
|
||||
- Commands: COMM_SET_RPM, COMM_SET_CURRENT, COMM_SET_DUTY
|
||||
- Telemetry: VESC Status 1 at 50 Hz (RPM, current, duty)
|
||||
> TBD pins — confirm in `esp32/balance/src/config.h`
|
||||
|
||||
### 5. Power Distribution
|
||||
### 3. ESP32-S3 BALANCE ↔ ESP32-S3 IO (Inter-Board UART)
|
||||
|
||||
| Signal | BALANCE GPIO | IO GPIO | Baud |
|
||||
|--------|-------------|---------|------|
|
||||
| TX | TBD | TBD (RX) | 460800 |
|
||||
| RX | TBD (RX) | TBD (TX) | 460800 |
|
||||
| GND | GND | GND | — |
|
||||
|
||||
Frame: `[0xAA][LEN][TYPE][PAYLOAD…][CRC8]` — see `esp32/shared/protocol.h`
|
||||
|
||||
### 4. ESP32-S3 IO ↔ TBS Crossfire RX (UART0)
|
||||
|
||||
| IO GPIO | Signal | Crossfire pin | Notes |
|
||||
|---------|--------|---------------|-------|
|
||||
| GPIO43 | TX | RX | CRSF telemetry to TX module |
|
||||
| GPIO44 | RX | TX | CRSF RC frames in |
|
||||
| GND | GND | GND | |
|
||||
| 5V | — | VCC | Power from 5V bus |
|
||||
|
||||
Baud: 420000 (CRSF). Failsafe: disarm after 300 ms without frame.
|
||||
|
||||
### 5. ESP32-S3 IO ↔ ELRS Receiver (UART2, failover)
|
||||
|
||||
| IO GPIO | Signal | ELRS pin |
|
||||
|---------|--------|----------|
|
||||
| TBD | TX | RX |
|
||||
| TBD | RX | TX |
|
||||
| GND | GND | GND |
|
||||
| 5V | — | VCC |
|
||||
|
||||
Baud: 420000 (CRSF). Activates automatically if Crossfire link lost >300 ms.
|
||||
|
||||
### 6. ESP32-S3 IO ↔ BTS7960 Motor Drivers (4×)
|
||||
|
||||
TBD GPIO assignments — see `esp32/io/src/config.h`.
|
||||
|
||||
| Signal | Per-driver | Notes |
|
||||
|--------|-----------|-------|
|
||||
| RPWM | GPIO TBD | Forward PWM |
|
||||
| LPWM | GPIO TBD | Reverse PWM |
|
||||
| R_EN | GPIO TBD | Enable H |
|
||||
| L_EN | GPIO TBD | Enable H |
|
||||
| Motor+ / Motor− | Hub motor | 36V via B+ / B− on BTS7960 |
|
||||
|
||||
### 7. ESP32-S3 IO I2C Sensors
|
||||
|
||||
| Device | I2C Address | Notes |
|
||||
|--------|-------------|-------|
|
||||
| NFC (PN532) | 0x24 | NFC tag read/write |
|
||||
| Barometer (BMP280/388) | 0x76 | Altitude + temp |
|
||||
| ToF range (VL53L0X) | 0x29 | Proximity/obstacle |
|
||||
|
||||
> SDA / SCL GPIOs TBD — confirm in `esp32/io/src/config.h`
|
||||
|
||||
### 8. Power Distribution
|
||||
|
||||
```
|
||||
BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
|
||||
├── VESC Right (36V direct -> BLDC right motor)
|
||||
36V BATTERY
|
||||
│
|
||||
├── 5V BEC/regulator ──┬── Orin (USB-C PD or barrel jack)
|
||||
│ ├── ESP32-S3 BALANCE (5V via USB-C)
|
||||
│ ├── ESP32-S3 IO (5V via USB-C)
|
||||
│ ├── WS2812B LEDs (5V)
|
||||
│ └── RPLIDAR (5V via USB)
|
||||
├── VESC left (36V) ─── Front-left + Rear-left hub motors
|
||||
├── VESC right (36V) ─── Front-right + Rear-right hub motors
|
||||
├── BTS7960 boards (B+/B−) — 36V motor power
|
||||
│
|
||||
└── Battery monitor ──── ESP32-S3 BALANCE ADC (voltage divider)
|
||||
├── DC-DC 12V ──── Fan / Headlight / Accessories
|
||||
│
|
||||
└── DC-DC 5V ─┬── Jetson Orin (USB-C PD)
|
||||
├── ESP32-S3 BALANCE (USB 5V)
|
||||
├── ESP32-S3 IO (USB 5V)
|
||||
├── TBS Crossfire RX (5V)
|
||||
├── ELRS RX (5V)
|
||||
├── WS2812B strip (5V)
|
||||
├── RPLIDAR A1M8 (5V via USB)
|
||||
└── Sensors (3.3V from ESP32-IO LDO)
|
||||
```
|
||||
|
||||
### 6. Sensors on Orin (USB/CSI)
|
||||
---
|
||||
|
||||
| Device | Interface | Orin Port | Device Node |
|
||||
|--------|-----------|-----------|-------------|
|
||||
| RealSense D435i | USB 3.1 | USB-A (blue) | `/dev/bus/usb/...` |
|
||||
| RPLIDAR A1M8 | USB-UART | USB-A | `/dev/rplidar` |
|
||||
| IMX219 front+left | MIPI CSI-2 | CSI-A (J5) | `/dev/video0,2` |
|
||||
| IMX219 rear+right | MIPI CSI-2 | CSI-B (J8) | `/dev/video4,6` |
|
||||
| 1TB NVMe | PCIe Gen3 x4 | M.2 Key M | `/dev/nvme0n1` |
|
||||
| CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
|
||||
## Orin USB Peripherals
|
||||
|
||||
| Device | Interface | Node |
|
||||
|--------|-----------|------|
|
||||
| CANable 2.0 | USB-A | `can0` (after `ip link set can0 up type can bitrate 500000`) |
|
||||
| RealSense D435i | USB 3.1 | `/dev/bus/usb/...` |
|
||||
| RPLIDAR A1M8 | USB-UART | `/dev/rplidar` |
|
||||
| SIM7600A 4G | USB | `/dev/ttyUSB0–2` |
|
||||
| ESP32-S3 BALANCE debug | USB-A (CH343) | `/dev/esp32-balance` |
|
||||
| ESP32-S3 IO debug | USB-A (JTAG/CDC) | `/dev/esp32-io` |
|
||||
|
||||
<<<<<<< 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)
|
||||
|
||||
|
||||
### 7. ReSpeaker 2-Mic HAT (on Orin 40-pin header)
|
||||
## ReSpeaker 2-Mic HAT (Orin 40-pin)
|
||||
|
||||
| Orin Pin | Signal | Function |
|
||||
|----------|--------|----------|
|
||||
@ -214,117 +176,20 @@ VESC Left CAN ID = 56 (0x38), VESC Right CAN ID = 68 (0x44).
|
||||
| Pin 35 (GPIO 19) | I2S LRCLK | Audio left/right clock |
|
||||
| Pin 38 (GPIO 20) | I2S DIN | Audio data in (from mics) |
|
||||
| Pin 40 (GPIO 21) | I2S DOUT | Audio data out (to speaker) |
|
||||
| Pin 3 (GPIO 2) | I2C SDA | WM8960 codec control (i2c-7) |
|
||||
| Pin 5 (GPIO 3) | I2C SCL | WM8960 codec control (i2c-7) |
|
||||
| Pin 32 (GPIO 12) | GPIO | Button input |
|
||||
| Pin 11 (GPIO 17) | GPIO | RGB LED (APA102 data) |
|
||||
| Pin 3 (GPIO 2) | I2C SDA | WM8960 codec (i2c-7) |
|
||||
| Pin 5 (GPIO 3) | I2C SCL | WM8960 codec (i2c-7) |
|
||||
| Pin 2, 4 | 5V | Power |
|
||||
| Pin 6, 9 | GND | Ground |
|
||||
|
||||
- Codec: Wolfson WM8960 (I2C addr 0x1A)
|
||||
- Mics: 2x MEMS (left + right) --- basic stereo / sound localization
|
||||
- Speaker: 3W class-D amp output (JST connector)
|
||||
- Headset: 3.5mm TRRS jack
|
||||
- Requires: WM8960 device tree overlay for Jetson (community port)
|
||||
- Use: Voice commands (faster-whisper), wake word (openWakeWord), audio feedback, status announcements
|
||||
|
||||
### 8. SIM7600A 4G/LTE HAT (via USB)
|
||||
|
||||
| Connection | Detail |
|
||||
|-----------|--------|
|
||||
| Interface | USB (micro-B on HAT -> USB-A/C on Orin) |
|
||||
| Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) |
|
||||
| Power | 5V from USB or separate 5V supply (peak 2A during TX) |
|
||||
| SIM | Nano-SIM slot on HAT |
|
||||
| Antenna | 4G LTE + GPS/GNSS (external SMA antennas --- mount high on chassis) |
|
||||
|
||||
- Data: PPP or QMI for internet connectivity
|
||||
- GPS/GNSS: Built-in receiver, NMEA sentences on ttyUSB2 --- outdoor positioning
|
||||
- AT commands: `AT+CGPS=1` (enable GPS), `AT+CGPSINFO` (get fix)
|
||||
- Connected via USB (not 40-pin) --- avoids UART conflict with BALANCE fallback, flexible antenna placement
|
||||
- Use: Remote telemetry, 4G connectivity outdoors, GPS positioning, remote SSH/control
|
||||
|
||||
### 9. Leap Motion Controller (USB)
|
||||
|
||||
| Connection | Detail |
|
||||
|-----------|--------|
|
||||
| Interface | USB 3.0 (micro-B on controller -> USB-A on Orin) |
|
||||
| Power | ~0.5W |
|
||||
| Range | ~80cm, 150 deg FOV |
|
||||
| SDK | Ultraleap Gemini V5+ (Linux ARM64 support) |
|
||||
| ROS2 | `leap_motion_ros2` wrapper available |
|
||||
|
||||
- 2x IR cameras + 3x IR LEDs --- tracks all 10 fingers in 3D, sub-mm precision
|
||||
- Mount: Forward-facing on sensor tower or upward on Orin plate
|
||||
- Use: Gesture control (palm=stop, point=go, fist=arm), hand-following mode, demos
|
||||
- Combined with ReSpeaker: Voice + gesture control with zero hardware in hand
|
||||
|
||||
### 10. Power Budget (USB)
|
||||
|
||||
| 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) |
|
||||
| RPLIDAR A1M8 | USB-A | ~2.6W (motor on) |
|
||||
| SIM7600A | USB | ~1W idle, 3W TX peak |
|
||||
| Leap Motion | USB-A | ~0.5W |
|
||||
| ReSpeaker HAT | 40-pin | ~0.5W |
|
||||
| **Total USB** | | **~7.9W typical, ~11W peak** |
|
||||
|
||||
Orin Nano Super delivers up to 25W --- USB peripherals are well within budget.
|
||||
Codec: Wolfson WM8960 (0x1A). Use: voice commands, wake word, audio feedback.
|
||||
|
||||
---
|
||||
|
||||
## Data Flow
|
||||
## SIM7600A 4G/LTE HAT (Orin USB)
|
||||
|
||||
```
|
||||
┌──────────────┐
|
||||
│ RC TX │ (in your hand)
|
||||
│ (2.4GHz) │
|
||||
└──────┬───────┘
|
||||
│ radio
|
||||
┌──────▼───────┐
|
||||
│ RC RX │ CRSF 420kbaud (future)
|
||||
└──────┬───────┘
|
||||
│ UART
|
||||
┌────────────▼────────────┐
|
||||
<<<<<<< HEAD
|
||||
│ ESP32-S3 BALANCE │
|
||||
│ (Waveshare LCD 1.28) │
|
||||
=======
|
||||
│ ESP32-S3 BALANCE │
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
│ │
|
||||
│ 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)
|
||||
│ │
|
||||
┌────┴────────────┐ ▼
|
||||
│ CAN bus (500k) │ ┌───────────────────┐
|
||||
├─ VESC Left 56 │ │ Orin Nano Super │
|
||||
└─ VESC Right 68 │ │ │
|
||||
│ │ │ SLAM / Nav2 / AI │
|
||||
▼ ▼ │ Person following │
|
||||
LEFT RIGHT │ Voice commands │
|
||||
MOTOR MOTOR │ 4G telemetry │
|
||||
└──┬──────────┬───────┘
|
||||
│ │
|
||||
┌──────────▼─┐ ┌────▼──────────┐
|
||||
│ ReSpeaker │ │ SIM7600A │
|
||||
│ 2-Mic HAT │ │ 4G/LTE + GPS │
|
||||
└────────────┘ └───────────────┘
|
||||
```
|
||||
| Connection | Detail |
|
||||
|-----------|--------|
|
||||
| Interface | USB (micro-B on HAT → USB-A on Orin) |
|
||||
| Device nodes | `/dev/ttyUSB0` (AT), `/dev/ttyUSB1` (PPP/data), `/dev/ttyUSB2` (GPS NMEA) |
|
||||
| Power | 5V from USB (peak 2A during TX) |
|
||||
| SIM | Nano-SIM slot |
|
||||
|
||||
@ -1,3 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
|
||||
project(esp32s3_balance)
|
||||
@ -1,23 +0,0 @@
|
||||
idf_component_register(
|
||||
SRCS
|
||||
"main.c"
|
||||
"orin_serial.c"
|
||||
"vesc_can.c"
|
||||
"gc9a01.c"
|
||||
"gitea_ota.c"
|
||||
"ota_self.c"
|
||||
"uart_ota.c"
|
||||
"ota_display.c"
|
||||
INCLUDE_DIRS "."
|
||||
REQUIRES
|
||||
esp_wifi
|
||||
esp_http_client
|
||||
esp_https_ota
|
||||
nvs_flash
|
||||
app_update
|
||||
mbedtls
|
||||
espressif__cjson
|
||||
driver
|
||||
freertos
|
||||
esp_timer
|
||||
)
|
||||
@ -1,50 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
/* ── ESP32-S3 BALANCE board — Waveshare ESP32-S3-Touch-LCD-1.28 pinout ─────
|
||||
*
|
||||
* Orin comms: CH343 USB-to-serial on UART0 (GPIO43/44) → /dev/ttyACM0 on Orin
|
||||
* VESC CAN: SN65HVD230 transceiver on GPIO15 (TX) / GPIO16 (RX)
|
||||
* Display: GC9A01 on SPI2 — BL=GPIO40, RST=GPIO12
|
||||
*
|
||||
* GPIO2 is NOT used for CAN — it is free. Earlier versions had an erroneous
|
||||
* conflict between DISP_BL (GPIO2) and VESC_CAN_TX (GPIO2); corrected here
|
||||
* to match motor-test-firmware verified hardware layout (commit 8e66430).
|
||||
*/
|
||||
|
||||
/* ── Orin serial (CH343 USB-to-UART, 1a86:55d3 on Orin side) ── */
|
||||
#define ORIN_UART_PORT UART_NUM_0
|
||||
#define ORIN_UART_BAUD 460800
|
||||
#define ORIN_UART_TX_GPIO 43 /* ESP32 UART0 TX → CH343 RXD */
|
||||
#define ORIN_UART_RX_GPIO 44 /* CH343 TXD → ESP32 UART0 RX */
|
||||
#define ORIN_UART_RX_BUF 1024
|
||||
#define ORIN_TX_QUEUE_DEPTH 16
|
||||
|
||||
/* ── Inter-board UART (ESP32 BALANCE ↔ ESP32 IO) ── */
|
||||
#define IO_UART_TX_GPIO 17
|
||||
#define IO_UART_RX_GPIO 18
|
||||
|
||||
/* ── VESC CAN TWAI (SN65HVD230 on Waveshare header GPIO15/16) ── */
|
||||
#define VESC_CAN_TX_GPIO 15 /* ESP32 TWAI TX → SN65HVD230 TXD */
|
||||
#define VESC_CAN_RX_GPIO 16 /* SN65HVD230 RXD → ESP32 TWAI RX */
|
||||
#define VESC_CAN_RX_QUEUE 32
|
||||
|
||||
/* VESC node IDs */
|
||||
#define VESC_ID_A 61u /* FRONT VESC — drive + telemetry (0x81) */
|
||||
#define VESC_ID_B 79u /* REAR VESC — telemetry only (0x82) */
|
||||
|
||||
/* ── GC9A01 240×240 round display (Waveshare ESP32-S3-Touch-LCD-1.28, SPI2) ── */
|
||||
#define DISP_DC_GPIO 8
|
||||
#define DISP_CS_GPIO 9
|
||||
#define DISP_SCK_GPIO 10
|
||||
#define DISP_MOSI_GPIO 11
|
||||
#define DISP_RST_GPIO 12
|
||||
#define DISP_BL_GPIO 40
|
||||
|
||||
/* ── Safety / timing ── */
|
||||
#define HB_TIMEOUT_MS 500u /* heartbeat watchdog: disarm if exceeded */
|
||||
#define DRIVE_TIMEOUT_MS 500u /* drive command staleness timeout */
|
||||
#define TELEM_STATUS_PERIOD_MS 100u /* 10 Hz status telemetry to Orin */
|
||||
#define TELEM_VESC_PERIOD_MS 100u /* 10 Hz VESC telemetry to Orin */
|
||||
|
||||
/* ── Drive → VESC RPM scaling ── */
|
||||
#define RPM_PER_SPEED_UNIT 5 /* speed_units=1000 → 5000 ERPM */
|
||||
@ -1,269 +0,0 @@
|
||||
/* gc9a01.c — GC9A01 240×240 round LCD SPI driver (bd-1yr8 display bead) */
|
||||
|
||||
#include "gc9a01.h"
|
||||
#include "config.h"
|
||||
#include "driver/spi_master.h"
|
||||
#include "driver/gpio.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_log.h"
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
static const char *TAG = "gc9a01";
|
||||
static spi_device_handle_t s_spi;
|
||||
|
||||
/* ── 5×7 bitmap font, one byte per column (bit0 = top row), ASCII 32..126 ── */
|
||||
static const uint8_t s_font[95][5] = {
|
||||
{0x00,0x00,0x00,0x00,0x00}, /* ' ' */ {0x00,0x00,0x5F,0x00,0x00}, /* '!' */
|
||||
{0x00,0x07,0x00,0x07,0x00}, /* '"' */ {0x14,0x7F,0x14,0x7F,0x14}, /* '#' */
|
||||
{0x24,0x2A,0x7F,0x2A,0x12}, /* '$' */ {0x23,0x13,0x08,0x64,0x62}, /* '%' */
|
||||
{0x36,0x49,0x55,0x22,0x50}, /* '&' */ {0x00,0x05,0x03,0x00,0x00}, /* '\'' */
|
||||
{0x00,0x1C,0x22,0x41,0x00}, /* '(' */ {0x00,0x41,0x22,0x1C,0x00}, /* ')' */
|
||||
{0x14,0x08,0x3E,0x08,0x14}, /* '*' */ {0x08,0x08,0x3E,0x08,0x08}, /* '+' */
|
||||
{0x00,0x50,0x30,0x00,0x00}, /* ',' */ {0x08,0x08,0x08,0x08,0x08}, /* '-' */
|
||||
{0x00,0x60,0x60,0x00,0x00}, /* '.' */ {0x20,0x10,0x08,0x04,0x02}, /* '/' */
|
||||
{0x3E,0x51,0x49,0x45,0x3E}, /* '0' */ {0x00,0x42,0x7F,0x40,0x00}, /* '1' */
|
||||
{0x42,0x61,0x51,0x49,0x46}, /* '2' */ {0x21,0x41,0x45,0x4B,0x31}, /* '3' */
|
||||
{0x18,0x14,0x12,0x7F,0x10}, /* '4' */ {0x27,0x45,0x45,0x45,0x39}, /* '5' */
|
||||
{0x3C,0x4A,0x49,0x49,0x30}, /* '6' */ {0x01,0x71,0x09,0x05,0x03}, /* '7' */
|
||||
{0x36,0x49,0x49,0x49,0x36}, /* '8' */ {0x06,0x49,0x49,0x29,0x1E}, /* '9' */
|
||||
{0x00,0x36,0x36,0x00,0x00}, /* ':' */ {0x00,0x56,0x36,0x00,0x00}, /* ';' */
|
||||
{0x08,0x14,0x22,0x41,0x00}, /* '<' */ {0x14,0x14,0x14,0x14,0x14}, /* '=' */
|
||||
{0x00,0x41,0x22,0x14,0x08}, /* '>' */ {0x02,0x01,0x51,0x09,0x06}, /* '?' */
|
||||
{0x32,0x49,0x79,0x41,0x3E}, /* '@' */ {0x7E,0x11,0x11,0x11,0x7E}, /* 'A' */
|
||||
{0x7F,0x49,0x49,0x49,0x36}, /* 'B' */ {0x3E,0x41,0x41,0x41,0x22}, /* 'C' */
|
||||
{0x7F,0x41,0x41,0x22,0x1C}, /* 'D' */ {0x7F,0x49,0x49,0x49,0x41}, /* 'E' */
|
||||
{0x7F,0x09,0x09,0x09,0x01}, /* 'F' */ {0x3E,0x41,0x49,0x49,0x3A}, /* 'G' */
|
||||
{0x7F,0x08,0x08,0x08,0x7F}, /* 'H' */ {0x00,0x41,0x7F,0x41,0x00}, /* 'I' */
|
||||
{0x20,0x40,0x41,0x3F,0x01}, /* 'J' */ {0x7F,0x08,0x14,0x22,0x41}, /* 'K' */
|
||||
{0x7F,0x40,0x40,0x40,0x40}, /* 'L' */ {0x7F,0x02,0x0C,0x02,0x7F}, /* 'M' */
|
||||
{0x7F,0x04,0x08,0x10,0x7F}, /* 'N' */ {0x3E,0x41,0x41,0x41,0x3E}, /* 'O' */
|
||||
{0x7F,0x09,0x09,0x09,0x06}, /* 'P' */ {0x3E,0x41,0x51,0x21,0x5E}, /* 'Q' */
|
||||
{0x7F,0x09,0x19,0x29,0x46}, /* 'R' */ {0x46,0x49,0x49,0x49,0x31}, /* 'S' */
|
||||
{0x01,0x01,0x7F,0x01,0x01}, /* 'T' */ {0x3F,0x40,0x40,0x40,0x3F}, /* 'U' */
|
||||
{0x1F,0x20,0x40,0x20,0x1F}, /* 'V' */ {0x3F,0x40,0x38,0x40,0x3F}, /* 'W' */
|
||||
{0x63,0x14,0x08,0x14,0x63}, /* 'X' */ {0x07,0x08,0x70,0x08,0x07}, /* 'Y' */
|
||||
{0x61,0x51,0x49,0x45,0x43}, /* 'Z' */ {0x00,0x7F,0x41,0x41,0x00}, /* '[' */
|
||||
{0x02,0x04,0x08,0x10,0x20}, /* '\\' */ {0x00,0x41,0x41,0x7F,0x00}, /* ']' */
|
||||
{0x04,0x02,0x01,0x02,0x04}, /* '^' */ {0x40,0x40,0x40,0x40,0x40}, /* '_' */
|
||||
{0x00,0x01,0x02,0x04,0x00}, /* '`' */ {0x20,0x54,0x54,0x54,0x78}, /* 'a' */
|
||||
{0x7F,0x48,0x44,0x44,0x38}, /* 'b' */ {0x38,0x44,0x44,0x44,0x20}, /* 'c' */
|
||||
{0x38,0x44,0x44,0x48,0x7F}, /* 'd' */ {0x38,0x54,0x54,0x54,0x18}, /* 'e' */
|
||||
{0x08,0x7E,0x09,0x01,0x02}, /* 'f' */ {0x0C,0x52,0x52,0x52,0x3E}, /* 'g' */
|
||||
{0x7F,0x08,0x04,0x04,0x78}, /* 'h' */ {0x00,0x44,0x7D,0x40,0x00}, /* 'i' */
|
||||
{0x20,0x40,0x44,0x3D,0x00}, /* 'j' */ {0x7F,0x10,0x28,0x44,0x00}, /* 'k' */
|
||||
{0x00,0x41,0x7F,0x40,0x00}, /* 'l' */ {0x7C,0x04,0x18,0x04,0x78}, /* 'm' */
|
||||
{0x7C,0x08,0x04,0x04,0x78}, /* 'n' */ {0x38,0x44,0x44,0x44,0x38}, /* 'o' */
|
||||
{0x7C,0x14,0x14,0x14,0x08}, /* 'p' */ {0x08,0x14,0x14,0x18,0x7C}, /* 'q' */
|
||||
{0x7C,0x08,0x04,0x04,0x08}, /* 'r' */ {0x48,0x54,0x54,0x54,0x20}, /* 's' */
|
||||
{0x04,0x3F,0x44,0x40,0x20}, /* 't' */ {0x3C,0x40,0x40,0x20,0x7C}, /* 'u' */
|
||||
{0x1C,0x20,0x40,0x20,0x1C}, /* 'v' */ {0x3C,0x40,0x30,0x40,0x3C}, /* 'w' */
|
||||
{0x44,0x28,0x10,0x28,0x44}, /* 'x' */ {0x0C,0x50,0x50,0x50,0x3C}, /* 'y' */
|
||||
{0x44,0x64,0x54,0x4C,0x44}, /* 'z' */ {0x00,0x08,0x36,0x41,0x00}, /* '{' */
|
||||
{0x00,0x00,0x7F,0x00,0x00}, /* '|' */ {0x00,0x41,0x36,0x08,0x00}, /* '}' */
|
||||
{0x10,0x08,0x08,0x10,0x08}, /* '~' */
|
||||
};
|
||||
|
||||
/* ── Static buffers (internal SRAM → DMA-safe) ── */
|
||||
static uint8_t s_line_buf[240 * 2];
|
||||
static uint8_t s_char_buf[5 * 5 * 7 * 5 * 2]; /* max scale=5: 25×35×2 */
|
||||
|
||||
/* ── Low-level SPI helpers ── */
|
||||
static void write_cmd(uint8_t cmd)
|
||||
{
|
||||
gpio_set_level(DISP_DC_GPIO, 0);
|
||||
spi_transaction_t t = { .length = 8, .flags = SPI_TRANS_USE_TXDATA };
|
||||
t.tx_data[0] = cmd;
|
||||
spi_device_polling_transmit(s_spi, &t);
|
||||
}
|
||||
|
||||
static void write_bytes(const uint8_t *data, size_t len)
|
||||
{
|
||||
if (!len) return;
|
||||
gpio_set_level(DISP_DC_GPIO, 1);
|
||||
spi_transaction_t t = { .length = len * 8, .tx_buffer = data };
|
||||
spi_device_polling_transmit(s_spi, &t);
|
||||
}
|
||||
|
||||
static inline void write_byte(uint8_t b) { write_bytes(&b, 1); }
|
||||
|
||||
/* ── Address window ── */
|
||||
static void set_window(int x1, int y1, int x2, int y2)
|
||||
{
|
||||
uint8_t d[4];
|
||||
d[0] = x1 >> 8; d[1] = x1 & 0xFF; d[2] = x2 >> 8; d[3] = x2 & 0xFF;
|
||||
write_cmd(0x2A); write_bytes(d, 4);
|
||||
d[0] = y1 >> 8; d[1] = y1 & 0xFF; d[2] = y2 >> 8; d[3] = y2 & 0xFF;
|
||||
write_cmd(0x2B); write_bytes(d, 4);
|
||||
}
|
||||
|
||||
/* ── GC9A01 register init ── */
|
||||
static void init_regs(void)
|
||||
{
|
||||
write_cmd(0xEF);
|
||||
write_cmd(0xEB); write_byte(0x14);
|
||||
write_cmd(0xFE);
|
||||
write_cmd(0xEF);
|
||||
write_cmd(0xEB); write_byte(0x14);
|
||||
write_cmd(0x84); write_byte(0x40);
|
||||
write_cmd(0x85); write_byte(0xFF);
|
||||
write_cmd(0x86); write_byte(0xFF);
|
||||
write_cmd(0x87); write_byte(0xFF);
|
||||
write_cmd(0x88); write_byte(0x0A);
|
||||
write_cmd(0x89); write_byte(0x21);
|
||||
write_cmd(0x8A); write_byte(0x00);
|
||||
write_cmd(0x8B); write_byte(0x80);
|
||||
write_cmd(0x8C); write_byte(0x01);
|
||||
write_cmd(0x8D); write_byte(0x01);
|
||||
write_cmd(0x8E); write_byte(0xFF);
|
||||
write_cmd(0x8F); write_byte(0xFF);
|
||||
{ uint8_t d[] = {0x00,0x20}; write_cmd(0xB6); write_bytes(d,2); }
|
||||
write_cmd(0x36); write_byte(0x08); /* MADCTL: normal, BGR */
|
||||
write_cmd(0x3A); write_byte(0x05); /* COLMOD: 16-bit RGB565 */
|
||||
{ uint8_t d[] = {0x08,0x08,0x08,0x08}; write_cmd(0x90); write_bytes(d,4); }
|
||||
write_cmd(0xBD); write_byte(0x06);
|
||||
write_cmd(0xBC); write_byte(0x00);
|
||||
{ uint8_t d[] = {0x60,0x01,0x04}; write_cmd(0xFF); write_bytes(d,3); }
|
||||
write_cmd(0xC3); write_byte(0x13);
|
||||
write_cmd(0xC4); write_byte(0x13);
|
||||
write_cmd(0xC9); write_byte(0x22);
|
||||
write_cmd(0xBE); write_byte(0x11);
|
||||
{ uint8_t d[] = {0x10,0x0E}; write_cmd(0xE1); write_bytes(d,2); }
|
||||
{ uint8_t d[] = {0x21,0x0C,0x02}; write_cmd(0xDF); write_bytes(d,3); }
|
||||
{ uint8_t d[] = {0x45,0x09,0x08,0x08,0x26,0x2A}; write_cmd(0xF0); write_bytes(d,6); }
|
||||
{ uint8_t d[] = {0x43,0x70,0x72,0x36,0x37,0x6F}; write_cmd(0xF1); write_bytes(d,6); }
|
||||
{ uint8_t d[] = {0x45,0x09,0x08,0x08,0x26,0x2A}; write_cmd(0xF2); write_bytes(d,6); }
|
||||
{ uint8_t d[] = {0x43,0x70,0x72,0x36,0x37,0x6F}; write_cmd(0xF3); write_bytes(d,6); }
|
||||
{ uint8_t d[] = {0x1B,0x0B}; write_cmd(0xED); write_bytes(d,2); }
|
||||
write_cmd(0xAE); write_byte(0x77);
|
||||
write_cmd(0xCD); write_byte(0x63);
|
||||
{ uint8_t d[] = {0x07,0x07,0x04,0x0E,0x0F,0x09,0x07,0x08,0x03};
|
||||
write_cmd(0x70); write_bytes(d,9); }
|
||||
write_cmd(0xE8); write_byte(0x34);
|
||||
{ uint8_t d[] = {0x18,0x0D,0xB7,0x18,0x0D,0x8B,0x88,0x08};
|
||||
write_cmd(0x62); write_bytes(d,8); }
|
||||
{ uint8_t d[] = {0x18,0x0D,0xB7,0x58,0x1E,0x0B,0x00,0xA7,0x88,0x08};
|
||||
write_cmd(0x63); write_bytes(d,10); }
|
||||
{ uint8_t d[] = {0x20,0x07,0x04}; write_cmd(0x64); write_bytes(d,3); }
|
||||
{ uint8_t d[] = {0x10,0x85,0x80,0x00,0x00,0x4E,0x00};
|
||||
write_cmd(0x74); write_bytes(d,7); }
|
||||
{ uint8_t d[] = {0x3E,0x07}; write_cmd(0x98); write_bytes(d,2); }
|
||||
write_cmd(0x35); /* TEON */
|
||||
write_cmd(0x21); /* INVON */
|
||||
write_cmd(0x11); /* SLPOUT */
|
||||
vTaskDelay(pdMS_TO_TICKS(120));
|
||||
write_cmd(0x29); /* DISPON */
|
||||
vTaskDelay(pdMS_TO_TICKS(20));
|
||||
}
|
||||
|
||||
/* ── Public init ── */
|
||||
void gc9a01_init(void)
|
||||
{
|
||||
/* SPI bus */
|
||||
spi_bus_config_t bus = {
|
||||
.mosi_io_num = DISP_MOSI_GPIO,
|
||||
.miso_io_num = -1,
|
||||
.sclk_io_num = DISP_SCK_GPIO,
|
||||
.quadwp_io_num = -1,
|
||||
.quadhd_io_num = -1,
|
||||
.max_transfer_sz = 4096,
|
||||
};
|
||||
ESP_ERROR_CHECK(spi_bus_initialize(SPI2_HOST, &bus, SPI_DMA_CH_AUTO));
|
||||
|
||||
spi_device_interface_config_t dev = {
|
||||
.clock_speed_hz = 40 * 1000 * 1000,
|
||||
.mode = 0,
|
||||
.spics_io_num = DISP_CS_GPIO,
|
||||
.queue_size = 1,
|
||||
};
|
||||
ESP_ERROR_CHECK(spi_bus_add_device(SPI2_HOST, &dev, &s_spi));
|
||||
|
||||
/* DC, RST, BL GPIOs */
|
||||
gpio_set_direction(DISP_DC_GPIO, GPIO_MODE_OUTPUT);
|
||||
gpio_set_direction(DISP_RST_GPIO, GPIO_MODE_OUTPUT);
|
||||
gpio_set_direction(DISP_BL_GPIO, GPIO_MODE_OUTPUT);
|
||||
|
||||
/* Hardware reset */
|
||||
gpio_set_level(DISP_RST_GPIO, 0); vTaskDelay(pdMS_TO_TICKS(10));
|
||||
gpio_set_level(DISP_RST_GPIO, 1); vTaskDelay(pdMS_TO_TICKS(120));
|
||||
|
||||
init_regs();
|
||||
|
||||
/* Backlight on */
|
||||
gpio_set_level(DISP_BL_GPIO, 1);
|
||||
ESP_LOGI(TAG, "GC9A01 init OK: DC=%d CS=%d SCK=%d MOSI=%d RST=%d BL=%d",
|
||||
DISP_DC_GPIO, DISP_CS_GPIO, DISP_SCK_GPIO, DISP_MOSI_GPIO,
|
||||
DISP_RST_GPIO, DISP_BL_GPIO);
|
||||
}
|
||||
|
||||
/* ── display_fill_rect ── */
|
||||
void display_fill_rect(int x, int y, int w, int h, uint16_t rgb565)
|
||||
{
|
||||
if (w <= 0 || h <= 0) return;
|
||||
if (x < 0) { w += x; x = 0; }
|
||||
if (y < 0) { h += y; y = 0; }
|
||||
if (x + w > 240) w = 240 - x;
|
||||
if (y + h > 240) h = 240 - y;
|
||||
if (w <= 0 || h <= 0) return;
|
||||
|
||||
set_window(x, y, x + w - 1, y + h - 1);
|
||||
write_cmd(0x2C);
|
||||
|
||||
uint8_t hi = rgb565 >> 8, lo = rgb565 & 0xFF;
|
||||
for (int i = 0; i < w * 2; i += 2) { s_line_buf[i] = hi; s_line_buf[i+1] = lo; }
|
||||
for (int row = 0; row < h; row++) { write_bytes(s_line_buf, (size_t)(w * 2)); }
|
||||
}
|
||||
|
||||
/* ── Glyph rasteriser (handles scale 1..5) ── */
|
||||
static void draw_char_s(int x, int y, char c, uint16_t fg, uint16_t bg, int scale)
|
||||
{
|
||||
if ((uint8_t)c < 32 || (uint8_t)c > 126) return;
|
||||
if (scale < 1) scale = 1;
|
||||
if (scale > 5) scale = 5;
|
||||
const uint8_t *g = s_font[(uint8_t)c - 32];
|
||||
int cw = 5 * scale, ch = 7 * scale;
|
||||
|
||||
uint8_t *p = s_char_buf;
|
||||
for (int row = 0; row < 7; row++) {
|
||||
for (int sr = 0; sr < scale; sr++) {
|
||||
for (int col = 0; col < 5; col++) {
|
||||
uint16_t color = ((g[col] >> row) & 1) ? fg : bg;
|
||||
uint8_t hi = color >> 8, lo = color & 0xFF;
|
||||
for (int sc = 0; sc < scale; sc++) { *p++ = hi; *p++ = lo; }
|
||||
}
|
||||
}
|
||||
}
|
||||
set_window(x, y, x + cw - 1, y + ch - 1);
|
||||
write_cmd(0x2C);
|
||||
write_bytes(s_char_buf, (size_t)(cw * ch * 2));
|
||||
}
|
||||
|
||||
/* ── display_draw_string / display_draw_string_s ── */
|
||||
void display_draw_string(int x, int y, const char *str, uint16_t fg, uint16_t bg)
|
||||
{
|
||||
display_draw_string_s(x, y, str, fg, bg, 1);
|
||||
}
|
||||
|
||||
void display_draw_string_s(int x, int y, const char *str,
|
||||
uint16_t fg, uint16_t bg, int scale)
|
||||
{
|
||||
int cx = x;
|
||||
while (*str) {
|
||||
draw_char_s(cx, y, *str++, fg, bg, scale);
|
||||
cx += 6 * scale;
|
||||
}
|
||||
}
|
||||
|
||||
/* ── display_draw_arc ── */
|
||||
void display_draw_arc(int cx, int cy, int r, int start_deg, int end_deg,
|
||||
int thickness, uint16_t color)
|
||||
{
|
||||
for (int deg = start_deg; deg <= end_deg; deg++) {
|
||||
float rad = (float)deg * (3.14159265f / 180.0f);
|
||||
int px = cx + (int)((float)r * cosf(rad));
|
||||
int py = cy + (int)((float)r * sinf(rad));
|
||||
int half = thickness / 2;
|
||||
display_fill_rect(px - half, py - half, thickness, thickness, color);
|
||||
}
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
#pragma once
|
||||
/* gc9a01.h — GC9A01 240×240 round LCD SPI driver (bd-1yr8 display bead) */
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* ── Initialise SPI bus + GC9A01. Call once from app_main. ── */
|
||||
void gc9a01_init(void);
|
||||
|
||||
/* ── Display primitives (also satisfy ota_display.h contract) ── */
|
||||
void display_fill_rect(int x, int y, int w, int h, uint16_t rgb565);
|
||||
void display_draw_string(int x, int y, const char *str, uint16_t fg, uint16_t bg);
|
||||
void display_draw_string_s(int x, int y, const char *str,
|
||||
uint16_t fg, uint16_t bg, int scale);
|
||||
void display_draw_arc(int cx, int cy, int r,
|
||||
int start_deg, int end_deg, int thickness, uint16_t color);
|
||||
|
||||
/* ── Colour palette (RGB565) ── */
|
||||
#define COL_BG 0x0000u
|
||||
#define COL_WHITE 0xFFFFu
|
||||
#define COL_GREEN 0x07E0u
|
||||
#define COL_YELLOW 0xFFE0u
|
||||
#define COL_RED 0xF800u
|
||||
#define COL_BLUE 0x001Fu
|
||||
#define COL_ORANGE 0xFD20u
|
||||
@ -1,285 +0,0 @@
|
||||
/* gitea_ota.c — Gitea version checker (bd-3hte)
|
||||
*
|
||||
* Uses esp_http_client + cJSON to query:
|
||||
* GET /api/v1/repos/{repo}/releases?limit=10
|
||||
* Filters releases by tag prefix, extracts version and download URLs.
|
||||
*/
|
||||
|
||||
#include "gitea_ota.h"
|
||||
#include "version.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_wifi.h"
|
||||
#include "esp_event.h"
|
||||
#include "esp_netif.h"
|
||||
#include "esp_http_client.h"
|
||||
#include "nvs_flash.h"
|
||||
#include "nvs.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "freertos/event_groups.h"
|
||||
#include "cJSON.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
static const char *TAG = "gitea_ota";
|
||||
|
||||
ota_update_info_t g_balance_update = {0};
|
||||
ota_update_info_t g_io_update = {0};
|
||||
|
||||
/* ── WiFi connection ── */
|
||||
#define WIFI_CONNECTED_BIT BIT0
|
||||
#define WIFI_FAIL_BIT BIT1
|
||||
#define WIFI_MAX_RETRIES 5
|
||||
|
||||
/* Compile-time WiFi fallback (override in NVS "wifi"/"ssid","pass") */
|
||||
#define DEFAULT_WIFI_SSID "saltylab"
|
||||
#define DEFAULT_WIFI_PASS ""
|
||||
|
||||
static EventGroupHandle_t s_wifi_eg;
|
||||
static int s_wifi_retries = 0;
|
||||
|
||||
static void wifi_event_handler(void *arg, esp_event_base_t base,
|
||||
int32_t id, void *data)
|
||||
{
|
||||
if (base == WIFI_EVENT && id == WIFI_EVENT_STA_START) {
|
||||
esp_wifi_connect();
|
||||
} else if (base == WIFI_EVENT && id == WIFI_EVENT_STA_DISCONNECTED) {
|
||||
if (s_wifi_retries < WIFI_MAX_RETRIES) {
|
||||
esp_wifi_connect();
|
||||
s_wifi_retries++;
|
||||
} else {
|
||||
xEventGroupSetBits(s_wifi_eg, WIFI_FAIL_BIT);
|
||||
}
|
||||
} else if (base == IP_EVENT && id == IP_EVENT_STA_GOT_IP) {
|
||||
s_wifi_retries = 0;
|
||||
xEventGroupSetBits(s_wifi_eg, WIFI_CONNECTED_BIT);
|
||||
}
|
||||
}
|
||||
|
||||
static bool wifi_connect(void)
|
||||
{
|
||||
char ssid[64] = DEFAULT_WIFI_SSID;
|
||||
char pass[64] = DEFAULT_WIFI_PASS;
|
||||
|
||||
/* Try to read credentials from NVS */
|
||||
nvs_handle_t nvs;
|
||||
if (nvs_open("wifi", NVS_READONLY, &nvs) == ESP_OK) {
|
||||
size_t sz = sizeof(ssid);
|
||||
nvs_get_str(nvs, "ssid", ssid, &sz);
|
||||
sz = sizeof(pass);
|
||||
nvs_get_str(nvs, "pass", pass, &sz);
|
||||
nvs_close(nvs);
|
||||
}
|
||||
|
||||
s_wifi_eg = xEventGroupCreate();
|
||||
s_wifi_retries = 0;
|
||||
|
||||
ESP_ERROR_CHECK(esp_netif_init());
|
||||
ESP_ERROR_CHECK(esp_event_loop_create_default());
|
||||
esp_netif_create_default_wifi_sta();
|
||||
|
||||
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
|
||||
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
|
||||
|
||||
esp_event_handler_instance_t h1, h2;
|
||||
ESP_ERROR_CHECK(esp_event_handler_instance_register(
|
||||
WIFI_EVENT, ESP_EVENT_ANY_ID, wifi_event_handler, NULL, &h1));
|
||||
ESP_ERROR_CHECK(esp_event_handler_instance_register(
|
||||
IP_EVENT, IP_EVENT_STA_GOT_IP, wifi_event_handler, NULL, &h2));
|
||||
|
||||
wifi_config_t wcfg = {0};
|
||||
strlcpy((char *)wcfg.sta.ssid, ssid, sizeof(wcfg.sta.ssid));
|
||||
strlcpy((char *)wcfg.sta.password, pass, sizeof(wcfg.sta.password));
|
||||
|
||||
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
|
||||
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wcfg));
|
||||
ESP_ERROR_CHECK(esp_wifi_start());
|
||||
|
||||
EventBits_t bits = xEventGroupWaitBits(s_wifi_eg,
|
||||
WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, pdFALSE, pdFALSE,
|
||||
pdMS_TO_TICKS(15000));
|
||||
|
||||
esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, h2);
|
||||
esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, h1);
|
||||
vEventGroupDelete(s_wifi_eg);
|
||||
|
||||
if (bits & WIFI_CONNECTED_BIT) {
|
||||
ESP_LOGI(TAG, "WiFi connected SSID=%s", ssid);
|
||||
return true;
|
||||
}
|
||||
ESP_LOGW(TAG, "WiFi connect failed SSID=%s", ssid);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ── HTTP fetch into a heap buffer ── */
|
||||
#define HTTP_RESP_MAX (8 * 1024)
|
||||
|
||||
typedef struct { char *buf; int len; int cap; } http_buf_t;
|
||||
|
||||
static esp_err_t http_event_cb(esp_http_client_event_t *evt)
|
||||
{
|
||||
http_buf_t *b = (http_buf_t *)evt->user_data;
|
||||
if (evt->event_id == HTTP_EVENT_ON_DATA && b) {
|
||||
if (b->len + evt->data_len < b->cap) {
|
||||
memcpy(b->buf + b->len, evt->data, evt->data_len);
|
||||
b->len += evt->data_len;
|
||||
}
|
||||
}
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static char *http_get(const char *url)
|
||||
{
|
||||
char *buf = malloc(HTTP_RESP_MAX);
|
||||
if (!buf) return NULL;
|
||||
http_buf_t b = {.buf = buf, .len = 0, .cap = HTTP_RESP_MAX};
|
||||
buf[0] = '\0';
|
||||
|
||||
esp_http_client_config_t cfg = {
|
||||
.url = url,
|
||||
.event_handler = http_event_cb,
|
||||
.user_data = &b,
|
||||
.timeout_ms = GITEA_API_TIMEOUT_MS,
|
||||
.skip_cert_common_name_check = true,
|
||||
};
|
||||
esp_http_client_handle_t client = esp_http_client_init(&cfg);
|
||||
esp_err_t err = esp_http_client_perform(client);
|
||||
int status = esp_http_client_get_status_code(client);
|
||||
esp_http_client_cleanup(client);
|
||||
|
||||
if (err != ESP_OK || status != 200) {
|
||||
ESP_LOGW(TAG, "HTTP GET %s → err=%d status=%d", url, err, status);
|
||||
free(buf);
|
||||
return NULL;
|
||||
}
|
||||
buf[b.len] = '\0';
|
||||
return buf;
|
||||
}
|
||||
|
||||
/* ── Version comparison: returns true if remote > local ── */
|
||||
static bool version_newer(const char *local, const char *remote)
|
||||
{
|
||||
int la=0,lb=0,lc=0, ra=0,rb=0,rc=0;
|
||||
sscanf(local, "%d.%d.%d", &la, &lb, &lc);
|
||||
sscanf(remote, "%d.%d.%d", &ra, &rb, &rc);
|
||||
if (ra != la) return ra > la;
|
||||
if (rb != lb) return rb > lb;
|
||||
return rc > lc;
|
||||
}
|
||||
|
||||
/* ── Parse releases JSON array, fill ota_update_info_t ── */
|
||||
static void parse_releases(const char *json, const char *tag_prefix,
|
||||
const char *bin_asset, const char *sha_asset,
|
||||
const char *local_version,
|
||||
ota_update_info_t *out)
|
||||
{
|
||||
cJSON *arr = cJSON_Parse(json);
|
||||
if (!arr || !cJSON_IsArray(arr)) {
|
||||
ESP_LOGW(TAG, "JSON parse failed");
|
||||
cJSON_Delete(arr);
|
||||
return;
|
||||
}
|
||||
|
||||
cJSON *rel;
|
||||
cJSON_ArrayForEach(rel, arr) {
|
||||
cJSON *tag_j = cJSON_GetObjectItem(rel, "tag_name");
|
||||
if (!cJSON_IsString(tag_j)) continue;
|
||||
|
||||
const char *tag = tag_j->valuestring;
|
||||
if (strncmp(tag, tag_prefix, strlen(tag_prefix)) != 0) continue;
|
||||
|
||||
/* Extract version after prefix */
|
||||
const char *ver = tag + strlen(tag_prefix);
|
||||
if (*ver == 'v') ver++; /* strip leading 'v' */
|
||||
|
||||
if (!version_newer(local_version, ver)) continue;
|
||||
|
||||
/* Found a newer release — extract asset URLs */
|
||||
cJSON *assets = cJSON_GetObjectItem(rel, "assets");
|
||||
if (!cJSON_IsArray(assets)) continue;
|
||||
|
||||
out->available = false;
|
||||
out->download_url[0] = '\0';
|
||||
out->sha256[0] = '\0';
|
||||
strlcpy(out->version, ver, sizeof(out->version));
|
||||
|
||||
cJSON *asset;
|
||||
cJSON_ArrayForEach(asset, assets) {
|
||||
cJSON *name_j = cJSON_GetObjectItem(asset, "name");
|
||||
cJSON *url_j = cJSON_GetObjectItem(asset, "browser_download_url");
|
||||
if (!cJSON_IsString(name_j) || !cJSON_IsString(url_j)) continue;
|
||||
|
||||
if (strcmp(name_j->valuestring, bin_asset) == 0) {
|
||||
strlcpy(out->download_url, url_j->valuestring,
|
||||
sizeof(out->download_url));
|
||||
out->available = true;
|
||||
} else if (strcmp(name_j->valuestring, sha_asset) == 0) {
|
||||
/* Download the SHA256 asset inline */
|
||||
char *sha = http_get(url_j->valuestring);
|
||||
if (sha) {
|
||||
/* sha file is just hex+newline */
|
||||
size_t n = strspn(sha, "0123456789abcdefABCDEF");
|
||||
if (n == 64) {
|
||||
memcpy(out->sha256, sha, 64);
|
||||
out->sha256[64] = '\0';
|
||||
}
|
||||
free(sha);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (out->available) {
|
||||
ESP_LOGI(TAG, "update: tag=%s ver=%s", tag, out->version);
|
||||
}
|
||||
break; /* use first matching release */
|
||||
}
|
||||
|
||||
cJSON_Delete(arr);
|
||||
}
|
||||
|
||||
/* ── Main check ── */
|
||||
void gitea_ota_check_now(void)
|
||||
{
|
||||
char url[512];
|
||||
snprintf(url, sizeof(url),
|
||||
"%s/api/v1/repos/%s/releases?limit=10",
|
||||
GITEA_BASE_URL, GITEA_REPO);
|
||||
|
||||
char *json = http_get(url);
|
||||
if (!json) {
|
||||
ESP_LOGW(TAG, "releases fetch failed");
|
||||
return;
|
||||
}
|
||||
|
||||
parse_releases(json, BALANCE_TAG_PREFIX, BALANCE_BIN_ASSET,
|
||||
BALANCE_SHA256_ASSET, BALANCE_FW_VERSION, &g_balance_update);
|
||||
parse_releases(json, IO_TAG_PREFIX, IO_BIN_ASSET,
|
||||
IO_SHA256_ASSET, IO_FW_VERSION, &g_io_update);
|
||||
free(json);
|
||||
}
|
||||
|
||||
/* ── Background task ── */
|
||||
static void version_check_task(void *arg)
|
||||
{
|
||||
/* Initial check immediately after WiFi up */
|
||||
vTaskDelay(pdMS_TO_TICKS(2000));
|
||||
gitea_ota_check_now();
|
||||
|
||||
for (;;) {
|
||||
vTaskDelay(pdMS_TO_TICKS(VERSION_CHECK_PERIOD_MS));
|
||||
gitea_ota_check_now();
|
||||
}
|
||||
}
|
||||
|
||||
void gitea_ota_init(void)
|
||||
{
|
||||
ESP_ERROR_CHECK(nvs_flash_init());
|
||||
|
||||
if (!wifi_connect()) {
|
||||
ESP_LOGW(TAG, "WiFi unavailable — version checks disabled");
|
||||
return;
|
||||
}
|
||||
|
||||
xTaskCreate(version_check_task, "ver_check", 6144, NULL, 3, NULL);
|
||||
ESP_LOGI(TAG, "version check task started");
|
||||
}
|
||||
@ -1,42 +0,0 @@
|
||||
#pragma once
|
||||
/* gitea_ota.h — Gitea release version checker (bd-3hte)
|
||||
*
|
||||
* WiFi task: on boot and every 30 min, queries Gitea releases API,
|
||||
* compares tag version against embedded FW_VERSION, stores update info.
|
||||
*
|
||||
* WiFi credentials read from NVS namespace "wifi" keys "ssid"/"pass".
|
||||
* Fall back to compile-time defaults if NVS is empty.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* Gitea instance */
|
||||
#define GITEA_BASE_URL "http://gitea.vayrette.com"
|
||||
#define GITEA_REPO "seb/saltylab-firmware"
|
||||
#define GITEA_API_TIMEOUT_MS 10000
|
||||
|
||||
/* Version check interval */
|
||||
#define VERSION_CHECK_PERIOD_MS (30u * 60u * 1000u) /* 30 minutes */
|
||||
|
||||
/* Max URL/version string lengths */
|
||||
#define OTA_URL_MAX 384
|
||||
#define OTA_VER_MAX 32
|
||||
#define OTA_SHA256_MAX 65
|
||||
|
||||
typedef struct {
|
||||
bool available;
|
||||
char version[OTA_VER_MAX]; /* remote version string, e.g. "1.2.3" */
|
||||
char download_url[OTA_URL_MAX]; /* direct download URL for .bin */
|
||||
char sha256[OTA_SHA256_MAX]; /* hex SHA256 (from .sha256 asset), or "" */
|
||||
} ota_update_info_t;
|
||||
|
||||
/* Shared state — written by gitea_ota_check_task, read by display/OTA tasks */
|
||||
extern ota_update_info_t g_balance_update;
|
||||
extern ota_update_info_t g_io_update;
|
||||
|
||||
/* Initialize WiFi and start version check task */
|
||||
void gitea_ota_init(void);
|
||||
|
||||
/* One-shot sync check (can be called from any task) */
|
||||
void gitea_ota_check_now(void);
|
||||
@ -1,5 +0,0 @@
|
||||
dependencies:
|
||||
idf:
|
||||
version: '>=5.0'
|
||||
espressif/cjson:
|
||||
version: "^1.7.19~2"
|
||||
@ -1,110 +0,0 @@
|
||||
/* main.c — ESP32-S3 BALANCE app_main (bd-66hx + OTA beads) */
|
||||
|
||||
#include "orin_serial.h"
|
||||
#include "vesc_can.h"
|
||||
#include "gc9a01.h"
|
||||
#include "gitea_ota.h"
|
||||
#include "ota_self.h"
|
||||
#include "uart_ota.h"
|
||||
#include "ota_display.h"
|
||||
#include "config.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "freertos/queue.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include <string.h>
|
||||
|
||||
static const char *TAG = "main";
|
||||
|
||||
static QueueHandle_t s_orin_tx_q;
|
||||
|
||||
/* ── Telemetry task: sends TELEM_STATUS to Orin at 10 Hz ── */
|
||||
static void telem_task(void *arg)
|
||||
{
|
||||
for (;;) {
|
||||
vTaskDelay(pdMS_TO_TICKS(TELEM_STATUS_PERIOD_MS));
|
||||
|
||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
||||
bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
|
||||
|
||||
/* Determine balance state for telemetry */
|
||||
bal_state_t state;
|
||||
if (g_orin_ctrl.estop) {
|
||||
state = BAL_ESTOP;
|
||||
} else if (!g_orin_ctrl.armed) {
|
||||
state = BAL_DISARMED;
|
||||
} else {
|
||||
state = BAL_ARMED;
|
||||
}
|
||||
|
||||
/* flags: bit0=estop_active, bit1=heartbeat_timeout */
|
||||
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
|
||||
(hb_timeout ? 0x02u : 0x00u);
|
||||
|
||||
/* Battery voltage from VESC_ID_A STATUS_5 (V×10 → mV) */
|
||||
uint16_t vbat_mv = (uint16_t)((int32_t)g_vesc[0].voltage_x10 * 100);
|
||||
|
||||
orin_send_status(s_orin_tx_q,
|
||||
0, /* pitch_x10: stub — full IMU in future bead */
|
||||
0, /* motor_cmd: stub */
|
||||
vbat_mv,
|
||||
state,
|
||||
flags);
|
||||
}
|
||||
}
|
||||
|
||||
/* ── Drive task: applies Orin drive commands to VESCs @ 50 Hz ── */
|
||||
static void drive_task(void *arg)
|
||||
{
|
||||
for (;;) {
|
||||
vTaskDelay(pdMS_TO_TICKS(20)); /* 50 Hz */
|
||||
|
||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
||||
bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
|
||||
bool drive_stale = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS;
|
||||
|
||||
int32_t front_erpm = 0;
|
||||
|
||||
if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
|
||||
!hb_timeout && !drive_stale) {
|
||||
front_erpm = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
|
||||
}
|
||||
|
||||
vesc_can_send_rpm(VESC_ID_A, front_erpm);
|
||||
}
|
||||
}
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
ESP_LOGI(TAG, "ESP32-S3 BALANCE starting");
|
||||
|
||||
/* OTA rollback health check — must be called within OTA_ROLLBACK_WINDOW_S */
|
||||
ota_self_health_check();
|
||||
|
||||
/* Init peripherals — gc9a01 before vesc_can so BL/GPIO2 is high before TWAI takes it */
|
||||
gc9a01_init();
|
||||
orin_serial_init();
|
||||
vesc_can_init();
|
||||
|
||||
/* TX queue for outbound serial frames */
|
||||
s_orin_tx_q = xQueueCreate(ORIN_TX_QUEUE_DEPTH, sizeof(orin_tx_frame_t));
|
||||
configASSERT(s_orin_tx_q);
|
||||
|
||||
/* Seed heartbeat timer so we don't immediately timeout */
|
||||
g_orin_ctrl.hb_last_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
||||
|
||||
/* Create tasks */
|
||||
xTaskCreate(orin_serial_rx_task, "orin_rx", 4096, s_orin_tx_q, 10, NULL);
|
||||
xTaskCreate(orin_serial_tx_task, "orin_tx", 2048, s_orin_tx_q, 9, NULL);
|
||||
xTaskCreate(vesc_can_rx_task, "vesc_rx", 4096, s_orin_tx_q, 10, NULL);
|
||||
xTaskCreate(telem_task, "telem", 2048, NULL, 5, NULL);
|
||||
xTaskCreate(drive_task, "drive", 2048, NULL, 8, NULL);
|
||||
|
||||
/* OTA subsystem — WiFi version checker + display overlay */
|
||||
gitea_ota_init();
|
||||
ota_display_init();
|
||||
|
||||
ESP_LOGI(TAG, "all tasks started");
|
||||
/* app_main returns — FreeRTOS scheduler continues */
|
||||
}
|
||||
@ -1,334 +0,0 @@
|
||||
/* orin_serial.c — Orin↔ESP32-S3 serial protocol (bd-66hx + bd-1s1s OTA cmds) */
|
||||
|
||||
#include "orin_serial.h"
|
||||
#include "config.h"
|
||||
#include "gitea_ota.h"
|
||||
#include "ota_self.h"
|
||||
#include "uart_ota.h"
|
||||
#include "version.h"
|
||||
#include "driver/uart.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/queue.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
static const char *TAG = "orin";
|
||||
|
||||
/* ── Shared state ── */
|
||||
orin_drive_t g_orin_drive = {0};
|
||||
orin_control_t g_orin_ctrl = {.armed = false, .estop = false, .hb_last_ms = 0};
|
||||
|
||||
/* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */
|
||||
static uint8_t crc8(const uint8_t *data, uint8_t len)
|
||||
{
|
||||
uint8_t crc = 0x00u;
|
||||
for (uint8_t i = 0; i < len; i++) {
|
||||
crc ^= data[i];
|
||||
for (uint8_t b = 0; b < 8u; b++) {
|
||||
crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
/* ── Frame builder ── */
|
||||
static void build_frame(orin_tx_frame_t *f, uint8_t out[/* ORIN_MAX_PAYLOAD + 4 */], uint8_t *out_len)
|
||||
{
|
||||
/* [SYNC][LEN][TYPE][PAYLOAD...][CRC] */
|
||||
uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
|
||||
crc_buf[0] = f->len;
|
||||
crc_buf[1] = f->type;
|
||||
memcpy(&crc_buf[2], f->payload, f->len);
|
||||
uint8_t crc = crc8(crc_buf, (uint8_t)(2u + f->len));
|
||||
|
||||
out[0] = ORIN_SYNC;
|
||||
out[1] = f->len;
|
||||
out[2] = f->type;
|
||||
memcpy(&out[3], f->payload, f->len);
|
||||
out[3u + f->len] = crc;
|
||||
*out_len = (uint8_t)(4u + f->len);
|
||||
}
|
||||
|
||||
/* ── Enqueue helpers ── */
|
||||
static void enqueue(QueueHandle_t q, uint8_t type, const uint8_t *payload, uint8_t len)
|
||||
{
|
||||
orin_tx_frame_t f = {.type = type, .len = len};
|
||||
if (len > 0u && payload) {
|
||||
memcpy(f.payload, payload, len);
|
||||
}
|
||||
if (xQueueSend(q, &f, 0) != pdTRUE) {
|
||||
ESP_LOGW(TAG, "tx queue full, dropped type=0x%02x", type);
|
||||
}
|
||||
}
|
||||
|
||||
void orin_send_ack(QueueHandle_t q, uint8_t cmd_type)
|
||||
{
|
||||
enqueue(q, RESP_ACK, &cmd_type, 1u);
|
||||
}
|
||||
|
||||
void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err)
|
||||
{
|
||||
uint8_t p[2] = {cmd_type, err};
|
||||
enqueue(q, RESP_NACK, p, 2u);
|
||||
}
|
||||
|
||||
void orin_send_status(QueueHandle_t q,
|
||||
int16_t pitch_x10, int16_t motor_cmd,
|
||||
uint16_t vbat_mv, bal_state_t state, uint8_t flags)
|
||||
{
|
||||
/* int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags — BE */
|
||||
uint8_t p[8];
|
||||
p[0] = (uint8_t)((uint16_t)pitch_x10 >> 8u);
|
||||
p[1] = (uint8_t)((uint16_t)pitch_x10);
|
||||
p[2] = (uint8_t)((uint16_t)motor_cmd >> 8u);
|
||||
p[3] = (uint8_t)((uint16_t)motor_cmd);
|
||||
p[4] = (uint8_t)(vbat_mv >> 8u);
|
||||
p[5] = (uint8_t)(vbat_mv);
|
||||
p[6] = (uint8_t)state;
|
||||
p[7] = flags;
|
||||
enqueue(q, TELEM_STATUS, p, 8u);
|
||||
}
|
||||
|
||||
void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
|
||||
int32_t erpm, uint16_t voltage_mv,
|
||||
int16_t current_ma, uint16_t temp_c_x10)
|
||||
{
|
||||
/* int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10 — BE */
|
||||
uint8_t p[10];
|
||||
uint32_t u = (uint32_t)erpm;
|
||||
p[0] = (uint8_t)(u >> 24u);
|
||||
p[1] = (uint8_t)(u >> 16u);
|
||||
p[2] = (uint8_t)(u >> 8u);
|
||||
p[3] = (uint8_t)(u);
|
||||
p[4] = (uint8_t)(voltage_mv >> 8u);
|
||||
p[5] = (uint8_t)(voltage_mv);
|
||||
p[6] = (uint8_t)((uint16_t)current_ma >> 8u);
|
||||
p[7] = (uint8_t)((uint16_t)current_ma);
|
||||
p[8] = (uint8_t)(temp_c_x10 >> 8u);
|
||||
p[9] = (uint8_t)(temp_c_x10);
|
||||
enqueue(q, telem_type, p, 10u);
|
||||
}
|
||||
|
||||
/* ── UART init ── */
|
||||
void orin_serial_init(void)
|
||||
{
|
||||
uart_config_t cfg = {
|
||||
.baud_rate = ORIN_UART_BAUD,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
};
|
||||
ESP_ERROR_CHECK(uart_param_config(ORIN_UART_PORT, &cfg));
|
||||
ESP_ERROR_CHECK(uart_set_pin(ORIN_UART_PORT,
|
||||
ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO,
|
||||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
||||
ESP_ERROR_CHECK(uart_driver_install(ORIN_UART_PORT, ORIN_UART_RX_BUF, 0,
|
||||
0, NULL, 0));
|
||||
ESP_LOGI(TAG, "UART%d init OK: tx=%d rx=%d baud=%d",
|
||||
ORIN_UART_PORT, ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO, ORIN_UART_BAUD);
|
||||
}
|
||||
|
||||
/* ── RX parser state machine ── */
|
||||
typedef enum {
|
||||
WAIT_SYNC,
|
||||
WAIT_LEN,
|
||||
WAIT_TYPE,
|
||||
WAIT_PAYLOAD,
|
||||
WAIT_CRC,
|
||||
} rx_state_t;
|
||||
|
||||
static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len,
|
||||
QueueHandle_t tx_q)
|
||||
{
|
||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
||||
|
||||
switch (type) {
|
||||
case CMD_HEARTBEAT:
|
||||
g_orin_ctrl.hb_last_ms = now_ms;
|
||||
orin_send_ack(tx_q, type);
|
||||
break;
|
||||
|
||||
case CMD_DRIVE:
|
||||
if (len < 4u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
||||
if (g_orin_ctrl.estop) { orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE); break; }
|
||||
if (!g_orin_ctrl.armed) { orin_send_nack(tx_q, type, ERR_DISARMED); break; }
|
||||
g_orin_drive.speed = (int16_t)(((uint16_t)payload[0] << 8u) | payload[1]);
|
||||
g_orin_drive.steer = (int16_t)(((uint16_t)payload[2] << 8u) | payload[3]);
|
||||
g_orin_drive.updated_ms = now_ms;
|
||||
g_orin_ctrl.hb_last_ms = now_ms; /* drive counts as heartbeat */
|
||||
orin_send_ack(tx_q, type);
|
||||
break;
|
||||
|
||||
case CMD_ESTOP:
|
||||
if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
||||
g_orin_ctrl.estop = (payload[0] != 0u);
|
||||
if (g_orin_ctrl.estop) {
|
||||
g_orin_drive.speed = 0;
|
||||
g_orin_drive.steer = 0;
|
||||
}
|
||||
orin_send_ack(tx_q, type);
|
||||
break;
|
||||
|
||||
case CMD_ARM:
|
||||
if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
||||
if (g_orin_ctrl.estop && payload[0] != 0u) {
|
||||
/* cannot arm while estop is active */
|
||||
orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE);
|
||||
break;
|
||||
}
|
||||
g_orin_ctrl.armed = (payload[0] != 0u);
|
||||
if (!g_orin_ctrl.armed) {
|
||||
g_orin_drive.speed = 0;
|
||||
g_orin_drive.steer = 0;
|
||||
}
|
||||
orin_send_ack(tx_q, type);
|
||||
break;
|
||||
|
||||
case CMD_OTA_CHECK:
|
||||
/* Trigger an immediate Gitea version check */
|
||||
gitea_ota_check_now();
|
||||
orin_send_version_info(tx_q, OTA_TARGET_BALANCE,
|
||||
BALANCE_FW_VERSION,
|
||||
g_balance_update.available
|
||||
? g_balance_update.version : "");
|
||||
orin_send_version_info(tx_q, OTA_TARGET_IO,
|
||||
IO_FW_VERSION,
|
||||
g_io_update.available
|
||||
? g_io_update.version : "");
|
||||
orin_send_ack(tx_q, type);
|
||||
break;
|
||||
|
||||
case CMD_OTA_UPDATE:
|
||||
if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
||||
{
|
||||
uint8_t target = payload[0];
|
||||
bool triggered = false;
|
||||
if (target == OTA_TARGET_IO || target == OTA_TARGET_BOTH) {
|
||||
if (!uart_ota_trigger()) {
|
||||
orin_send_nack(tx_q, type,
|
||||
g_io_update.available ? ERR_OTA_BUSY : ERR_OTA_NO_UPDATE);
|
||||
break;
|
||||
}
|
||||
triggered = true;
|
||||
}
|
||||
if (target == OTA_TARGET_BALANCE || target == OTA_TARGET_BOTH) {
|
||||
if (!ota_self_trigger()) {
|
||||
if (!triggered) {
|
||||
orin_send_nack(tx_q, type,
|
||||
g_balance_update.available ? ERR_OTA_BUSY : ERR_OTA_NO_UPDATE);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
orin_send_ack(tx_q, type);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
ESP_LOGW(TAG, "unknown cmd type=0x%02x", type);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void orin_serial_rx_task(void *arg)
|
||||
{
|
||||
QueueHandle_t tx_q = (QueueHandle_t)arg;
|
||||
rx_state_t state = WAIT_SYNC;
|
||||
uint8_t rx_len = 0;
|
||||
uint8_t rx_type = 0;
|
||||
uint8_t payload[ORIN_MAX_PAYLOAD];
|
||||
uint8_t pay_idx = 0;
|
||||
|
||||
uint8_t byte;
|
||||
for (;;) {
|
||||
int r = uart_read_bytes(ORIN_UART_PORT, &byte, 1, pdMS_TO_TICKS(10));
|
||||
if (r <= 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case WAIT_SYNC:
|
||||
if (byte == ORIN_SYNC) { state = WAIT_LEN; }
|
||||
break;
|
||||
|
||||
case WAIT_LEN:
|
||||
if (byte > ORIN_MAX_PAYLOAD) {
|
||||
/* oversize — send NACK and reset */
|
||||
orin_send_nack(tx_q, 0x00u, ERR_BAD_LEN);
|
||||
state = WAIT_SYNC;
|
||||
} else {
|
||||
rx_len = byte;
|
||||
state = WAIT_TYPE;
|
||||
}
|
||||
break;
|
||||
|
||||
case WAIT_TYPE:
|
||||
rx_type = byte;
|
||||
pay_idx = 0u;
|
||||
state = (rx_len == 0u) ? WAIT_CRC : WAIT_PAYLOAD;
|
||||
break;
|
||||
|
||||
case WAIT_PAYLOAD:
|
||||
payload[pay_idx++] = byte;
|
||||
if (pay_idx == rx_len) { state = WAIT_CRC; }
|
||||
break;
|
||||
|
||||
case WAIT_CRC: {
|
||||
/* Verify CRC over [LEN, TYPE, PAYLOAD] */
|
||||
uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
|
||||
crc_buf[0] = rx_len;
|
||||
crc_buf[1] = rx_type;
|
||||
memcpy(&crc_buf[2], payload, rx_len);
|
||||
uint8_t expected = crc8(crc_buf, (uint8_t)(2u + rx_len));
|
||||
if (byte != expected) {
|
||||
ESP_LOGW(TAG, "CRC fail type=0x%02x got=0x%02x exp=0x%02x",
|
||||
rx_type, byte, expected);
|
||||
orin_send_nack(tx_q, rx_type, ERR_BAD_CRC);
|
||||
} else {
|
||||
dispatch_cmd(rx_type, payload, rx_len, tx_q);
|
||||
}
|
||||
state = WAIT_SYNC;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void orin_serial_tx_task(void *arg)
|
||||
{
|
||||
QueueHandle_t tx_q = (QueueHandle_t)arg;
|
||||
orin_tx_frame_t f;
|
||||
uint8_t wire[4u + ORIN_MAX_PAYLOAD];
|
||||
uint8_t wire_len;
|
||||
|
||||
for (;;) {
|
||||
if (xQueueReceive(tx_q, &f, portMAX_DELAY) == pdTRUE) {
|
||||
build_frame(&f, wire, &wire_len);
|
||||
uart_write_bytes(ORIN_UART_PORT, (const char *)wire, wire_len);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ── OTA telemetry helpers (bd-1s1s) ── */
|
||||
|
||||
void orin_send_ota_status(QueueHandle_t q, uint8_t target,
|
||||
uint8_t state, uint8_t progress, uint8_t err)
|
||||
{
|
||||
/* TELEM_OTA_STATUS: uint8 target, uint8 state, uint8 progress, uint8 err */
|
||||
uint8_t p[4] = {target, state, progress, err};
|
||||
enqueue(q, TELEM_OTA_STATUS, p, 4u);
|
||||
}
|
||||
|
||||
void orin_send_version_info(QueueHandle_t q, uint8_t target,
|
||||
const char *current, const char *available)
|
||||
{
|
||||
/* TELEM_VERSION_INFO: uint8 target, char current[16], char available[16] */
|
||||
uint8_t p[33];
|
||||
p[0] = target;
|
||||
strncpy((char *)&p[1], current, 16); p[16] = '\0';
|
||||
strncpy((char *)&p[17], available ? available : "", 16); p[32] = '\0';
|
||||
enqueue(q, TELEM_VERSION_INFO, p, 33u);
|
||||
}
|
||||
@ -1,105 +0,0 @@
|
||||
#pragma once
|
||||
/* orin_serial.h — Orin↔ESP32-S3 BALANCE USB/UART serial protocol (bd-66hx)
|
||||
*
|
||||
* Frame layout (matches bd-wim1 esp32_balance_protocol.py exactly):
|
||||
* [0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8-SMBUS]
|
||||
* CRC covers LEN + TYPE + PAYLOAD bytes.
|
||||
* All multi-byte payload fields are big-endian.
|
||||
*
|
||||
* Physical: UART0 → CH343 USB-serial → Orin /dev/esp32-balance @ 460800 baud
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/queue.h"
|
||||
|
||||
/* ── Frame constants ── */
|
||||
#define ORIN_SYNC 0xAAu
|
||||
#define ORIN_MAX_PAYLOAD 62u
|
||||
|
||||
/* ── Command types: Orin → ESP32 ── */
|
||||
#define CMD_HEARTBEAT 0x01u
|
||||
#define CMD_DRIVE 0x02u /* int16 speed + int16 steer, BE */
|
||||
#define CMD_ESTOP 0x03u /* uint8: 1=assert, 0=clear */
|
||||
#define CMD_ARM 0x04u /* uint8: 1=arm, 0=disarm */
|
||||
|
||||
/* ── Telemetry types: ESP32 → Orin ── */
|
||||
#define TELEM_STATUS 0x80u /* status @ 10 Hz */
|
||||
#define TELEM_VESC_LEFT 0x81u /* VESC ID 61 (front) telemetry @ 10 Hz */
|
||||
#define TELEM_VESC_RIGHT 0x82u /* VESC ID 79 (rear) telemetry @ 10 Hz */
|
||||
#define TELEM_OTA_STATUS 0x83u /* OTA state + progress (bd-1s1s) */
|
||||
#define TELEM_VERSION_INFO 0x84u /* firmware version report (bd-1s1s) */
|
||||
#define RESP_ACK 0xA0u
|
||||
#define RESP_NACK 0xA1u
|
||||
|
||||
/* ── OTA commands (Orin → ESP32, bd-1s1s) ── */
|
||||
#define CMD_OTA_CHECK 0x10u /* no payload: trigger Gitea version check */
|
||||
#define CMD_OTA_UPDATE 0x11u /* uint8 target: 0=balance, 1=io, 2=both */
|
||||
|
||||
/* ── OTA target constants ── */
|
||||
#define OTA_TARGET_BALANCE 0x00u
|
||||
#define OTA_TARGET_IO 0x01u
|
||||
#define OTA_TARGET_BOTH 0x02u
|
||||
|
||||
/* ── NACK error codes ── */
|
||||
#define ERR_BAD_CRC 0x01u
|
||||
#define ERR_BAD_LEN 0x02u
|
||||
#define ERR_ESTOP_ACTIVE 0x03u
|
||||
#define ERR_DISARMED 0x04u
|
||||
#define ERR_OTA_BUSY 0x05u
|
||||
#define ERR_OTA_NO_UPDATE 0x06u
|
||||
|
||||
/* ── Drive state (mirrored from TELEM_STATUS.balance_state) ── */
|
||||
typedef enum {
|
||||
BAL_DISARMED = 0,
|
||||
BAL_ARMED = 1,
|
||||
BAL_ESTOP = 3,
|
||||
} bal_state_t;
|
||||
|
||||
/* ── Shared state written by RX task, consumed by main/vesc tasks ── */
|
||||
typedef struct {
|
||||
volatile int16_t speed; /* -1000..+1000 */
|
||||
volatile int16_t steer; /* -1000..+1000 */
|
||||
volatile uint32_t updated_ms; /* esp_timer tick at last CMD_DRIVE */
|
||||
} orin_drive_t;
|
||||
|
||||
typedef struct {
|
||||
volatile bool armed;
|
||||
volatile bool estop;
|
||||
volatile uint32_t hb_last_ms; /* esp_timer tick at last CMD_HEARTBEAT/CMD_DRIVE */
|
||||
} orin_control_t;
|
||||
|
||||
/* ── TX frame queue item ── */
|
||||
typedef struct {
|
||||
uint8_t type;
|
||||
uint8_t len;
|
||||
uint8_t payload[ORIN_MAX_PAYLOAD];
|
||||
} orin_tx_frame_t;
|
||||
|
||||
/* ── Globals (defined in orin_serial.c, extern here) ── */
|
||||
extern orin_drive_t g_orin_drive;
|
||||
extern orin_control_t g_orin_ctrl;
|
||||
|
||||
/* ── API ── */
|
||||
void orin_serial_init(void);
|
||||
|
||||
/* Tasks — pass tx_queue as arg to both */
|
||||
void orin_serial_rx_task(void *arg); /* arg = QueueHandle_t tx_queue */
|
||||
void orin_serial_tx_task(void *arg); /* arg = QueueHandle_t tx_queue */
|
||||
|
||||
/* Enqueue outbound frames */
|
||||
void orin_send_status(QueueHandle_t q,
|
||||
int16_t pitch_x10, int16_t motor_cmd,
|
||||
uint16_t vbat_mv, bal_state_t state, uint8_t flags);
|
||||
void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
|
||||
int32_t erpm, uint16_t voltage_mv,
|
||||
int16_t current_ma, uint16_t temp_c_x10);
|
||||
void orin_send_ack(QueueHandle_t q, uint8_t cmd_type);
|
||||
void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err);
|
||||
|
||||
/* OTA telemetry helpers (bd-1s1s) */
|
||||
void orin_send_ota_status(QueueHandle_t q, uint8_t target,
|
||||
uint8_t state, uint8_t progress, uint8_t err);
|
||||
void orin_send_version_info(QueueHandle_t q, uint8_t target,
|
||||
const char *current, const char *available);
|
||||
@ -1,150 +0,0 @@
|
||||
/* ota_display.c — OTA notification/progress UI on GC9A01 (bd-1yr8)
|
||||
*
|
||||
* Renders OTA state overlaid on the 240×240 round HUD display:
|
||||
* - BADGE: small dot on top-right when update available (idle state)
|
||||
* - UPDATE SCREEN: version compare, Update Balance / Update IO / Update All
|
||||
* - PROGRESS: arc around display perimeter + % + status text
|
||||
* - ERROR: red banner + "RETRY" prompt
|
||||
*
|
||||
* The display_draw_* primitives must be provided by the GC9A01 driver.
|
||||
* Actual SPI driver implementation is in a separate driver bead.
|
||||
*/
|
||||
|
||||
#include "ota_display.h"
|
||||
#include "gitea_ota.h"
|
||||
#include "version.h"
|
||||
#include "esp_log.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static const char *TAG = "ota_disp";
|
||||
|
||||
/* Display centre and radius for the 240×240 GC9A01 */
|
||||
#define CX 120
|
||||
#define CY 120
|
||||
#define RAD 110
|
||||
|
||||
/* ── Availability badge: 8×8 dot at top-right of display ── */
|
||||
static void draw_badge(bool balance_avail, bool io_avail)
|
||||
{
|
||||
uint16_t col = (balance_avail || io_avail) ? COL_ORANGE : COL_BG;
|
||||
display_fill_rect(200, 15, 12, 12, col);
|
||||
}
|
||||
|
||||
/* ── Progress arc: sweeps 0→360° proportional to progress% ── */
|
||||
static void draw_progress_arc(uint8_t pct, uint16_t color)
|
||||
{
|
||||
int end_deg = (int)(360 * pct / 100);
|
||||
display_draw_arc(CX, CY, RAD, 0, end_deg, 6, color);
|
||||
}
|
||||
|
||||
/* ── Status banner: 2 lines of text centred on display ── */
|
||||
static void draw_status(const char *line1, const char *line2,
|
||||
uint16_t fg, uint16_t bg)
|
||||
{
|
||||
display_fill_rect(20, 90, 200, 60, bg);
|
||||
if (line1 && line1[0])
|
||||
display_draw_string(CX - (int)(strlen(line1) * 6 / 2), 96,
|
||||
line1, fg, bg);
|
||||
if (line2 && line2[0])
|
||||
display_draw_string(CX - (int)(strlen(line2) * 6 / 2), 116,
|
||||
line2, fg, bg);
|
||||
}
|
||||
|
||||
/* ── Main render logic ── */
|
||||
void ota_display_update(void)
|
||||
{
|
||||
/* Determine dominant OTA state */
|
||||
ota_self_state_t self = g_ota_self_state;
|
||||
uart_ota_send_state_t io_s = g_uart_ota_state;
|
||||
|
||||
switch (self) {
|
||||
case OTA_SELF_DOWNLOADING:
|
||||
case OTA_SELF_VERIFYING:
|
||||
case OTA_SELF_APPLYING: {
|
||||
/* Balance self-update in progress */
|
||||
char pct_str[16];
|
||||
snprintf(pct_str, sizeof(pct_str), "%d%%", g_ota_self_progress);
|
||||
const char *phase = (self == OTA_SELF_VERIFYING) ? "Verifying..." :
|
||||
(self == OTA_SELF_APPLYING) ? "Applying..." :
|
||||
"Downloading...";
|
||||
draw_progress_arc(g_ota_self_progress, COL_BLUE);
|
||||
draw_status("Updating Balance", pct_str, COL_WHITE, COL_BG);
|
||||
ESP_LOGD(TAG, "balance OTA %s %d%%", phase, g_ota_self_progress);
|
||||
return;
|
||||
}
|
||||
case OTA_SELF_REBOOTING:
|
||||
draw_status("Update complete", "Rebooting...", COL_GREEN, COL_BG);
|
||||
return;
|
||||
case OTA_SELF_FAILED:
|
||||
draw_progress_arc(0, COL_RED);
|
||||
draw_status("Balance update", "FAILED RETRY?", COL_RED, COL_BG);
|
||||
return;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (io_s) {
|
||||
case UART_OTA_S_DOWNLOADING:
|
||||
draw_progress_arc(g_uart_ota_progress, COL_YELLOW);
|
||||
draw_status("Downloading IO", "firmware...", COL_WHITE, COL_BG);
|
||||
return;
|
||||
case UART_OTA_S_SENDING: {
|
||||
char pct_str[16];
|
||||
snprintf(pct_str, sizeof(pct_str), "%d%%", g_uart_ota_progress);
|
||||
draw_progress_arc(g_uart_ota_progress, COL_YELLOW);
|
||||
draw_status("Updating IO", pct_str, COL_WHITE, COL_BG);
|
||||
return;
|
||||
}
|
||||
case UART_OTA_S_DONE:
|
||||
draw_status("IO update done", "", COL_GREEN, COL_BG);
|
||||
return;
|
||||
case UART_OTA_S_FAILED:
|
||||
draw_progress_arc(0, COL_RED);
|
||||
draw_status("IO update", "FAILED RETRY?", COL_RED, COL_BG);
|
||||
return;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* Idle — show badge if update available */
|
||||
bool bal_avail = g_balance_update.available;
|
||||
bool io_avail = g_io_update.available;
|
||||
draw_badge(bal_avail, io_avail);
|
||||
|
||||
if (bal_avail || io_avail) {
|
||||
/* Show available versions on display when idle */
|
||||
char verline[48];
|
||||
if (bal_avail) {
|
||||
snprintf(verline, sizeof(verline), "Bal v%s rdy",
|
||||
g_balance_update.version);
|
||||
draw_status(verline, io_avail ? "IO update rdy" : "",
|
||||
COL_ORANGE, COL_BG);
|
||||
} else if (io_avail) {
|
||||
snprintf(verline, sizeof(verline), "IO v%s rdy",
|
||||
g_io_update.version);
|
||||
draw_status(verline, "", COL_ORANGE, COL_BG);
|
||||
}
|
||||
} else {
|
||||
/* Clear OTA overlay area */
|
||||
display_fill_rect(20, 90, 200, 60, COL_BG);
|
||||
draw_badge(false, false);
|
||||
}
|
||||
}
|
||||
|
||||
/* ── Background display task (5 Hz) ── */
|
||||
static void ota_display_task(void *arg)
|
||||
{
|
||||
for (;;) {
|
||||
vTaskDelay(pdMS_TO_TICKS(200));
|
||||
ota_display_update();
|
||||
}
|
||||
}
|
||||
|
||||
void ota_display_init(void)
|
||||
{
|
||||
xTaskCreate(ota_display_task, "ota_disp", 2048, NULL, 3, NULL);
|
||||
ESP_LOGI(TAG, "OTA display task started");
|
||||
}
|
||||
@ -1,33 +0,0 @@
|
||||
#pragma once
|
||||
/* ota_display.h — OTA notification UI on GC9A01 round LCD (bd-1yr8)
|
||||
*
|
||||
* GC9A01 240×240 round display via SPI (IO12 CS, IO11 DC, IO10 RST, IO9 BL).
|
||||
* Calls into display_draw_* primitives (provided by display driver layer).
|
||||
* This module owns the "OTA notification overlay" rendered over the HUD.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "ota_self.h"
|
||||
#include "uart_ota.h"
|
||||
|
||||
/* ── Display primitives API (must be provided by display driver) ── */
|
||||
void display_fill_rect(int x, int y, int w, int h, uint16_t rgb565);
|
||||
void display_draw_string(int x, int y, const char *str, uint16_t fg, uint16_t bg);
|
||||
void display_draw_arc(int cx, int cy, int r, int start_deg, int end_deg,
|
||||
int thickness, uint16_t color);
|
||||
|
||||
/* ── Colour palette (RGB565) ── */
|
||||
#define COL_BG 0x0000u /* black */
|
||||
#define COL_WHITE 0xFFFFu
|
||||
#define COL_GREEN 0x07E0u
|
||||
#define COL_YELLOW 0xFFE0u
|
||||
#define COL_RED 0xF800u
|
||||
#define COL_BLUE 0x001Fu
|
||||
#define COL_ORANGE 0xFD20u
|
||||
|
||||
/* ── OTA display task: runs at 5 Hz, overlays OTA state on HUD ── */
|
||||
void ota_display_init(void);
|
||||
|
||||
/* Called from main loop or display task to render the OTA overlay */
|
||||
void ota_display_update(void);
|
||||
@ -1,183 +0,0 @@
|
||||
/* ota_self.c — Balance self-OTA (bd-18nb)
|
||||
*
|
||||
* Uses esp_https_ota / esp_ota_ops to download from Gitea release URL,
|
||||
* stream-verify SHA256 with mbedTLS, set new boot partition, and reboot.
|
||||
* CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE in sdkconfig allows auto-rollback
|
||||
* if the new image doesn't call esp_ota_mark_app_valid_cancel_rollback()
|
||||
* within OTA_ROLLBACK_WINDOW_S seconds.
|
||||
*/
|
||||
|
||||
#include "ota_self.h"
|
||||
#include "gitea_ota.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_ota_ops.h"
|
||||
#include "esp_http_client.h"
|
||||
#include "esp_timer.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "mbedtls/sha256.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
static const char *TAG = "ota_self";
|
||||
|
||||
volatile ota_self_state_t g_ota_self_state = OTA_SELF_IDLE;
|
||||
volatile uint8_t g_ota_self_progress = 0;
|
||||
|
||||
#define OTA_CHUNK_SIZE 4096
|
||||
|
||||
/* ── SHA256 verify helper ── */
|
||||
static bool sha256_matches(const uint8_t *digest, const char *expected_hex)
|
||||
{
|
||||
if (!expected_hex || expected_hex[0] == '\0') {
|
||||
ESP_LOGW(TAG, "no SHA256 to verify — skipping");
|
||||
return true;
|
||||
}
|
||||
char got[65] = {0};
|
||||
for (int i = 0; i < 32; i++) {
|
||||
snprintf(&got[i*2], 3, "%02x", digest[i]);
|
||||
}
|
||||
bool ok = (strncasecmp(got, expected_hex, 64) == 0);
|
||||
if (!ok) {
|
||||
ESP_LOGE(TAG, "SHA256 mismatch: got=%s exp=%s", got, expected_hex);
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
/* ── OTA download + flash task ── */
|
||||
static void ota_self_task(void *arg)
|
||||
{
|
||||
const char *url = g_balance_update.download_url;
|
||||
const char *sha256 = g_balance_update.sha256;
|
||||
|
||||
g_ota_self_state = OTA_SELF_DOWNLOADING;
|
||||
g_ota_self_progress = 0;
|
||||
|
||||
ESP_LOGI(TAG, "OTA start: %s", url);
|
||||
|
||||
esp_ota_handle_t handle = 0;
|
||||
const esp_partition_t *ota_part = esp_ota_get_next_update_partition(NULL);
|
||||
if (!ota_part) {
|
||||
ESP_LOGE(TAG, "no OTA partition");
|
||||
g_ota_self_state = OTA_SELF_FAILED;
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
esp_err_t err = esp_ota_begin(ota_part, OTA_WITH_SEQUENTIAL_WRITES, &handle);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "ota_begin: %s", esp_err_to_name(err));
|
||||
g_ota_self_state = OTA_SELF_FAILED;
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Setup HTTP client */
|
||||
esp_http_client_config_t hcfg = {
|
||||
.url = url,
|
||||
.timeout_ms = 30000,
|
||||
.buffer_size = OTA_CHUNK_SIZE,
|
||||
.skip_cert_common_name_check = true,
|
||||
};
|
||||
esp_http_client_handle_t client = esp_http_client_init(&hcfg);
|
||||
err = esp_http_client_open(client, 0);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "http_open: %s", esp_err_to_name(err));
|
||||
esp_ota_abort(handle);
|
||||
esp_http_client_cleanup(client);
|
||||
g_ota_self_state = OTA_SELF_FAILED;
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
int content_len = esp_http_client_fetch_headers(client);
|
||||
ESP_LOGI(TAG, "content-length: %d", content_len);
|
||||
|
||||
mbedtls_sha256_context sha_ctx;
|
||||
mbedtls_sha256_init(&sha_ctx);
|
||||
mbedtls_sha256_starts(&sha_ctx, 0); /* 0 = SHA-256 */
|
||||
|
||||
static uint8_t buf[OTA_CHUNK_SIZE];
|
||||
int total = 0;
|
||||
int rd;
|
||||
while ((rd = esp_http_client_read(client, (char *)buf, sizeof(buf))) > 0) {
|
||||
mbedtls_sha256_update(&sha_ctx, buf, rd);
|
||||
err = esp_ota_write(handle, buf, rd);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "ota_write: %s", esp_err_to_name(err));
|
||||
esp_ota_abort(handle);
|
||||
goto cleanup;
|
||||
}
|
||||
total += rd;
|
||||
if (content_len > 0) {
|
||||
g_ota_self_progress = (uint8_t)((total * 100) / content_len);
|
||||
}
|
||||
}
|
||||
esp_http_client_close(client);
|
||||
|
||||
/* Verify SHA256 */
|
||||
g_ota_self_state = OTA_SELF_VERIFYING;
|
||||
uint8_t digest[32];
|
||||
mbedtls_sha256_finish(&sha_ctx, digest);
|
||||
if (!sha256_matches(digest, sha256)) {
|
||||
ESP_LOGE(TAG, "SHA256 verification failed");
|
||||
esp_ota_abort(handle);
|
||||
g_ota_self_state = OTA_SELF_FAILED;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
/* Finalize + set boot partition */
|
||||
g_ota_self_state = OTA_SELF_APPLYING;
|
||||
err = esp_ota_end(handle);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "ota_end: %s", esp_err_to_name(err));
|
||||
g_ota_self_state = OTA_SELF_FAILED;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
err = esp_ota_set_boot_partition(ota_part);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG, "set_boot_partition: %s", esp_err_to_name(err));
|
||||
g_ota_self_state = OTA_SELF_FAILED;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
g_ota_self_state = OTA_SELF_REBOOTING;
|
||||
g_ota_self_progress = 100;
|
||||
ESP_LOGI(TAG, "OTA success — rebooting");
|
||||
vTaskDelay(pdMS_TO_TICKS(500));
|
||||
esp_restart();
|
||||
|
||||
cleanup:
|
||||
mbedtls_sha256_free(&sha_ctx);
|
||||
esp_http_client_cleanup(client);
|
||||
handle = 0;
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
bool ota_self_trigger(void)
|
||||
{
|
||||
if (!g_balance_update.available) {
|
||||
ESP_LOGW(TAG, "no update available");
|
||||
return false;
|
||||
}
|
||||
if (g_ota_self_state != OTA_SELF_IDLE) {
|
||||
ESP_LOGW(TAG, "OTA already in progress (state=%d)", g_ota_self_state);
|
||||
return false;
|
||||
}
|
||||
xTaskCreate(ota_self_task, "ota_self", 8192, NULL, 5, NULL);
|
||||
return true;
|
||||
}
|
||||
|
||||
void ota_self_health_check(void)
|
||||
{
|
||||
/* Mark running image as valid — prevents rollback */
|
||||
esp_err_t err = esp_ota_mark_app_valid_cancel_rollback();
|
||||
if (err == ESP_OK) {
|
||||
ESP_LOGI(TAG, "image marked valid");
|
||||
} else if (err == ESP_ERR_NOT_SUPPORTED) {
|
||||
/* Not an OTA image (e.g., flashed via JTAG) — ignore */
|
||||
} else {
|
||||
ESP_LOGW(TAG, "mark_valid: %s", esp_err_to_name(err));
|
||||
}
|
||||
}
|
||||
@ -1,34 +0,0 @@
|
||||
#pragma once
|
||||
/* ota_self.h — Balance self-OTA (bd-18nb)
|
||||
*
|
||||
* Downloads balance-firmware.bin from Gitea release URL to the inactive
|
||||
* OTA partition, verifies SHA256, sets boot partition, reboots.
|
||||
* Auto-rollback if health check not called within ROLLBACK_WINDOW_S seconds.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#define OTA_ROLLBACK_WINDOW_S 30
|
||||
|
||||
typedef enum {
|
||||
OTA_SELF_IDLE = 0,
|
||||
OTA_SELF_CHECKING, /* (unused — gitea_ota handles this) */
|
||||
OTA_SELF_DOWNLOADING,
|
||||
OTA_SELF_VERIFYING,
|
||||
OTA_SELF_APPLYING,
|
||||
OTA_SELF_REBOOTING,
|
||||
OTA_SELF_FAILED,
|
||||
} ota_self_state_t;
|
||||
|
||||
extern volatile ota_self_state_t g_ota_self_state;
|
||||
extern volatile uint8_t g_ota_self_progress; /* 0-100 % */
|
||||
|
||||
/* Trigger a Balance self-update.
|
||||
* Uses g_balance_update (from gitea_ota). Non-blocking: starts in a task.
|
||||
* Returns false if no update available or OTA already in progress. */
|
||||
bool ota_self_trigger(void);
|
||||
|
||||
/* Called from app_main after boot to mark the running image as valid.
|
||||
* Must be called within OTA_ROLLBACK_WINDOW_S after boot or rollback fires. */
|
||||
void ota_self_health_check(void);
|
||||
@ -1,241 +0,0 @@
|
||||
/* uart_ota.c — UART OTA sender: Balance→IO board (bd-21hv)
|
||||
*
|
||||
* Downloads io-firmware.bin from Gitea, then sends to IO board via UART1.
|
||||
* IO board must update itself BEFORE Balance self-update (per spec).
|
||||
*/
|
||||
|
||||
#include "uart_ota.h"
|
||||
#include "gitea_ota.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_http_client.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "mbedtls/sha256.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
static const char *TAG = "uart_ota";
|
||||
|
||||
volatile uart_ota_send_state_t g_uart_ota_state = UART_OTA_S_IDLE;
|
||||
volatile uint8_t g_uart_ota_progress = 0;
|
||||
|
||||
/* ── CRC8-SMBUS ── */
|
||||
static uint8_t crc8(const uint8_t *d, uint16_t len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
for (uint16_t i = 0; i < len; i++) {
|
||||
crc ^= d[i];
|
||||
for (uint8_t b = 0; b < 8; b++)
|
||||
crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
/* ── Build and send one UART OTA frame ── */
|
||||
static void send_frame(uint8_t type, uint16_t seq,
|
||||
const uint8_t *payload, uint16_t plen)
|
||||
{
|
||||
/* [TYPE:1][SEQ:2 BE][LEN:2 BE][PAYLOAD][CRC8:1] */
|
||||
uint8_t hdr[5];
|
||||
hdr[0] = type;
|
||||
hdr[1] = (uint8_t)(seq >> 8u);
|
||||
hdr[2] = (uint8_t)(seq);
|
||||
hdr[3] = (uint8_t)(plen >> 8u);
|
||||
hdr[4] = (uint8_t)(plen);
|
||||
|
||||
/* CRC over hdr + payload */
|
||||
uint8_t crc_buf[5 + OTA_UART_CHUNK_SIZE];
|
||||
memcpy(crc_buf, hdr, 5);
|
||||
if (plen > 0 && payload) memcpy(crc_buf + 5, payload, plen);
|
||||
uint8_t crc = crc8(crc_buf, (uint16_t)(5 + plen));
|
||||
|
||||
uart_write_bytes(UART_OTA_PORT, (char *)hdr, 5);
|
||||
if (plen > 0 && payload)
|
||||
uart_write_bytes(UART_OTA_PORT, (char *)payload, plen);
|
||||
uart_write_bytes(UART_OTA_PORT, (char *)&crc, 1);
|
||||
}
|
||||
|
||||
/* ── Wait for ACK/NACK from IO board ── */
|
||||
static bool wait_ack(uint16_t expected_seq)
|
||||
{
|
||||
/* Response frame: [TYPE:1][SEQ:2][LEN:2][PAYLOAD][CRC:1] */
|
||||
uint8_t buf[16];
|
||||
int timeout = OTA_UART_ACK_TIMEOUT_MS;
|
||||
int got = 0;
|
||||
|
||||
while (timeout > 0 && got < 6) {
|
||||
int r = uart_read_bytes(UART_OTA_PORT, buf + got, 1, pdMS_TO_TICKS(50));
|
||||
if (r > 0) got++;
|
||||
else timeout -= 50;
|
||||
}
|
||||
|
||||
if (got < 3) return false;
|
||||
|
||||
uint8_t type = buf[0];
|
||||
uint16_t seq = (uint16_t)((buf[1] << 8u) | buf[2]);
|
||||
|
||||
if (type == UART_OTA_ACK && seq == expected_seq) return true;
|
||||
if (type == UART_OTA_NACK) {
|
||||
uint8_t err = (got >= 6) ? buf[5] : 0;
|
||||
ESP_LOGW(TAG, "NACK seq=%u err=%u", seq, err);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ── Download firmware to RAM buffer (max 1.75 MB) ── */
|
||||
static uint8_t *download_io_firmware(uint32_t *out_size)
|
||||
{
|
||||
const char *url = g_io_update.download_url;
|
||||
ESP_LOGI(TAG, "downloading IO fw: %s", url);
|
||||
|
||||
esp_http_client_config_t cfg = {
|
||||
.url = url, .timeout_ms = 30000,
|
||||
.skip_cert_common_name_check = true,
|
||||
};
|
||||
esp_http_client_handle_t client = esp_http_client_init(&cfg);
|
||||
if (esp_http_client_open(client, 0) != ESP_OK) {
|
||||
esp_http_client_cleanup(client);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int content_len = esp_http_client_fetch_headers(client);
|
||||
if (content_len <= 0 || content_len > (int)(0x1B0000)) {
|
||||
ESP_LOGE(TAG, "bad content-length: %d", content_len);
|
||||
esp_http_client_cleanup(client);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
uint8_t *buf = malloc(content_len);
|
||||
if (!buf) {
|
||||
ESP_LOGE(TAG, "malloc %d failed", content_len);
|
||||
esp_http_client_cleanup(client);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int total = 0, rd;
|
||||
while ((rd = esp_http_client_read(client, (char *)buf + total,
|
||||
content_len - total)) > 0) {
|
||||
total += rd;
|
||||
g_uart_ota_progress = (uint8_t)((total * 50) / content_len); /* 0-50% for download */
|
||||
}
|
||||
esp_http_client_cleanup(client);
|
||||
|
||||
if (total != content_len) {
|
||||
free(buf);
|
||||
return NULL;
|
||||
}
|
||||
*out_size = (uint32_t)total;
|
||||
return buf;
|
||||
}
|
||||
|
||||
/* ── UART OTA send task ── */
|
||||
static void uart_ota_task(void *arg)
|
||||
{
|
||||
g_uart_ota_state = UART_OTA_S_DOWNLOADING;
|
||||
g_uart_ota_progress = 0;
|
||||
|
||||
uint32_t fw_size = 0;
|
||||
uint8_t *fw = download_io_firmware(&fw_size);
|
||||
if (!fw) {
|
||||
ESP_LOGE(TAG, "download failed");
|
||||
g_uart_ota_state = UART_OTA_S_FAILED;
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Compute SHA256 of downloaded firmware */
|
||||
uint8_t digest[32];
|
||||
mbedtls_sha256_context sha;
|
||||
mbedtls_sha256_init(&sha);
|
||||
mbedtls_sha256_starts(&sha, 0);
|
||||
mbedtls_sha256_update(&sha, fw, fw_size);
|
||||
mbedtls_sha256_finish(&sha, digest);
|
||||
mbedtls_sha256_free(&sha);
|
||||
|
||||
g_uart_ota_state = UART_OTA_S_SENDING;
|
||||
|
||||
/* Send OTA_BEGIN: uint32 size + uint8[32] sha256 */
|
||||
uint8_t begin_payload[36];
|
||||
begin_payload[0] = (uint8_t)(fw_size >> 24u);
|
||||
begin_payload[1] = (uint8_t)(fw_size >> 16u);
|
||||
begin_payload[2] = (uint8_t)(fw_size >> 8u);
|
||||
begin_payload[3] = (uint8_t)(fw_size);
|
||||
memcpy(&begin_payload[4], digest, 32);
|
||||
|
||||
for (int retry = 0; retry < OTA_UART_MAX_RETRIES; retry++) {
|
||||
send_frame(UART_OTA_BEGIN, 0, begin_payload, 36);
|
||||
if (wait_ack(0)) goto send_data;
|
||||
ESP_LOGW(TAG, "BEGIN retry %d", retry);
|
||||
}
|
||||
ESP_LOGE(TAG, "BEGIN failed");
|
||||
free(fw);
|
||||
g_uart_ota_state = UART_OTA_S_FAILED;
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
|
||||
send_data: {
|
||||
uint32_t offset = 0;
|
||||
uint16_t seq = 1;
|
||||
while (offset < fw_size) {
|
||||
uint16_t chunk = (uint16_t)((fw_size - offset) < OTA_UART_CHUNK_SIZE
|
||||
? (fw_size - offset) : OTA_UART_CHUNK_SIZE);
|
||||
bool acked = false;
|
||||
for (int retry = 0; retry < OTA_UART_MAX_RETRIES; retry++) {
|
||||
send_frame(UART_OTA_DATA, seq, fw + offset, chunk);
|
||||
if (wait_ack(seq)) { acked = true; break; }
|
||||
ESP_LOGW(TAG, "DATA seq=%u retry=%d", seq, retry);
|
||||
}
|
||||
if (!acked) {
|
||||
ESP_LOGE(TAG, "DATA seq=%u failed", seq);
|
||||
send_frame(UART_OTA_ABORT, seq, NULL, 0);
|
||||
free(fw);
|
||||
g_uart_ota_state = UART_OTA_S_FAILED;
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
offset += chunk;
|
||||
seq++;
|
||||
/* 50-100% for sending phase */
|
||||
g_uart_ota_progress = (uint8_t)(50u + (offset * 50u) / fw_size);
|
||||
}
|
||||
|
||||
/* Send OTA_END */
|
||||
for (int retry = 0; retry < OTA_UART_MAX_RETRIES; retry++) {
|
||||
send_frame(UART_OTA_END, seq, NULL, 0);
|
||||
if (wait_ack(seq)) break;
|
||||
}
|
||||
}
|
||||
|
||||
free(fw);
|
||||
g_uart_ota_progress = 100;
|
||||
g_uart_ota_state = UART_OTA_S_DONE;
|
||||
ESP_LOGI(TAG, "IO OTA complete — %lu bytes sent", (unsigned long)fw_size);
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
bool uart_ota_trigger(void)
|
||||
{
|
||||
if (!g_io_update.available) {
|
||||
ESP_LOGW(TAG, "no IO update available");
|
||||
return false;
|
||||
}
|
||||
if (g_uart_ota_state != UART_OTA_S_IDLE) {
|
||||
ESP_LOGW(TAG, "UART OTA busy (state=%d)", g_uart_ota_state);
|
||||
return false;
|
||||
}
|
||||
/* Init UART1 for OTA */
|
||||
uart_config_t ucfg = {
|
||||
.baud_rate = UART_OTA_BAUD,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
};
|
||||
uart_param_config(UART_OTA_PORT, &ucfg);
|
||||
uart_set_pin(UART_OTA_PORT, UART_OTA_TX_GPIO, UART_OTA_RX_GPIO,
|
||||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||||
uart_driver_install(UART_OTA_PORT, 2048, 0, 0, NULL, 0);
|
||||
|
||||
xTaskCreate(uart_ota_task, "uart_ota", 16384, NULL, 4, NULL);
|
||||
return true;
|
||||
}
|
||||
@ -1,64 +0,0 @@
|
||||
#pragma once
|
||||
/* uart_ota.h — UART OTA protocol for Balance→IO firmware update (bd-21hv)
|
||||
*
|
||||
* Balance downloads io-firmware.bin from Gitea, then streams it to the IO
|
||||
* board over UART1 (GPIO17/18, 460800 baud) in 1 KB chunks with ACK.
|
||||
*
|
||||
* Protocol frame format (both directions):
|
||||
* [TYPE:1][SEQ:2 BE][LEN:2 BE][PAYLOAD:LEN][CRC8:1]
|
||||
* CRC8-SMBUS over TYPE+SEQ+LEN+PAYLOAD.
|
||||
*
|
||||
* Balance→IO:
|
||||
* OTA_BEGIN (0xC0) payload: uint32 total_size BE + uint8[32] sha256
|
||||
* OTA_DATA (0xC1) payload: uint8[] chunk (up to 1024 bytes)
|
||||
* OTA_END (0xC2) no payload
|
||||
* OTA_ABORT (0xC3) no payload
|
||||
*
|
||||
* IO→Balance:
|
||||
* OTA_ACK (0xC4) payload: uint16 acked_seq BE
|
||||
* OTA_NACK (0xC5) payload: uint16 failed_seq BE + uint8 err_code
|
||||
* OTA_STATUS (0xC6) payload: uint8 state + uint8 progress%
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* UART for Balance→IO OTA */
|
||||
#include "driver/uart.h"
|
||||
#define UART_OTA_PORT UART_NUM_1
|
||||
#define UART_OTA_BAUD 460800
|
||||
#define UART_OTA_TX_GPIO 17
|
||||
#define UART_OTA_RX_GPIO 18
|
||||
|
||||
#define OTA_UART_CHUNK_SIZE 1024
|
||||
#define OTA_UART_ACK_TIMEOUT_MS 3000
|
||||
#define OTA_UART_MAX_RETRIES 3
|
||||
|
||||
/* Frame type bytes */
|
||||
#define UART_OTA_BEGIN 0xC0u
|
||||
#define UART_OTA_DATA 0xC1u
|
||||
#define UART_OTA_END 0xC2u
|
||||
#define UART_OTA_ABORT 0xC3u
|
||||
#define UART_OTA_ACK 0xC4u
|
||||
#define UART_OTA_NACK 0xC5u
|
||||
#define UART_OTA_STATUS 0xC6u
|
||||
|
||||
/* NACK error codes */
|
||||
#define OTA_ERR_BAD_CRC 0x01u
|
||||
#define OTA_ERR_WRITE 0x02u
|
||||
#define OTA_ERR_SIZE 0x03u
|
||||
|
||||
typedef enum {
|
||||
UART_OTA_S_IDLE = 0,
|
||||
UART_OTA_S_DOWNLOADING, /* downloading from Gitea */
|
||||
UART_OTA_S_SENDING, /* sending to IO board */
|
||||
UART_OTA_S_DONE,
|
||||
UART_OTA_S_FAILED,
|
||||
} uart_ota_send_state_t;
|
||||
|
||||
extern volatile uart_ota_send_state_t g_uart_ota_state;
|
||||
extern volatile uint8_t g_uart_ota_progress;
|
||||
|
||||
/* Trigger IO firmware update. Uses g_io_update (from gitea_ota).
|
||||
* Downloads bin, then streams via UART. Returns false if busy or no update. */
|
||||
bool uart_ota_trigger(void);
|
||||
@ -1,14 +0,0 @@
|
||||
#pragma once
|
||||
/* Embedded firmware version — bump on each release */
|
||||
#define BALANCE_FW_VERSION "1.0.0"
|
||||
#define IO_FW_VERSION "1.0.0"
|
||||
|
||||
/* Gitea release tag prefixes */
|
||||
#define BALANCE_TAG_PREFIX "esp32-balance/"
|
||||
#define IO_TAG_PREFIX "esp32-io/"
|
||||
|
||||
/* Gitea release asset filenames */
|
||||
#define BALANCE_BIN_ASSET "balance-firmware.bin"
|
||||
#define IO_BIN_ASSET "io-firmware.bin"
|
||||
#define BALANCE_SHA256_ASSET "balance-firmware.sha256"
|
||||
#define IO_SHA256_ASSET "io-firmware.sha256"
|
||||
@ -1,119 +0,0 @@
|
||||
/* vesc_can.c — VESC CAN TWAI driver (bd-66hx)
|
||||
*
|
||||
* Receives VESC STATUS/4/5 frames via TWAI, proxies to Orin over serial.
|
||||
* Transmits SET_RPM commands from Orin drive requests.
|
||||
*/
|
||||
|
||||
#include "vesc_can.h"
|
||||
#include "orin_serial.h"
|
||||
#include "config.h"
|
||||
#include "driver/twai.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include <string.h>
|
||||
|
||||
static const char *TAG = "vesc_can";
|
||||
|
||||
vesc_state_t g_vesc[2] = {0};
|
||||
|
||||
/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */
|
||||
static int vesc_idx(uint8_t id)
|
||||
{
|
||||
if (id == VESC_ID_A) return 0;
|
||||
if (id == VESC_ID_B) return 1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void vesc_can_init(void)
|
||||
{
|
||||
twai_general_config_t gcfg = TWAI_GENERAL_CONFIG_DEFAULT(
|
||||
(gpio_num_t)VESC_CAN_TX_GPIO,
|
||||
(gpio_num_t)VESC_CAN_RX_GPIO,
|
||||
TWAI_MODE_NORMAL);
|
||||
gcfg.rx_queue_len = VESC_CAN_RX_QUEUE;
|
||||
|
||||
twai_timing_config_t tcfg = TWAI_TIMING_CONFIG_500KBITS();
|
||||
twai_filter_config_t fcfg = TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
||||
|
||||
ESP_ERROR_CHECK(twai_driver_install(&gcfg, &tcfg, &fcfg));
|
||||
ESP_ERROR_CHECK(twai_start());
|
||||
ESP_LOGI(TAG, "TWAI init OK: tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
|
||||
}
|
||||
|
||||
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
|
||||
{
|
||||
uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | vesc_id;
|
||||
twai_message_t msg = {
|
||||
.extd = 1,
|
||||
.identifier = ext_id,
|
||||
.data_length_code = 4,
|
||||
};
|
||||
uint32_t u = (uint32_t)erpm;
|
||||
msg.data[0] = (uint8_t)(u >> 24u);
|
||||
msg.data[1] = (uint8_t)(u >> 16u);
|
||||
msg.data[2] = (uint8_t)(u >> 8u);
|
||||
msg.data[3] = (uint8_t)(u);
|
||||
twai_transmit(&msg, pdMS_TO_TICKS(5));
|
||||
}
|
||||
|
||||
void vesc_can_rx_task(void *arg)
|
||||
{
|
||||
QueueHandle_t tx_q = (QueueHandle_t)arg;
|
||||
twai_message_t msg;
|
||||
|
||||
for (;;) {
|
||||
if (twai_receive(&msg, pdMS_TO_TICKS(50)) != ESP_OK) {
|
||||
continue;
|
||||
}
|
||||
if (!msg.extd) {
|
||||
continue; /* ignore standard frames */
|
||||
}
|
||||
|
||||
uint8_t pkt_type = (uint8_t)(msg.identifier >> 8u);
|
||||
uint8_t vesc_id = (uint8_t)(msg.identifier & 0xFFu);
|
||||
int idx = vesc_idx(vesc_id);
|
||||
if (idx < 0) {
|
||||
continue; /* not our VESC */
|
||||
}
|
||||
|
||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
||||
vesc_state_t *s = &g_vesc[idx];
|
||||
|
||||
switch (pkt_type) {
|
||||
case VESC_PKT_STATUS:
|
||||
if (msg.data_length_code < 8u) { break; }
|
||||
s->erpm = (int32_t)(
|
||||
((uint32_t)msg.data[0] << 24u) | ((uint32_t)msg.data[1] << 16u) |
|
||||
((uint32_t)msg.data[2] << 8u) | (uint32_t)msg.data[3]);
|
||||
s->current_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
|
||||
s->last_rx_ms = now_ms;
|
||||
/* Proxy to Orin: voltage from STATUS_5 (may be zero until received) */
|
||||
{
|
||||
uint8_t ttype = (vesc_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT;
|
||||
/* voltage_mv: V×10 → mV (/10 * 1000 = *100); current_ma: A×10 → mA (*100) */
|
||||
uint16_t vmv = (uint16_t)((int32_t)s->voltage_x10 * 100);
|
||||
int16_t ima = (int16_t)((int32_t)s->current_x10 * 100);
|
||||
orin_send_vesc(tx_q, ttype, s->erpm, vmv, ima,
|
||||
(uint16_t)s->temp_mot_x10);
|
||||
}
|
||||
break;
|
||||
|
||||
case VESC_PKT_STATUS_4:
|
||||
if (msg.data_length_code < 6u) { break; }
|
||||
/* T_fet×10, T_mot×10, I_in×10 */
|
||||
s->temp_mot_x10 = (int16_t)(((uint16_t)msg.data[2] << 8u) | msg.data[3]);
|
||||
break;
|
||||
|
||||
case VESC_PKT_STATUS_5:
|
||||
if (msg.data_length_code < 6u) { break; }
|
||||
/* int32 tacho (ignored), int16 V_in×10 */
|
||||
s->voltage_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,36 +0,0 @@
|
||||
#pragma once
|
||||
/* vesc_can.h — VESC CAN TWAI driver for ESP32-S3 BALANCE (bd-66hx)
|
||||
*
|
||||
* VESC extended CAN ID: (packet_type << 8) | vesc_node_id
|
||||
* Physical layer: TWAI peripheral → SN65HVD230 → 500 kbps shared bus
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/queue.h"
|
||||
|
||||
/* ── VESC packet types ── */
|
||||
#define VESC_PKT_SET_RPM 3u
|
||||
#define VESC_PKT_STATUS 9u /* int32 erpm, int16 I×10, int16 duty×1000 */
|
||||
#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */
|
||||
#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */
|
||||
|
||||
/* ── VESC telemetry snapshot ── */
|
||||
typedef struct {
|
||||
int32_t erpm; /* electrical RPM (STATUS) */
|
||||
int16_t current_x10; /* phase current A×10 (STATUS) */
|
||||
int16_t voltage_x10; /* bus voltage V×10 (STATUS_5) */
|
||||
int16_t temp_mot_x10; /* motor temp °C×10 (STATUS_4) */
|
||||
uint32_t last_rx_ms; /* esp_timer ms of last STATUS frame */
|
||||
} vesc_state_t;
|
||||
|
||||
/* ── Globals (two VESC nodes: index 0 = VESC_ID_A=56, 1 = VESC_ID_B=68) ── */
|
||||
extern vesc_state_t g_vesc[2];
|
||||
|
||||
/* ── API ── */
|
||||
void vesc_can_init(void);
|
||||
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm);
|
||||
|
||||
/* RX task — pass tx_queue as arg; forwards STATUS frames to Orin over serial */
|
||||
void vesc_can_rx_task(void *arg); /* arg = QueueHandle_t orin_tx_queue */
|
||||
@ -1,7 +0,0 @@
|
||||
# ESP32-S3 BALANCE — 4 MB flash, dual OTA partitions
|
||||
# Name, Type, SubType, Offset, Size
|
||||
nvs, data, nvs, 0x9000, 0x5000,
|
||||
otadata, data, ota, 0xe000, 0x2000,
|
||||
app0, app, ota_0, 0x10000, 0x1B0000,
|
||||
app1, app, ota_1, 0x1C0000, 0x1B0000,
|
||||
nvs_user, data, nvs, 0x370000, 0x50000,
|
||||
|
@ -1,23 +0,0 @@
|
||||
CONFIG_IDF_TARGET="esp32s3"
|
||||
CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
|
||||
CONFIG_FREERTOS_HZ=1000
|
||||
CONFIG_ESP_TASK_WDT_EN=y
|
||||
CONFIG_ESP_TASK_WDT_TIMEOUT_S=5
|
||||
CONFIG_TWAI_ISR_IN_IRAM=y
|
||||
CONFIG_UART_ISR_IN_IRAM=y
|
||||
CONFIG_ESP_CONSOLE_UART_DEFAULT=y
|
||||
CONFIG_ESP_CONSOLE_UART_NUM=0
|
||||
CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200
|
||||
CONFIG_LOG_DEFAULT_LEVEL_INFO=y
|
||||
|
||||
# OTA — bd-3gwo: dual OTA partitions + rollback
|
||||
CONFIG_PARTITION_TABLE_CUSTOM=y
|
||||
CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
|
||||
CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE=y
|
||||
CONFIG_OTA_ALLOW_HTTP=y
|
||||
CONFIG_ESP_HTTPS_OTA_ALLOW_HTTP=y
|
||||
CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=y
|
||||
|
||||
# bd-66hx hardware: disable brownout detection — power supply dips during SPI/init
|
||||
# The gc9a01 display SPI init causes ~50mA transient that trips level-0 brownout
|
||||
CONFIG_ESP_BROWNOUT_DET=n
|
||||
@ -1,3 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
|
||||
project(esp32s3_io)
|
||||
@ -1,10 +0,0 @@
|
||||
idf_component_register(
|
||||
SRCS "main.c" "uart_ota_recv.c"
|
||||
INCLUDE_DIRS "."
|
||||
REQUIRES
|
||||
app_update
|
||||
mbedtls
|
||||
driver
|
||||
freertos
|
||||
esp_timer
|
||||
)
|
||||
@ -1,35 +0,0 @@
|
||||
#pragma once
|
||||
/* ESP32-S3 IO board — pin assignments (SAUL-TEE-SYSTEM-REFERENCE.md) */
|
||||
|
||||
/* ── Inter-board UART (to/from BALANCE board) ── */
|
||||
#define IO_UART_PORT UART_NUM_0
|
||||
#define IO_UART_BAUD 460800
|
||||
#define IO_UART_TX_GPIO 43 /* IO board UART0_TXD → BALANCE RX */
|
||||
#define IO_UART_RX_GPIO 44 /* IO board UART0_RXD ← BALANCE TX */
|
||||
/* Note: SAUL-TEE spec says IO TX=IO18, RX=IO21; BALANCE TX=IO17, RX=IO18.
|
||||
* This is UART0 on the IO devkit (GPIO43/44). Adjust to match actual wiring. */
|
||||
|
||||
/* ── BTS7960 Left motor driver ── */
|
||||
#define MOTOR_L_RPWM 1
|
||||
#define MOTOR_L_LPWM 2
|
||||
#define MOTOR_L_EN_R 3
|
||||
#define MOTOR_L_EN_L 4
|
||||
|
||||
/* ── BTS7960 Right motor driver ── */
|
||||
#define MOTOR_R_RPWM 5
|
||||
#define MOTOR_R_LPWM 6
|
||||
#define MOTOR_R_EN_R 7
|
||||
#define MOTOR_R_EN_L 8
|
||||
|
||||
/* ── Arming button / kill switch ── */
|
||||
#define ARM_BTN_GPIO 9
|
||||
#define KILL_GPIO 10
|
||||
|
||||
/* ── WS2812B LED strip ── */
|
||||
#define LED_DATA_GPIO 13
|
||||
|
||||
/* ── OTA UART — receives firmware from BALANCE (bd-21hv) ── */
|
||||
/* Uses same IO_UART_PORT since Balance drives OTA over the inter-board link */
|
||||
|
||||
/* ── Firmware version ── */
|
||||
#define IO_FW_VERSION "1.0.0"
|
||||
@ -1,42 +0,0 @@
|
||||
/* main.c — ESP32-S3 IO board app_main */
|
||||
|
||||
#include "uart_ota_recv.h"
|
||||
#include "config.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_ota_ops.h"
|
||||
#include "driver/uart.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
|
||||
static const char *TAG = "io_main";
|
||||
|
||||
static void uart_init(void)
|
||||
{
|
||||
uart_config_t cfg = {
|
||||
.baud_rate = IO_UART_BAUD,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
};
|
||||
uart_param_config(IO_UART_PORT, &cfg);
|
||||
uart_set_pin(IO_UART_PORT, IO_UART_TX_GPIO, IO_UART_RX_GPIO,
|
||||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||||
uart_driver_install(IO_UART_PORT, 4096, 0, 0, NULL, 0);
|
||||
}
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
ESP_LOGI(TAG, "ESP32-S3 IO v%s starting", IO_FW_VERSION);
|
||||
|
||||
/* Mark running image valid (OTA rollback support) */
|
||||
esp_ota_mark_app_valid_cancel_rollback();
|
||||
|
||||
uart_init();
|
||||
uart_ota_recv_init();
|
||||
|
||||
/* IO board main loop placeholder — RC/motor/sensor tasks added in later beads */
|
||||
while (1) {
|
||||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||||
}
|
||||
}
|
||||
@ -1,210 +0,0 @@
|
||||
/* uart_ota_recv.c — IO board OTA receiver (bd-21hv)
|
||||
*
|
||||
* Listens on UART0 for OTA frames from Balance board.
|
||||
* Writes incoming chunks to the inactive OTA partition, verifies SHA256,
|
||||
* then reboots into new firmware.
|
||||
*/
|
||||
|
||||
#include "uart_ota_recv.h"
|
||||
#include "config.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_ota_ops.h"
|
||||
#include "driver/uart.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "mbedtls/sha256.h"
|
||||
#include <string.h>
|
||||
|
||||
static const char *TAG = "io_ota";
|
||||
|
||||
volatile io_ota_state_t g_io_ota_state = IO_OTA_IDLE;
|
||||
volatile uint8_t g_io_ota_progress = 0;
|
||||
|
||||
/* Frame type bytes (same as uart_ota.h sender side) */
|
||||
#define OTA_BEGIN 0xC0u
|
||||
#define OTA_DATA 0xC1u
|
||||
#define OTA_END 0xC2u
|
||||
#define OTA_ABORT 0xC3u
|
||||
#define OTA_ACK 0xC4u
|
||||
#define OTA_NACK 0xC5u
|
||||
|
||||
#define CHUNK_MAX 1024
|
||||
|
||||
static uint8_t crc8(const uint8_t *d, uint16_t len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
for (uint16_t i = 0; i < len; i++) {
|
||||
crc ^= d[i];
|
||||
for (uint8_t b = 0; b < 8; b++)
|
||||
crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
static void send_ack(uint16_t seq)
|
||||
{
|
||||
uint8_t frame[6];
|
||||
frame[0] = OTA_ACK;
|
||||
frame[1] = (uint8_t)(seq >> 8u);
|
||||
frame[2] = (uint8_t)(seq);
|
||||
frame[3] = 0; frame[4] = 0; /* LEN=0 */
|
||||
uint8_t crc = crc8(frame, 5);
|
||||
frame[5] = crc;
|
||||
uart_write_bytes(IO_UART_PORT, (char *)frame, 6);
|
||||
}
|
||||
|
||||
static void send_nack(uint16_t seq, uint8_t err)
|
||||
{
|
||||
uint8_t frame[8];
|
||||
frame[0] = OTA_NACK;
|
||||
frame[1] = (uint8_t)(seq >> 8u);
|
||||
frame[2] = (uint8_t)(seq);
|
||||
frame[3] = 0; frame[4] = 1; /* LEN=1 */
|
||||
frame[5] = err;
|
||||
uint8_t crc = crc8(frame, 6);
|
||||
frame[6] = crc;
|
||||
uart_write_bytes(IO_UART_PORT, (char *)frame, 7);
|
||||
}
|
||||
|
||||
/* Read exact n bytes with timeout */
|
||||
static bool uart_read_exact(uint8_t *buf, int n, int timeout_ms)
|
||||
{
|
||||
int got = 0;
|
||||
while (got < n && timeout_ms > 0) {
|
||||
int r = uart_read_bytes(IO_UART_PORT, buf + got, n - got,
|
||||
pdMS_TO_TICKS(50));
|
||||
if (r > 0) got += r;
|
||||
else timeout_ms -= 50;
|
||||
}
|
||||
return got == n;
|
||||
}
|
||||
|
||||
static void ota_recv_task(void *arg)
|
||||
{
|
||||
esp_ota_handle_t handle = 0;
|
||||
const esp_partition_t *ota_part = esp_ota_get_next_update_partition(NULL);
|
||||
mbedtls_sha256_context sha;
|
||||
mbedtls_sha256_init(&sha);
|
||||
uint32_t expected_size = 0;
|
||||
uint8_t expected_digest[32] = {0};
|
||||
uint32_t received = 0;
|
||||
bool ota_started = false;
|
||||
static uint8_t payload[CHUNK_MAX];
|
||||
|
||||
for (;;) {
|
||||
/* Read frame header: TYPE(1) + SEQ(2) + LEN(2) = 5 bytes */
|
||||
uint8_t hdr[5];
|
||||
if (!uart_read_exact(hdr, 5, 5000)) continue;
|
||||
|
||||
uint8_t type = hdr[0];
|
||||
uint16_t seq = (uint16_t)((hdr[1] << 8u) | hdr[2]);
|
||||
uint16_t plen = (uint16_t)((hdr[3] << 8u) | hdr[4]);
|
||||
|
||||
if (plen > CHUNK_MAX + 36) {
|
||||
ESP_LOGW(TAG, "oversized frame plen=%u", plen);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Read payload + CRC */
|
||||
if (plen > 0 && !uart_read_exact(payload, plen, 2000)) continue;
|
||||
uint8_t crc_rx;
|
||||
if (!uart_read_exact(&crc_rx, 1, 500)) continue;
|
||||
|
||||
/* Verify CRC over hdr+payload */
|
||||
uint8_t crc_buf[5 + CHUNK_MAX + 36];
|
||||
memcpy(crc_buf, hdr, 5);
|
||||
if (plen > 0) memcpy(crc_buf + 5, payload, plen);
|
||||
uint8_t expected_crc = crc8(crc_buf, (uint16_t)(5 + plen));
|
||||
if (crc_rx != expected_crc) {
|
||||
ESP_LOGW(TAG, "CRC fail seq=%u", seq);
|
||||
send_nack(seq, 0x01u); /* OTA_ERR_BAD_CRC */
|
||||
continue;
|
||||
}
|
||||
|
||||
switch (type) {
|
||||
case OTA_BEGIN:
|
||||
if (plen < 36) { send_nack(seq, 0x03u); break; }
|
||||
expected_size = ((uint32_t)payload[0] << 24u) |
|
||||
((uint32_t)payload[1] << 16u) |
|
||||
((uint32_t)payload[2] << 8u) |
|
||||
(uint32_t)payload[3];
|
||||
memcpy(expected_digest, &payload[4], 32);
|
||||
|
||||
if (!ota_part || esp_ota_begin(ota_part, OTA_WITH_SEQUENTIAL_WRITES,
|
||||
&handle) != ESP_OK) {
|
||||
send_nack(seq, 0x02u);
|
||||
break;
|
||||
}
|
||||
mbedtls_sha256_starts(&sha, 0);
|
||||
received = 0;
|
||||
ota_started = true;
|
||||
g_io_ota_state = IO_OTA_RECEIVING;
|
||||
g_io_ota_progress = 0;
|
||||
ESP_LOGI(TAG, "OTA begin: %lu bytes", (unsigned long)expected_size);
|
||||
send_ack(seq);
|
||||
break;
|
||||
|
||||
case OTA_DATA:
|
||||
if (!ota_started) { send_nack(seq, 0x02u); break; }
|
||||
if (esp_ota_write(handle, payload, plen) != ESP_OK) {
|
||||
send_nack(seq, 0x02u);
|
||||
esp_ota_abort(handle);
|
||||
ota_started = false;
|
||||
g_io_ota_state = IO_OTA_FAILED;
|
||||
break;
|
||||
}
|
||||
mbedtls_sha256_update(&sha, payload, plen);
|
||||
received += plen;
|
||||
if (expected_size > 0)
|
||||
g_io_ota_progress = (uint8_t)((received * 100u) / expected_size);
|
||||
send_ack(seq);
|
||||
break;
|
||||
|
||||
case OTA_END: {
|
||||
if (!ota_started) { send_nack(seq, 0x02u); break; }
|
||||
g_io_ota_state = IO_OTA_VERIFYING;
|
||||
|
||||
uint8_t digest[32];
|
||||
mbedtls_sha256_finish(&sha, digest);
|
||||
if (memcmp(digest, expected_digest, 32) != 0) {
|
||||
ESP_LOGE(TAG, "SHA256 mismatch");
|
||||
esp_ota_abort(handle);
|
||||
send_nack(seq, 0x01u);
|
||||
g_io_ota_state = IO_OTA_FAILED;
|
||||
break;
|
||||
}
|
||||
|
||||
if (esp_ota_end(handle) != ESP_OK ||
|
||||
esp_ota_set_boot_partition(ota_part) != ESP_OK) {
|
||||
send_nack(seq, 0x02u);
|
||||
g_io_ota_state = IO_OTA_FAILED;
|
||||
break;
|
||||
}
|
||||
|
||||
g_io_ota_state = IO_OTA_REBOOTING;
|
||||
g_io_ota_progress = 100;
|
||||
ESP_LOGI(TAG, "OTA done — rebooting");
|
||||
send_ack(seq);
|
||||
vTaskDelay(pdMS_TO_TICKS(500));
|
||||
esp_restart();
|
||||
break;
|
||||
}
|
||||
|
||||
case OTA_ABORT:
|
||||
if (ota_started) { esp_ota_abort(handle); ota_started = false; }
|
||||
g_io_ota_state = IO_OTA_IDLE;
|
||||
ESP_LOGW(TAG, "OTA aborted");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void uart_ota_recv_init(void)
|
||||
{
|
||||
/* UART0 already initialized for inter-board comms; just create the task */
|
||||
xTaskCreate(ota_recv_task, "io_ota_recv", 8192, NULL, 6, NULL);
|
||||
ESP_LOGI(TAG, "OTA receiver task started");
|
||||
}
|
||||
@ -1,20 +0,0 @@
|
||||
#pragma once
|
||||
/* uart_ota_recv.h — IO board: receives OTA firmware from Balance (bd-21hv) */
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef enum {
|
||||
IO_OTA_IDLE = 0,
|
||||
IO_OTA_RECEIVING,
|
||||
IO_OTA_VERIFYING,
|
||||
IO_OTA_APPLYING,
|
||||
IO_OTA_REBOOTING,
|
||||
IO_OTA_FAILED,
|
||||
} io_ota_state_t;
|
||||
|
||||
extern volatile io_ota_state_t g_io_ota_state;
|
||||
extern volatile uint8_t g_io_ota_progress;
|
||||
|
||||
/* Start listening for OTA frames on UART0 */
|
||||
void uart_ota_recv_init(void);
|
||||
@ -1,7 +0,0 @@
|
||||
# ESP32-S3 IO — 4 MB flash, dual OTA partitions
|
||||
# Name, Type, SubType, Offset, Size
|
||||
nvs, data, nvs, 0x9000, 0x5000,
|
||||
otadata, data, ota, 0xe000, 0x2000,
|
||||
app0, app, ota_0, 0x10000, 0x1B0000,
|
||||
app1, app, ota_1, 0x1C0000, 0x1B0000,
|
||||
nvs_user, data, nvs, 0x370000, 0x50000,
|
||||
|
@ -1,13 +0,0 @@
|
||||
CONFIG_IDF_TARGET="esp32s3"
|
||||
CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
|
||||
CONFIG_FREERTOS_HZ=1000
|
||||
CONFIG_ESP_TASK_WDT_EN=y
|
||||
CONFIG_ESP_TASK_WDT_TIMEOUT_S=5
|
||||
CONFIG_UART_ISR_IN_IRAM=y
|
||||
CONFIG_ESP_CONSOLE_UART_DEFAULT=y
|
||||
CONFIG_LOG_DEFAULT_LEVEL_INFO=y
|
||||
|
||||
# OTA — bd-3gwo: dual OTA partitions + rollback
|
||||
CONFIG_PARTITION_TABLE_CUSTOM=y
|
||||
CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
|
||||
CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE=y
|
||||
@ -6,19 +6,15 @@ Self-balancing robot: Jetson Orin Nano Super dev environment for ROS2 Humble + S
|
||||
|
||||
| Component | Version / Part |
|
||||
|-----------|---------------|
|
||||
| Platform | Jetson Orin Nano Super 4GB |
|
||||
| JetPack | 4.6 (L4T R32.6.1, CUDA 10.2) |
|
||||
| Platform | Jetson Orin Nano Super 8GB |
|
||||
| JetPack | 6.x (L4T R36.x, CUDA 12.x) |
|
||||
| ROS2 | Humble Hawksbill |
|
||||
| DDS | CycloneDDS |
|
||||
| SLAM | slam_toolbox |
|
||||
| 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)
|
||||
| MCU bridge | ESP32-S3 BALANCE (CAN bus @ 500 kbps via CANable 2.0) |
|
||||
|
||||
## Quick Start
|
||||
|
||||
@ -46,15 +42,11 @@ 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)
|
||||
├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, CAN bridge)
|
||||
├── README.md # This file
|
||||
├── docs/
|
||||
│ ├── pinout.md # GPIO/I2C/UART pinout reference
|
||||
│ └── power-budget.md # Power budget analysis (10W envelope)
|
||||
│ └── power-budget.md # Power budget analysis (25W envelope)
|
||||
└── scripts/
|
||||
├── entrypoint.sh # Docker container entrypoint
|
||||
├── setup-jetson.sh # Host setup (udev, Docker, nvpmodel)
|
||||
@ -66,8 +58,8 @@ jetson/
|
||||
| Scenario | Total |
|
||||
|---------|-------|
|
||||
| Idle | 2.9W |
|
||||
| Nominal (SLAM active) | ~10.2W |
|
||||
| Peak | 15.4W |
|
||||
| Nominal (SLAM active) | ~19.9W |
|
||||
| Peak | ~28.2W |
|
||||
|
||||
Target: 10W (MAXN nvpmodel). Use RPLIDAR standby + 640p D435i for compliance.
|
||||
Target: 25W (MAXN nvpmodel). 5W headroom at nominal load.
|
||||
See [`docs/power-budget.md`](docs/power-budget.md) for full analysis.
|
||||
|
||||
@ -10,7 +10,7 @@ Recovery behaviors are triggered when Nav2 encounters navigation failures (path
|
||||
|
||||
### Backup Recovery (Issue #479)
|
||||
- **Distance**: 0.3 meters reverse
|
||||
- **Speed**: 0.1 m/s (very conservative for FC + Hoverboard ESC)
|
||||
- **Speed**: 0.1 m/s (very conservative for ESP32-S3 BALANCE + VESC)
|
||||
- **Max velocity**: 0.15 m/s (absolute limit)
|
||||
- **Time limit**: 5 seconds maximum
|
||||
|
||||
@ -34,20 +34,16 @@ 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)
|
||||
Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32-S3 BALANCE firmware.
|
||||
|
||||
## Behavior Tree Sequence
|
||||
|
||||
Recovery runs in a round-robin fashion with up to 6 retry cycles.
|
||||
|
||||
## Constraints for FC + Hoverboard ESC
|
||||
## Constraints for ESP32-S3 BALANCE + VESC
|
||||
|
||||
This configuration is specifically tuned for:
|
||||
- **Drivetrain**: Flux Capacitor (FC) balancing controller + Hoverboard brushless ESC
|
||||
- **Drivetrain**: ESP32-S3 BALANCE + VESC (CAN bus)
|
||||
- **Max linear velocity**: 1.0 m/s
|
||||
- **Max angular velocity**: 1.5 rad/s
|
||||
- **Recovery velocity constraints**: 50% of normal for stability
|
||||
|
||||
@ -97,7 +97,11 @@ services:
|
||||
rgb_camera.profile:=640x480x30
|
||||
"
|
||||
|
||||
# ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ───────────────────────
|
||||
<<<<<<< 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:
|
||||
image: saltybot/ros2-humble:jetson-orin
|
||||
build:
|
||||
@ -208,8 +212,13 @@ services:
|
||||
"
|
||||
|
||||
|
||||
# -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ---------------------
|
||||
<<<<<<< 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).
|
||||
remote-estop:
|
||||
image: saltybot/ros2-humble:jetson-orin
|
||||
@ -357,50 +366,6 @@ services:
|
||||
"
|
||||
|
||||
|
||||
# ── Here4 DroneCAN GPS + NTRIP RTK client ────────────────────────────────
|
||||
# Issue #725 — CubePilot Here4 RTK GPS via DroneCAN (1Mbps, SocketCAN)
|
||||
# Start: docker compose up -d here4-gps
|
||||
# Monitor fix: docker compose exec here4-gps ros2 topic echo /gps/rtk_status
|
||||
# Configure NTRIP: set NTRIP_MOUNT, NTRIP_USER, NTRIP_PASSWORD env vars
|
||||
here4-gps:
|
||||
image: saltybot/ros2-humble:jetson-orin
|
||||
build:
|
||||
context: .
|
||||
dockerfile: Dockerfile
|
||||
container_name: saltybot-here4-gps
|
||||
restart: unless-stopped
|
||||
runtime: nvidia
|
||||
network_mode: host
|
||||
depends_on:
|
||||
- saltybot-nav2
|
||||
environment:
|
||||
- ROS_DOMAIN_ID=42
|
||||
- RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
||||
# NTRIP credentials — set these in your .env or override at runtime
|
||||
- NTRIP_MOUNT=${NTRIP_MOUNT:-}
|
||||
- NTRIP_USER=${NTRIP_USER:-}
|
||||
- NTRIP_PASSWORD=${NTRIP_PASSWORD:-}
|
||||
volumes:
|
||||
- ./ros2_ws/src:/ros2_ws/src:rw
|
||||
- ./config:/config:ro
|
||||
devices:
|
||||
- /dev/can0:/dev/can0
|
||||
cap_add:
|
||||
- NET_ADMIN # needed for SocketCAN ip link set can0 up inside container
|
||||
command: >
|
||||
bash -c "
|
||||
source /opt/ros/humble/setup.bash &&
|
||||
source /ros2_ws/install/local_setup.bash 2>/dev/null || true &&
|
||||
pip install python-dronecan --quiet 2>/dev/null || true &&
|
||||
ros2 launch saltybot_dronecan_gps here4_gps.launch.py
|
||||
can_interface:=can0
|
||||
can_bitrate:=1000000
|
||||
ntrip_caster:=rtk2go.com
|
||||
ntrip_mount:=${NTRIP_MOUNT:-}
|
||||
ntrip_user:=${NTRIP_USER:-}
|
||||
ntrip_password:=${NTRIP_PASSWORD:-}
|
||||
"
|
||||
|
||||
volumes:
|
||||
saltybot-maps:
|
||||
driver: local
|
||||
|
||||
@ -1,9 +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)
|
||||
## Self-Balancing Robot: ESP32-S3 BALANCE (CAN bus) + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
|
||||
|
||||
Last updated: 2026-02-28
|
||||
JetPack version: 6.x (L4T R36.x / Ubuntu 22.04)
|
||||
@ -47,75 +43,36 @@ i2cdetect -l
|
||||
|
||||
---
|
||||
|
||||
<<<<<<< HEAD
|
||||
## 1. ESP32 Bridge (USB CDC — Primary)
|
||||
## 1. ESP32-S3 BALANCE Bridge (CAN bus — 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 BALANCE acts as a real-time motor + IMU controller. Communication is via **CAN bus** through a CANable 2.0 USB-CAN adapter.
|
||||
|
||||
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
|
||||
### CAN Bus 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) |
|
||||
| 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)
|
||||
|-----------|--------|-----------|-------|
|
||||
| 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)
|
||||
| Interface | CANable 2.0 USB-CAN adapter → USB-A on Jetson |
|
||||
| Device node | `/dev/can0` (via CANable 2.0, SocketCAN) |
|
||||
| Bitrate | 500 kbps |
|
||||
| Protocol | Binary CAN frames (see balance_protocol.py) |
|
||||
| Power | ESP32-S3 BALANCE powered from robot 5V DC-DC |
|
||||
|
||||
### Bring Up CAN Interface
|
||||
```bash
|
||||
# Verify UART
|
||||
ls /dev/ttyTHS0
|
||||
sudo usermod -aG dialout $USER
|
||||
# Quick test
|
||||
picocom -b 921600 /dev/ttyTHS0
|
||||
# Bring up can0 at 500 kbps
|
||||
sudo ip link set can0 up type can bitrate 500000
|
||||
ip link show can0
|
||||
|
||||
# Quick test — dump CAN frames
|
||||
candump can0
|
||||
```
|
||||
|
||||
<<<<<<< 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 topics (CAN bridge node):**
|
||||
| ROS2 Topic | Direction | Content |
|
||||
|-----------|-----------|---------
|
||||
| `/saltybot/imu` | ESP32-S3→Jetson | IMU data (accel, gyro) at 50Hz |
|
||||
| `/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)
|
||||
| `/cmd_vel` | Jetson→CAN 0x300 | Velocity commands via CAN |
|
||||
| `/saltybot/estop` | Jetson→CAN 0x302 | Emergency stop |
|
||||
|
||||
---
|
||||
|
||||
@ -300,11 +257,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)
|
||||
| USB-C | USB 3.1 Gen 1 (+ DP) | CANable 2.0 or host flash |
|
||||
| Micro-USB | Debug/flash | JetPack flash only |
|
||||
|
||||
---
|
||||
@ -315,17 +268,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) |
|
||||
| 8 | TXD0 | 3.3V | UART TX → ESP32-S3 IO (inter-board, fallback) |
|
||||
| 10 | RXD0 | 3.3V | UART RX ← ESP32-S3 IO (inter-board, 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)
|
||||
| USB-C | — | 5V | CANable 2.0 (CAN bus) |
|
||||
| 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,13 +289,10 @@ 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)
|
||||
# CANable 2.0 USB-CAN adapter
|
||||
# (bring up with: sudo ip link set can0 up type can bitrate 500000)
|
||||
KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
|
||||
SYMLINK+="esp32-bridge", MODE="0666"
|
||||
SYMLINK+="balance", MODE="0666"
|
||||
|
||||
# Intel RealSense D435i
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \
|
||||
|
||||
@ -56,11 +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)
|
||||
| ESP32-S3 BALANCE | 0.5 | 0.5 | 0.5 | CAN bus (SN65HVD230) | Powered from 5V DC-DC |
|
||||
| 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** | | |
|
||||
|
||||
@ -76,7 +72,7 @@ sudo jtop
|
||||
|
||||
## Budget Analysis vs Previous Platform
|
||||
|
||||
| Metric | Jetson Orin Nano Super | Jetson Orin Nano Super |
|
||||
| Metric | Jetson Nano | Jetson Orin Nano Super |
|
||||
|--------|------------|------------------------|
|
||||
| TDP | 10W | 25W |
|
||||
| CPU | 4× Cortex-A57 @ 1.43GHz | 6× A78AE @ 1.5GHz |
|
||||
@ -155,13 +151,9 @@ 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 BALANCE + ESP32-S3 IO + logic 5V rail
|
||||
│
|
||||
└─► Hoverboard ESC ──► Hub motors (48V loop)
|
||||
└─► VESC left (ID 68) + VESC right (ID 56) ──► Hub motors
|
||||
```
|
||||
|
||||
Using a 4S LiPo (vs 3S previously) gives better efficiency for the 5V buck converter
|
||||
|
||||
@ -1,64 +0,0 @@
|
||||
# saul-tee.conf — nginx site for saul-t-mote.evthings.app reverse proxy
|
||||
#
|
||||
# Replaces: python3 -m http.server 8080 --directory /home/seb
|
||||
# Router (ASUS Merlin / OpenResty) terminates TLS on :443 and proxies to Orin:8080
|
||||
#
|
||||
# Deploy:
|
||||
# sudo cp saul-tee.conf /etc/nginx/sites-available/saul-tee
|
||||
# sudo ln -sf /etc/nginx/sites-available/saul-tee /etc/nginx/sites-enabled/saul-tee
|
||||
# sudo systemctl reload nginx
|
||||
#
|
||||
# ROUTER REQUIREMENT for WSS to work:
|
||||
# The ASUS Merlin router's nginx/OpenResty must proxy /rosbridge with
|
||||
# WebSocket headers. Add to /jffs/configs/nginx.conf.add:
|
||||
#
|
||||
# map $http_upgrade $connection_upgrade {
|
||||
# default upgrade;
|
||||
# '' close;
|
||||
# }
|
||||
#
|
||||
# And in the server block that proxies to 192.168.86.158:8080, add:
|
||||
#
|
||||
# location /rosbridge {
|
||||
# proxy_pass http://192.168.86.158:8080/rosbridge;
|
||||
# proxy_http_version 1.1;
|
||||
# proxy_set_header Upgrade $http_upgrade;
|
||||
# proxy_set_header Connection $connection_upgrade;
|
||||
# proxy_read_timeout 86400;
|
||||
# }
|
||||
|
||||
# Infer WebSocket upgrade from Connection header.
|
||||
# Handles the case where an upstream proxy (e.g. ASUS Merlin OpenResty)
|
||||
# passes Connection: upgrade but strips the Upgrade: websocket header.
|
||||
map $http_connection $ws_upgrade {
|
||||
"upgrade" "websocket";
|
||||
default "";
|
||||
}
|
||||
|
||||
server {
|
||||
listen 8080 default_server;
|
||||
listen [::]:8080 default_server;
|
||||
|
||||
root /home/seb;
|
||||
index index.html;
|
||||
server_name _;
|
||||
|
||||
# Static files — replaces python3 -m http.server 8080 --directory /home/seb
|
||||
location / {
|
||||
try_files $uri $uri/ =404;
|
||||
autoindex on;
|
||||
}
|
||||
|
||||
# rosbridge WebSocket reverse proxy
|
||||
# wss://saul-t-mote.evthings.app/rosbridge --> ws://127.0.0.1:9090
|
||||
location /rosbridge {
|
||||
proxy_pass http://127.0.0.1:9090/;
|
||||
proxy_http_version 1.1;
|
||||
proxy_set_header Upgrade $ws_upgrade;
|
||||
proxy_set_header Connection "upgrade";
|
||||
proxy_set_header Host $host;
|
||||
proxy_set_header X-Real-IP $remote_addr;
|
||||
proxy_read_timeout 86400;
|
||||
proxy_send_timeout 86400;
|
||||
}
|
||||
}
|
||||
@ -2,11 +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)
|
||||
Bridges ESP32-S3 BALANCE telemetry from inter-board UART into ROS2.
|
||||
|
||||
Published topics (same as USB CDC bridge):
|
||||
/saltybot/imu sensor_msgs/Imu — pitch/roll/yaw as angular velocity
|
||||
@ -24,11 +20,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)
|
||||
- STM32 firmware transmitting JSON telemetry frames (50 Hz)
|
||||
- ROS2 environment sourced (source install/setup.bash)
|
||||
|
||||
Note:
|
||||
|
||||
@ -1,9 +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)
|
||||
cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 drive command bridge.
|
||||
|
||||
Extends the basic saltybot_cmd_node with four additions required for safe
|
||||
autonomous operation on a self-balancing robot:
|
||||
@ -16,11 +12,7 @@ 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)
|
||||
4. Mode gate — only issue non-zero drive commands when STM32 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
|
||||
@ -28,15 +20,9 @@ autonomous operation on a self-balancing robot:
|
||||
|
||||
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.
|
||||
H\\n — heartbeat. STM32 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)
|
||||
Telemetry (50 Hz from STM32):
|
||||
Same RX/publish pipeline as saltybot_cmd_node.
|
||||
The "md" field (0=MANUAL,1=ASSISTED,2=AUTO) is parsed for the mode gate.
|
||||
|
||||
@ -148,7 +134,7 @@ class CmdVelBridgeNode(Node):
|
||||
self._current_speed = 0 # ramped output actually sent
|
||||
self._current_steer = 0
|
||||
self._last_cmd_vel = 0.0 # wall clock (seconds) of last /cmd_vel msg
|
||||
self._esp32_mode = 0 # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO
|
||||
self._stm32_mode = 0 # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO
|
||||
self._last_state = -1
|
||||
self._frame_count = 0
|
||||
self._error_count = 0
|
||||
@ -164,11 +150,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)
|
||||
# Telemetry read at 100 Hz (STM32 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)
|
||||
@ -243,7 +225,7 @@ class CmdVelBridgeNode(Node):
|
||||
|
||||
# Mode gate: in non-AUTONOMOUS mode, zero and reset ramp state so
|
||||
# re-entry always accelerates smoothly from 0.
|
||||
if self._esp32_mode != MODE_AUTONOMOUS:
|
||||
if self._stm32_mode != MODE_AUTONOMOUS:
|
||||
self._current_speed = 0
|
||||
self._current_steer = 0
|
||||
speed, steer = 0, 0
|
||||
@ -256,11 +238,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)
|
||||
# Send to STM32
|
||||
frame = f"C{speed},{steer}\n".encode("ascii")
|
||||
if not self._write(frame):
|
||||
self.get_logger().warn(
|
||||
@ -278,11 +256,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)
|
||||
"""H\\n keeps STM32 jetson_cmd heartbeat alive regardless of mode."""
|
||||
self._write(b"H\n")
|
||||
|
||||
# ── Telemetry RX ──────────────────────────────────────────────────────────
|
||||
@ -345,7 +319,7 @@ class CmdVelBridgeNode(Node):
|
||||
state = int(data["s"])
|
||||
mode = int(data.get("md", 0)) # 0=MANUAL if not present
|
||||
|
||||
self._esp32_mode = mode
|
||||
self._stm32_mode = mode
|
||||
self._frame_count += 1
|
||||
|
||||
self._publish_imu(pitch_deg, roll_deg, yaw_deg, now)
|
||||
@ -404,11 +378,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.hardware_id = "esp32s3_balance"
|
||||
status.message = f"{state_label} [{mode_label}]"
|
||||
status.level = (
|
||||
DiagnosticStatus.OK if state == 1 else
|
||||
@ -436,19 +406,11 @@ class CmdVelBridgeNode(Node):
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
status.hardware_id = "esp32s3_balance"
|
||||
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)
|
||||
self.get_logger().error(f"STM32 IMU fault: errno={errno}")
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
@ -1,66 +1,45 @@
|
||||
<<<<<<< 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
|
||||
"""esp32_cmd_node.py — Full bidirectional binary-framed ESP32-S3 BALANCE↔Jetson bridge.
|
||||
|
||||
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).
|
||||
Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
|
||||
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 460800 baud (inter-board UART).
|
||||
|
||||
<<<<<<< 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):
|
||||
TX commands (Jetson → ESP32-S3 BALANCE):
|
||||
SPEED_STEER — 50 Hz from /cmd_vel subscription
|
||||
HEARTBEAT — 200 ms timer (ESP32-S3 watchdog fires at 500 ms)
|
||||
HEARTBEAT — 200 ms timer (ESP32-S3 BALANCE watchdog fires at 500 ms)
|
||||
ARM — via /saltybot/arm service
|
||||
SET_MODE — via /saltybot/set_mode service
|
||||
PID_UPDATE — via /saltybot/pid_update topic
|
||||
>>>>>>> 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
|
||||
Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
|
||||
|
||||
<<<<<<< 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):
|
||||
RX telemetry (ESP32-S3 BALANCE → Jetson):
|
||||
IMU → /saltybot/imu (sensor_msgs/Imu)
|
||||
BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON)
|
||||
MOTOR_RPM → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
|
||||
ARM_STATE → /saltybot/arm_state (std_msgs/String JSON)
|
||||
ERROR → /saltybot/error (std_msgs/String JSON)
|
||||
All frames → /diagnostics (diagnostic_msgs/DiagnosticArray)
|
||||
>>>>>>> 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)
|
||||
Auto-reconnect: USB disconnect is detected when serial.read() raises; node
|
||||
continuously retries at reconnect_delay interval.
|
||||
|
||||
This node owns /dev/can0 exclusively — do NOT run alongside
|
||||
serial_bridge_node or saltybot_cmd_node on the same port.
|
||||
|
||||
<<<<<<< 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
|
||||
serial_port /dev/can0
|
||||
baud_rate 460800
|
||||
reconnect_delay 2.0 (seconds)
|
||||
heartbeat_period 0.2 (seconds)
|
||||
watchdog_timeout 0.5 (seconds — no /cmd_vel → send zero-speed)
|
||||
speed_scale 1000.0 (linear.x m/s → ESC units)
|
||||
steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only):jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/esp32_cmd_node.py
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
|
||||
@ -71,82 +50,119 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
import serial
|
||||
|
||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
from geometry_msgs.msg import Twist
|
||||
from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import SetBool, Trigger
|
||||
|
||||
<<<<<<< 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,
|
||||
RcChannels,
|
||||
SensorData,
|
||||
encode_heartbeat,
|
||||
encode_led_cmd,
|
||||
encode_output_cmd,
|
||||
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
||||
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
|
||||
encode_pid_update,
|
||||
)
|
||||
|
||||
# ── Constants ─────────────────────────────────────────────────────────────────
|
||||
|
||||
class Stm32CmdNode(Node):
|
||||
<<<<<<< HEAD:jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
|
||||
"""Orin ↔ ESP32-S3 IO auxiliary bridge 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
|
||||
IMU_FRAME_ID = "imu_link"
|
||||
_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
||||
|
||||
|
||||
def _clamp(v: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, v))
|
||||
|
||||
|
||||
# ── Node ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
class Esp32CmdNode(Node):
|
||||
"""Binary-framed Jetson↔ESP32-S3 BALANCE bridge node."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("esp32_cmd_node")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/esp32-io")
|
||||
self.declare_parameter("baud_rate", BAUD_RATE)
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/can0")
|
||||
self.declare_parameter("baud_rate", 460800)
|
||||
self.declare_parameter("reconnect_delay", 2.0)
|
||||
self.declare_parameter("heartbeat_period", 0.2)
|
||||
self.declare_parameter("watchdog_timeout", 0.5)
|
||||
self.declare_parameter("speed_scale", 1000.0)
|
||||
self.declare_parameter("steer_scale", -500.0)
|
||||
|
||||
self._port_name = self.get_parameter("serial_port").value
|
||||
self._baud = self.get_parameter("baud_rate").value
|
||||
port = self.get_parameter("serial_port").value
|
||||
baud = self.get_parameter("baud_rate").value
|
||||
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
||||
self._hb_period = self.get_parameter("heartbeat_period").value
|
||||
self._wd_timeout = self.get_parameter("watchdog_timeout").value
|
||||
self._speed_scale = self.get_parameter("speed_scale").value
|
||||
self._steer_scale = self.get_parameter("steer_scale").value
|
||||
|
||||
# ── QoS ───────────────────────────────────────────────────────────
|
||||
# ── QoS ───────────────────────────────────────────────────────────────
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
||||
)
|
||||
rel_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
||||
)
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos)
|
||||
self._sens_pub = self.create_publisher(String, "/saltybot/sensors", rel_qos)
|
||||
# ── Publishers ────────────────────────────────────────────────────────
|
||||
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos)
|
||||
self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos)
|
||||
self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos)
|
||||
self._battery_pub = self.create_publisher(String, "/saltybot/telemetry/battery", rel_qos)
|
||||
self._rpm_pub = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
|
||||
self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos)
|
||||
self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
|
||||
# ── Subscribers ───────────────────────────────────────────────────────
|
||||
self._cmd_vel_sub = self.create_subscription(
|
||||
Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
|
||||
)
|
||||
self._pid_sub = self.create_subscription(
|
||||
String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
|
||||
)
|
||||
|
||||
# ── Serial state ──────────────────────────────────────────────────
|
||||
# ── Services ──────────────────────────────────────────────────────────
|
||||
self._arm_srv = self.create_service(SetBool, "/saltybot/arm", self._svc_arm)
|
||||
self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode)
|
||||
|
||||
# ── Serial state ──────────────────────────────────────────────────────
|
||||
self._port_name = port
|
||||
self._baud = baud
|
||||
self._ser: serial.Serial | None = None
|
||||
self._ser_lock = threading.Lock()
|
||||
self._parser = FrameParser()
|
||||
self._rx_count = 0
|
||||
|
||||
# ── Open serial and start timers ──────────────────────────────────
|
||||
# ── TX state ──────────────────────────────────────────────────────────
|
||||
self._last_speed = 0
|
||||
self._last_steer = 0
|
||||
self._last_cmd_t = time.monotonic()
|
||||
self._watchdog_sent = False # tracks whether we already sent zero
|
||||
|
||||
# ── Diagnostics state ──────────────────────────────────────────────────
|
||||
self._last_arm_state = -1
|
||||
self._last_battery_mv = 0
|
||||
self._rx_frame_count = 0
|
||||
|
||||
# ── Open serial and start timers ──────────────────────────────────────
|
||||
self._open_serial()
|
||||
|
||||
# Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean)
|
||||
self._read_timer = self.create_timer(0.005, self._read_cb)
|
||||
# Heartbeat TX
|
||||
self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb)
|
||||
# Watchdog check (fires at 2× watchdog_timeout for quick detection)
|
||||
self._wd_timer = self.create_timer(self._wd_timeout / 2, self._watchdog_cb)
|
||||
# Periodic diagnostics
|
||||
self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
|
||||
|
||||
self.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
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────
|
||||
# ── Serial management ─────────────────────────────────────────────────────
|
||||
|
||||
def _open_serial(self) -> bool:
|
||||
with self._ser_lock:
|
||||
@ -154,7 +170,7 @@ class Stm32CmdNode(Node):
|
||||
self._ser = serial.Serial(
|
||||
port=self._port_name,
|
||||
baudrate=self._baud,
|
||||
timeout=0.005,
|
||||
timeout=0.005, # non-blocking reads
|
||||
write_timeout=0.1,
|
||||
)
|
||||
self._ser.reset_input_buffer()
|
||||
@ -169,7 +185,17 @@ class Stm32CmdNode(Node):
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
def _close_serial(self) -> None:
|
||||
with self._ser_lock:
|
||||
if self._ser and self._ser.is_open:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
|
||||
def _write(self, data: bytes) -> bool:
|
||||
"""Thread-safe serial write. Returns False if port is not open."""
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
return False
|
||||
@ -181,15 +207,16 @@ class Stm32CmdNode(Node):
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
# ── RX ────────────────────────────────────────────────────────────────
|
||||
# ── RX — read callback ────────────────────────────────────────────────────
|
||||
|
||||
def _read_cb(self) -> None:
|
||||
"""Read bytes from serial and feed them to the frame parser."""
|
||||
raw: bytes | None = None
|
||||
reconnect = False
|
||||
reconnect_needed = False
|
||||
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
reconnect = True
|
||||
reconnect_needed = True
|
||||
else:
|
||||
try:
|
||||
n = self._ser.in_waiting
|
||||
@ -198,9 +225,9 @@ class Stm32CmdNode(Node):
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial read error: {exc}")
|
||||
self._ser = None
|
||||
reconnect = True
|
||||
reconnect_needed = True
|
||||
|
||||
if reconnect:
|
||||
if reconnect_needed:
|
||||
self.get_logger().warn(
|
||||
"Serial disconnected — will retry",
|
||||
throttle_duration_sec=self._reconnect_delay,
|
||||
@ -213,41 +240,24 @@ class Stm32CmdNode(Node):
|
||||
return
|
||||
|
||||
for byte in raw:
|
||||
msg = self._parser.feed(byte)
|
||||
if msg is not None:
|
||||
self._rx_count += 1
|
||||
self._dispatch(msg)
|
||||
frame = self._parser.feed(byte)
|
||||
if frame is not None:
|
||||
self._rx_frame_count += 1
|
||||
self._dispatch_frame(frame)
|
||||
|
||||
def _dispatch(self, msg) -> None:
|
||||
def _dispatch_frame(self, frame) -> None:
|
||||
"""Route a decoded frame to the appropriate publisher."""
|
||||
now = self.get_clock().now().to_msg()
|
||||
ts = f"{now.sec}.{now.nanosec:09d}"
|
||||
|
||||
if isinstance(msg, RcChannels):
|
||||
out = String()
|
||||
out.data = json.dumps({
|
||||
"channels": msg.channels,
|
||||
"source": msg.source,
|
||||
"ts": ts,
|
||||
})
|
||||
self._rc_pub.publish(out)
|
||||
if isinstance(frame, ImuFrame):
|
||||
self._publish_imu(frame, now)
|
||||
|
||||
elif isinstance(msg, SensorData):
|
||||
out = String()
|
||||
out.data = json.dumps({
|
||||
"pressure_pa": msg.pressure_pa,
|
||||
"temperature_c": msg.temperature_c,
|
||||
"tof_mm": msg.tof_mm,
|
||||
"ts": ts,
|
||||
})
|
||||
self._sens_pub.publish(out)
|
||||
elif isinstance(frame, BatteryFrame):
|
||||
self._publish_battery(frame, now)
|
||||
|
||||
elif isinstance(msg, tuple):
|
||||
type_code, _ = msg
|
||||
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
|
||||
elif isinstance(frame, MotorRpmFrame):
|
||||
self._publish_motor_rpm(frame, now)
|
||||
|
||||
<<<<<<< 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,85 +368,108 @@ 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:
|
||||
"""Send HEARTBEAT every heartbeat_period (default 200ms)."""
|
||||
self._write(encode_heartbeat())
|
||||
|
||||
def _on_leds(self, msg: String) -> None:
|
||||
"""Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
|
||||
try:
|
||||
d = json.loads(msg.data)
|
||||
frame = encode_led_cmd(
|
||||
int(d.get("pattern", 0)),
|
||||
int(d.get("r", 0)),
|
||||
int(d.get("g", 0)),
|
||||
int(d.get("b", 0)),
|
||||
def _watchdog_cb(self) -> None:
|
||||
"""Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
|
||||
if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
|
||||
if not self._watchdog_sent:
|
||||
self.get_logger().warn(
|
||||
f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
|
||||
)
|
||||
except (ValueError, KeyError, json.JSONDecodeError) as exc:
|
||||
self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
|
||||
return
|
||||
self._write(frame)
|
||||
self._watchdog_sent = True
|
||||
self._last_speed = 0
|
||||
self._last_steer = 0
|
||||
self._write(encode_speed_steer(0, 0))
|
||||
|
||||
def _on_outputs(self, msg: String) -> None:
|
||||
"""Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
|
||||
def _on_pid_update(self, msg: String) -> None:
|
||||
"""Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
|
||||
try:
|
||||
d = json.loads(msg.data)
|
||||
frame = encode_output_cmd(
|
||||
bool(d.get("horn", False)),
|
||||
bool(d.get("buzzer", False)),
|
||||
int(d.get("headlight", 0)),
|
||||
int(d.get("fan", 0)),
|
||||
)
|
||||
data = json.loads(msg.data)
|
||||
kp = float(data["kp"])
|
||||
ki = float(data["ki"])
|
||||
kd = float(data["kd"])
|
||||
except (ValueError, KeyError, json.JSONDecodeError) as exc:
|
||||
self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}")
|
||||
self.get_logger().error(f"Bad PID update JSON: {exc}")
|
||||
return
|
||||
self._write(frame)
|
||||
frame = encode_pid_update(kp, ki, kd)
|
||||
if self._write(frame):
|
||||
self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
|
||||
else:
|
||||
self.get_logger().warn("PID_UPDATE dropped — serial not open")
|
||||
|
||||
# ── Diagnostics ───────────────────────────────────────────────────────
|
||||
# ── Services ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _svc_arm(self, request: SetBool.Request, response: SetBool.Response):
|
||||
"""SetBool(True) = arm, SetBool(False) = disarm."""
|
||||
arm = request.data
|
||||
frame = encode_arm(arm)
|
||||
ok = self._write(frame)
|
||||
response.success = ok
|
||||
response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open"
|
||||
self.get_logger().info(
|
||||
f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}"
|
||||
)
|
||||
return response
|
||||
|
||||
def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response):
|
||||
"""SetBool: data maps to mode byte (True=1, False=0)."""
|
||||
mode = 1 if request.data else 0
|
||||
frame = encode_set_mode(mode)
|
||||
ok = self._write(frame)
|
||||
response.success = ok
|
||||
response.message = f"mode={mode}" if ok else "serial not open"
|
||||
return response
|
||||
|
||||
# ── Diagnostics ───────────────────────────────────────────────────────────
|
||||
|
||||
def _publish_diagnostics(self) -> None:
|
||||
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
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/esp32_cmd_node"
|
||||
status.hardware_id = "esp32s3_balance"
|
||||
|
||||
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}"
|
||||
if port_ok:
|
||||
status.level = DiagnosticStatus.OK
|
||||
status.message = "Serial OK"
|
||||
else:
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.message = f"Serial disconnected: {self._port_name}"
|
||||
|
||||
wd_age = time.monotonic() - self._last_cmd_t
|
||||
status.values = [
|
||||
KeyValue(key="serial_port", value=self._port_name),
|
||||
KeyValue(key="baud_rate", value=str(self._baud)),
|
||||
KeyValue(key="port_open", value=str(port_ok)),
|
||||
KeyValue(key="rx_frames", value=str(self._rx_count)),
|
||||
KeyValue(key="rx_frames", value=str(self._rx_frame_count)),
|
||||
KeyValue(key="rx_errors", value=str(self._parser.frames_error)),
|
||||
KeyValue(key="last_speed", value=str(self._last_speed)),
|
||||
KeyValue(key="last_steer", value=str(self._last_steer)),
|
||||
KeyValue(key="cmd_vel_age_s", value=f"{wd_age:.2f}"),
|
||||
KeyValue(key="battery_mv", value=str(self._last_battery_mv)),
|
||||
KeyValue(key="arm_state", value=_ARM_LABEL.get(self._last_arm_state, "?")),
|
||||
]
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._write(encode_heartbeat(state=0))
|
||||
with self._ser_lock:
|
||||
if self._ser and self._ser.is_open:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
# Send zero-speed + disarm on shutdown
|
||||
self._write(encode_speed_steer(0, 0))
|
||||
self._write(encode_arm(False))
|
||||
self._close_serial()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = Stm32CmdNode()
|
||||
node = Esp32CmdNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
"""esp32_protocol.py — Binary frame codec for Jetson↔ESP32-S3 communication.
|
||||
"""esp32_protocol.py — Binary frame codec for Jetson↔ESP32-S3 BALANCE communication.
|
||||
|
||||
Issue #119: defines the binary serial protocol between the Jetson Orin Nano Super and the
|
||||
ESP32-S3 ESP32-S3 BALANCE over USB CDC @ 921600 baud.
|
||||
ESP32-S3 BALANCE board over inter-board UART @ 460800 baud.
|
||||
|
||||
Frame layout (all multi-byte fields are big-endian):
|
||||
┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
|
||||
@ -12,14 +12,14 @@ Frame layout (all multi-byte fields are big-endian):
|
||||
CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
|
||||
CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
|
||||
|
||||
Command types (Jetson → ESP32-S3):
|
||||
Command types (Jetson → ESP32-S3 BALANCE):
|
||||
0x01 HEARTBEAT — no payload (len=0)
|
||||
0x02 SPEED_STEER — int16 speed + int16 steer (len=4) range: -1000..+1000
|
||||
0x03 ARM — uint8 (0=disarm, 1=arm) (len=1)
|
||||
0x04 SET_MODE — uint8 mode (len=1)
|
||||
0x05 PID_UPDATE — float32 kp + ki + kd (len=12)
|
||||
|
||||
Telemetry types (ESP32-S3 → Jetson):
|
||||
Telemetry types (ESP32-S3 BALANCE → Jetson):
|
||||
0x10 IMU — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12)
|
||||
0x11 BATTERY — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5)
|
||||
0x12 MOTOR_RPM — int16 left_rpm + int16 right_rpm (len=4)
|
||||
@ -27,11 +27,11 @@ Telemetry types (ESP32-S3 → Jetson):
|
||||
0x14 ERROR — uint8 error_code + uint8 subcode (len=2)
|
||||
|
||||
Usage:
|
||||
# Encoding (Jetson → ESP32-S3)
|
||||
# Encoding (Jetson → ESP32-S3 BALANCE)
|
||||
frame = encode_speed_steer(300, -150)
|
||||
ser.write(frame)
|
||||
|
||||
# Decoding (ESP32-S3 → Jetson), one byte at a time
|
||||
# Decoding (ESP32-S3 BALANCE → Jetson), one byte at a time
|
||||
parser = FrameParser()
|
||||
for byte in incoming_bytes:
|
||||
result = parser.feed(byte)
|
||||
@ -87,7 +87,7 @@ class ImuFrame:
|
||||
class BatteryFrame:
|
||||
voltage_mv: int # millivolts (e.g. 11100 = 11.1 V)
|
||||
current_ma: int # milliamps (negative = charging)
|
||||
soc_pct: int # state of charge 0–100 (from ESP32-S3 fuel gauge or lookup)
|
||||
soc_pct: int # state of charge 0–100 (from ESP32-S3 BALANCE fuel gauge or lookup)
|
||||
|
||||
|
||||
@dataclass
|
||||
@ -183,7 +183,7 @@ class ParseError(Exception):
|
||||
|
||||
|
||||
class FrameParser:
|
||||
"""Byte-by-byte streaming parser for ESP32-S3 telemetry frames.
|
||||
"""Byte-by-byte streaming parser for ESP32-S3 BALANCE telemetry frames.
|
||||
|
||||
Feed individual bytes via feed(); returns a decoded TelemetryFrame (or raw
|
||||
bytes tuple) when a complete valid frame is received.
|
||||
|
||||
@ -322,11 +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.hardware_id = "esp32s3_balance"
|
||||
st.message = state_label
|
||||
st.level = (DiagnosticStatus.OK if state == 1 else
|
||||
DiagnosticStatus.WARN if state == 0 else
|
||||
|
||||
@ -1,38 +1,20 @@
|
||||
"""
|
||||
<<<<<<< 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)
|
||||
saltybot_cmd_node — full bidirectional STM32↔Jetson bridge
|
||||
Combines 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):
|
||||
RX path (50Hz from STM32):
|
||||
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
|
||||
/cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n → STM32
|
||||
Heartbeat timer (200ms) → H\\n → STM32
|
||||
|
||||
Protocol:
|
||||
H\\n — heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms.
|
||||
H\\n — heartbeat. STM32 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
|
||||
|
||||
TX path:
|
||||
/cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n → ESP32-S3
|
||||
Heartbeat timer (200ms) → H\\n → ESP32-S3
|
||||
|
||||
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)
|
||||
C command also refreshes STM32 heartbeat timer.
|
||||
|
||||
Twist mapping (configurable via ROS2 params):
|
||||
speed = clamp(linear.x * speed_scale, -1000, 1000)
|
||||
@ -118,11 +100,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)
|
||||
# Telemetry read at 100Hz (STM32 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,11 +266,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.hardware_id = "esp32s3_balance"
|
||||
status.message = state_label
|
||||
if state == 1:
|
||||
status.level = DiagnosticStatus.OK
|
||||
@ -320,19 +294,11 @@ class SaltybotCmdNode(Node):
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
status.hardware_id = "esp32s3_balance"
|
||||
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)
|
||||
self.get_logger().error(f"STM32 IMU fault: errno={errno}")
|
||||
|
||||
# ── TX — command send ─────────────────────────────────────────────────────
|
||||
|
||||
@ -350,11 +316,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)
|
||||
"""Send H\\n heartbeat. STM32 reverts steer to 0 if gap > 500ms."""
|
||||
self._write(b"H\n")
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
|
||||
@ -1,10 +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)
|
||||
ESP32-S3 BALANCE → ROS2 topic publisher (inter-board UART)
|
||||
|
||||
Telemetry frame (50 Hz, newline-delimited JSON):
|
||||
{"p":<pitch×10>,"r":<roll×10>,"e":<err×10>,"ig":<integral×10>,
|
||||
@ -33,11 +29,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)
|
||||
# Balance state labels matching STM32 balance_state_t enum
|
||||
_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
||||
|
||||
# Sensor frame_id published in Imu header
|
||||
@ -46,7 +38,7 @@ IMU_FRAME_ID = "imu_link"
|
||||
|
||||
class SerialBridgeNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__("esp32_serial_bridge")
|
||||
super().__init__("stm32_serial_bridge")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
||||
@ -91,11 +83,7 @@ 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)
|
||||
# Poll at 100 Hz — STM32 sends at 50 Hz, so we never miss a frame
|
||||
self._timer = self.create_timer(0.01, self._read_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
@ -129,11 +117,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)
|
||||
Send raw bytes to STM32 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.
|
||||
"""
|
||||
@ -222,11 +206,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)
|
||||
The STM32 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.
|
||||
|
||||
@ -284,11 +264,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.hardware_id = "esp32s3_balance"
|
||||
status.message = state_label
|
||||
|
||||
if state == 1: # ARMED
|
||||
@ -317,19 +293,11 @@ class SerialBridgeNode(Node):
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
<<<<<<< HEAD
|
||||
status.hardware_id = "esp32"
|
||||
status.hardware_id = "esp32s3_balance"
|
||||
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)
|
||||
self.get_logger().error(f"STM32 reported IMU fault: errno={errno}")
|
||||
|
||||
def destroy_node(self):
|
||||
self._close_serial()
|
||||
|
||||
@ -53,14 +53,7 @@ rosbridge_websocket:
|
||||
"/vesc/left/state",
|
||||
"/vesc/right/state",
|
||||
"/tf",
|
||||
"/tf_static",
|
||||
"/saltybot/phone/gps",
|
||||
"/saltybot/phone/imu",
|
||||
"/saltybot/phone/battery",
|
||||
"/saltybot/phone/bridge/status",
|
||||
"/gps/fix",
|
||||
"/gps/vel",
|
||||
"/saltybot/ios/gps"]
|
||||
"/tf_static"]
|
||||
|
||||
services_glob: "[]" # no service calls via WebSocket
|
||||
params_glob: "[]" # no parameter access via WebSocket
|
||||
@ -74,7 +67,7 @@ rosbridge_websocket:
|
||||
|
||||
# Delay between consecutive outgoing messages (ms). 0 = unlimited.
|
||||
# Set > 0 (e.g. 10) if browser JS event loop is overwhelmed.
|
||||
delay_between_messages: 0.0
|
||||
delay_between_messages: 0
|
||||
|
||||
# ── Logging ───────────────────────────────────────────────────────────────
|
||||
# Set to true to log every publish/subscribe call (verbose, dev only).
|
||||
|
||||
@ -39,7 +39,11 @@ 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)
|
||||
|
||||
outdoor
|
||||
@ -57,8 +61,13 @@ 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= 4s UWB driver (independent serial device)
|
||||
t= 4s CSI cameras (optional, independent)
|
||||
@ -71,10 +80,17 @@ 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.
|
||||
• To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}'
|
||||
|
||||
@ -91,7 +107,11 @@ 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/balance_state String
|
||||
"""
|
||||
@ -209,7 +229,11 @@ 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)
|
||||
)
|
||||
|
||||
enable_rosbridge_arg = DeclareLaunchArgument(
|
||||
@ -270,7 +294,11 @@ def generate_launch_description():
|
||||
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)
|
||||
)
|
||||
|
||||
# ── Shared substitution handles ───────────────────────────────────────────
|
||||
@ -290,7 +318,11 @@ def generate_launch_description():
|
||||
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||
)
|
||||
|
||||
# ── t=0s ESP32-S3 bidirectional serial bridge ───────────────────────────────
|
||||
<<<<<<< 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(
|
||||
condition=IfCondition(LaunchConfiguration("enable_bridge")),
|
||||
actions=[
|
||||
@ -320,7 +352,11 @@ def generate_launch_description():
|
||||
],
|
||||
)
|
||||
|
||||
<<<<<<< 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(
|
||||
period=2.0,
|
||||
actions=[
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
[Unit]
|
||||
Description=CANable 2.0 CAN bus bringup (can0, 1 Mbps DroneCAN — Here4 GPS)
|
||||
Description=CANable 2.0 CAN bus bringup (can0, 500 kbps)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
|
||||
# Wait until the gs_usb net device appears; udev fires After=sys-subsystem-net-devices-can0.device
|
||||
After=network.target sys-subsystem-net-devices-can0.device
|
||||
@ -10,7 +10,7 @@ BindsTo=sys-subsystem-net-devices-can0.device
|
||||
Type=oneshot
|
||||
RemainAfterExit=yes
|
||||
|
||||
ExecStart=/usr/sbin/ip link set can0 up type can bitrate 1000000
|
||||
ExecStart=/usr/sbin/ip link set can0 up type can bitrate 500000
|
||||
ExecStop=/usr/sbin/ip link set can0 down
|
||||
|
||||
StandardOutput=journal
|
||||
|
||||
@ -61,11 +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)
|
||||
- ESP32-S3 CAN Bridge
|
||||
- cmd_vel Bridge
|
||||
- Rosbridge WebSocket
|
||||
|
||||
@ -129,15 +125,11 @@ 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)
|
||||
# Verify CAN bridge is running first
|
||||
ros2 node list | grep bridge
|
||||
|
||||
# Check serial port
|
||||
ls -l /dev/esp32-bridge
|
||||
# Check CAN interface
|
||||
ip link show can0
|
||||
```
|
||||
|
||||
## Performance Baseline
|
||||
|
||||
@ -2,10 +2,6 @@
|
||||
# Exposes the adapter as can0 via the SocketCAN subsystem.
|
||||
# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
|
||||
#
|
||||
# Bitrate is set by can-bringup.service (1 Mbps DroneCAN for Here4 GPS, bd-p47c).
|
||||
# This rule only assigns the interface name and tags it for systemd; it does NOT
|
||||
# bring up the interface — that is handled by can-bringup.service.
|
||||
#
|
||||
# Install:
|
||||
# sudo cp 70-canable.rules /etc/udev/rules.d/
|
||||
# sudo udevadm control --reload && sudo udevadm trigger
|
||||
@ -20,4 +16,4 @@
|
||||
SUBSYSTEM=="net", ACTION=="add", \
|
||||
ATTRS{idVendor}=="1d50", ATTRS{idProduct}=="606f", \
|
||||
NAME="can0", \
|
||||
TAG+="systemd"
|
||||
RUN+="/sbin/ip link set can0 up type can bitrate 500000"
|
||||
|
||||
@ -1,31 +0,0 @@
|
||||
# ESP32-S3 USB serial devices (bd-wim1)
|
||||
#
|
||||
# ESP32-S3 BALANCE (Waveshare LCD 1.28) — USB CDC via CH343 USB-UART chip
|
||||
# Appears as /dev/ttyACM0, symlinked to /dev/esp32-balance
|
||||
# idVendor = 1a86 (QinHeng Electronics / WCH)
|
||||
# idProduct = 55d4 (CH343 USB-UART)
|
||||
#
|
||||
# ESP32-S3 IO (bare board JTAG USB) — native USB CDC
|
||||
# Appears as /dev/ttyACM1, symlinked to /dev/esp32-io
|
||||
# idVendor = 303a (Espressif)
|
||||
# idProduct = 1001 (ESP32-S3 USB CDC)
|
||||
#
|
||||
# Install:
|
||||
# sudo cp 80-esp32.rules /etc/udev/rules.d/
|
||||
# sudo udevadm control --reload && sudo udevadm trigger
|
||||
#
|
||||
# Verify:
|
||||
# ls -la /dev/esp32-*
|
||||
# python3 -c "import serial; s=serial.Serial('/dev/esp32-balance', 460800); s.close(); print('OK')"
|
||||
|
||||
# ESP32-S3 BALANCE — CH343 USB-UART (bd-wim1 UART serial bridge)
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", \
|
||||
SYMLINK+="esp32-balance", \
|
||||
TAG+="systemd", \
|
||||
MODE="0660", GROUP="dialout"
|
||||
|
||||
# ESP32-S3 IO — Espressif native USB CDC
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", \
|
||||
SYMLINK+="esp32-io", \
|
||||
TAG+="systemd", \
|
||||
MODE="0660", GROUP="dialout"
|
||||
@ -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
|
||||
----------
|
||||
|
||||
@ -1,9 +1,10 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32-S3 BALANCE motor
|
||||
can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32-S3 BALANCE
|
||||
controller and VESC motor controllers over CAN bus.
|
||||
|
||||
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
|
||||
The node opens the SocketCAN interface (slcan0 by default), spawns a background
|
||||
reader thread to process incoming telemetry, and exposes the following interface:
|
||||
|
||||
Subscriptions
|
||||
-------------
|
||||
@ -18,15 +19,9 @@ Publications
|
||||
/can/vesc/right/state std_msgs/Float32MultiArray Right VESC state
|
||||
/can/connection_status std_msgs/String "connected" | "disconnected"
|
||||
|
||||
Parameters
|
||||
----------
|
||||
can_interface str CAN socket name (default: slcan0)
|
||||
speed_scale float /cmd_vel linear.x (m/s) → motor units (default: 1000.0)
|
||||
steer_scale float /cmd_vel angular.z (rad/s) → motor units (default: -500.0)
|
||||
command_timeout_s float watchdog zero-vel threshold (default: 0.5)
|
||||
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
|
||||
"""
|
||||
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
@ -35,32 +30,36 @@ import can
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from rcl_interfaces.msg import SetParametersResult
|
||||
from sensor_msgs.msg import BatteryState, Imu
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
|
||||
from saltybot_can_bridge.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_ESTOP,
|
||||
MODE_IDLE,
|
||||
encode_drive_cmd,
|
||||
encode_arm_cmd,
|
||||
encode_estop_cmd,
|
||||
decode_attitude,
|
||||
decode_battery,
|
||||
decode_vesc_status1,
|
||||
encode_mode_cmd,
|
||||
encode_velocity_cmd,
|
||||
encode_pid_set_cmd,
|
||||
decode_battery_telem,
|
||||
decode_imu_telem,
|
||||
decode_pid_ack,
|
||||
decode_vesc_state,
|
||||
)
|
||||
|
||||
# Reconnect attempt interval when CAN bus is lost
|
||||
_RECONNECT_INTERVAL_S: float = 5.0
|
||||
|
||||
# Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent
|
||||
# Watchdog timer tick rate (Hz)
|
||||
_WATCHDOG_HZ: float = 10.0
|
||||
|
||||
|
||||
@ -71,38 +70,47 @@ class CanBridgeNode(Node):
|
||||
super().__init__("can_bridge_node")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("can_interface", "slcan0")
|
||||
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
|
||||
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
|
||||
self.declare_parameter("speed_scale", 1000.0)
|
||||
self.declare_parameter("steer_scale", -500.0)
|
||||
self.declare_parameter("can_interface", "can0")
|
||||
self.declare_parameter("left_vesc_can_id", 68)
|
||||
self.declare_parameter("right_vesc_can_id", 56)
|
||||
self.declare_parameter("balance_can_id", 1)
|
||||
self.declare_parameter("command_timeout_s", 0.5)
|
||||
self.declare_parameter("pid/kp", 0.0)
|
||||
self.declare_parameter("pid/ki", 0.0)
|
||||
self.declare_parameter("pid/kd", 0.0)
|
||||
|
||||
self._iface = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
|
||||
self._speed_scale = self.get_parameter("speed_scale").value
|
||||
self._steer_scale = self.get_parameter("steer_scale").value
|
||||
self._cmd_timeout = self.get_parameter("command_timeout_s").value
|
||||
self._iface: str = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
|
||||
self._balance_id: int = self.get_parameter("balance_can_id").value
|
||||
self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
|
||||
self._pid_kp: float = self.get_parameter("pid/kp").value
|
||||
self._pid_ki: float = self.get_parameter("pid/ki").value
|
||||
self._pid_kd: float = self.get_parameter("pid/kd").value
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────
|
||||
self._bus: Optional[can.BusABC] = None
|
||||
self._connected: bool = False
|
||||
self._last_cmd_time: float = time.monotonic()
|
||||
self._lock = threading.Lock()
|
||||
self._lock = threading.Lock() # protects _bus / _connected
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10)
|
||||
self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10)
|
||||
self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
|
||||
self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
|
||||
self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state", 10)
|
||||
self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state", 10)
|
||||
self._pub_status = self.create_publisher(String, "/can/connection_status", 10)
|
||||
self._pub_vesc_left = self.create_publisher(
|
||||
Float32MultiArray, "/can/vesc/left/state", 10
|
||||
)
|
||||
self._pub_vesc_right = self.create_publisher(
|
||||
Float32MultiArray, "/can/vesc/right/state", 10
|
||||
)
|
||||
self._pub_status = self.create_publisher(
|
||||
String, "/can/connection_status", 10
|
||||
)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
||||
self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
|
||||
self.add_on_set_parameters_callback(self._on_set_parameters)
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||
@ -120,17 +128,46 @@ class CanBridgeNode(Node):
|
||||
self.get_logger().info(
|
||||
f"can_bridge_node ready — iface={self._iface} "
|
||||
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
|
||||
f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
|
||||
f"balance={self._balance_id}"
|
||||
)
|
||||
|
||||
# -- PID parameter callback (Issue #693) --
|
||||
|
||||
def _on_set_parameters(self, params) -> SetParametersResult:
|
||||
"""Send new PID gains over CAN when pid/* params change."""
|
||||
for p in params:
|
||||
if p.name == "pid/kp":
|
||||
self._pid_kp = float(p.value)
|
||||
elif p.name == "pid/ki":
|
||||
self._pid_ki = float(p.value)
|
||||
elif p.name == "pid/kd":
|
||||
self._pid_kd = float(p.value)
|
||||
else:
|
||||
continue
|
||||
try:
|
||||
payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
|
||||
self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
|
||||
self.get_logger().info(
|
||||
f"PID gains sent: Kp={self._pid_kp:.2f} "
|
||||
f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
|
||||
)
|
||||
except ValueError as exc:
|
||||
return SetParametersResult(successful=False, reason=str(exc))
|
||||
return SetParametersResult(successful=True)
|
||||
|
||||
# ── Connection management ──────────────────────────────────────────────
|
||||
|
||||
def _try_connect(self) -> None:
|
||||
"""Attempt to open the CAN interface; silently skip if already connected."""
|
||||
with self._lock:
|
||||
if self._connected:
|
||||
return
|
||||
try:
|
||||
self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
|
||||
bus = can.interface.Bus(
|
||||
channel=self._iface,
|
||||
bustype="socketcan",
|
||||
)
|
||||
self._bus = bus
|
||||
self._connected = True
|
||||
self.get_logger().info(f"CAN bus connected: {self._iface}")
|
||||
self._publish_status("connected")
|
||||
@ -143,10 +180,12 @@ class CanBridgeNode(Node):
|
||||
self._publish_status("disconnected")
|
||||
|
||||
def _reconnect_cb(self) -> None:
|
||||
"""Periodic timer: try to reconnect when disconnected."""
|
||||
if not self._connected:
|
||||
self._try_connect()
|
||||
|
||||
def _handle_can_error(self, exc: Exception, context: str) -> None:
|
||||
"""Mark bus as disconnected on any CAN error."""
|
||||
self.get_logger().warning(f"CAN error in {context}: {exc}")
|
||||
with self._lock:
|
||||
if self._bus is not None:
|
||||
@ -161,8 +200,9 @@ class CanBridgeNode(Node):
|
||||
# ── ROS callbacks ─────────────────────────────────────────────────────
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist) -> None:
|
||||
"""Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN."""
|
||||
"""Convert /cmd_vel Twist to VESC speed commands over CAN."""
|
||||
self._last_cmd_time = time.monotonic()
|
||||
|
||||
if not self._connected:
|
||||
return
|
||||
|
||||
@ -179,40 +219,54 @@ 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."""
|
||||
if not self._connected:
|
||||
return
|
||||
payload = encode_estop_cmd(msg.data)
|
||||
self._send_can(BALANCE_CMD_ESTOP, payload, "estop")
|
||||
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")
|
||||
|
||||
# ── Watchdog ──────────────────────────────────────────────────────────
|
||||
|
||||
def _watchdog_cb(self) -> None:
|
||||
"""If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive)."""
|
||||
"""If no /cmd_vel arrives within the timeout, send zero velocity."""
|
||||
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")
|
||||
elapsed = time.monotonic() - self._last_cmd_time
|
||||
if elapsed > self._cmd_timeout:
|
||||
self._send_can(
|
||||
BALANCE_CMD_VELOCITY,
|
||||
encode_velocity_cmd(0.0, 0.0),
|
||||
"watchdog zero-vel",
|
||||
)
|
||||
self._send_can(
|
||||
BALANCE_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
|
||||
)
|
||||
|
||||
# ── CAN send helper ───────────────────────────────────────────────────
|
||||
|
||||
def _send_can(self, arb_id: int, data: bytes, context: str,
|
||||
extended: bool = False) -> None:
|
||||
def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
|
||||
"""Send a standard CAN frame; handle errors gracefully."""
|
||||
with self._lock:
|
||||
if not self._connected or self._bus is None:
|
||||
return
|
||||
bus = self._bus
|
||||
msg = can.Message(arbitration_id=arb_id, data=data,
|
||||
is_extended_id=extended)
|
||||
|
||||
msg = can.Message(
|
||||
arbitration_id=arb_id,
|
||||
data=data,
|
||||
is_extended_id=False,
|
||||
)
|
||||
try:
|
||||
bus.send(msg, timeout=0.05)
|
||||
except can.CanError as exc:
|
||||
@ -221,41 +275,55 @@ class CanBridgeNode(Node):
|
||||
# ── Background CAN reader ─────────────────────────────────────────────
|
||||
|
||||
def _reader_loop(self) -> None:
|
||||
"""
|
||||
Blocking CAN read loop executed in a daemon thread.
|
||||
Dispatches incoming frames to the appropriate handler.
|
||||
"""
|
||||
while rclpy.ok():
|
||||
with self._lock:
|
||||
connected, bus = self._connected, self._bus
|
||||
connected = self._connected
|
||||
bus = self._bus
|
||||
|
||||
if not connected or bus is None:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
|
||||
try:
|
||||
frame = bus.recv(timeout=0.5)
|
||||
except can.CanError as exc:
|
||||
self._handle_can_error(exc, "reader_loop recv")
|
||||
continue
|
||||
|
||||
if frame is None:
|
||||
# Timeout — no frame within 0.5 s, loop again
|
||||
continue
|
||||
|
||||
self._dispatch_frame(frame)
|
||||
|
||||
def _dispatch_frame(self, frame: can.Message) -> None:
|
||||
"""Route an incoming CAN frame to the correct publisher."""
|
||||
arb_id = frame.arbitration_id
|
||||
data = bytes(frame.data)
|
||||
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||
vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||
|
||||
try:
|
||||
if arb_id == ESP32_TELEM_ATTITUDE:
|
||||
self._handle_attitude(data)
|
||||
elif arb_id == ESP32_TELEM_BATTERY:
|
||||
self._handle_battery(data)
|
||||
elif arb_id == vesc_l:
|
||||
t = decode_vesc_status1(self._left_vesc_id, data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
self._pub_vesc_left.publish(m)
|
||||
elif arb_id == vesc_r:
|
||||
t = decode_vesc_status1(self._right_vesc_id, data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
self._pub_vesc_right.publish(m)
|
||||
if arb_id == BALANCE_TELEM_IMU:
|
||||
self._handle_imu(data, frame.timestamp)
|
||||
|
||||
elif arb_id == BALANCE_TELEM_BATTERY:
|
||||
self._handle_battery(data, frame.timestamp)
|
||||
|
||||
elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
|
||||
self._handle_vesc_state(data, frame.timestamp, side="left")
|
||||
|
||||
elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
|
||||
self._handle_vesc_state(data, frame.timestamp, side="right")
|
||||
|
||||
elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
|
||||
gains = decode_pid_ack(data)
|
||||
self.get_logger().debug(
|
||||
f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
|
||||
)
|
||||
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
|
||||
@ -263,36 +331,52 @@ class CanBridgeNode(Node):
|
||||
|
||||
# ── Frame handlers ────────────────────────────────────────────────────
|
||||
|
||||
_STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
|
||||
def _handle_imu(self, data: bytes, timestamp: float) -> None:
|
||||
telem = decode_imu_telem(data)
|
||||
|
||||
def _handle_attitude(self, data: bytes) -> None:
|
||||
"""ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
|
||||
t = decode_attitude(data)
|
||||
now = self.get_clock().now().to_msg()
|
||||
payload = {
|
||||
"pitch_deg": round(t.pitch_deg, 2),
|
||||
"speed_mps": round(t.speed, 3),
|
||||
"yaw_rate": round(t.yaw_rate, 3),
|
||||
"state": t.state,
|
||||
"state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
|
||||
"flags": t.flags,
|
||||
"ts": f"{now.sec}.{now.nanosec:09d}",
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload)
|
||||
self._pub_attitude.publish(msg)
|
||||
self._pub_balance.publish(msg) # keep /saltybot/balance_state alive
|
||||
msg = Imu()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = "imu_link"
|
||||
|
||||
msg.linear_acceleration.x = telem.accel_x
|
||||
msg.linear_acceleration.y = telem.accel_y
|
||||
msg.linear_acceleration.z = telem.accel_z
|
||||
|
||||
msg.angular_velocity.x = telem.gyro_x
|
||||
msg.angular_velocity.y = telem.gyro_y
|
||||
msg.angular_velocity.z = telem.gyro_z
|
||||
|
||||
# Covariance unknown; mark as -1 per REP-145
|
||||
msg.orientation_covariance[0] = -1.0
|
||||
|
||||
self._pub_imu.publish(msg)
|
||||
|
||||
def _handle_battery(self, data: bytes, timestamp: float) -> None:
|
||||
telem = decode_battery_telem(data)
|
||||
|
||||
def _handle_battery(self, data: bytes) -> None:
|
||||
"""BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
|
||||
t = decode_battery(data)
|
||||
msg = BatteryState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.voltage = t.vbat_mv / 1000.0
|
||||
msg.voltage = telem.voltage
|
||||
msg.current = telem.current
|
||||
msg.present = True
|
||||
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
|
||||
self._pub_battery.publish(msg)
|
||||
|
||||
def _handle_vesc_state(
|
||||
self, data: bytes, timestamp: float, side: str
|
||||
) -> None:
|
||||
telem = decode_vesc_state(data)
|
||||
|
||||
msg = Float32MultiArray()
|
||||
# Layout: [erpm, duty, voltage, current]
|
||||
msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
|
||||
|
||||
if side == "left":
|
||||
self._pub_vesc_left.publish(msg)
|
||||
else:
|
||||
self._pub_vesc_right.publish(msg)
|
||||
|
||||
# ── Status helper ─────────────────────────────────────────────────────
|
||||
|
||||
def _publish_status(self, status: str) -> None:
|
||||
@ -303,10 +387,17 @@ class CanBridgeNode(Node):
|
||||
# ── Shutdown ──────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
"""Send zero velocity and shut down the CAN bus cleanly."""
|
||||
if self._connected and self._bus is not None:
|
||||
try:
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
|
||||
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "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:
|
||||
@ -316,6 +407,8 @@ class CanBridgeNode(Node):
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = CanBridgeNode()
|
||||
|
||||
@ -15,11 +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",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
|
||||
@ -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,31 +4,28 @@ 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↔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 → 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_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):
|
||||
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
|
||||
|
||||
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
|
||||
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
|
||||
@ -39,7 +36,7 @@ import struct
|
||||
from typing import Tuple
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Orin → FC (ESP32-S3 BALANCE) command IDs (from orin_can.h)
|
||||
# Orin → ESP32-S3 BALANCE command IDs (from orin_can.h)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
ORIN_CMD_HEARTBEAT: int = 0x300
|
||||
@ -48,7 +45,7 @@ ORIN_CMD_MODE: int = 0x302
|
||||
ORIN_CMD_ESTOP: int = 0x303
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# FC (ESP32-S3 BALANCE) → Orin telemetry IDs (from orin_can.h)
|
||||
# ESP32-S3 BALANCE → Orin telemetry IDs (from orin_can.h)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
FC_STATUS: int = 0x400
|
||||
@ -57,18 +54,15 @@ FC_IMU: int = 0x402
|
||||
FC_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)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
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
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
@ -142,15 +136,12 @@ def build_estop_cmd(action: int = 1) -> bytes:
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 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 × float32 big-endian).
|
||||
|
||||
Matches encode_velocity_cmd() in balance_protocol.py.
|
||||
"""
|
||||
@ -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 × 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])
|
||||
|
||||
@ -14,8 +14,8 @@ import struct
|
||||
import pytest
|
||||
|
||||
from saltybot_can_e2e_test.protocol_defs import (
|
||||
MAMBA_CMD_VELOCITY,
|
||||
MAMBA_CMD_MODE,
|
||||
BALANCE_CMD_VELOCITY,
|
||||
BALANCE_CMD_MODE,
|
||||
FC_VESC,
|
||||
MODE_DRIVE,
|
||||
MODE_IDLE,
|
||||
@ -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,7 +77,7 @@ 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])
|
||||
|
||||
@ -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))
|
||||
@ -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])
|
||||
|
||||
|
||||
@ -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,
|
||||
ORIN_CMD_ESTOP,
|
||||
FC_STATUS,
|
||||
MODE_IDLE,
|
||||
@ -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"
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
@ -5,11 +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
|
||||
- Boot result TTS + face animation
|
||||
- JSON logging
|
||||
|
||||
@ -1,9 +0,0 @@
|
||||
# dronecan_gps_params.yaml — CubePilot Here4 DroneCAN GPS driver defaults
|
||||
# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
|
||||
|
||||
dronecan_gps:
|
||||
ros__parameters:
|
||||
can_interface: "can0"
|
||||
can_bitrate: 1000000 # Here4 default: 1Mbps DroneCAN
|
||||
node_id: 127 # DroneCAN local node ID (GPS driver)
|
||||
publish_compass: true # publish MagneticFieldStrength if available
|
||||
@ -1,22 +0,0 @@
|
||||
here4_can_node:
|
||||
ros__parameters:
|
||||
# SocketCAN interface — freed from ESP32 BALANCE comms by bd-wim1
|
||||
can_interface: can0
|
||||
|
||||
# DroneCAN bitrate — Here4 runs at 1 Mbps (not 500 kbps used previously)
|
||||
bitrate: 1000000
|
||||
|
||||
# Local DroneCAN node ID for this Orin instance (1-127, must be unique on bus)
|
||||
local_node_id: 126
|
||||
|
||||
# Set true to run: ip link set can0 type can bitrate 1000000 && ip link set can0 up
|
||||
# Requires: sudo setcap cap_net_admin+eip $(which python3) or run as root
|
||||
# Default false — manage interface externally (systemd-networkd or udev)
|
||||
bring_up_can: false
|
||||
|
||||
# Here4 DroneCAN node ID — 0 means auto-detect from first Fix2 message
|
||||
node_id_filter: 0
|
||||
|
||||
# TF frame IDs
|
||||
fix_frame_id: gps
|
||||
imu_frame_id: here4_imu
|
||||
@ -1,109 +0,0 @@
|
||||
"""here4.launch.py — Launch the Here4 GPS DroneCAN bridge on CANable2.
|
||||
|
||||
bd-p47c: CANable2 freed by bd-wim1 (ESP32 moved to UART/USB) now used for
|
||||
Here4 GPS at 1 Mbps DroneCAN/UAVCAN v0.
|
||||
|
||||
CAN setup (one-time, survives reboot via systemd-networkd or udev)
|
||||
------------------------------------------------------------------
|
||||
# Option A — manual (quick test):
|
||||
sudo ip link set can0 down
|
||||
sudo ip link set can0 type can bitrate 1000000
|
||||
sudo ip link set can0 up
|
||||
|
||||
# Option B — let the node do it (requires CAP_NET_ADMIN):
|
||||
ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true
|
||||
|
||||
# Option C — systemd-networkd (/etc/systemd/network/80-can0.network):
|
||||
[Match]
|
||||
Name=can0
|
||||
[CAN]
|
||||
BitRate=1M
|
||||
|
||||
Published topics
|
||||
----------------
|
||||
/gps/fix sensor_msgs/NavSatFix → navsat_transform_node (EKF)
|
||||
/gps/velocity geometry_msgs/TwistWithCovarianceStamped
|
||||
/here4/fix sensor_msgs/NavSatFix (alias)
|
||||
/here4/imu sensor_msgs/Imu
|
||||
/here4/mag sensor_msgs/MagneticField
|
||||
/here4/baro sensor_msgs/FluidPressure
|
||||
/here4/status std_msgs/String JSON
|
||||
/here4/node_id std_msgs/Int32
|
||||
|
||||
Subscribed topics
|
||||
-----------------
|
||||
/rtcm std_msgs/ByteMultiArray RTCM corrections (NTRIP client)
|
||||
/rtcm_hex std_msgs/String hex-encoded RTCM (fallback)
|
||||
|
||||
Usage
|
||||
-----
|
||||
# Default (interface already up):
|
||||
ros2 launch saltybot_dronecan_gps here4.launch.py
|
||||
|
||||
# Bring up interface automatically (CAP_NET_ADMIN required):
|
||||
ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true
|
||||
|
||||
# Override interface (e.g. second CAN adapter):
|
||||
ros2 launch saltybot_dronecan_gps here4.launch.py can_interface:=can1
|
||||
|
||||
# Pin to specific Here4 node ID (skip auto-detect):
|
||||
ros2 launch saltybot_dronecan_gps here4.launch.py node_id_filter:=10
|
||||
|
||||
System dependency
|
||||
-----------------
|
||||
pip install dronecan # DroneCAN/UAVCAN v0 Python library
|
||||
apt install can-utils # optional: candump, cansend for debugging
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
pkg_share = get_package_share_directory("saltybot_dronecan_gps")
|
||||
params_file = os.path.join(pkg_share, "config", "here4_params.yaml")
|
||||
|
||||
args = [
|
||||
DeclareLaunchArgument(
|
||||
"can_interface",
|
||||
default_value="can0",
|
||||
description="SocketCAN interface (CANable2 @ 1 Mbps)",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"bring_up_can",
|
||||
default_value="false",
|
||||
description="Bring up can_interface via ip link (requires CAP_NET_ADMIN)",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"node_id_filter",
|
||||
default_value="0",
|
||||
description="DroneCAN node ID of Here4 (0=auto-detect from first Fix2)",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"local_node_id",
|
||||
default_value="126",
|
||||
description="DroneCAN node ID for this Orin (must be unique on bus)",
|
||||
),
|
||||
]
|
||||
|
||||
node = Node(
|
||||
package="saltybot_dronecan_gps",
|
||||
executable="here4_node",
|
||||
name="here4_can_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
params_file,
|
||||
{
|
||||
"can_interface": LaunchConfiguration("can_interface"),
|
||||
"bring_up_can": LaunchConfiguration("bring_up_can"),
|
||||
"node_id_filter": LaunchConfiguration("node_id_filter"),
|
||||
"local_node_id": LaunchConfiguration("local_node_id"),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([*args, node])
|
||||
@ -1,110 +0,0 @@
|
||||
"""
|
||||
here4_gps.launch.py — CubePilot Here4 RTK GPS full stack
|
||||
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
|
||||
|
||||
Launches:
|
||||
- dronecan_gps_node (saltybot_dronecan_gps)
|
||||
- ntrip_client_node (saltybot_ntrip_client)
|
||||
|
||||
Usage (minimal):
|
||||
ros2 launch saltybot_dronecan_gps here4_gps.launch.py \\
|
||||
ntrip_mount:=RTCM3_GENERIC ntrip_user:=you@email.com
|
||||
|
||||
Full options:
|
||||
ros2 launch saltybot_dronecan_gps here4_gps.launch.py \\
|
||||
can_interface:=can0 \\
|
||||
can_bitrate:=1000000 \\
|
||||
ntrip_caster:=rtk2go.com \\
|
||||
ntrip_port:=2101 \\
|
||||
ntrip_mount:=MYBASE \\
|
||||
ntrip_user:=you@email.com \\
|
||||
ntrip_password:=secret
|
||||
"""
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
gps_cfg = os.path.join(
|
||||
get_package_share_directory('saltybot_dronecan_gps'),
|
||||
'config', 'dronecan_gps_params.yaml',
|
||||
)
|
||||
ntrip_cfg = os.path.join(
|
||||
get_package_share_directory('saltybot_ntrip_client'),
|
||||
'config', 'ntrip_params.yaml',
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
# ── Shared CAN args ───────────────────────────────────────────────────
|
||||
DeclareLaunchArgument(
|
||||
'can_interface', default_value='can0',
|
||||
description='SocketCAN interface name',
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'can_bitrate', default_value='1000000',
|
||||
description='CAN bus bitrate — Here4 default is 1000000 (1 Mbps)',
|
||||
),
|
||||
|
||||
# ── NTRIP args ────────────────────────────────────────────────────────
|
||||
DeclareLaunchArgument(
|
||||
'ntrip_caster', default_value='rtk2go.com',
|
||||
description='NTRIP caster hostname',
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'ntrip_port', default_value='2101',
|
||||
description='NTRIP caster port',
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'ntrip_mount', default_value='',
|
||||
description='NTRIP mount point (REQUIRED)',
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'ntrip_user', default_value='',
|
||||
description='NTRIP username (rtk2go.com requires email address)',
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'ntrip_password', default_value='',
|
||||
description='NTRIP password',
|
||||
),
|
||||
|
||||
# ── DroneCAN GPS node ─────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_dronecan_gps',
|
||||
executable='dronecan_gps_node',
|
||||
name='dronecan_gps',
|
||||
output='screen',
|
||||
parameters=[
|
||||
gps_cfg,
|
||||
{
|
||||
'can_interface': LaunchConfiguration('can_interface'),
|
||||
'can_bitrate': LaunchConfiguration('can_bitrate'),
|
||||
},
|
||||
],
|
||||
),
|
||||
|
||||
# ── NTRIP client node ─────────────────────────────────────────────────
|
||||
Node(
|
||||
package='saltybot_ntrip_client',
|
||||
executable='ntrip_client_node',
|
||||
name='ntrip_client',
|
||||
output='screen',
|
||||
parameters=[
|
||||
ntrip_cfg,
|
||||
{
|
||||
'can_interface': LaunchConfiguration('can_interface'),
|
||||
'can_bitrate': LaunchConfiguration('can_bitrate'),
|
||||
'ntrip_caster': LaunchConfiguration('ntrip_caster'),
|
||||
'ntrip_port': LaunchConfiguration('ntrip_port'),
|
||||
'ntrip_mount': LaunchConfiguration('ntrip_mount'),
|
||||
'ntrip_user': LaunchConfiguration('ntrip_user'),
|
||||
'ntrip_password': LaunchConfiguration('ntrip_password'),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -1,37 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_dronecan_gps</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
DroneCAN/UAVCAN v0 bridge for Here4 GPS on the CANable2 SocketCAN adapter.
|
||||
Publishes GPS fix, IMU, magnetometer, and barometer data to ROS2.
|
||||
Injects RTCM corrections for RTK.
|
||||
|
||||
bd-p47c: CANable2 freed from ESP32 BALANCE comms by bd-wim1.
|
||||
Here4 operates at 1 Mbps on can0 (was 500 kbps for VESC comms).
|
||||
|
||||
System dependency: dronecan Python library (pip install dronecan)
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
|
||||
<!-- dronecan: pip install dronecan (DroneCAN/UAVCAN v0 Python library) -->
|
||||
<!-- python3-can is a transitive dependency of dronecan -->
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1 +0,0 @@
|
||||
saltybot_dronecan_gps
|
||||
@ -1 +0,0 @@
|
||||
# saltybot_dronecan_gps — Here4 GPS DroneCAN bridge for CANable2 (bd-p47c)
|
||||
@ -1,204 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
dronecan_gps_node.py — DroneCAN GPS driver for CubePilot Here4 RTK
|
||||
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
|
||||
|
||||
Subscribes to:
|
||||
uavcan.equipment.gnss.Fix2 (msg ID 1063) — position + fix status
|
||||
uavcan.equipment.ahrs.MagneticFieldStrength — compass (optional)
|
||||
|
||||
Publishes:
|
||||
/gps/fix sensor_msgs/NavSatFix
|
||||
/gps/vel geometry_msgs/TwistStamped
|
||||
/gps/rtk_status std_msgs/String
|
||||
|
||||
DroneCAN fix_type → sensor_msgs status mapping:
|
||||
0 = NO_FIX → STATUS_NO_FIX (-1)
|
||||
1 = TIME_ONLY → STATUS_NO_FIX (-1)
|
||||
2 = 2D_FIX → STATUS_FIX (0)
|
||||
3 = 3D_FIX → STATUS_FIX (0)
|
||||
4 = DGPS → STATUS_SBAS_FIX (1)
|
||||
5 = RTK_FLOAT → STATUS_GBAS_FIX (2)
|
||||
6 = RTK_FIXED → STATUS_GBAS_FIX (2)
|
||||
"""
|
||||
|
||||
import math
|
||||
import threading
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
from sensor_msgs.msg import NavSatFix, NavSatStatus
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from std_msgs.msg import String
|
||||
|
||||
try:
|
||||
import dronecan
|
||||
except ImportError:
|
||||
dronecan = None
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
|
||||
# DroneCAN fix_type → (NavSatStatus.status, rtk_label)
|
||||
_FIX_MAP = {
|
||||
0: (NavSatStatus.STATUS_NO_FIX, 'NO_FIX'),
|
||||
1: (NavSatStatus.STATUS_NO_FIX, 'TIME_ONLY'),
|
||||
2: (NavSatStatus.STATUS_FIX, '2D_FIX'),
|
||||
3: (NavSatStatus.STATUS_FIX, '3D_FIX'),
|
||||
4: (NavSatStatus.STATUS_SBAS_FIX, 'DGPS'),
|
||||
5: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FLOAT'),
|
||||
6: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FIXED'),
|
||||
}
|
||||
|
||||
|
||||
class DroneCanGpsNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('dronecan_gps')
|
||||
|
||||
self.declare_parameter('can_interface', 'can0')
|
||||
self.declare_parameter('can_bitrate', 1000000)
|
||||
self.declare_parameter('node_id', 127) # DroneCAN local node ID
|
||||
self.declare_parameter('publish_compass', True)
|
||||
|
||||
self._iface = self.get_parameter('can_interface').value
|
||||
self._bitrate = self.get_parameter('can_bitrate').value
|
||||
self._node_id = self.get_parameter('node_id').value
|
||||
self._publish_compass = self.get_parameter('publish_compass').value
|
||||
|
||||
self._fix_pub = self.create_publisher(NavSatFix, '/gps/fix', _SENSOR_QOS)
|
||||
self._vel_pub = self.create_publisher(TwistStamped, '/gps/vel', _SENSOR_QOS)
|
||||
self._rtk_pub = self.create_publisher(String, '/gps/rtk_status', 10)
|
||||
|
||||
if dronecan is None:
|
||||
self.get_logger().error(
|
||||
'python-dronecan not installed. '
|
||||
'Run: pip install python-dronecan'
|
||||
)
|
||||
return
|
||||
|
||||
self._lock = threading.Lock()
|
||||
self._dc_node = None
|
||||
self._spin_thread = threading.Thread(
|
||||
target=self._dronecan_spin, daemon=True
|
||||
)
|
||||
self._spin_thread.start()
|
||||
self.get_logger().info(
|
||||
f'DroneCanGpsNode started — interface={self._iface} '
|
||||
f'bitrate={self._bitrate}'
|
||||
)
|
||||
|
||||
# ── DroneCAN spin (runs in background thread) ─────────────────────────────
|
||||
|
||||
def _dronecan_spin(self) -> None:
|
||||
try:
|
||||
self._dc_node = dronecan.make_node(
|
||||
self._iface,
|
||||
node_id=self._node_id,
|
||||
bitrate=self._bitrate,
|
||||
)
|
||||
self._dc_node.add_handler(
|
||||
dronecan.uavcan.equipment.gnss.Fix2,
|
||||
self._on_fix2,
|
||||
)
|
||||
if self._publish_compass:
|
||||
self._dc_node.add_handler(
|
||||
dronecan.uavcan.equipment.ahrs.MagneticFieldStrength,
|
||||
self._on_mag,
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'DroneCAN node online on {self._iface}'
|
||||
)
|
||||
while rclpy.ok():
|
||||
self._dc_node.spin(timeout=0.1)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f'DroneCAN spin error: {exc}')
|
||||
|
||||
# ── Message handlers ──────────────────────────────────────────────────────
|
||||
|
||||
def _on_fix2(self, event) -> None:
|
||||
msg = event.message
|
||||
now = self.get_clock().now().to_msg()
|
||||
|
||||
fix_type = int(msg.fix_type)
|
||||
nav_status, rtk_label = _FIX_MAP.get(
|
||||
fix_type, (NavSatStatus.STATUS_NO_FIX, 'UNKNOWN')
|
||||
)
|
||||
|
||||
# NavSatFix
|
||||
fix = NavSatFix()
|
||||
fix.header.stamp = now
|
||||
fix.header.frame_id = 'gps'
|
||||
fix.status.status = nav_status
|
||||
fix.status.service = NavSatStatus.SERVICE_GPS
|
||||
|
||||
fix.latitude = math.degrees(msg.latitude_deg_1e8 * 1e-8)
|
||||
fix.longitude = math.degrees(msg.longitude_deg_1e8 * 1e-8)
|
||||
fix.altitude = msg.height_msl_mm * 1e-3 # mm → m
|
||||
|
||||
# Covariance from position_covariance if available, else diagonal guess
|
||||
if hasattr(msg, 'position_covariance') and len(msg.position_covariance) >= 9:
|
||||
fix.position_covariance = list(msg.position_covariance)
|
||||
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_FULL
|
||||
else:
|
||||
h_var = (msg.horizontal_pos_accuracy_m_1e2 * 1e-2) ** 2 \
|
||||
if hasattr(msg, 'horizontal_pos_accuracy_m_1e2') else 4.0
|
||||
v_var = (msg.vertical_pos_accuracy_m_1e2 * 1e-2) ** 2 \
|
||||
if hasattr(msg, 'vertical_pos_accuracy_m_1e2') else 4.0
|
||||
fix.position_covariance = [
|
||||
h_var, 0.0, 0.0,
|
||||
0.0, h_var, 0.0,
|
||||
0.0, 0.0, v_var,
|
||||
]
|
||||
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
|
||||
|
||||
self._fix_pub.publish(fix)
|
||||
|
||||
# TwistStamped velocity
|
||||
if hasattr(msg, 'ned_velocity'):
|
||||
vel = TwistStamped()
|
||||
vel.header.stamp = now
|
||||
vel.header.frame_id = 'gps'
|
||||
vel.twist.linear.x = float(msg.ned_velocity[0]) # North m/s
|
||||
vel.twist.linear.y = float(msg.ned_velocity[1]) # East m/s
|
||||
vel.twist.linear.z = float(msg.ned_velocity[2]) # Down m/s (ROS: up+)
|
||||
self._vel_pub.publish(vel)
|
||||
|
||||
# RTK status string
|
||||
rtk_msg = String()
|
||||
rtk_msg.data = rtk_label
|
||||
self._rtk_pub.publish(rtk_msg)
|
||||
|
||||
self.get_logger().debug(
|
||||
f'Fix2: {fix.latitude:.6f},{fix.longitude:.6f} '
|
||||
f'alt={fix.altitude:.1f}m status={rtk_label}'
|
||||
)
|
||||
|
||||
def _on_mag(self, event) -> None:
|
||||
# Compass data logged; extend to publish /imu/mag if needed
|
||||
msg = event.message
|
||||
self.get_logger().debug(
|
||||
f'Mag: {msg.magnetic_field_ga[0]:.3f} '
|
||||
f'{msg.magnetic_field_ga[1]:.3f} '
|
||||
f'{msg.magnetic_field_ga[2]:.3f} Ga'
|
||||
)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = DroneCanGpsNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,333 +0,0 @@
|
||||
"""dronecan_parser.py — Pure-Python conversion of DroneCAN/UAVCAN v0 message
|
||||
objects to plain Python dicts, independent of ROS2.
|
||||
|
||||
Keeps the dronecan library dependency isolated so that unit tests can run
|
||||
without the dronecan package or CAN hardware.
|
||||
|
||||
DroneCAN message types handled
|
||||
-------------------------------
|
||||
uavcan.equipment.gnss.Fix2 DTID 1063
|
||||
uavcan.equipment.gnss.Auxiliary DTID 1060
|
||||
uavcan.equipment.ahrs.RawIMU DTID 1003
|
||||
uavcan.equipment.ahrs.MagneticFieldStrength2 DTID 1001
|
||||
uavcan.equipment.air_data.StaticPressure DTID 1028
|
||||
uavcan.equipment.air_data.StaticTemperature DTID 1029
|
||||
uavcan.protocol.NodeStatus DTID 341
|
||||
uavcan.equipment.gnss.RTCMStream DTID 1062 (TX: RTCM injection)
|
||||
|
||||
Fix2 status values
|
||||
------------------
|
||||
0 NO_FIX
|
||||
1 TIME_ONLY
|
||||
2 2D_FIX
|
||||
3 3D_FIX
|
||||
|
||||
NodeStatus health / mode
|
||||
-------------------------
|
||||
health 0=OK 1=WARNING 2=ERROR 3=CRITICAL
|
||||
mode 0=OPERATIONAL 1=INITIALIZATION 2=MAINTENANCE 3=SOFTWARE_UPDATE 7=OFFLINE
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
# ── DSDL constants ────────────────────────────────────────────────────────────
|
||||
|
||||
DTID_FIX2 = 1063
|
||||
DTID_AUXILIARY = 1060
|
||||
DTID_RAW_IMU = 1003
|
||||
DTID_MAG_FIELD = 1001
|
||||
DTID_STATIC_PRESSURE = 1028
|
||||
DTID_STATIC_TEMPERATURE = 1029
|
||||
DTID_NODE_STATUS = 341
|
||||
DTID_RTCM_STREAM = 1062
|
||||
|
||||
# Fix2 status
|
||||
FIX_NO_FIX = 0
|
||||
FIX_TIME_ONLY = 1
|
||||
FIX_2D = 2
|
||||
FIX_3D = 3
|
||||
|
||||
FIX_LABEL = {FIX_NO_FIX: "NO_FIX", FIX_TIME_ONLY: "TIME_ONLY",
|
||||
FIX_2D: "2D_FIX", FIX_3D: "3D_FIX"}
|
||||
|
||||
# NodeStatus health
|
||||
HEALTH_OK = 0
|
||||
HEALTH_WARNING = 1
|
||||
HEALTH_ERROR = 2
|
||||
HEALTH_CRITICAL = 3
|
||||
|
||||
HEALTH_LABEL = {0: "OK", 1: "WARNING", 2: "ERROR", 3: "CRITICAL"}
|
||||
MODE_LABEL = {0: "OPERATIONAL", 1: "INITIALIZATION", 2: "MAINTENANCE",
|
||||
3: "SOFTWARE_UPDATE", 7: "OFFLINE"}
|
||||
|
||||
# RTCM stream: max bytes per DroneCAN frame
|
||||
RTCM_MAX_CHUNK = 128
|
||||
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────────
|
||||
|
||||
def _getf(obj: Any, *attrs: str, default: Any = None) -> Any:
|
||||
"""Get nested attribute with fallback, tolerant of dronecan proxy objects."""
|
||||
for attr in attrs:
|
||||
try:
|
||||
obj = getattr(obj, attr)
|
||||
except AttributeError:
|
||||
return default
|
||||
return obj
|
||||
|
||||
|
||||
def _list3(vec: Any, default: float = 0.0) -> List[float]:
|
||||
"""Convert a dronecan 3-element array to a plain list of floats."""
|
||||
try:
|
||||
return [float(vec[0]), float(vec[1]), float(vec[2])]
|
||||
except (TypeError, IndexError):
|
||||
return [default, default, default]
|
||||
|
||||
|
||||
def _list9(mat: Any, default: float = 0.0) -> List[float]:
|
||||
"""Convert a dronecan 9-element array (row-major 3×3) to a plain list."""
|
||||
try:
|
||||
return [float(mat[i]) for i in range(9)]
|
||||
except (TypeError, IndexError):
|
||||
return [default] * 9
|
||||
|
||||
|
||||
# ── Fix2 ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def parse_fix2(msg: Any) -> Optional[Dict[str, Any]]:
|
||||
"""Parse a uavcan.equipment.gnss.Fix2 message.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
msg : dronecan message object (event.message / event.transfer.payload)
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with keys:
|
||||
lat_deg float degrees (WGS84)
|
||||
lon_deg float degrees (WGS84)
|
||||
alt_msl_m float metres above MSL
|
||||
alt_ellipsoid_m float metres above WGS84 ellipsoid
|
||||
ned_vel list[3] [north, east, down] m/s
|
||||
fix_type int 0..3
|
||||
fix_label str
|
||||
sats_used int
|
||||
pdop float
|
||||
hdop float
|
||||
vdop float
|
||||
pos_cov list[9] row-major 3×3 position covariance, m²
|
||||
undulation_m float WGS84 geoid undulation (m)
|
||||
gnss_timestamp_us int microseconds (GPS epoch)
|
||||
"""
|
||||
if msg is None:
|
||||
return None
|
||||
try:
|
||||
lat = float(_getf(msg, "latitude_deg_1e8", default=0)) * 1e-8
|
||||
lon = float(_getf(msg, "longitude_deg_1e8", default=0)) * 1e-8
|
||||
alt_msl = float(_getf(msg, "height_msl_mm", default=0)) * 1e-3
|
||||
alt_ell = float(_getf(msg, "height_ellipsoid_mm", default=0)) * 1e-3
|
||||
|
||||
ned = _list3(_getf(msg, "ned_velocity"))
|
||||
|
||||
fix_type = int(_getf(msg, "status", default=FIX_NO_FIX))
|
||||
|
||||
# sats_used is uint7, may be named differently across DSDL revisions
|
||||
sats = int(_getf(msg, "sats_used", default=0))
|
||||
|
||||
pdop = float(_getf(msg, "pdop", default=99.0))
|
||||
hdop = float(_getf(msg, "hdop", default=99.0))
|
||||
vdop = float(_getf(msg, "vdop", default=99.0))
|
||||
undulation = float(_getf(msg, "undulation", default=0.0))
|
||||
|
||||
# position_covariance: optional, may be absent in older DSDL
|
||||
pos_cov_raw = _getf(msg, "position_covariance")
|
||||
pos_cov = _list9(pos_cov_raw) if pos_cov_raw is not None else [0.0] * 9
|
||||
|
||||
gnss_ts = int(_getf(msg, "gnss_timestamp", default=0))
|
||||
|
||||
return {
|
||||
"lat_deg": lat,
|
||||
"lon_deg": lon,
|
||||
"alt_msl_m": alt_msl,
|
||||
"alt_ellipsoid_m": alt_ell,
|
||||
"ned_vel": ned,
|
||||
"fix_type": fix_type,
|
||||
"fix_label": FIX_LABEL.get(fix_type, f"UNKNOWN({fix_type})"),
|
||||
"sats_used": sats,
|
||||
"pdop": pdop,
|
||||
"hdop": hdop,
|
||||
"vdop": vdop,
|
||||
"pos_cov": pos_cov,
|
||||
"undulation_m": undulation,
|
||||
"gnss_timestamp_us": gnss_ts,
|
||||
}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ── RawIMU ────────────────────────────────────────────────────────────────────
|
||||
|
||||
def parse_raw_imu(msg: Any) -> Optional[Dict[str, Any]]:
|
||||
"""Parse a uavcan.equipment.ahrs.RawIMU message.
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with keys:
|
||||
gyro_xyz list[3] rad/s
|
||||
accel_xyz list[3] m/s²
|
||||
dt_s float integration interval (seconds)
|
||||
gyro_cov list[9] row-major 3×3 covariance
|
||||
accel_cov list[9]
|
||||
"""
|
||||
if msg is None:
|
||||
return None
|
||||
try:
|
||||
gyro = _list3(_getf(msg, "rate_gyro_latest"))
|
||||
accel = _list3(_getf(msg, "accelerometer_latest"))
|
||||
dt = float(_getf(msg, "integration_interval", default=0.0))
|
||||
|
||||
gyro_cov = _list9(_getf(msg, "rate_gyro_covariance"))
|
||||
accel_cov = _list9(_getf(msg, "accelerometer_covariance"))
|
||||
|
||||
return {
|
||||
"gyro_xyz": gyro,
|
||||
"accel_xyz": accel,
|
||||
"dt_s": dt,
|
||||
"gyro_cov": gyro_cov,
|
||||
"accel_cov": accel_cov,
|
||||
}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ── MagneticFieldStrength2 ────────────────────────────────────────────────────
|
||||
|
||||
def parse_mag(msg: Any) -> Optional[Dict[str, Any]]:
|
||||
"""Parse a uavcan.equipment.ahrs.MagneticFieldStrength2 message.
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with keys:
|
||||
sensor_id int
|
||||
field_ga list[3] [x, y, z] Gauss
|
||||
field_cov list[9] row-major 3×3 covariance (Gauss²)
|
||||
"""
|
||||
if msg is None:
|
||||
return None
|
||||
try:
|
||||
sensor_id = int(_getf(msg, "sensor_id", default=0))
|
||||
field_ga = _list3(_getf(msg, "magnetic_field_ga"))
|
||||
field_cov = _list9(_getf(msg, "magnetic_field_covariance"))
|
||||
return {
|
||||
"sensor_id": sensor_id,
|
||||
"field_ga": field_ga,
|
||||
"field_cov": field_cov,
|
||||
}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ── StaticPressure / StaticTemperature ───────────────────────────────────────
|
||||
|
||||
def parse_static_pressure(msg: Any) -> Optional[Dict[str, Any]]:
|
||||
"""Parse a uavcan.equipment.air_data.StaticPressure message.
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with keys:
|
||||
pressure_pa float Pascals
|
||||
pressure_variance float Pa²
|
||||
"""
|
||||
if msg is None:
|
||||
return None
|
||||
try:
|
||||
return {
|
||||
"pressure_pa": float(_getf(msg, "static_pressure", default=0.0)),
|
||||
"pressure_variance": float(_getf(msg, "static_pressure_variance", default=0.0)),
|
||||
}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def parse_static_temperature(msg: Any) -> Optional[Dict[str, Any]]:
|
||||
"""Parse a uavcan.equipment.air_data.StaticTemperature message.
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with keys:
|
||||
temperature_k float Kelvin
|
||||
"""
|
||||
if msg is None:
|
||||
return None
|
||||
try:
|
||||
return {
|
||||
"temperature_k": float(_getf(msg, "static_temperature", default=273.15)),
|
||||
}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ── NodeStatus ────────────────────────────────────────────────────────────────
|
||||
|
||||
def parse_node_status(msg: Any, source_node_id: int) -> Optional[Dict[str, Any]]:
|
||||
"""Parse a uavcan.protocol.NodeStatus message.
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with keys:
|
||||
node_id int
|
||||
uptime_sec int
|
||||
health int 0=OK .. 3=CRITICAL
|
||||
health_label str
|
||||
mode int
|
||||
mode_label str
|
||||
vendor_code int
|
||||
"""
|
||||
if msg is None:
|
||||
return None
|
||||
try:
|
||||
health = int(_getf(msg, "health", default=0))
|
||||
mode = int(_getf(msg, "mode", default=0))
|
||||
uptime = int(_getf(msg, "uptime_sec", default=0))
|
||||
vendor_code = int(_getf(msg, "vendor_specific_status_code", default=0))
|
||||
return {
|
||||
"node_id": source_node_id,
|
||||
"uptime_sec": uptime,
|
||||
"health": health,
|
||||
"health_label": HEALTH_LABEL.get(health, f"UNKNOWN({health})"),
|
||||
"mode": mode,
|
||||
"mode_label": MODE_LABEL.get(mode, f"UNKNOWN({mode})"),
|
||||
"vendor_code": vendor_code,
|
||||
}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ── RTCM stream chunking ──────────────────────────────────────────────────────
|
||||
|
||||
def chunk_rtcm(data: bytes, sequence_id: int) -> List[Dict[str, Any]]:
|
||||
"""Split a raw RTCM byte string into DroneCAN RTCMStream message dicts.
|
||||
|
||||
Each chunk carries up to RTCM_MAX_CHUNK bytes. The caller must convert
|
||||
these dicts into actual dronecan.uavcan.equipment.gnss.RTCMStream objects
|
||||
and broadcast them on the bus.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
data : raw RTCM correction bytes
|
||||
sequence_id : monotonically increasing counter (uint16, wraps at 65535)
|
||||
|
||||
Returns
|
||||
-------
|
||||
List of dicts: {"sequence_id": int, "data": bytes}
|
||||
"""
|
||||
chunks = []
|
||||
for i in range(0, len(data), RTCM_MAX_CHUNK):
|
||||
chunks.append({
|
||||
"sequence_id": sequence_id & 0xFFFF,
|
||||
"data": data[i:i + RTCM_MAX_CHUNK],
|
||||
})
|
||||
sequence_id += 1
|
||||
return chunks
|
||||
@ -1,532 +0,0 @@
|
||||
"""here4_node.py — ROS2 node for Here4 GPS via DroneCAN/UAVCAN v0 on CANable2.
|
||||
|
||||
bd-p47c: CANable2 (freed from ESP32 BALANCE comms by bd-wim1) is now used
|
||||
for Here4 GPS at 1 Mbps DroneCAN.
|
||||
|
||||
CAN setup (run once before launching, or use bring_up_can:=true arg)
|
||||
-----------------------------------------------------------------------
|
||||
sudo ip link set can0 down
|
||||
sudo ip link set can0 type can bitrate 1000000
|
||||
sudo ip link set can0 up
|
||||
|
||||
DroneCAN messages received from Here4
|
||||
--------------------------------------
|
||||
uavcan.equipment.gnss.Fix2 DTID 1063 ~5 Hz
|
||||
uavcan.equipment.gnss.Auxiliary DTID 1060 ~5 Hz
|
||||
uavcan.equipment.ahrs.RawIMU DTID 1003 ~50 Hz
|
||||
uavcan.equipment.ahrs.MagneticFieldStrength2 DTID 1001 ~10 Hz
|
||||
uavcan.equipment.air_data.StaticPressure DTID 1028 ~10 Hz
|
||||
uavcan.equipment.air_data.StaticTemperature DTID 1029 ~10 Hz
|
||||
uavcan.protocol.NodeStatus DTID 341 ~1 Hz
|
||||
|
||||
DroneCAN messages transmitted to Here4
|
||||
----------------------------------------
|
||||
uavcan.equipment.gnss.RTCMStream DTID 1062 on /rtcm input
|
||||
|
||||
ROS2 topics published
|
||||
----------------------
|
||||
/gps/fix sensor_msgs/NavSatFix 5 Hz — feeds navsat_transform_node
|
||||
/gps/velocity geometry_msgs/TwistWithCovarianceStamped 5 Hz
|
||||
/here4/fix sensor_msgs/NavSatFix alias of /gps/fix
|
||||
/here4/imu sensor_msgs/Imu 50 Hz
|
||||
/here4/mag sensor_msgs/MagneticField 10 Hz
|
||||
/here4/baro sensor_msgs/FluidPressure 10 Hz
|
||||
/here4/status std_msgs/String JSON 1 Hz (node ID, health, fix quality)
|
||||
/here4/node_id std_msgs/Int32 node ID once discovered
|
||||
|
||||
ROS2 topics subscribed
|
||||
-----------------------
|
||||
/rtcm std_msgs/ByteMultiArray — RTCM corrections (from NTRIP client)
|
||||
or std_msgs/String (hex-encoded, fallback)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
can_interface str SocketCAN interface name default: can0
|
||||
local_node_id int This node's DroneCAN ID (1-127) default: 126
|
||||
bitrate int CAN bitrate, bps default: 1000000
|
||||
bring_up_can bool Bring up can_interface via ip default: false
|
||||
node_id_filter int Specific Here4 node ID (0=auto) default: 0
|
||||
fix_frame_id str Frame ID for NavSatFix default: gps
|
||||
imu_frame_id str Frame ID for Imu default: here4_imu
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from typing import Any, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import TwistWithCovarianceStamped
|
||||
from sensor_msgs.msg import FluidPressure, Imu, MagneticField, NavSatFix, NavSatStatus
|
||||
from std_msgs.msg import ByteMultiArray, Int32, String
|
||||
|
||||
from .dronecan_parser import (
|
||||
FIX_3D,
|
||||
FIX_NO_FIX,
|
||||
chunk_rtcm,
|
||||
parse_fix2,
|
||||
parse_mag,
|
||||
parse_raw_imu,
|
||||
parse_static_pressure,
|
||||
parse_static_temperature,
|
||||
parse_node_status,
|
||||
)
|
||||
|
||||
# Gauss → Tesla conversion
|
||||
_GA_TO_T = 1e-4
|
||||
|
||||
# NavSatStatus mapping from DroneCAN fix type
|
||||
_FIX_TO_NAV_STATUS = {
|
||||
0: NavSatStatus.STATUS_NO_FIX, # NO_FIX
|
||||
1: NavSatStatus.STATUS_NO_FIX, # TIME_ONLY
|
||||
2: NavSatStatus.STATUS_FIX, # 2D_FIX
|
||||
3: NavSatStatus.STATUS_FIX, # 3D_FIX (upgraded to SBAS/RTK when applicable)
|
||||
}
|
||||
|
||||
# Here4 supports GPS+GLONASS+BeiDou+Galileo
|
||||
_SERVICE_FLAGS = (NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS |
|
||||
NavSatStatus.SERVICE_GALILEO | NavSatStatus.SERVICE_COMPASS)
|
||||
|
||||
# RTCM injection sequence counter (per-session, wraps at 65535)
|
||||
_rtcm_seq = 0
|
||||
_rtcm_lock = threading.Lock()
|
||||
|
||||
|
||||
def _bring_up_can(iface: str, bitrate: int, logger) -> bool:
|
||||
"""Bring up the SocketCAN interface at the requested bitrate."""
|
||||
try:
|
||||
subprocess.run(["ip", "link", "set", iface, "down"], check=False)
|
||||
subprocess.run(["ip", "link", "set", iface, "type", "can",
|
||||
"bitrate", str(bitrate)], check=True)
|
||||
subprocess.run(["ip", "link", "set", iface, "up"], check=True)
|
||||
logger.info(f"SocketCAN {iface} up at {bitrate} bps")
|
||||
return True
|
||||
except subprocess.CalledProcessError as exc:
|
||||
logger.error(f"Failed to bring up {iface}: {exc} — check sudo/udev rules")
|
||||
return False
|
||||
|
||||
|
||||
class Here4CanNode(Node):
|
||||
"""DroneCAN bridge publishing Here4 GPS, IMU, mag, and baro to ROS2."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("here4_can_node")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("can_interface", "can0")
|
||||
self.declare_parameter("local_node_id", 126)
|
||||
self.declare_parameter("bitrate", 1_000_000)
|
||||
self.declare_parameter("bring_up_can", False)
|
||||
self.declare_parameter("node_id_filter", 0) # 0 = auto-detect
|
||||
self.declare_parameter("fix_frame_id", "gps")
|
||||
self.declare_parameter("imu_frame_id", "here4_imu")
|
||||
|
||||
self._iface = self.get_parameter("can_interface").value
|
||||
self._local_nid = self.get_parameter("local_node_id").value
|
||||
self._bitrate = self.get_parameter("bitrate").value
|
||||
self._bring_up = self.get_parameter("bring_up_can").value
|
||||
self._nid_filter = self.get_parameter("node_id_filter").value
|
||||
self._fix_frame = self.get_parameter("fix_frame_id").value
|
||||
self._imu_frame = self.get_parameter("imu_frame_id").value
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────
|
||||
self._here4_nid: Optional[int] = self._nid_filter if self._nid_filter else None
|
||||
self._last_fix: Optional[dict] = None
|
||||
self._last_temp_k: float = 293.15 # 20 °C default until first reading
|
||||
self._dc_node = None # dronecan node, set up after CAN is ready
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._pub_fix = self.create_publisher(NavSatFix,
|
||||
"/gps/fix", 10)
|
||||
self._pub_fix_h4 = self.create_publisher(NavSatFix,
|
||||
"/here4/fix", 10)
|
||||
self._pub_vel = self.create_publisher(TwistWithCovarianceStamped,
|
||||
"/gps/velocity", 10)
|
||||
self._pub_imu = self.create_publisher(Imu,
|
||||
"/here4/imu", 10)
|
||||
self._pub_mag = self.create_publisher(MagneticField,
|
||||
"/here4/mag", 10)
|
||||
self._pub_baro = self.create_publisher(FluidPressure,
|
||||
"/here4/baro", 10)
|
||||
self._pub_status = self.create_publisher(String,
|
||||
"/here4/status", 10)
|
||||
self._pub_node_id = self.create_publisher(Int32,
|
||||
"/here4/node_id", 10)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(ByteMultiArray, "/rtcm",
|
||||
self._on_rtcm_bytes, 10)
|
||||
self.create_subscription(String, "/rtcm_hex",
|
||||
self._on_rtcm_hex, 10)
|
||||
|
||||
# ── Bring up CAN if requested ─────────────────────────────────────
|
||||
if self._bring_up:
|
||||
_bring_up_can(self._iface, self._bitrate, self.get_logger())
|
||||
|
||||
# ── Initialise DroneCAN node ──────────────────────────────────────
|
||||
self._init_dronecan()
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────
|
||||
# Poll DroneCAN at 200 Hz (5 ms) to drain the RX queue in real-time
|
||||
self.create_timer(0.005, self._spin_dronecan)
|
||||
# Status publish at 1 Hz
|
||||
self.create_timer(1.0, self._publish_status)
|
||||
|
||||
# ── DroneCAN initialisation ───────────────────────────────────────────
|
||||
|
||||
def _init_dronecan(self) -> None:
|
||||
try:
|
||||
import dronecan # type: ignore[import]
|
||||
except ImportError:
|
||||
self.get_logger().error(
|
||||
"dronecan package not found — install with: pip install dronecan\n"
|
||||
"Here4 node will not receive GPS data until dronecan is installed."
|
||||
)
|
||||
return
|
||||
|
||||
try:
|
||||
self._dc_node = dronecan.make_node(
|
||||
f"socketcan:{self._iface}",
|
||||
node_id=self._local_nid,
|
||||
bitrate=self._bitrate,
|
||||
)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"Failed to open DroneCAN on {self._iface}: {exc}\n"
|
||||
f"Verify: ip link show {self._iface} and try bring_up_can:=true"
|
||||
)
|
||||
return
|
||||
|
||||
# ── Register message handlers ──────────────────────────────────────
|
||||
dc = dronecan
|
||||
self._dc_node.add_handler(dc.uavcan.equipment.gnss.Fix2,
|
||||
self._on_fix2)
|
||||
self._dc_node.add_handler(dc.uavcan.equipment.ahrs.RawIMU,
|
||||
self._on_raw_imu)
|
||||
self._dc_node.add_handler(dc.uavcan.equipment.ahrs.MagneticFieldStrength2,
|
||||
self._on_mag)
|
||||
self._dc_node.add_handler(dc.uavcan.equipment.air_data.StaticPressure,
|
||||
self._on_pressure)
|
||||
self._dc_node.add_handler(dc.uavcan.equipment.air_data.StaticTemperature,
|
||||
self._on_temperature)
|
||||
self._dc_node.add_handler(dc.uavcan.protocol.NodeStatus,
|
||||
self._on_node_status)
|
||||
|
||||
self.get_logger().info(
|
||||
f"DroneCAN node ready: {self._iface} @ {self._bitrate} bps "
|
||||
f"local_id={self._local_nid}"
|
||||
)
|
||||
|
||||
# ── DroneCAN spin (called from 5 ms timer) ────────────────────────────
|
||||
|
||||
def _spin_dronecan(self) -> None:
|
||||
if self._dc_node is None:
|
||||
return
|
||||
try:
|
||||
self._dc_node.spin(timeout=0)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
f"DroneCAN spin error: {exc}",
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
|
||||
# ── Source node filter ────────────────────────────────────────────────
|
||||
|
||||
def _accept(self, event: Any) -> bool: # type: ignore[valid-type]
|
||||
"""Return True if this event comes from the tracked Here4 node."""
|
||||
src = getattr(getattr(event, "transfer", None), "source_node_id", None)
|
||||
if src is None:
|
||||
return True # unknown source — accept
|
||||
if self._here4_nid is None:
|
||||
return True # auto-detect mode — accept any
|
||||
return src == self._here4_nid
|
||||
|
||||
# ── DroneCAN message handlers ─────────────────────────────────────────
|
||||
|
||||
def _on_fix2(self, event) -> None:
|
||||
src = getattr(getattr(event, "transfer", None), "source_node_id", None)
|
||||
|
||||
# Auto-latch: first node that sends Fix2 becomes our Here4
|
||||
if self._here4_nid is None and src is not None:
|
||||
self._here4_nid = src
|
||||
self.get_logger().info(
|
||||
f"Here4 auto-discovered: DroneCAN node ID {src}"
|
||||
)
|
||||
nid_msg = Int32()
|
||||
nid_msg.data = src
|
||||
self._pub_node_id.publish(nid_msg)
|
||||
|
||||
if not self._accept(event):
|
||||
return
|
||||
d = parse_fix2(event.message)
|
||||
if d is None:
|
||||
return
|
||||
self._last_fix = d
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
self._publish_navsatfix(d, stamp)
|
||||
self._publish_velocity(d, stamp)
|
||||
|
||||
def _on_raw_imu(self, event) -> None:
|
||||
if not self._accept(event):
|
||||
return
|
||||
d = parse_raw_imu(event.message)
|
||||
if d is None:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
msg = Imu()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = self._imu_frame
|
||||
|
||||
# Orientation: not provided by DroneCAN RawIMU — signal unknown
|
||||
msg.orientation_covariance[0] = -1.0
|
||||
|
||||
gx, gy, gz = d["gyro_xyz"]
|
||||
msg.angular_velocity.x = gx
|
||||
msg.angular_velocity.y = gy
|
||||
msg.angular_velocity.z = gz
|
||||
_fill_cov9(msg.angular_velocity_covariance, d["gyro_cov"])
|
||||
|
||||
ax, ay, az = d["accel_xyz"]
|
||||
msg.linear_acceleration.x = ax
|
||||
msg.linear_acceleration.y = ay
|
||||
msg.linear_acceleration.z = az
|
||||
_fill_cov9(msg.linear_acceleration_covariance, d["accel_cov"])
|
||||
|
||||
self._pub_imu.publish(msg)
|
||||
|
||||
def _on_mag(self, event) -> None:
|
||||
if not self._accept(event):
|
||||
return
|
||||
d = parse_mag(event.message)
|
||||
if d is None:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
msg = MagneticField()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = self._imu_frame
|
||||
gx, gy, gz = d["field_ga"]
|
||||
msg.magnetic_field.x = gx * _GA_TO_T
|
||||
msg.magnetic_field.y = gy * _GA_TO_T
|
||||
msg.magnetic_field.z = gz * _GA_TO_T
|
||||
_fill_cov9(msg.magnetic_field_covariance,
|
||||
[v * _GA_TO_T * _GA_TO_T for v in d["field_cov"]])
|
||||
self._pub_mag.publish(msg)
|
||||
|
||||
def _on_pressure(self, event) -> None:
|
||||
if not self._accept(event):
|
||||
return
|
||||
d = parse_static_pressure(event.message)
|
||||
if d is None:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
msg = FluidPressure()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = self._imu_frame
|
||||
msg.fluid_pressure = d["pressure_pa"]
|
||||
msg.variance = d["pressure_variance"]
|
||||
self._pub_baro.publish(msg)
|
||||
|
||||
def _on_temperature(self, event) -> None:
|
||||
if not self._accept(event):
|
||||
return
|
||||
d = parse_static_temperature(event.message)
|
||||
if d is None:
|
||||
return
|
||||
self._last_temp_k = d["temperature_k"]
|
||||
|
||||
def _on_node_status(self, event) -> None:
|
||||
src = getattr(getattr(event, "transfer", None), "source_node_id", None)
|
||||
if src is None:
|
||||
return
|
||||
d = parse_node_status(event.message, src)
|
||||
if d is None:
|
||||
return
|
||||
|
||||
# Auto-discovery: latch first node that sends GPS-relevant data
|
||||
# NodeStatus alone is not enough — we track whichever source_node_id
|
||||
# first delivered a Fix2 message; if none yet, log all nodes seen.
|
||||
if self._here4_nid is None:
|
||||
# Any node could be Here4 — we'll latch on first Fix2 source
|
||||
self.get_logger().debug(
|
||||
f"DroneCAN node {src} alive: health={d['health_label']} "
|
||||
f"mode={d['mode_label']} uptime={d['uptime_sec']}s"
|
||||
)
|
||||
elif src == self._here4_nid:
|
||||
self._last_node_status = d
|
||||
|
||||
# ── NavSatFix publishing ─────────────────────────────────────────────
|
||||
|
||||
def _publish_navsatfix(self, d: dict, stamp) -> None:
|
||||
fix_type = d["fix_type"]
|
||||
|
||||
status = NavSatStatus()
|
||||
status.service = _SERVICE_FLAGS
|
||||
|
||||
# Upgrade status for RTK: hdop < 0.5 and 3D fix → treat as SBAS/RTK
|
||||
if fix_type == FIX_3D:
|
||||
if d["hdop"] < 0.5:
|
||||
status.status = NavSatStatus.STATUS_GBAS_FIX # RTK fixed
|
||||
elif d["hdop"] < 1.5:
|
||||
status.status = NavSatStatus.STATUS_SBAS_FIX # SBAS/DGPS
|
||||
else:
|
||||
status.status = NavSatStatus.STATUS_FIX
|
||||
else:
|
||||
status.status = _FIX_TO_NAV_STATUS.get(fix_type, NavSatStatus.STATUS_NO_FIX)
|
||||
|
||||
msg = NavSatFix()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = self._fix_frame
|
||||
msg.status = status
|
||||
msg.latitude = d["lat_deg"]
|
||||
msg.longitude = d["lon_deg"]
|
||||
msg.altitude = d["alt_msl_m"]
|
||||
|
||||
# Position covariance (diagonal from DroneCAN; full matrix if available)
|
||||
pos_cov = d["pos_cov"]
|
||||
msg.position_covariance_type = (
|
||||
NavSatFix.COVARIANCE_TYPE_FULL if any(pos_cov) else
|
||||
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
|
||||
)
|
||||
if any(pos_cov):
|
||||
for i, v in enumerate(pos_cov):
|
||||
msg.position_covariance[i] = v
|
||||
else:
|
||||
# Approximate from HDOP/VDOP: σ_h ≈ hdop × 3m, σ_v ≈ vdop × 5m
|
||||
sh2 = (d["hdop"] * 3.0) ** 2
|
||||
sv2 = (d["vdop"] * 5.0) ** 2
|
||||
msg.position_covariance[0] = sh2 # east
|
||||
msg.position_covariance[4] = sh2 # north
|
||||
msg.position_covariance[8] = sv2 # up
|
||||
|
||||
self._pub_fix.publish(msg)
|
||||
self._pub_fix_h4.publish(msg)
|
||||
|
||||
def _publish_velocity(self, d: dict, stamp) -> None:
|
||||
vn, ve, vd = d["ned_vel"]
|
||||
msg = TwistWithCovarianceStamped()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = self._fix_frame
|
||||
# NED → ENU: x=east=ve, y=north=vn, z=up=-vd
|
||||
msg.twist.twist.linear.x = ve
|
||||
msg.twist.twist.linear.y = vn
|
||||
msg.twist.twist.linear.z = -vd
|
||||
# Velocity covariance: approximate from HDOP
|
||||
vel_var = (d["hdop"] * 0.2) ** 2
|
||||
msg.twist.covariance[0] = vel_var # xx (east)
|
||||
msg.twist.covariance[7] = vel_var # yy (north)
|
||||
msg.twist.covariance[14] = (d["vdop"] * 0.3) ** 2 # zz (up)
|
||||
self._pub_vel.publish(msg)
|
||||
|
||||
# ── Status publish (1 Hz) ────────────────────────────────────────────
|
||||
|
||||
def _publish_status(self) -> None:
|
||||
fix = self._last_fix
|
||||
ns = getattr(self, "_last_node_status", None)
|
||||
|
||||
payload: dict = {
|
||||
"here4_node_id": self._here4_nid,
|
||||
"can_interface": self._iface,
|
||||
"dc_node_ready": self._dc_node is not None,
|
||||
"fix_type": fix["fix_type"] if fix else FIX_NO_FIX,
|
||||
"fix_label": fix["fix_label"] if fix else "NO_FIX",
|
||||
"sats_used": fix["sats_used"] if fix else 0,
|
||||
"hdop": round(fix["hdop"], 2) if fix else 99.0,
|
||||
"lat_deg": round(fix["lat_deg"], 7) if fix else 0.0,
|
||||
"lon_deg": round(fix["lon_deg"], 7) if fix else 0.0,
|
||||
"alt_msl_m": round(fix["alt_msl_m"], 2) if fix else 0.0,
|
||||
"temp_c": round(self._last_temp_k - 273.15, 1),
|
||||
"node_health": ns["health_label"] if ns else "UNKNOWN",
|
||||
"node_uptime_s": ns["uptime_sec"] if ns else 0,
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload)
|
||||
self._pub_status.publish(msg)
|
||||
|
||||
# Also publish discovered node ID
|
||||
if self._here4_nid is not None:
|
||||
nid_msg = Int32()
|
||||
nid_msg.data = self._here4_nid
|
||||
self._pub_node_id.publish(nid_msg)
|
||||
|
||||
# ── RTCM injection ────────────────────────────────────────────────────
|
||||
|
||||
def _on_rtcm_bytes(self, msg: ByteMultiArray) -> None:
|
||||
"""Receive RTCM bytes from /rtcm and inject via DroneCAN RTCMStream."""
|
||||
raw = bytes(msg.data)
|
||||
self._inject_rtcm(raw)
|
||||
|
||||
def _on_rtcm_hex(self, msg: String) -> None:
|
||||
"""Receive hex-encoded RTCM from /rtcm_hex (fallback topic)."""
|
||||
try:
|
||||
raw = bytes.fromhex(msg.data.strip())
|
||||
except ValueError as exc:
|
||||
self.get_logger().error(f"Bad /rtcm_hex: {exc}")
|
||||
return
|
||||
self._inject_rtcm(raw)
|
||||
|
||||
def _inject_rtcm(self, raw: bytes) -> None:
|
||||
if self._dc_node is None or not raw:
|
||||
return
|
||||
|
||||
global _rtcm_seq
|
||||
try:
|
||||
import dronecan # type: ignore[import]
|
||||
except ImportError:
|
||||
return
|
||||
|
||||
with _rtcm_lock:
|
||||
chunks = chunk_rtcm(raw, _rtcm_seq)
|
||||
_rtcm_seq = (_rtcm_seq + len(chunks)) & 0xFFFF
|
||||
|
||||
for chunk in chunks:
|
||||
try:
|
||||
rtcm_msg = dronecan.uavcan.equipment.gnss.RTCMStream()
|
||||
rtcm_msg.sequence_id = chunk["sequence_id"]
|
||||
rtcm_msg.data = list(chunk["data"])
|
||||
self._dc_node.broadcast(rtcm_msg)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
f"RTCM inject error (seq {chunk['sequence_id']}): {exc}",
|
||||
throttle_duration_sec=2.0,
|
||||
)
|
||||
|
||||
# ── Shutdown ──────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
if self._dc_node is not None:
|
||||
try:
|
||||
self._dc_node.close()
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
# ── Covariance helper ─────────────────────────────────────────────────────────
|
||||
|
||||
def _fill_cov9(ros_cov, values) -> None:
|
||||
"""Fill a 9-element ROS2 covariance array from a list."""
|
||||
for i, v in enumerate(values[:9]):
|
||||
ros_cov[i] = float(v)
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = Here4CanNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_dronecan_gps
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_dronecan_gps
|
||||
@ -1,27 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_dronecan_gps"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/config", ["config/here4_params.yaml"]),
|
||||
(f"share/{package_name}/launch", ["launch/here4.launch.py"]),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-perception",
|
||||
maintainer_email="sl-perception@saltylab.local",
|
||||
description="DroneCAN/UAVCAN v0 bridge for Here4 GPS via CANable2 (bd-p47c)",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"here4_node = saltybot_dronecan_gps.here4_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,367 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Unit tests for saltybot_dronecan_gps.dronecan_parser.
|
||||
|
||||
No dronecan library, ROS2, or CAN hardware required.
|
||||
Uses simple Python objects (namespaces) to simulate dronecan message objects.
|
||||
|
||||
Run with: pytest test/ -v
|
||||
"""
|
||||
|
||||
import math
|
||||
import types
|
||||
import unittest
|
||||
|
||||
from saltybot_dronecan_gps.dronecan_parser import (
|
||||
DTID_FIX2,
|
||||
DTID_RAW_IMU,
|
||||
DTID_MAG_FIELD,
|
||||
DTID_STATIC_PRESSURE,
|
||||
DTID_STATIC_TEMPERATURE,
|
||||
DTID_NODE_STATUS,
|
||||
DTID_RTCM_STREAM,
|
||||
FIX_NO_FIX,
|
||||
FIX_TIME_ONLY,
|
||||
FIX_2D,
|
||||
FIX_3D,
|
||||
RTCM_MAX_CHUNK,
|
||||
chunk_rtcm,
|
||||
parse_fix2,
|
||||
parse_mag,
|
||||
parse_node_status,
|
||||
parse_raw_imu,
|
||||
parse_static_pressure,
|
||||
parse_static_temperature,
|
||||
_getf,
|
||||
_list3,
|
||||
_list9,
|
||||
)
|
||||
|
||||
|
||||
# ── Helpers ───────────────────────────────────────────────────────────────────
|
||||
|
||||
def _ns(**kwargs) -> types.SimpleNamespace:
|
||||
"""Build a simple namespace to mimic a dronecan message object."""
|
||||
return types.SimpleNamespace(**kwargs)
|
||||
|
||||
|
||||
def _ns_vec(*values):
|
||||
"""Return an object that is indexable like a dronecan vector field."""
|
||||
return list(values)
|
||||
|
||||
|
||||
# ── DTID constants ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestDtidConstants(unittest.TestCase):
|
||||
def test_known_values(self):
|
||||
self.assertEqual(DTID_FIX2, 1063)
|
||||
self.assertEqual(DTID_RAW_IMU, 1003)
|
||||
self.assertEqual(DTID_MAG_FIELD, 1001)
|
||||
self.assertEqual(DTID_STATIC_PRESSURE, 1028)
|
||||
self.assertEqual(DTID_STATIC_TEMPERATURE, 1029)
|
||||
self.assertEqual(DTID_NODE_STATUS, 341)
|
||||
self.assertEqual(DTID_RTCM_STREAM, 1062)
|
||||
|
||||
def test_rtcm_chunk_size(self):
|
||||
self.assertEqual(RTCM_MAX_CHUNK, 128)
|
||||
|
||||
|
||||
# ── _getf / _list3 / _list9 ──────────────────────────────────────────────────
|
||||
|
||||
class TestHelpers(unittest.TestCase):
|
||||
def test_getf_simple(self):
|
||||
obj = _ns(x=42)
|
||||
self.assertEqual(_getf(obj, "x"), 42)
|
||||
|
||||
def test_getf_missing(self):
|
||||
obj = _ns(x=1)
|
||||
self.assertIsNone(_getf(obj, "y"))
|
||||
|
||||
def test_getf_default(self):
|
||||
obj = _ns()
|
||||
self.assertEqual(_getf(obj, "missing", default=99), 99)
|
||||
|
||||
def test_list3_ok(self):
|
||||
self.assertEqual(_list3([1.0, 2.0, 3.0]), [1.0, 2.0, 3.0])
|
||||
|
||||
def test_list3_bad(self):
|
||||
self.assertEqual(_list3(None), [0.0, 0.0, 0.0])
|
||||
|
||||
def test_list9_ok(self):
|
||||
vals = list(range(9))
|
||||
self.assertEqual(_list9(vals), [float(v) for v in vals])
|
||||
|
||||
def test_list9_short(self):
|
||||
self.assertEqual(_list9(None), [0.0] * 9)
|
||||
|
||||
|
||||
# ── parse_fix2 ────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestParseFix2(unittest.TestCase):
|
||||
|
||||
def _make_fix(self, lat_1e8=0, lon_1e8=0, alt_msl_mm=0, alt_ell_mm=0,
|
||||
ned_vel=None, status=FIX_3D, sats=12,
|
||||
pdop=1.2, hdop=0.9, vdop=1.5,
|
||||
pos_cov=None, undulation=5.0, gnss_ts=0):
|
||||
return _ns(
|
||||
latitude_deg_1e8 = lat_1e8,
|
||||
longitude_deg_1e8 = lon_1e8,
|
||||
height_msl_mm = alt_msl_mm,
|
||||
height_ellipsoid_mm = alt_ell_mm,
|
||||
ned_velocity = ned_vel if ned_vel is not None else _ns_vec(0.0, 0.0, 0.0),
|
||||
status = status,
|
||||
sats_used = sats,
|
||||
pdop = pdop,
|
||||
hdop = hdop,
|
||||
vdop = vdop,
|
||||
position_covariance = pos_cov,
|
||||
undulation = undulation,
|
||||
gnss_timestamp = gnss_ts,
|
||||
)
|
||||
|
||||
def test_basic_3d_fix(self):
|
||||
msg = self._make_fix(
|
||||
lat_1e8 = 4_576_543_210, # 45.7654321°
|
||||
lon_1e8 = -7_345_678_901, # -73.45678901°
|
||||
alt_msl_mm = 50_000, # 50 m
|
||||
alt_ell_mm = 52_000, # 52 m
|
||||
ned_vel = _ns_vec(1.5, -0.3, 0.0),
|
||||
sats = 14,
|
||||
hdop = 0.8,
|
||||
)
|
||||
d = parse_fix2(msg)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertAlmostEqual(d["lat_deg"], 45.7654321, places=5)
|
||||
self.assertAlmostEqual(d["lon_deg"], -73.45678901, places=5)
|
||||
self.assertAlmostEqual(d["alt_msl_m"], 50.0, places=3)
|
||||
self.assertAlmostEqual(d["alt_ellipsoid_m"], 52.0, places=3)
|
||||
self.assertEqual(d["ned_vel"], [1.5, -0.3, 0.0])
|
||||
self.assertEqual(d["fix_type"], FIX_3D)
|
||||
self.assertEqual(d["fix_label"], "3D_FIX")
|
||||
self.assertEqual(d["sats_used"], 14)
|
||||
self.assertAlmostEqual(d["hdop"], 0.8, places=5)
|
||||
|
||||
def test_no_fix(self):
|
||||
msg = self._make_fix(status=FIX_NO_FIX, sats=0)
|
||||
d = parse_fix2(msg)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertEqual(d["fix_type"], FIX_NO_FIX)
|
||||
self.assertEqual(d["fix_label"], "NO_FIX")
|
||||
self.assertEqual(d["sats_used"], 0)
|
||||
|
||||
def test_time_only(self):
|
||||
msg = self._make_fix(status=FIX_TIME_ONLY)
|
||||
d = parse_fix2(msg)
|
||||
self.assertEqual(d["fix_label"], "TIME_ONLY")
|
||||
|
||||
def test_2d_fix(self):
|
||||
msg = self._make_fix(status=FIX_2D)
|
||||
d = parse_fix2(msg)
|
||||
self.assertEqual(d["fix_label"], "2D_FIX")
|
||||
|
||||
def test_position_covariance(self):
|
||||
cov = [float(i) for i in range(9)]
|
||||
msg = self._make_fix(pos_cov=cov)
|
||||
d = parse_fix2(msg)
|
||||
self.assertEqual(d["pos_cov"], cov)
|
||||
|
||||
def test_no_covariance_returns_zeros(self):
|
||||
msg = self._make_fix(pos_cov=None)
|
||||
d = parse_fix2(msg)
|
||||
self.assertEqual(d["pos_cov"], [0.0] * 9)
|
||||
|
||||
def test_zero_lat_lon(self):
|
||||
msg = self._make_fix(lat_1e8=0, lon_1e8=0)
|
||||
d = parse_fix2(msg)
|
||||
self.assertAlmostEqual(d["lat_deg"], 0.0, places=8)
|
||||
self.assertAlmostEqual(d["lon_deg"], 0.0, places=8)
|
||||
|
||||
def test_returns_none_on_bad_input(self):
|
||||
self.assertIsNone(parse_fix2(None))
|
||||
|
||||
def test_gnss_timestamp(self):
|
||||
msg = self._make_fix(gnss_ts=1_000_000_000)
|
||||
d = parse_fix2(msg)
|
||||
self.assertEqual(d["gnss_timestamp_us"], 1_000_000_000)
|
||||
|
||||
|
||||
# ── parse_raw_imu ─────────────────────────────────────────────────────────────
|
||||
|
||||
class TestParseRawImu(unittest.TestCase):
|
||||
|
||||
def _make_imu(self, gyro=None, accel=None, dt=0.02,
|
||||
gyro_cov=None, accel_cov=None):
|
||||
return _ns(
|
||||
rate_gyro_latest = gyro if gyro else _ns_vec(0.0, 0.0, 0.0),
|
||||
accelerometer_latest = accel if accel else _ns_vec(0.0, 0.0, 9.81),
|
||||
integration_interval = dt,
|
||||
rate_gyro_covariance = gyro_cov if gyro_cov else [0.0] * 9,
|
||||
accelerometer_covariance = accel_cov if accel_cov else [0.0] * 9,
|
||||
)
|
||||
|
||||
def test_basic(self):
|
||||
msg = self._make_imu(
|
||||
gyro = _ns_vec(0.01, -0.02, 0.03),
|
||||
accel = _ns_vec(0.1, -0.2, 9.8),
|
||||
dt = 0.02,
|
||||
)
|
||||
d = parse_raw_imu(msg)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertEqual(d["gyro_xyz"], [0.01, -0.02, 0.03])
|
||||
self.assertEqual(d["accel_xyz"], [0.1, -0.2, 9.8])
|
||||
self.assertAlmostEqual(d["dt_s"], 0.02)
|
||||
|
||||
def test_covariances_returned(self):
|
||||
cov = [float(i * 0.001) for i in range(9)]
|
||||
msg = self._make_imu(gyro_cov=cov, accel_cov=cov)
|
||||
d = parse_raw_imu(msg)
|
||||
self.assertEqual(d["gyro_cov"], cov)
|
||||
self.assertEqual(d["accel_cov"], cov)
|
||||
|
||||
def test_none_input(self):
|
||||
self.assertIsNone(parse_raw_imu(None))
|
||||
|
||||
|
||||
# ── parse_mag ─────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestParseMag(unittest.TestCase):
|
||||
|
||||
def _make_mag(self, sensor_id=0, field=None, cov=None):
|
||||
return _ns(
|
||||
sensor_id = sensor_id,
|
||||
magnetic_field_ga = field if field else _ns_vec(0.1, 0.2, -0.5),
|
||||
magnetic_field_covariance = cov if cov else [0.0] * 9,
|
||||
)
|
||||
|
||||
def test_basic(self):
|
||||
msg = self._make_mag(sensor_id=1, field=_ns_vec(0.1, 0.2, -0.5))
|
||||
d = parse_mag(msg)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertEqual(d["sensor_id"], 1)
|
||||
self.assertEqual(d["field_ga"], [0.1, 0.2, -0.5])
|
||||
|
||||
def test_none_input(self):
|
||||
self.assertIsNone(parse_mag(None))
|
||||
|
||||
|
||||
# ── parse_static_pressure ────────────────────────────────────────────────────
|
||||
|
||||
class TestParseStaticPressure(unittest.TestCase):
|
||||
|
||||
def test_basic(self):
|
||||
msg = _ns(static_pressure=101325.0, static_pressure_variance=100.0)
|
||||
d = parse_static_pressure(msg)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertAlmostEqual(d["pressure_pa"], 101325.0)
|
||||
self.assertAlmostEqual(d["pressure_variance"], 100.0)
|
||||
|
||||
def test_none_input(self):
|
||||
self.assertIsNone(parse_static_pressure(None))
|
||||
|
||||
|
||||
# ── parse_static_temperature ─────────────────────────────────────────────────
|
||||
|
||||
class TestParseStaticTemperature(unittest.TestCase):
|
||||
|
||||
def test_celsius_to_kelvin(self):
|
||||
msg = _ns(static_temperature=293.15) # 20 °C
|
||||
d = parse_static_temperature(msg)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertAlmostEqual(d["temperature_k"], 293.15)
|
||||
|
||||
def test_none_input(self):
|
||||
self.assertIsNone(parse_static_temperature(None))
|
||||
|
||||
|
||||
# ── parse_node_status ────────────────────────────────────────────────────────
|
||||
|
||||
class TestParseNodeStatus(unittest.TestCase):
|
||||
|
||||
def _make_status(self, health=0, mode=0, uptime=120, vendor_code=0):
|
||||
return _ns(
|
||||
health = health,
|
||||
mode = mode,
|
||||
uptime_sec = uptime,
|
||||
vendor_specific_status_code = vendor_code,
|
||||
)
|
||||
|
||||
def test_ok_operational(self):
|
||||
msg = self._make_status(health=0, mode=0, uptime=300)
|
||||
d = parse_node_status(msg, source_node_id=10)
|
||||
self.assertIsNotNone(d)
|
||||
self.assertEqual(d["node_id"], 10)
|
||||
self.assertEqual(d["health"], 0)
|
||||
self.assertEqual(d["health_label"], "OK")
|
||||
self.assertEqual(d["mode_label"], "OPERATIONAL")
|
||||
self.assertEqual(d["uptime_sec"], 300)
|
||||
|
||||
def test_error_state(self):
|
||||
msg = self._make_status(health=2, mode=0)
|
||||
d = parse_node_status(msg, source_node_id=5)
|
||||
self.assertEqual(d["health_label"], "ERROR")
|
||||
|
||||
def test_critical_state(self):
|
||||
msg = self._make_status(health=3)
|
||||
d = parse_node_status(msg, 7)
|
||||
self.assertEqual(d["health_label"], "CRITICAL")
|
||||
|
||||
def test_none_input(self):
|
||||
self.assertIsNone(parse_node_status(None, 1))
|
||||
|
||||
|
||||
# ── chunk_rtcm ────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestChunkRtcm(unittest.TestCase):
|
||||
|
||||
def test_empty(self):
|
||||
chunks = chunk_rtcm(b"", 0)
|
||||
self.assertEqual(chunks, [])
|
||||
|
||||
def test_single_chunk(self):
|
||||
data = bytes(range(64))
|
||||
chunks = chunk_rtcm(data, 5)
|
||||
self.assertEqual(len(chunks), 1)
|
||||
self.assertEqual(chunks[0]["sequence_id"], 5)
|
||||
self.assertEqual(chunks[0]["data"], data)
|
||||
|
||||
def test_exact_chunk_boundary(self):
|
||||
data = bytes(RTCM_MAX_CHUNK)
|
||||
chunks = chunk_rtcm(data, 0)
|
||||
self.assertEqual(len(chunks), 1)
|
||||
self.assertEqual(len(chunks[0]["data"]), RTCM_MAX_CHUNK)
|
||||
|
||||
def test_two_chunks(self):
|
||||
data = bytes(RTCM_MAX_CHUNK + 1)
|
||||
chunks = chunk_rtcm(data, 0)
|
||||
self.assertEqual(len(chunks), 2)
|
||||
self.assertEqual(len(chunks[0]["data"]), RTCM_MAX_CHUNK)
|
||||
self.assertEqual(len(chunks[1]["data"]), 1)
|
||||
|
||||
def test_sequence_id_increments(self):
|
||||
data = bytes(RTCM_MAX_CHUNK * 3)
|
||||
chunks = chunk_rtcm(data, 10)
|
||||
for i, chunk in enumerate(chunks):
|
||||
self.assertEqual(chunk["sequence_id"], 10 + i)
|
||||
|
||||
def test_sequence_id_wraps_at_16bit(self):
|
||||
data = bytes(RTCM_MAX_CHUNK * 2)
|
||||
chunks = chunk_rtcm(data, 0xFFFF)
|
||||
self.assertEqual(chunks[0]["sequence_id"], 0xFFFF)
|
||||
self.assertEqual(chunks[1]["sequence_id"], 0x0000) # wraps
|
||||
|
||||
def test_data_round_trip(self):
|
||||
original = bytes(range(200))
|
||||
chunks = chunk_rtcm(original, 0)
|
||||
reassembled = b"".join(c["data"] for c in chunks)
|
||||
self.assertEqual(reassembled, original)
|
||||
|
||||
def test_large_message(self):
|
||||
data = bytes(1024)
|
||||
chunks = chunk_rtcm(data, 0)
|
||||
total_bytes = sum(len(c["data"]) for c in chunks)
|
||||
self.assertEqual(total_bytes, 1024)
|
||||
for c in chunks:
|
||||
self.assertLessEqual(len(c["data"]), RTCM_MAX_CHUNK)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -1,20 +0,0 @@
|
||||
esp32_balance_node:
|
||||
ros__parameters:
|
||||
# USB-CDC serial port: CH343 chip, VID:PID 1a86:55d3
|
||||
# Udev symlink set by jetson/docs/pinout.md udev rules
|
||||
serial_port: /dev/esp32-balance
|
||||
baud_rate: 460800
|
||||
|
||||
# /cmd_vel linear.x (m/s) → speed units sent to ESP32-S3 BALANCE
|
||||
speed_scale: 1000.0
|
||||
# /cmd_vel angular.z (rad/s) → steer units (negative = flip turn convention)
|
||||
steer_scale: -500.0
|
||||
|
||||
# Safety: send CMD_DRIVE(0,0) if /cmd_vel silent for this many seconds
|
||||
command_timeout_s: 0.5
|
||||
|
||||
# CMD_HEARTBEAT TX interval in seconds (ESP32-S3 watchdog fires at 500 ms)
|
||||
heartbeat_period: 0.2
|
||||
|
||||
# Reconnect attempt interval when serial is lost
|
||||
reconnect_delay: 2.0
|
||||
@ -1,73 +0,0 @@
|
||||
"""esp32_balance.launch.py — Launch the Orin↔ESP32-S3 BALANCE USB-serial bridge.
|
||||
|
||||
bd-wim1: replaces can_bridge_node (CANable2/python-can) with USB-CDC serial.
|
||||
The freed CANable2 slot is used by Here4 GPS (bd-p47c).
|
||||
|
||||
USB device
|
||||
----------
|
||||
CH343 USB-CDC chip on ESP32-S3 BALANCE board
|
||||
VID:PID 1a86:55d3
|
||||
Symlink /dev/esp32-balance (via udev rule in jetson/docs/pinout.md)
|
||||
Baud 460800
|
||||
|
||||
Usage
|
||||
-----
|
||||
# Default:
|
||||
ros2 launch saltybot_esp32_serial esp32_balance.launch.py
|
||||
|
||||
# Override port (e.g. direct ACM path):
|
||||
ros2 launch saltybot_esp32_serial esp32_balance.launch.py serial_port:=/dev/ttyACM0
|
||||
|
||||
# Custom velocity scales:
|
||||
ros2 launch saltybot_esp32_serial esp32_balance.launch.py speed_scale:=800.0 steer_scale:=-400.0
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
pkg_share = get_package_share_directory("saltybot_esp32_serial")
|
||||
params_file = os.path.join(pkg_share, "config", "esp32_balance_params.yaml")
|
||||
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/esp32-balance",
|
||||
description="USB-CDC serial port for ESP32-S3 BALANCE (CH343, 1a86:55d3)",
|
||||
)
|
||||
speed_scale_arg = DeclareLaunchArgument(
|
||||
"speed_scale",
|
||||
default_value="1000.0",
|
||||
description="linear.x (m/s) → motor speed units",
|
||||
)
|
||||
steer_scale_arg = DeclareLaunchArgument(
|
||||
"steer_scale",
|
||||
default_value="-500.0",
|
||||
description="angular.z (rad/s) → steer units",
|
||||
)
|
||||
|
||||
node = Node(
|
||||
package="saltybot_esp32_serial",
|
||||
executable="esp32_balance_node",
|
||||
name="esp32_balance_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
params_file,
|
||||
{
|
||||
"serial_port": LaunchConfiguration("serial_port"),
|
||||
"speed_scale": LaunchConfiguration("speed_scale"),
|
||||
"steer_scale": LaunchConfiguration("steer_scale"),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
serial_port_arg,
|
||||
speed_scale_arg,
|
||||
steer_scale_arg,
|
||||
node,
|
||||
])
|
||||
@ -1,40 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_esp32_serial</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
UART/USB-CDC serial bridge for SaltyBot Orin — interfaces with the
|
||||
ESP32-S3 BALANCE board (CH343, 1a86:55d3) using a binary framing protocol
|
||||
[0xAA][LEN][TYPE][PAYLOAD][CRC8]. Replaces saltybot_can_bridge (CANable2)
|
||||
to free the CAN bus for Here4 GPS (bd-p47c).
|
||||
|
||||
Publishes the same ROS2 topics as saltybot_can_bridge for backward compat:
|
||||
/saltybot/attitude, /can/battery, /can/vesc/{left,right}/state,
|
||||
/can/connection_status
|
||||
|
||||
System dependency: pyserial (apt: python3-serial or pip: pyserial)
|
||||
Bead: bd-wim1 Counterpart firmware: bd-66hx (hal)
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
|
||||
<!-- pyserial: apt install python3-serial or pip install pyserial -->
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1 +0,0 @@
|
||||
# saltybot_esp32_serial — UART/USB-CDC bridge for ESP32-S3 BALANCE (bd-wim1)
|
||||
@ -1,369 +0,0 @@
|
||||
"""esp32_balance_node.py — ROS2 bridge: Orin ↔ ESP32-S3 BALANCE via USB-CDC serial.
|
||||
|
||||
bd-wim1: replaces saltybot_can_bridge (CANable2/python-can) with USB serial.
|
||||
The CANable2 is freed for Here4 GPS (bd-p47c).
|
||||
|
||||
Physical connection
|
||||
-------------------
|
||||
Device : /dev/esp32-balance (CH343 USB-CDC, VID:PID 1a86:55d3)
|
||||
Baud : 460800, 8N1
|
||||
Protocol: [0xAA][LEN][TYPE][PAYLOAD][CRC8] (see esp32_balance_protocol.py)
|
||||
|
||||
Subscriptions
|
||||
-------------
|
||||
/cmd_vel geometry_msgs/Twist → CMD_DRIVE (at up to 50 Hz)
|
||||
/estop std_msgs/Bool → CMD_ESTOP
|
||||
/saltybot/arm std_msgs/Bool → CMD_ARM
|
||||
/saltybot/pid_update std_msgs/String JSON → CMD_PID {"kp":…,"ki":…,"kd":…}
|
||||
|
||||
Publications
|
||||
------------
|
||||
/saltybot/attitude std_msgs/String JSON pitch, motor_cmd, state, etc.
|
||||
/saltybot/balance_state std_msgs/String same as /saltybot/attitude (alias)
|
||||
/can/battery sensor_msgs/BatteryState from TELEM_STATUS.vbat_mv
|
||||
/can/vesc/left/state std_msgs/Float32MultiArray [erpm, duty≡0, voltage_mv/1000, current_ma/1000]
|
||||
/can/vesc/right/state std_msgs/Float32MultiArray same layout
|
||||
/can/connection_status std_msgs/String "connected" | "disconnected"
|
||||
|
||||
NOTE: /can/* topic names are preserved for backward compatibility with
|
||||
nodes that subscribed to saltybot_can_bridge outputs.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
serial_port str USB-CDC port default: /dev/esp32-balance
|
||||
baud_rate int Baud rate default: 460800
|
||||
speed_scale float linear.x (m/s) → units default: 1000.0
|
||||
steer_scale float angular.z (rad/s) → units default: -500.0
|
||||
command_timeout_s float /cmd_vel watchdog default: 0.5
|
||||
heartbeat_period float CMD_HEARTBEAT interval default: 0.2
|
||||
reconnect_delay float Retry interval, s default: 2.0
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
import serial
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
|
||||
from .esp32_balance_protocol import (
|
||||
BAUD_RATE,
|
||||
SERIAL_PORT_DEFAULT,
|
||||
AckFrame,
|
||||
BalanceState,
|
||||
BALANCE_STATE_LABEL,
|
||||
FrameParser,
|
||||
NackFrame,
|
||||
StatusFrame,
|
||||
VescFrame,
|
||||
encode_arm,
|
||||
encode_drive,
|
||||
encode_estop,
|
||||
encode_heartbeat,
|
||||
encode_pid,
|
||||
)
|
||||
|
||||
_WATCHDOG_HZ: float = 20.0 # TX watchdog check rate
|
||||
|
||||
|
||||
class Esp32BalanceNode(Node):
|
||||
"""Serial bridge replacing CANable2 CAN comms with USB-CDC to ESP32-S3 BALANCE."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("esp32_balance_node")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", SERIAL_PORT_DEFAULT)
|
||||
self.declare_parameter("baud_rate", BAUD_RATE)
|
||||
self.declare_parameter("speed_scale", 1000.0)
|
||||
self.declare_parameter("steer_scale", -500.0)
|
||||
self.declare_parameter("command_timeout_s", 0.5)
|
||||
self.declare_parameter("heartbeat_period", 0.2)
|
||||
self.declare_parameter("reconnect_delay", 2.0)
|
||||
|
||||
self._port_name = self.get_parameter("serial_port").value
|
||||
self._baud = self.get_parameter("baud_rate").value
|
||||
self._speed_scale = self.get_parameter("speed_scale").value
|
||||
self._steer_scale = self.get_parameter("steer_scale").value
|
||||
self._cmd_timeout = self.get_parameter("command_timeout_s").value
|
||||
self._hb_period = self.get_parameter("heartbeat_period").value
|
||||
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────
|
||||
self._ser: Optional[serial.Serial] = None
|
||||
self._ser_lock = threading.Lock()
|
||||
self._parser = FrameParser()
|
||||
self._last_cmd_time = time.monotonic()
|
||||
self._watchdog_sent = False
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10)
|
||||
self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10)
|
||||
self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
|
||||
self._pub_vesc_left = self.create_publisher(Float32MultiArray, "/can/vesc/left/state", 10)
|
||||
self._pub_vesc_right = self.create_publisher(Float32MultiArray, "/can/vesc/right/state", 10)
|
||||
self._pub_status = self.create_publisher(String, "/can/connection_status", 10)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
||||
self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
|
||||
self.create_subscription(String, "/saltybot/pid_update", self._pid_cb, 10)
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────
|
||||
self.create_timer(self._hb_period, self._heartbeat_cb)
|
||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||
self.create_timer(self._reconnect_delay, self._reconnect_cb)
|
||||
|
||||
# ── Open port + start reader thread ───────────────────────────────
|
||||
self._try_connect()
|
||||
self._reader_thread = threading.Thread(
|
||||
target=self._reader_loop, daemon=True, name="balance_serial_reader"
|
||||
)
|
||||
self._reader_thread.start()
|
||||
|
||||
self.get_logger().info(
|
||||
f"esp32_balance_node — {self._port_name} @ {self._baud} baud | "
|
||||
f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────
|
||||
|
||||
def _try_connect(self) -> None:
|
||||
with self._ser_lock:
|
||||
if self._ser is not None and self._ser.is_open:
|
||||
return
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
port=self._port_name,
|
||||
baudrate=self._baud,
|
||||
timeout=0.05,
|
||||
write_timeout=0.1,
|
||||
)
|
||||
self._ser.reset_input_buffer()
|
||||
self._parser.reset()
|
||||
self.get_logger().info(f"Serial open: {self._port_name}")
|
||||
self._publish_status("connected")
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().warning(
|
||||
f"Cannot open {self._port_name}: {exc} — retry in {self._reconnect_delay:.0f} s",
|
||||
throttle_duration_sec=self._reconnect_delay,
|
||||
)
|
||||
self._ser = None
|
||||
self._publish_status("disconnected")
|
||||
|
||||
def _reconnect_cb(self) -> None:
|
||||
with self._ser_lock:
|
||||
open_ok = self._ser is not None and self._ser.is_open
|
||||
if not open_ok:
|
||||
self._try_connect()
|
||||
|
||||
def _handle_serial_error(self, exc: Exception, context: str) -> None:
|
||||
self.get_logger().warning(f"Serial error in {context}: {exc}")
|
||||
with self._ser_lock:
|
||||
if self._ser is not None:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
self._publish_status("disconnected")
|
||||
|
||||
# ── Serial write helper ───────────────────────────────────────────────
|
||||
|
||||
def _write(self, data: bytes, context: str = "") -> bool:
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
return False
|
||||
ser = self._ser
|
||||
try:
|
||||
ser.write(data)
|
||||
return True
|
||||
except serial.SerialException as exc:
|
||||
self._handle_serial_error(exc, f"write({context})")
|
||||
return False
|
||||
|
||||
# ── ROS subscriptions ─────────────────────────────────────────────────
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist) -> None:
|
||||
"""Convert /cmd_vel to CMD_DRIVE at up to 50 Hz."""
|
||||
self._last_cmd_time = time.monotonic()
|
||||
self._watchdog_sent = False
|
||||
|
||||
speed = int(msg.linear.x * self._speed_scale)
|
||||
steer = int(msg.angular.z * self._steer_scale)
|
||||
self._write(encode_drive(speed, steer), "cmd_vel")
|
||||
|
||||
def _estop_cb(self, msg: Bool) -> None:
|
||||
frame = encode_estop(msg.data)
|
||||
self._write(frame, "estop")
|
||||
if msg.data:
|
||||
self.get_logger().warning("E-stop asserted → CMD_ESTOP sent to ESP32-S3 BALANCE")
|
||||
|
||||
def _arm_cb(self, msg: Bool) -> None:
|
||||
self._write(encode_arm(msg.data), "arm")
|
||||
|
||||
def _pid_cb(self, msg: String) -> None:
|
||||
"""Parse JSON {"kp":…,"ki":…,"kd":…} and send CMD_PID."""
|
||||
try:
|
||||
d = json.loads(msg.data)
|
||||
kp = float(d["kp"])
|
||||
ki = float(d["ki"])
|
||||
kd = float(d["kd"])
|
||||
except (KeyError, ValueError, json.JSONDecodeError) as exc:
|
||||
self.get_logger().error(f"Bad /saltybot/pid_update JSON: {exc}")
|
||||
return
|
||||
self._write(encode_pid(kp, ki, kd), "pid_update")
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────────
|
||||
|
||||
def _heartbeat_cb(self) -> None:
|
||||
self._write(encode_heartbeat(), "heartbeat")
|
||||
|
||||
def _watchdog_cb(self) -> None:
|
||||
"""Send CMD_DRIVE(0,0) if /cmd_vel has been silent for command_timeout_s."""
|
||||
if self._watchdog_sent:
|
||||
return
|
||||
if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
|
||||
self._write(encode_drive(0, 0), "watchdog")
|
||||
self._watchdog_sent = True
|
||||
|
||||
# ── Background reader ─────────────────────────────────────────────────
|
||||
|
||||
def _reader_loop(self) -> None:
|
||||
while rclpy.ok():
|
||||
with self._ser_lock:
|
||||
ser = self._ser if (self._ser and self._ser.is_open) else None
|
||||
if ser is None:
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
try:
|
||||
n = ser.in_waiting
|
||||
if n:
|
||||
raw = ser.read(n)
|
||||
else:
|
||||
time.sleep(0.002)
|
||||
continue
|
||||
except serial.SerialException as exc:
|
||||
self._handle_serial_error(exc, "reader_loop")
|
||||
continue
|
||||
|
||||
for byte in raw:
|
||||
frame = self._parser.feed(byte)
|
||||
if frame is not None:
|
||||
self._dispatch(frame)
|
||||
|
||||
def _dispatch(self, frame) -> None:
|
||||
try:
|
||||
if isinstance(frame, StatusFrame):
|
||||
self._handle_status(frame)
|
||||
elif isinstance(frame, VescFrame):
|
||||
self._handle_vesc(frame)
|
||||
elif isinstance(frame, AckFrame):
|
||||
self.get_logger().debug(
|
||||
f"ACK for cmd type 0x{frame.echoed_type:02X}"
|
||||
)
|
||||
elif isinstance(frame, NackFrame):
|
||||
self.get_logger().warning(
|
||||
f"NACK cmd=0x{frame.cmd_type:02X} err=0x{frame.error_code:02X}"
|
||||
)
|
||||
elif isinstance(frame, tuple):
|
||||
type_code, _ = frame
|
||||
self.get_logger().debug(f"Unknown telemetry type 0x{type_code:02X}")
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f"Error dispatching frame {type(frame).__name__}: {exc}")
|
||||
|
||||
# ── Frame handlers ────────────────────────────────────────────────────
|
||||
|
||||
def _handle_status(self, f: StatusFrame) -> None:
|
||||
"""Publish balance controller status — replaces CAN 0x400 handler."""
|
||||
now = self.get_clock().now().to_msg()
|
||||
payload = {
|
||||
"pitch_deg": round(f.pitch_deg, 2),
|
||||
"motor_cmd": f.motor_cmd,
|
||||
"vbat_mv": f.vbat_mv,
|
||||
"balance_state": f.balance_state,
|
||||
"state_label": BALANCE_STATE_LABEL.get(f.balance_state,
|
||||
f"UNKNOWN({f.balance_state})"),
|
||||
"flags": f.flags,
|
||||
"estop_active": bool(f.flags & 0x01),
|
||||
"hb_timeout": bool(f.flags & 0x02),
|
||||
"ts": f"{now.sec}.{now.nanosec:09d}",
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload)
|
||||
self._pub_attitude.publish(msg)
|
||||
self._pub_balance.publish(msg)
|
||||
|
||||
# Battery piggyback on STATUS frame for nodes that use /can/battery
|
||||
bat = BatteryState()
|
||||
bat.header.stamp = now
|
||||
bat.voltage = f.vbat_mv / 1000.0
|
||||
bat.present = True
|
||||
bat.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
self._pub_battery.publish(bat)
|
||||
|
||||
def _handle_vesc(self, f: VescFrame) -> None:
|
||||
"""Publish proxied VESC telemetry — replaces CAN VESC_STATUS_1 handler.
|
||||
|
||||
Float32MultiArray layout: [erpm, voltage_v, current_a, temp_c]
|
||||
(duty not available via serial proxy; 0.0 placeholder)
|
||||
"""
|
||||
arr = Float32MultiArray()
|
||||
arr.data = [
|
||||
float(f.erpm),
|
||||
float(f.voltage_mv) / 1000.0,
|
||||
float(f.current_ma) / 1000.0,
|
||||
f.temp_c,
|
||||
]
|
||||
if f.vesc_id == 56:
|
||||
self._pub_vesc_left.publish(arr)
|
||||
elif f.vesc_id == 68:
|
||||
self._pub_vesc_right.publish(arr)
|
||||
else:
|
||||
self.get_logger().warning(f"Unknown VESC ID {f.vesc_id} in telemetry frame")
|
||||
|
||||
# ── Status helper ─────────────────────────────────────────────────────
|
||||
|
||||
def _publish_status(self, status: str) -> None:
|
||||
msg = String()
|
||||
msg.data = status
|
||||
self._pub_status.publish(msg)
|
||||
|
||||
# ── Shutdown ──────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
# Send zero drive + estop before closing
|
||||
self._write(encode_drive(0, 0), "shutdown")
|
||||
self._write(encode_estop(True), "shutdown")
|
||||
with self._ser_lock:
|
||||
if self._ser is not None:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = Esp32BalanceNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,355 +0,0 @@
|
||||
"""esp32_balance_protocol.py — Binary frame codec for Orin↔ESP32-S3 BALANCE UART/USB comms.
|
||||
|
||||
bd-wim1: replaces CANable2/python-can ESP32 interface with USB-CDC serial.
|
||||
Counterpart firmware bead: bd-66hx (hal's ESP32-S3 BALANCE side).
|
||||
|
||||
Frame layout:
|
||||
┌───────┬─────┬──────┬──────────────────┬───────┐
|
||||
│ SYNC │ LEN │ TYPE │ PAYLOAD │ CRC8 │
|
||||
│ 0xAA │ 1B │ 1B │ LEN bytes │ 1B │
|
||||
└───────┴─────┴──────┴──────────────────┴───────┘
|
||||
|
||||
LEN = number of payload bytes (0–62)
|
||||
CRC8 = CRC8-SMBUS (poly=0x07, init=0x00), computed over LEN+TYPE+PAYLOAD
|
||||
|
||||
Physical layer
|
||||
Device : /dev/esp32-balance (CH343 USB-CDC, VID:PID 1a86:55d3)
|
||||
Baud : 460800, 8N1
|
||||
|
||||
Command types (Orin → ESP32-S3 BALANCE):
|
||||
0x01 CMD_HEARTBEAT — no payload len=0
|
||||
0x02 CMD_DRIVE — int16 speed_units, int16 steer_units len=4 range −1000..+1000
|
||||
0x03 CMD_ESTOP — uint8 (1=assert, 0=clear) len=1
|
||||
0x04 CMD_ARM — uint8 (1=arm, 0=disarm) len=1
|
||||
0x05 CMD_PID — float32 kp, float32 ki, float32 kd len=12
|
||||
|
||||
Telemetry types (ESP32-S3 BALANCE → Orin):
|
||||
0x80 TELEM_STATUS — int16 pitch_x10, int16 motor_cmd,
|
||||
uint16 vbat_mv, uint8 balance_state, uint8 flags len=8
|
||||
0x81 TELEM_VESC_LEFT — int32 erpm, uint16 voltage_mv,
|
||||
int16 current_ma, uint16 temp_c_x10 len=10
|
||||
0x82 TELEM_VESC_RIGHT — same layout as TELEM_VESC_LEFT len=10
|
||||
0xA0 RESP_ACK — uint8 echoed cmd type len=1
|
||||
0xA1 RESP_NACK — uint8 cmd type, uint8 error_code len=2
|
||||
|
||||
Balance state values (TELEM_STATUS.balance_state):
|
||||
0 DISARMED
|
||||
1 ARMED
|
||||
2 TILT_FAULT
|
||||
3 ESTOP
|
||||
|
||||
NACK error codes:
|
||||
0x01 ERR_BAD_CRC
|
||||
0x02 ERR_BAD_LEN
|
||||
0x03 ERR_ESTOP_ACTIVE
|
||||
0x04 ERR_DISARMED
|
||||
|
||||
NOTE: Message type IDs coordinated with hal (bd-66hx) on 2026-04-17.
|
||||
Confirm against bd-66hx PR before deploying on hardware.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
from enum import IntEnum
|
||||
from typing import Optional
|
||||
|
||||
# ── Physical layer constants ──────────────────────────────────────────────────
|
||||
|
||||
SERIAL_PORT_DEFAULT = "/dev/esp32-balance"
|
||||
BAUD_RATE = 460800
|
||||
MAX_PAYLOAD_LEN = 62 # hard cap; larger frames are considered corrupt
|
||||
|
||||
# ── Framing ───────────────────────────────────────────────────────────────────
|
||||
|
||||
SYNC_BYTE = 0xAA
|
||||
|
||||
|
||||
# ── Command type codes (Orin → ESP32-S3 BALANCE) ─────────────────────────────
|
||||
|
||||
class CmdType(IntEnum):
|
||||
HEARTBEAT = 0x01
|
||||
DRIVE = 0x02
|
||||
ESTOP = 0x03
|
||||
ARM = 0x04
|
||||
PID = 0x05
|
||||
|
||||
|
||||
# ── Telemetry type codes (ESP32-S3 BALANCE → Orin) ───────────────────────────
|
||||
|
||||
class TelType(IntEnum):
|
||||
STATUS = 0x80
|
||||
VESC_LEFT = 0x81
|
||||
VESC_RIGHT = 0x82
|
||||
ACK = 0xA0
|
||||
NACK = 0xA1
|
||||
|
||||
|
||||
# ── Balance state enum ────────────────────────────────────────────────────────
|
||||
|
||||
class BalanceState(IntEnum):
|
||||
DISARMED = 0
|
||||
ARMED = 1
|
||||
TILT_FAULT = 2
|
||||
ESTOP = 3
|
||||
|
||||
|
||||
BALANCE_STATE_LABEL = {
|
||||
BalanceState.DISARMED: "DISARMED",
|
||||
BalanceState.ARMED: "ARMED",
|
||||
BalanceState.TILT_FAULT: "TILT_FAULT",
|
||||
BalanceState.ESTOP: "ESTOP",
|
||||
}
|
||||
|
||||
|
||||
# ── Parsed telemetry dataclasses ─────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class StatusFrame:
|
||||
"""TELEM_STATUS (0x80): balance controller status at ~10 Hz."""
|
||||
pitch_deg: float # degrees (positive = forward tilt); raw pitch_x10 / 10.0
|
||||
motor_cmd: int # ESC command, −1000..+1000
|
||||
vbat_mv: int # battery voltage, millivolts
|
||||
balance_state: int # BalanceState enum
|
||||
flags: int # bitmask — bit 0: estop_active, bit 1: heartbeat_timeout
|
||||
|
||||
|
||||
@dataclass
|
||||
class VescFrame:
|
||||
"""TELEM_VESC_LEFT (0x81) or TELEM_VESC_RIGHT (0x82): VESC proxied telemetry at ~10 Hz."""
|
||||
vesc_id: int # 56 = left, 68 = right (maps to CAN IDs from hardware spec)
|
||||
erpm: int # electrical RPM (signed)
|
||||
voltage_mv: int # bus voltage, millivolts
|
||||
current_ma: int # phase current, milliamps (signed)
|
||||
temp_c: float # motor temperature, °C; raw temp_c_x10 / 10.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class AckFrame:
|
||||
"""RESP_ACK (0xA0): acknowledgement for a successfully parsed command."""
|
||||
echoed_type: int
|
||||
|
||||
|
||||
@dataclass
|
||||
class NackFrame:
|
||||
"""RESP_NACK (0xA1): rejection; contains original cmd type and error code."""
|
||||
cmd_type: int
|
||||
error_code: int
|
||||
|
||||
|
||||
TelemetryFrame = StatusFrame | VescFrame | AckFrame | NackFrame
|
||||
|
||||
|
||||
# ── CRC8-SMBUS ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _crc8(data: bytes) -> int:
|
||||
"""CRC8-SMBUS: polynomial 0x07, init 0x00, no final XOR.
|
||||
|
||||
Coverage: LEN + TYPE + PAYLOAD bytes.
|
||||
"""
|
||||
crc = 0
|
||||
for byte in data:
|
||||
crc ^= byte
|
||||
for _ in range(8):
|
||||
if crc & 0x80:
|
||||
crc = (crc << 1) ^ 0x07
|
||||
else:
|
||||
crc <<= 1
|
||||
crc &= 0xFF
|
||||
return crc
|
||||
|
||||
|
||||
# ── Frame encoder ─────────────────────────────────────────────────────────────
|
||||
|
||||
def _build_frame(cmd_type: int, payload: bytes) -> bytes:
|
||||
"""Build a complete binary frame: [0xAA][LEN][TYPE][PAYLOAD][CRC8]."""
|
||||
if len(payload) > MAX_PAYLOAD_LEN:
|
||||
raise ValueError(f"Payload length {len(payload)} exceeds max {MAX_PAYLOAD_LEN}")
|
||||
length = len(payload)
|
||||
crc_data = bytes([length, cmd_type]) + payload
|
||||
crc = _crc8(crc_data)
|
||||
return bytes([SYNC_BYTE, length, cmd_type]) + payload + bytes([crc])
|
||||
|
||||
|
||||
def encode_heartbeat() -> bytes:
|
||||
"""CMD_HEARTBEAT — no payload. Send every ≤500 ms to keep watchdog alive."""
|
||||
return _build_frame(CmdType.HEARTBEAT, b"")
|
||||
|
||||
|
||||
def encode_drive(speed: int, steer: int) -> bytes:
|
||||
"""CMD_DRIVE — int16 speed_units + int16 steer_units (big-endian, −1000..+1000).
|
||||
|
||||
Parameters
|
||||
----------
|
||||
speed : target longitudinal speed in motor units (positive = forward)
|
||||
steer : steering demand in motor units (positive = right turn)
|
||||
"""
|
||||
speed = max(-1000, min(1000, int(speed)))
|
||||
steer = max(-1000, min(1000, int(steer)))
|
||||
return _build_frame(CmdType.DRIVE, struct.pack(">hh", speed, steer))
|
||||
|
||||
|
||||
def encode_estop(assert_stop: bool) -> bytes:
|
||||
"""CMD_ESTOP — uint8, 1=assert, 0=clear."""
|
||||
return _build_frame(CmdType.ESTOP, struct.pack("B", 1 if assert_stop else 0))
|
||||
|
||||
|
||||
def encode_arm(arm: bool) -> bytes:
|
||||
"""CMD_ARM — uint8, 1=arm, 0=disarm."""
|
||||
return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
|
||||
|
||||
|
||||
def encode_pid(kp: float, ki: float, kd: float) -> bytes:
|
||||
"""CMD_PID — three float32 values, big-endian."""
|
||||
if kp < 0.0 or ki < 0.0 or kd < 0.0:
|
||||
raise ValueError("PID gains must be non-negative")
|
||||
return _build_frame(CmdType.PID, struct.pack(">fff", kp, ki, kd))
|
||||
|
||||
|
||||
# ── Frame decoder (streaming state-machine) ───────────────────────────────────
|
||||
|
||||
class _ParseState(IntEnum):
|
||||
WAIT_SYNC = 0
|
||||
WAIT_LEN = 1
|
||||
WAIT_TYPE = 2
|
||||
PAYLOAD = 3
|
||||
WAIT_CRC = 4
|
||||
|
||||
|
||||
class FrameParser:
|
||||
"""Byte-by-byte streaming parser for ESP32-S3 BALANCE telemetry frames.
|
||||
|
||||
Feed individual bytes via ``feed()``; returns a decoded dataclass when a
|
||||
complete, CRC-valid frame arrives, otherwise returns ``None``.
|
||||
|
||||
Not thread-safe — guard with a lock if accessed from multiple threads.
|
||||
|
||||
Counters
|
||||
--------
|
||||
frames_ok : successfully decoded frames
|
||||
frames_error : CRC failures or oversized payloads
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.frames_ok = 0
|
||||
self.frames_error = 0
|
||||
self._reset()
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Reset to initial state — call after serial reconnect."""
|
||||
self._reset()
|
||||
|
||||
def _reset(self) -> None:
|
||||
self._state = _ParseState.WAIT_SYNC
|
||||
self._length = 0
|
||||
self._type = 0
|
||||
self._payload = bytearray()
|
||||
|
||||
def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
|
||||
"""Process one byte.
|
||||
|
||||
Returns a decoded dataclass on success, ``None`` while waiting for more
|
||||
bytes, or a ``(type_code, raw_payload)`` tuple for unrecognised types.
|
||||
"""
|
||||
s = self._state
|
||||
|
||||
if s == _ParseState.WAIT_SYNC:
|
||||
if byte == SYNC_BYTE:
|
||||
self._state = _ParseState.WAIT_LEN
|
||||
return None
|
||||
|
||||
if s == _ParseState.WAIT_LEN:
|
||||
if byte > MAX_PAYLOAD_LEN:
|
||||
self.frames_error += 1
|
||||
self._reset()
|
||||
return None
|
||||
self._length = byte
|
||||
self._state = _ParseState.WAIT_TYPE
|
||||
return None
|
||||
|
||||
if s == _ParseState.WAIT_TYPE:
|
||||
self._type = byte
|
||||
self._payload = bytearray()
|
||||
if self._length == 0:
|
||||
self._state = _ParseState.WAIT_CRC
|
||||
else:
|
||||
self._state = _ParseState.PAYLOAD
|
||||
return None
|
||||
|
||||
if s == _ParseState.PAYLOAD:
|
||||
self._payload.append(byte)
|
||||
if len(self._payload) == self._length:
|
||||
self._state = _ParseState.WAIT_CRC
|
||||
return None
|
||||
|
||||
if s == _ParseState.WAIT_CRC:
|
||||
# Verify CRC before resetting parser state
|
||||
crc_data = bytes([self._length, self._type]) + self._payload
|
||||
expected = _crc8(crc_data)
|
||||
if byte != expected:
|
||||
self.frames_error += 1
|
||||
self._reset()
|
||||
return None
|
||||
self.frames_ok += 1
|
||||
result = _decode(self._type, bytes(self._payload))
|
||||
self._reset()
|
||||
return result
|
||||
|
||||
self._reset()
|
||||
return None
|
||||
|
||||
|
||||
# ── Telemetry decoder ─────────────────────────────────────────────────────────
|
||||
|
||||
# Maps VESC_LEFT/RIGHT type codes to hardware CAN IDs (from memory)
|
||||
_VESC_ID_MAP = {
|
||||
TelType.VESC_LEFT: 56, # left VESC CAN ID
|
||||
TelType.VESC_RIGHT: 68, # right VESC CAN ID
|
||||
}
|
||||
|
||||
|
||||
def _decode(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
|
||||
"""Decode a validated telemetry payload into a typed dataclass."""
|
||||
try:
|
||||
if type_code == TelType.STATUS:
|
||||
# int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags
|
||||
if len(payload) < 8:
|
||||
return None
|
||||
pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hhHBB", payload)
|
||||
return StatusFrame(
|
||||
pitch_deg=pitch_x10 / 10.0,
|
||||
motor_cmd=motor_cmd,
|
||||
vbat_mv=vbat_mv,
|
||||
balance_state=state,
|
||||
flags=flags,
|
||||
)
|
||||
|
||||
if type_code in (TelType.VESC_LEFT, TelType.VESC_RIGHT):
|
||||
# int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10
|
||||
if len(payload) < 10:
|
||||
return None
|
||||
erpm, voltage_mv, current_ma, temp_x10 = struct.unpack_from(">iHhH", payload)
|
||||
return VescFrame(
|
||||
vesc_id=_VESC_ID_MAP[type_code],
|
||||
erpm=erpm,
|
||||
voltage_mv=voltage_mv,
|
||||
current_ma=current_ma,
|
||||
temp_c=temp_x10 / 10.0,
|
||||
)
|
||||
|
||||
if type_code == TelType.ACK:
|
||||
if len(payload) < 1:
|
||||
return None
|
||||
return AckFrame(echoed_type=payload[0])
|
||||
|
||||
if type_code == TelType.NACK:
|
||||
if len(payload) < 2:
|
||||
return None
|
||||
return NackFrame(cmd_type=payload[0], error_code=payload[1])
|
||||
|
||||
except struct.error:
|
||||
return None
|
||||
|
||||
# Unknown type — return raw for forward-compatibility
|
||||
return (type_code, payload)
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_esp32_serial
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_esp32_serial
|
||||
@ -1,27 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_esp32_serial"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/config", ["config/esp32_balance_params.yaml"]),
|
||||
(f"share/{package_name}/launch", ["launch/esp32_balance.launch.py"]),
|
||||
],
|
||||
install_requires=["setuptools", "pyserial"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-perception",
|
||||
maintainer_email="sl-perception@saltylab.local",
|
||||
description="UART/USB-CDC serial bridge for ESP32-S3 BALANCE (bd-wim1)",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"esp32_balance_node = saltybot_esp32_serial.esp32_balance_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
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