Compare commits
1 Commits
main
...
serkan/par
| Author | SHA1 | Date | |
|---|---|---|---|
|
ffda15e3ec |
@ -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}"
|
||||
534
docs/PARKING-MARKING-SPEC.md
Normal file
534
docs/PARKING-MARKING-SPEC.md
Normal file
@ -0,0 +1,534 @@
|
||||
# Parking Lot Line Marking System — Hardware + Software Spec
|
||||
|
||||
**Platform:** SAUL-TEE Robot
|
||||
**Use case:** Serkan — Autonomous parking lot striping
|
||||
**Author:** mark (parking marking agent)
|
||||
**Date:** 2026-04-07
|
||||
**Status:** Draft v1.0
|
||||
|
||||
---
|
||||
|
||||
## Table of Contents
|
||||
|
||||
1. [System Overview](#1-system-overview)
|
||||
2. [Paint Dispensing Hardware](#2-paint-dispensing-hardware)
|
||||
3. [Control Electronics](#3-control-electronics)
|
||||
4. [Positioning Strategy](#4-positioning-strategy)
|
||||
5. [Software Architecture](#5-software-architecture)
|
||||
6. [Parking Lot Templates](#6-parking-lot-templates)
|
||||
7. [Bill of Materials](#7-bill-of-materials)
|
||||
8. [Safety](#8-safety)
|
||||
9. [Open Items](#9-open-items)
|
||||
|
||||
---
|
||||
|
||||
## 1. System Overview
|
||||
|
||||
SAUL-TEE autonomously traverses a parking lot surface while a rear-mounted paint dispensing system sprays crisp lines. The Jetson Orin drives RTK-corrected path following via differential VESC drive. The ESP32 IO board triggers a solenoid valve via relay on demand. Lines are generated from a layout file (SVG or JSON) and executed as a sequence of waypoints with paint-on/off events.
|
||||
|
||||
```
|
||||
[Layout File] → [Layout Planner node]
|
||||
↓
|
||||
[Path Follower node] ← RTK/UWB pose
|
||||
↓
|
||||
[Paint Controller node] → GPIO → relay → solenoid → spray
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 2. Paint Dispensing Hardware
|
||||
|
||||
### 2.1 Spray Mechanism — Selected: Pressurized Airless Canister
|
||||
|
||||
Three options evaluated:
|
||||
|
||||
| Option | Pros | Cons | Verdict |
|
||||
|--------|------|------|---------|
|
||||
| **Pressurized aerosol canister** | Cheap, no pump, self-contained, easy swap | ~500 mL, limited volume, cold-sensitive | **Selected for v1** |
|
||||
| Airless diaphragm pump + reservoir | Large capacity, refillable, consistent | Pump adds complexity, power draw ~3 A | v2 option |
|
||||
| Gravity-fed dip tube | Simple, zero power | Requires height differential, drips on stop | Rejected |
|
||||
|
||||
**v1 approach:** Modified professional marking paint canister (e.g., Rustoleum Marking Chalk, inverted-spray capable) + normally-closed solenoid valve inline with canister outlet tube. Canister pressure (~50 PSI) propels paint through the solenoid to a fan nozzle aimed at the ground.
|
||||
|
||||
### 2.2 Nozzle Selection
|
||||
|
||||
| Line Width | Nozzle | Tip Orifice | Height from Ground | Notes |
|
||||
|------------|--------|-------------|-------------------|-------|
|
||||
| 4" standard parking line | Flat fan 40° | 0.017" | 8–10 cm | Covers standard stripe width at 50 PSI |
|
||||
| 2" detail / curb line | Flat fan 20° | 0.013" | 6–8 cm | Narrower pattern |
|
||||
| 6" fire lane | Flat fan 65° | 0.021" | 10–12 cm | Wide spray, lower pressure preferred |
|
||||
|
||||
**Recommended nozzle:** Graco RAC X 515 (flat fan, 10" fan width at 12" height) for 4" lines. Reversible tip for unclogging.
|
||||
|
||||
**Critical:** Nozzle tip height must be fixed relative to ground. Spring-loaded mount or rigid bracket set to calibrated height.
|
||||
|
||||
### 2.3 Solenoid Valve
|
||||
|
||||
- **Type:** Normally-closed (NC) 12 V DC solenoid valve
|
||||
- **Body:** Brass or stainless, 1/4" NPT female ports
|
||||
- **Pressure rating:** ≥ 100 PSI (canister runs at ~50 PSI)
|
||||
- **Response time:** < 20 ms open/close (critical for line start/stop accuracy)
|
||||
- **Current draw:** ~500 mA @ 12 V when open
|
||||
- **Recommended part:** U.S. Solid USS-MSV00006 1/4" NC 12 V (≈ $18) or equivalent
|
||||
|
||||
**Flyback diode:** 1N4007 across solenoid coil terminals (cathode to +12 V).
|
||||
|
||||
### 2.4 Paint Reservoir / Canister Mount
|
||||
|
||||
**v1 — Inverted aerosol canister:**
|
||||
- Standard 17 oz / 500 mL marking paint canister (inverted nozzle type)
|
||||
- Custom bracket: 3 mm aluminum plate, two 3" U-bolt clamps for canister body
|
||||
- Mount behind rear castor, centered on robot longitudinal axis
|
||||
- Canister outlet → 1/4" OD PTFE tube → solenoid → nozzle
|
||||
|
||||
**v2 — Refillable reservoir:**
|
||||
- 2 L HDPE pressure pot (Harbor Freight or similar, ~$35)
|
||||
- External 12 V mini air compressor regulating to 30–60 PSI
|
||||
- Float-level sensor (reed switch) for paint level monitoring
|
||||
|
||||
### 2.5 Mounting Bracket
|
||||
|
||||
```
|
||||
Top rail (80/20 T-slot or robot frame)
|
||||
│
|
||||
├─── Canister bracket (rear, vertical, above castor)
|
||||
│ U-bolts × 2 clamping canister body
|
||||
│ QD fitting for tube swap
|
||||
│
|
||||
└─── Nozzle arm (extends aft ~200 mm, 50 mm above ground target height)
|
||||
Adjustable height slot (bolt + lock nut)
|
||||
Solenoid mounted inline, protected by splash shield
|
||||
```
|
||||
|
||||
**Key dimension:** Nozzle center is 150 mm behind rear axle center. This offset is compensated in software — spray-on is triggered when the nozzle position (not robot center) reaches the line start point.
|
||||
|
||||
### 2.6 Power Requirements
|
||||
|
||||
| Component | Voltage | Current | Source |
|
||||
|-----------|---------|---------|--------|
|
||||
| Solenoid valve | 12 V | 0.5 A peak (intermittent) | Robot 12 V rail |
|
||||
| Relay module | 5 V logic, 12 V switched | < 50 mA control | 5 V rail via IO GPIO |
|
||||
| RTK GPS module | 5 V | 100–200 mA | 5 V rail |
|
||||
| (v2) Mini compressor | 12 V | 3–5 A | Direct from 36 V via 12 V DC-DC |
|
||||
|
||||
Total paint system power budget: **< 1 A continuous** (solenoid duty cycle ~20%).
|
||||
|
||||
---
|
||||
|
||||
## 3. Control Electronics
|
||||
|
||||
### 3.1 Signal Chain: Orin → Spray
|
||||
|
||||
```
|
||||
Jetson Orin (ROS2 node)
|
||||
│ publishes /paint/cmd (Bool)
|
||||
│
|
||||
[CAN or MQTT] ← preferred: direct GPIO on Orin Jetson 40-pin header
|
||||
│
|
||||
Orin GPIO pin (3.3 V logic)
|
||||
│
|
||||
Level shifter (3.3 V → 5 V) [optional, if relay module needs 5 V]
|
||||
│
|
||||
5 V relay module (optocoupler isolated, SPDT, 10 A contacts)
|
||||
│ NO contact → +12 V
|
||||
│ COM → solenoid +
|
||||
│ solenoid − → GND
|
||||
│
|
||||
Solenoid valve (NC, 12 V)
|
||||
│
|
||||
Spray nozzle
|
||||
```
|
||||
|
||||
### 3.2 Jetson Orin GPIO
|
||||
|
||||
The Jetson Orin Nano / AGX Orin 40-pin expansion header exposes GPIOs via `Jetson.GPIO` Python library or `/sys/class/gpio`.
|
||||
|
||||
| Signal | Orin Pin | GPIO # | Notes |
|
||||
|--------|----------|--------|-------|
|
||||
| SPRAY_EN | Pin 11 | GPIO09 (Tegra SOC) | Active high → relay ON → solenoid opens |
|
||||
| ESTOP_IN | Pin 12 | GPIO08 | Monitor hardware kill switch state |
|
||||
|
||||
Use `Jetson.GPIO` in ROS2 node:
|
||||
```python
|
||||
import Jetson.GPIO as GPIO
|
||||
SPRAY_PIN = 11
|
||||
GPIO.setmode(GPIO.BOARD)
|
||||
GPIO.setup(SPRAY_PIN, GPIO.OUT, initial=GPIO.LOW)
|
||||
GPIO.output(SPRAY_PIN, GPIO.HIGH) # spray on
|
||||
GPIO.output(SPRAY_PIN, GPIO.LOW) # spray off
|
||||
```
|
||||
|
||||
### 3.3 ESP32 IO Board — Alternative / Backup Path
|
||||
|
||||
If direct Orin GPIO is not preferred, route through the IO board.
|
||||
|
||||
**Available IO pins (from SAUL-TEE-SYSTEM-REFERENCE.md):**
|
||||
|
||||
| GPIO | Current Use | Available? |
|
||||
|------|-------------|-----------|
|
||||
| IO14 | Horn/buzzer | Yes (shared, low duty) |
|
||||
| IO15 | Headlight | Yes (can multiplex) |
|
||||
| **IO16** | Fan (if ELRS not fitted) | **Yes — recommended for spray relay** |
|
||||
|
||||
**Recommended:** IO16 on ESP32 IO board → relay → solenoid.
|
||||
|
||||
Add new UART message type for spray control:
|
||||
|
||||
```
|
||||
IO Board extension — TYPE 0x04: SPRAY_CMD
|
||||
Payload: uint8 enable (0=off, 1=on), uint16 duration_ms (0=indefinite)
|
||||
```
|
||||
|
||||
Orin sends via MQTT → ESP32 MQTT bridge → UART to IO board → GPIO16.
|
||||
|
||||
### 3.4 Safety Interlocks
|
||||
|
||||
| Condition | Action |
|
||||
|-----------|--------|
|
||||
| E-STOP (RC CH6 > 1500 µs) | IO board immediately pulls IO16 LOW (spray off) before forwarding FAULT to BALANCE |
|
||||
| Tilt > ±25° | BALANCE sends FAULT → Orin paint controller kills spray |
|
||||
| Robot velocity = 0 (stationary) | paint_controller node holds spray off (prevents puddles) |
|
||||
| RC loss > 100 ms | FAULT propagated → spray off |
|
||||
| MQTT disconnected > 5 s | paint_controller enters SAFE mode, spray off |
|
||||
|
||||
**IO board firmware addition:** In the FAULT frame handler (TYPE 0x03), ensure IO16 is driven LOW before sending any FAULT frame upward. Zero-latency interlock at MCU level.
|
||||
|
||||
### 3.5 Flow Rate Sensor (Optional — v2)
|
||||
|
||||
- **Type:** YF-S201 Hall-effect flow sensor, 1/2" inline
|
||||
- **Signal:** Pulse output, 7.5 pulses per mL
|
||||
- **Connected to:** IO board I2C bus interrupt pin or free GPIO
|
||||
- **Purpose:** Detect clog (rate drops to zero during spray command), estimate paint consumed
|
||||
|
||||
---
|
||||
|
||||
## 4. Positioning Strategy
|
||||
|
||||
### 4.1 Accuracy Requirements
|
||||
|
||||
| Parameter | Requirement | Rationale |
|
||||
|-----------|-------------|-----------|
|
||||
| Lateral position error | < ±2 cm | 4" (10 cm) line — 2 cm error leaves 3 cm margin |
|
||||
| Heading error | < ±0.5° | Over 20 m line, 0.5° heading error = 17 cm drift — marginal but acceptable with frequent corrections |
|
||||
| Longitudinal (along-line) error | < ±5 cm | Controls spray on/off timing |
|
||||
| Update rate | ≥ 5 Hz | At 0.3 m/s robot speed, 5 Hz gives 6 cm per update |
|
||||
|
||||
**Conclusion:** Phone GPS (±5 m) alone is **insufficient**. RTK GPS (±2 cm) is required for production use.
|
||||
|
||||
### 4.2 RTK GPS Module
|
||||
|
||||
**Recommended:** u-blox ZED-F9P (standalone RTK receiver)
|
||||
|
||||
| Spec | Value |
|
||||
|------|-------|
|
||||
| RTK accuracy (fixed) | 1 cm + 1 ppm horizontal |
|
||||
| Heading (dual antenna) | 0.3° RMS |
|
||||
| Update rate | 10 Hz (RTK), 20 Hz raw |
|
||||
| Interface | UART (NMEA + UBX), USB |
|
||||
| Power | 5 V, ~150 mA |
|
||||
| Module board | SparkFun GPS-RTK2 (ZED-F9P) ~$250, or ArduSimple simpleRTK2B ~$200 |
|
||||
|
||||
**NTRIP correction source:** Use a local CORS network or set up a second ZED-F9P base station at a known point on the parking lot boundary. Base → Orin NTRIP client → RTK corrections to rover.
|
||||
|
||||
**Antenna placement:** Center of robot, clear sky view. Keep > 30 cm from metal frame edges.
|
||||
|
||||
### 4.3 Dual Antenna Heading (Moving Baseline RTK)
|
||||
|
||||
Use two ZED-F9P modules (or one ZED-F9P + ZED-F9H) with antennas at robot front and rear, separated by ≥ 50 cm. This gives direct heading from GPS — no magnetometer drift issues.
|
||||
|
||||
| Antenna separation | Heading accuracy |
|
||||
|-------------------|-----------------|
|
||||
| 50 cm | ~1.1° RMS |
|
||||
| 100 cm | ~0.57° RMS |
|
||||
| 150 cm | ~0.38° RMS |
|
||||
|
||||
**Recommended:** 100 cm baseline along robot longitudinal axis → heading accuracy 0.57° RMS.
|
||||
|
||||
### 4.4 Sensor Fusion Architecture
|
||||
|
||||
```
|
||||
[ZED-F9P RTK rover] ── UART ──→ robot_localization EKF
|
||||
[Phone GPS MQTT bridge] ──────→ (fallback, coarse)
|
||||
[UWB tag] ────────────────────→ (near base station, ±2 cm augment)
|
||||
[IMU - QMI8658 via CAN 0x400]──→ dead reckoning between GPS updates
|
||||
↓
|
||||
/saltybot/pose (geometry_msgs/PoseWithCovarianceStamped)
|
||||
```
|
||||
|
||||
- **robot_localization** (ROS2 EKF node) fuses GPS + IMU
|
||||
- RTK GPS is primary when fixed; covariance set high when float/no fix
|
||||
- UWB active near parking lot entry/exit points (anchor infrastructure needed)
|
||||
- Phone GPS used for coarse positioning only, filtered out when RTK fixed
|
||||
|
||||
### 4.5 Wheel Odometry
|
||||
|
||||
At 0.3 m/s nominal painting speed, the VESC RPM telemetry (CAN 0x401) provides ~1 cm odometry resolution. Combine with IMU heading for dead-reckoning between RTK updates.
|
||||
|
||||
---
|
||||
|
||||
## 5. Software Architecture
|
||||
|
||||
### 5.1 ROS2 Node Graph
|
||||
|
||||
```
|
||||
/layout_planner
|
||||
Reads: layout JSON file
|
||||
Publishes: /paint/path (nav_msgs/Path), /paint/zones (PaintZone[])
|
||||
|
||||
/path_follower
|
||||
Subscribes: /paint/path, /saltybot/pose
|
||||
Publishes: /cmd_vel (geometry_msgs/Twist) → CAN 0x300 bridge
|
||||
Algorithm: Pure Pursuit with adaptive lookahead (0.3 m at low speed)
|
||||
|
||||
/paint_controller
|
||||
Subscribes: /saltybot/pose, /paint/zones
|
||||
Publishes: /paint/spray_cmd (std_msgs/Bool) → GPIO
|
||||
Logic: nozzle-offset compensation, edge start/stop, velocity interlock
|
||||
|
||||
/rtk_bridge
|
||||
Reads: ZED-F9P UART (NMEA GGA, UBX-NAV-PVT)
|
||||
Publishes: /gps/fix (sensor_msgs/NavSatFix), /gps/heading (Float64)
|
||||
|
||||
/paint_monitor (MQTT bridge)
|
||||
Bridges: /paint/status → MQTT saltybot/mark/status
|
||||
Bridges: MQTT saltybot/mark/cmd → /paint/cmd
|
||||
```
|
||||
|
||||
### 5.2 MQTT Topics
|
||||
|
||||
| Topic | Direction | Payload | Description |
|
||||
|-------|-----------|---------|-------------|
|
||||
| `saltybot/mark/status` | Orin → broker | JSON | Current state: pose, spray_state, progress_pct, paint_remaining |
|
||||
| `saltybot/mark/cmd` | broker → Orin | JSON | Commands: start, stop, pause, load_layout |
|
||||
| `saltybot/mark/layout` | broker → Orin | JSON | Full layout upload |
|
||||
| `saltybot/mark/fault` | Orin → broker | JSON | Fault events: e-stop, gps_lost, clog_detected |
|
||||
| `saltybot/mark/spray` | Orin → ESP32 IO | `{"en":1}` | Direct spray relay command (fallback path) |
|
||||
|
||||
### 5.3 Layout File Format (JSON)
|
||||
|
||||
```json
|
||||
{
|
||||
"version": 1,
|
||||
"origin": {"lat": 37.4219983, "lon": -122.084},
|
||||
"lines": [
|
||||
{
|
||||
"id": "row_A_space_1_left",
|
||||
"type": "stripe",
|
||||
"width_mm": 100,
|
||||
"start": {"x_m": 0.0, "y_m": 0.0},
|
||||
"end": {"x_m": 5.5, "y_m": 0.0}
|
||||
}
|
||||
],
|
||||
"symbols": [
|
||||
{
|
||||
"id": "hc_symbol_1",
|
||||
"type": "handicap_iso",
|
||||
"center": {"x_m": 12.0, "y_m": 2.3},
|
||||
"heading_deg": 0
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
Coordinates are in a local ENU (East-North-Up) frame anchored at `origin`.
|
||||
|
||||
### 5.4 path_follower — Pure Pursuit
|
||||
|
||||
```python
|
||||
class PathFollower(Node):
|
||||
LOOKAHEAD = 0.35 # m — tunable
|
||||
MAX_SPEED = 0.3 # m/s during painting
|
||||
NOZZLE_OFFSET = -0.15 # m aft of rear axle
|
||||
|
||||
def compute_cmd_vel(self, pose, path):
|
||||
# Find lookahead point on path
|
||||
# Compute curvature κ = 2*sin(α) / L
|
||||
# angular_vel = κ * linear_vel
|
||||
...
|
||||
```
|
||||
|
||||
**Speed:** 0.3 m/s nominal (≈ 1 ft/s). At this speed, 100 m line ≈ 5.5 min.
|
||||
|
||||
### 5.5 paint_controller — Spray Logic
|
||||
|
||||
```python
|
||||
NOZZLE_OFFSET_M = -0.15 # nozzle is 15 cm behind robot center (rear)
|
||||
|
||||
def nozzle_pose(robot_pose):
|
||||
# Project robot pose backward by offset along heading
|
||||
...
|
||||
|
||||
def on_pose_update(self, pose):
|
||||
nozzle = nozzle_pose(pose)
|
||||
# Check if nozzle is within any active paint zone
|
||||
in_zone = any(zone.contains(nozzle) for zone in self.active_zones)
|
||||
# Velocity interlock: must be moving > 0.05 m/s
|
||||
moving = self.current_speed > 0.05
|
||||
spray = in_zone and moving and not self.fault
|
||||
self.set_spray(spray)
|
||||
```
|
||||
|
||||
### 5.6 Integration with Existing Nodes
|
||||
|
||||
| Existing node | Integration point |
|
||||
|---------------|------------------|
|
||||
| `ios_gps_bridge` (Issue #681) | Provides fallback /gps/fix when RTK not fixed |
|
||||
| VESC CAN bridge | path_follower publishes to /cmd_vel → existing bridge forwards to CAN 0x300 |
|
||||
| `saul_tee_driver` | Subscribes /cmd_vel — no changes needed |
|
||||
| UWB tag node | robot_localization already fuses UWB range — paint system uses same /pose topic |
|
||||
|
||||
---
|
||||
|
||||
## 6. Parking Lot Templates
|
||||
|
||||
All templates follow **MUTCD (Manual on Uniform Traffic Control Devices)** and **ADA** standards.
|
||||
|
||||
### 6.1 Standard Dimensions
|
||||
|
||||
| Space Type | Width | Depth | Line Width | Notes |
|
||||
|------------|-------|-------|-----------|-------|
|
||||
| Standard | 9 ft (2.74 m) | 18 ft (5.49 m) | 4 in (102 mm) | Most common |
|
||||
| Compact | 8 ft (2.44 m) | 16 ft (4.88 m) | 4 in | Must be labeled |
|
||||
| ADA accessible | 13 ft (3.96 m) | 18 ft | 4 in | Includes 5' access aisle |
|
||||
| Van accessible | 16 ft (4.88 m) | 18 ft | 4 in | 8' space + 8' aisle |
|
||||
| Fire lane | varies | — | 6 in (152 mm) | Red or yellow, MUTCD |
|
||||
|
||||
### 6.2 Stall Angle Templates
|
||||
|
||||
| Angle | Module width | Drive aisle |
|
||||
|-------|-------------|------------|
|
||||
| 90° (perpendicular) | 9.0 ft stall + 9.0 ft module | 24 ft two-way |
|
||||
| 60° angled | 10.4 ft module | 18 ft one-way |
|
||||
| 45° angled | 12.7 ft module | 13 ft one-way |
|
||||
| Parallel | 23 ft length | 12 ft one-way |
|
||||
|
||||
### 6.3 Symbol Templates
|
||||
|
||||
#### Handicap Symbol (ISA — International Symbol of Access)
|
||||
|
||||
Standard 60" × 60" (1.52 m × 1.52 m) wheelchair figure, blue paint. Consists of:
|
||||
- Head circle: 6" diameter
|
||||
- Body and wheel segments: 4–6" strokes
|
||||
|
||||
Robot approach: rasterize ISA symbol into a grid of parallel stripe passes at 2" nozzle width, ~30 passes per symbol.
|
||||
|
||||
#### Arrow Templates
|
||||
|
||||
| Type | Dimensions | Use |
|
||||
|------|-----------|-----|
|
||||
| Straight arrow | 6" wide × 36" long shaft + 18" head | One-way lane |
|
||||
| Curved arrow | 6" wide, 10 ft radius | Turn lane |
|
||||
| Double-headed | 6" wide | Two-way reminder |
|
||||
|
||||
#### Fire Lane Markings
|
||||
|
||||
- "FIRE LANE — NO PARKING" text: 12" tall letters, 4" stroke
|
||||
- Red curb paint: continuous stripe, 6" wide on curb face (robot with angle-mounted nozzle)
|
||||
- MUTCD crosshatch: 6" lines at 45°, 18" spacing
|
||||
|
||||
### 6.4 Layout Design Tool (CLI)
|
||||
|
||||
A Python script `tools/parking_layout.py` will generate a layout JSON from simple parameters:
|
||||
|
||||
```bash
|
||||
# Generate a 10-space 90° lot section
|
||||
python tools/parking_layout.py \
|
||||
--type 90deg \
|
||||
--spaces 10 \
|
||||
--rows 2 \
|
||||
--origin 37.4219983,-122.084 \
|
||||
--output lot_A.json
|
||||
|
||||
# Preview as SVG
|
||||
python tools/parking_layout.py --input lot_A.json --svg lot_A.svg
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 7. Bill of Materials
|
||||
|
||||
### 7.1 Paint Dispensing System
|
||||
|
||||
| Item | Part / Source | Qty | Unit Cost | Total |
|
||||
|------|--------------|-----|-----------|-------|
|
||||
| Inverted marking paint canister (Rustoleum Professional) | Home Depot / Rustoleum | 4 | $8 | $32 |
|
||||
| NC solenoid valve 12 V 1/4" NPT | U.S. Solid / Amazon | 1 | $18 | $18 |
|
||||
| 5 V single-channel relay module | Amazon | 1 | $5 | $5 |
|
||||
| Graco RAC X 515 reversible tip | Paint supply | 1 | $25 | $25 |
|
||||
| 1/4" OD PTFE tubing (1 m) | Amazon | 1 | $8 | $8 |
|
||||
| 1/4" NPT push-to-connect fittings | Amazon | 4 | $3 | $12 |
|
||||
| 1N4007 flyback diode | electronics | 2 | $0.10 | $1 |
|
||||
| 3 mm aluminum plate (bracket stock) | Metal supermarket | 1 | $15 | $15 |
|
||||
| M5 hardware (bolts, nuts, standoffs) | — | lot | $5 | $5 |
|
||||
| 80/20 T-slot bracket 10-series | 80/20 Inc. | 2 | $8 | $16 |
|
||||
| PTFE thread tape | — | 1 | $2 | $2 |
|
||||
| Splash shield (acrylic 3 mm) | — | 1 | $5 | $5 |
|
||||
| **Subtotal — paint system** | | | | **$144** |
|
||||
|
||||
### 7.2 Positioning (RTK GPS)
|
||||
|
||||
| Item | Part / Source | Qty | Unit Cost | Total |
|
||||
|------|--------------|-----|-----------|-------|
|
||||
| u-blox ZED-F9P RTK board (rover) | SparkFun GPS-RTK2 | 1 | $250 | $250 |
|
||||
| u-blox ZED-F9P RTK board (base) | SparkFun GPS-RTK2 | 1 | $250 | $250 |
|
||||
| Survey GNSS antenna (L1/L2) | SparkFun / u-blox | 2 | $65 | $130 |
|
||||
| SMA cable (3 m) | Amazon | 2 | $12 | $24 |
|
||||
| USB-A to USB-C cable (Orin) | — | 1 | $8 | $8 |
|
||||
| **Subtotal — RTK GPS** | | | | **$662** |
|
||||
|
||||
### 7.3 Software / Labor
|
||||
|
||||
All software is open-source / in-house. No licensing cost.
|
||||
|
||||
### 7.4 Total Estimated BOM
|
||||
|
||||
| Category | Cost |
|
||||
|----------|------|
|
||||
| Paint dispensing system | $144 |
|
||||
| RTK GPS system | $662 |
|
||||
| Contingency (10%) | $81 |
|
||||
| **Total** | **$887** |
|
||||
|
||||
---
|
||||
|
||||
## 8. Safety
|
||||
|
||||
### 8.1 Paint Hazard
|
||||
|
||||
- Aerosol paint is flammable. Keep robot battery terminals isolated from paint canister.
|
||||
- Ensure ventilation if operating in enclosed areas.
|
||||
- Overspray: operate only in calm wind conditions (< 10 mph / 16 km/h).
|
||||
|
||||
### 8.2 Electrical Safety
|
||||
|
||||
- Relay module must be rated for 12 V / 1 A minimum (solenoid inrush).
|
||||
- Flyback diode mandatory across solenoid coil — unclamped kickback can damage relay contacts and IO GPIO.
|
||||
- Fuse solenoid 12 V line with a 2 A polyfuse.
|
||||
|
||||
### 8.3 Operational Safety
|
||||
|
||||
- Never enable autonomous movement unless lot is clear.
|
||||
- Use RC ESTOP (CH6) as immediate abort — confirm IO board firmware pulls spray pin LOW in FAULT handler.
|
||||
- Run at max 0.5 m/s with human observer for initial field trials.
|
||||
- Paint-on while stationary is locked out in software (puddle prevention).
|
||||
|
||||
---
|
||||
|
||||
## 9. Open Items
|
||||
|
||||
| # | Item | Owner | Priority |
|
||||
|---|------|-------|----------|
|
||||
| 1 | Confirm Orin GPIO pin accessibility (40-pin header on Jetson Orin Nano vs AGX) | mark / hal | High |
|
||||
| 2 | Source RTK CORS network credentials for target parking lot area | Serkan | High |
|
||||
| 3 | Determine parking lot GPS coordinates / anchor point for origin | Serkan | High |
|
||||
| 4 | Fabricate aluminum canister bracket + nozzle arm | Serkan / mech | Medium |
|
||||
| 5 | Write IO board firmware extension (TYPE 0x04 SPRAY_CMD) | mark | Medium |
|
||||
| 6 | Implement `tools/parking_layout.py` layout designer | mark | Medium |
|
||||
| 7 | Field test RTK fix acquisition time in target lot (tree/building multipath) | mark | Medium |
|
||||
| 8 | Calibrate nozzle offset constant (15 cm assumed — measure actual) | mark | Low |
|
||||
| 9 | Evaluate YF-S201 flow sensor for clog detection (v2) | mark | Low |
|
||||
|
||||
---
|
||||
|
||||
*Spec written by mark — parking lot marking agent.*
|
||||
*Questions/corrections → MQTT: `saltybot/mark/status` or open GitHub issue in saltylab-firmware.*
|
||||
@ -1,3 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
|
||||
project(esp32s3_balance)
|
||||
@ -1,22 +0,0 @@
|
||||
idf_component_register(
|
||||
SRCS
|
||||
"main.c"
|
||||
"orin_serial.c"
|
||||
"vesc_can.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
|
||||
cJSON
|
||||
driver
|
||||
freertos
|
||||
esp_timer
|
||||
)
|
||||
@ -1,42 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
/* ── ESP32-S3 BALANCE board — bd-66hx pin/config definitions ───────────────
|
||||
*
|
||||
* Hardware change from pre-bd-66hx design:
|
||||
* Previously: IO43/IO44 = CAN SN65HVD230 (shared Orin+VESC bus via CANable2)
|
||||
* After bd-66hx: IO43/IO44 = CH343 UART0 (Orin serial comms)
|
||||
* IO2/IO1 = CAN SN65HVD230 rewired (VESC-only bus)
|
||||
*
|
||||
* The SN65HVD230 transceiver physical wiring must be updated from IO43/44
|
||||
* to IO2/IO1 when deploying this firmware. See docs/SAUL-TEE-SYSTEM-REFERENCE.md.
|
||||
*/
|
||||
|
||||
/* ── 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→CH343 RXD */
|
||||
#define ORIN_UART_RX_GPIO 44 /* CH343 TXD→ESP32 */
|
||||
#define ORIN_UART_RX_BUF 1024
|
||||
#define ORIN_TX_QUEUE_DEPTH 16
|
||||
|
||||
/* ── VESC CAN TWAI (SN65HVD230 transceiver, rewired for bd-66hx) ── */
|
||||
#define VESC_CAN_TX_GPIO 2 /* ESP32 TWAI TX → SN65HVD230 TXD */
|
||||
#define VESC_CAN_RX_GPIO 1 /* SN65HVD230 RXD → ESP32 TWAI RX */
|
||||
#define VESC_CAN_RX_QUEUE 32
|
||||
|
||||
/* VESC node IDs — matched to bd-wim1 TELEM_VESC_LEFT/RIGHT mapping */
|
||||
#define VESC_ID_A 56u /* TELEM_VESC_LEFT (0x81) */
|
||||
#define VESC_ID_B 68u /* TELEM_VESC_RIGHT (0x82) */
|
||||
|
||||
/* ── 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 */
|
||||
#define RPM_PER_STEER_UNIT 3 /* steer differential scale */
|
||||
|
||||
/* ── Tilt cutoff ── */
|
||||
#define TILT_CUTOFF_DEG 25.0f
|
||||
@ -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,114 +0,0 @@
|
||||
/* main.c — ESP32-S3 BALANCE app_main (bd-66hx + OTA beads) */
|
||||
|
||||
#include "orin_serial.h"
|
||||
#include "vesc_can.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 left_erpm = 0;
|
||||
int32_t right_erpm = 0;
|
||||
|
||||
if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
|
||||
!hb_timeout && !drive_stale) {
|
||||
int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
|
||||
int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT;
|
||||
left_erpm = spd + str;
|
||||
right_erpm = spd - str;
|
||||
}
|
||||
|
||||
/* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */
|
||||
vesc_can_send_rpm(VESC_ID_A, left_erpm);
|
||||
vesc_can_send_rpm(VESC_ID_B, right_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 */
|
||||
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,354 +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_pid_t g_orin_pid = {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_PID:
|
||||
if (len < 12u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
||||
/* float32 big-endian: copy and swap bytes */
|
||||
{
|
||||
uint32_t raw;
|
||||
raw = ((uint32_t)payload[0] << 24u) | ((uint32_t)payload[1] << 16u) |
|
||||
((uint32_t)payload[2] << 8u) | (uint32_t)payload[3];
|
||||
memcpy((void*)&g_orin_pid.kp, &raw, 4u);
|
||||
raw = ((uint32_t)payload[4] << 24u) | ((uint32_t)payload[5] << 16u) |
|
||||
((uint32_t)payload[6] << 8u) | (uint32_t)payload[7];
|
||||
memcpy((void*)&g_orin_pid.ki, &raw, 4u);
|
||||
raw = ((uint32_t)payload[8] << 24u) | ((uint32_t)payload[9] << 16u) |
|
||||
((uint32_t)payload[10] << 8u) | (uint32_t)payload[11];
|
||||
memcpy((void*)&g_orin_pid.kd, &raw, 4u);
|
||||
g_orin_pid.updated = true;
|
||||
}
|
||||
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,113 +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 */
|
||||
#define CMD_PID 0x05u /* float32 kp, ki, kd, BE */
|
||||
|
||||
/* ── Telemetry types: ESP32 → Orin ── */
|
||||
#define TELEM_STATUS 0x80u /* status @ 10 Hz */
|
||||
#define TELEM_VESC_LEFT 0x81u /* VESC ID 56 telemetry @ 10 Hz */
|
||||
#define TELEM_VESC_RIGHT 0x82u /* VESC ID 68 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
|
||||
|
||||
/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */
|
||||
typedef enum {
|
||||
BAL_DISARMED = 0,
|
||||
BAL_ARMED = 1,
|
||||
BAL_TILT_FAULT = 2,
|
||||
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 float kp, ki, kd;
|
||||
volatile bool updated;
|
||||
} orin_pid_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_pid_t g_orin_pid;
|
||||
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[32];
|
||||
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,19 +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
|
||||
@ -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
|
||||
@ -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(
|
||||
@ -218,7 +242,7 @@ def generate_launch_description():
|
||||
description="Launch rosbridge WebSocket server (port 9090)",
|
||||
)
|
||||
|
||||
enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
"enable_mission_logging",
|
||||
default_value="true",
|
||||
description="Launch ROS2 bag recorder for mission logging (Issue #488)",
|
||||
@ -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
|
||||
|
||||
@ -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"
|
||||
@ -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,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 — Here4 GPS DroneCAN bridge for CANable2 (bd-p47c)
|
||||
@ -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",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,311 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Unit tests for saltybot_esp32_serial.esp32_balance_protocol.
|
||||
|
||||
No ROS2 or hardware required — exercises encode/decode round-trips,
|
||||
framing, CRC8, and parser state-machine in pure Python.
|
||||
|
||||
Run with: pytest test/ -v
|
||||
"""
|
||||
|
||||
import struct
|
||||
import unittest
|
||||
|
||||
from saltybot_esp32_serial.esp32_balance_protocol import (
|
||||
SYNC_BYTE,
|
||||
CmdType,
|
||||
TelType,
|
||||
BalanceState,
|
||||
StatusFrame,
|
||||
VescFrame,
|
||||
AckFrame,
|
||||
NackFrame,
|
||||
FrameParser,
|
||||
_crc8,
|
||||
encode_heartbeat,
|
||||
encode_drive,
|
||||
encode_estop,
|
||||
encode_arm,
|
||||
encode_pid,
|
||||
)
|
||||
|
||||
|
||||
# ── CRC8 tests ────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestCrc8(unittest.TestCase):
|
||||
"""Verify CRC8-SMBUS implementation."""
|
||||
|
||||
def test_empty(self):
|
||||
self.assertEqual(_crc8(b""), 0x00)
|
||||
|
||||
def test_single_byte_zero(self):
|
||||
# CRC of 0x00: no XOR, no poly — stays 0
|
||||
self.assertEqual(_crc8(b"\x00"), 0x00)
|
||||
|
||||
def test_single_byte_nonzero(self):
|
||||
# Pre-computed expected value for 0xFF
|
||||
result = _crc8(b"\xFF")
|
||||
self.assertIsInstance(result, int)
|
||||
self.assertGreaterEqual(result, 0)
|
||||
self.assertLessEqual(result, 0xFF)
|
||||
|
||||
def test_deterministic(self):
|
||||
data = b"\x01\x02\x04\x08"
|
||||
self.assertEqual(_crc8(data), _crc8(data))
|
||||
|
||||
def test_different_data_different_crc(self):
|
||||
self.assertNotEqual(_crc8(b"\x01"), _crc8(b"\x02"))
|
||||
|
||||
def test_byte_range(self):
|
||||
for b in range(256):
|
||||
r = _crc8(bytes([b]))
|
||||
self.assertGreaterEqual(r, 0)
|
||||
self.assertLessEqual(r, 255)
|
||||
|
||||
|
||||
# ── Frame structure tests ─────────────────────────────────────────────────────
|
||||
|
||||
class TestFrameStructure(unittest.TestCase):
|
||||
"""Verify encode functions produce correctly structured frames."""
|
||||
|
||||
def _check_frame(self, data: bytes, expected_type: int, expected_payload: bytes):
|
||||
"""Assert frame matches [SYNC][LEN][TYPE][PAYLOAD][CRC8]."""
|
||||
self.assertGreaterEqual(len(data), 4)
|
||||
self.assertEqual(data[0], SYNC_BYTE, "SYNC byte mismatch")
|
||||
length = data[1]
|
||||
self.assertEqual(data[2], expected_type, "TYPE byte mismatch")
|
||||
self.assertEqual(length, len(expected_payload), "LEN byte mismatch")
|
||||
self.assertEqual(data[3:3 + length], expected_payload, "PAYLOAD mismatch")
|
||||
# Verify CRC over [LEN][TYPE][PAYLOAD]
|
||||
crc_data = bytes([length, expected_type]) + expected_payload
|
||||
expected_crc = _crc8(crc_data)
|
||||
self.assertEqual(data[-1], expected_crc, "CRC8 mismatch")
|
||||
|
||||
def test_heartbeat_frame(self):
|
||||
frame = encode_heartbeat()
|
||||
self._check_frame(frame, CmdType.HEARTBEAT, b"")
|
||||
|
||||
def test_drive_frame_zeros(self):
|
||||
frame = encode_drive(0, 0)
|
||||
payload = struct.pack(">hh", 0, 0)
|
||||
self._check_frame(frame, CmdType.DRIVE, payload)
|
||||
|
||||
def test_drive_frame_values(self):
|
||||
frame = encode_drive(500, -300)
|
||||
payload = struct.pack(">hh", 500, -300)
|
||||
self._check_frame(frame, CmdType.DRIVE, payload)
|
||||
|
||||
def test_drive_clamping(self):
|
||||
# Values outside ±1000 must be clamped
|
||||
frame = encode_drive(5000, -9999)
|
||||
_, _, _, p0, p1, p2, p3, _ = frame
|
||||
speed, steer = struct.unpack(">hh", bytes([p0, p1, p2, p3]))
|
||||
self.assertEqual(speed, 1000)
|
||||
self.assertEqual(steer, -1000)
|
||||
|
||||
def test_estop_assert(self):
|
||||
frame = encode_estop(True)
|
||||
self._check_frame(frame, CmdType.ESTOP, b"\x01")
|
||||
|
||||
def test_estop_clear(self):
|
||||
frame = encode_estop(False)
|
||||
self._check_frame(frame, CmdType.ESTOP, b"\x00")
|
||||
|
||||
def test_arm_frame(self):
|
||||
for arm_val in (True, False):
|
||||
frame = encode_arm(arm_val)
|
||||
self._check_frame(frame, CmdType.ARM, bytes([1 if arm_val else 0]))
|
||||
|
||||
def test_pid_frame(self):
|
||||
frame = encode_pid(1.0, 0.1, 0.01)
|
||||
payload = struct.pack(">fff", 1.0, 0.1, 0.01)
|
||||
self._check_frame(frame, CmdType.PID, payload)
|
||||
|
||||
def test_pid_negative_raises(self):
|
||||
with self.assertRaises(ValueError):
|
||||
encode_pid(-1.0, 0.1, 0.01)
|
||||
|
||||
|
||||
# ── Parser round-trip tests ───────────────────────────────────────────────────
|
||||
|
||||
class TestFrameParser(unittest.TestCase):
|
||||
"""Feed encoded frames back through the parser and verify decode."""
|
||||
|
||||
def _parse_all(self, data: bytes):
|
||||
"""Feed all bytes; return list of decoded frames."""
|
||||
parser = FrameParser()
|
||||
results = []
|
||||
for byte in data:
|
||||
r = parser.feed(byte)
|
||||
if r is not None:
|
||||
results.append(r)
|
||||
return parser, results
|
||||
|
||||
# ── Command echo round-trips (parser recognises unknown cmd types as raw tuples) ──
|
||||
|
||||
def test_heartbeat_roundtrip(self):
|
||||
frame = encode_heartbeat()
|
||||
parser, results = self._parse_all(frame)
|
||||
self.assertEqual(len(results), 1)
|
||||
# HEARTBEAT (0x01) is a command type — no dataclass decoder, returns raw tuple
|
||||
self.assertIsInstance(results[0], tuple)
|
||||
type_code, payload = results[0]
|
||||
self.assertEqual(type_code, CmdType.HEARTBEAT)
|
||||
self.assertEqual(payload, b"")
|
||||
|
||||
def test_drive_roundtrip(self):
|
||||
frame = encode_drive(250, -100)
|
||||
parser, results = self._parse_all(frame)
|
||||
self.assertEqual(len(results), 1)
|
||||
type_code, payload = results[0]
|
||||
self.assertEqual(type_code, CmdType.DRIVE)
|
||||
speed, steer = struct.unpack(">hh", payload)
|
||||
self.assertEqual(speed, 250)
|
||||
self.assertEqual(steer, -100)
|
||||
|
||||
# ── Telemetry decode round-trips ────────────────────────────────────────
|
||||
|
||||
def _build_telem_frame(self, type_code: int, payload: bytes) -> bytes:
|
||||
length = len(payload)
|
||||
crc = _crc8(bytes([length, type_code]) + payload)
|
||||
return bytes([SYNC_BYTE, length, type_code]) + payload + bytes([crc])
|
||||
|
||||
def test_status_frame_decode(self):
|
||||
raw = struct.pack(">hhHBB", 152, 300, 11400, BalanceState.ARMED, 0x00)
|
||||
frame = self._build_telem_frame(TelType.STATUS, raw)
|
||||
_, results = self._parse_all(frame)
|
||||
self.assertEqual(len(results), 1)
|
||||
f = results[0]
|
||||
self.assertIsInstance(f, StatusFrame)
|
||||
self.assertAlmostEqual(f.pitch_deg, 15.2, places=5)
|
||||
self.assertEqual(f.motor_cmd, 300)
|
||||
self.assertEqual(f.vbat_mv, 11400)
|
||||
self.assertEqual(f.balance_state, BalanceState.ARMED)
|
||||
self.assertEqual(f.flags, 0x00)
|
||||
|
||||
def test_status_estop_flag(self):
|
||||
raw = struct.pack(">hhHBB", 0, 0, 10000, BalanceState.ESTOP, 0x01)
|
||||
frame = self._build_telem_frame(TelType.STATUS, raw)
|
||||
_, results = self._parse_all(frame)
|
||||
f = results[0]
|
||||
self.assertIsInstance(f, StatusFrame)
|
||||
self.assertTrue(bool(f.flags & 0x01)) # estop_active flag set
|
||||
|
||||
def test_vesc_left_frame_decode(self):
|
||||
# erpm=12000, voltage_mv=22400, current_ma=4500, temp_x10=352
|
||||
raw = struct.pack(">iHhH", 12000, 22400, 4500, 352)
|
||||
frame = self._build_telem_frame(TelType.VESC_LEFT, raw)
|
||||
_, results = self._parse_all(frame)
|
||||
self.assertEqual(len(results), 1)
|
||||
f = results[0]
|
||||
self.assertIsInstance(f, VescFrame)
|
||||
self.assertEqual(f.vesc_id, 56)
|
||||
self.assertEqual(f.erpm, 12000)
|
||||
self.assertEqual(f.voltage_mv, 22400)
|
||||
self.assertEqual(f.current_ma, 4500)
|
||||
self.assertAlmostEqual(f.temp_c, 35.2, places=5)
|
||||
|
||||
def test_vesc_right_frame_decode(self):
|
||||
raw = struct.pack(">iHhH", -6000, 22000, -200, 280)
|
||||
frame = self._build_telem_frame(TelType.VESC_RIGHT, raw)
|
||||
_, results = self._parse_all(frame)
|
||||
f = results[0]
|
||||
self.assertIsInstance(f, VescFrame)
|
||||
self.assertEqual(f.vesc_id, 68)
|
||||
self.assertEqual(f.erpm, -6000)
|
||||
self.assertAlmostEqual(f.temp_c, 28.0, places=5)
|
||||
|
||||
def test_ack_frame_decode(self):
|
||||
raw = bytes([CmdType.DRIVE])
|
||||
frame = self._build_telem_frame(TelType.ACK, raw)
|
||||
_, results = self._parse_all(frame)
|
||||
f = results[0]
|
||||
self.assertIsInstance(f, AckFrame)
|
||||
self.assertEqual(f.echoed_type, CmdType.DRIVE)
|
||||
|
||||
def test_nack_frame_decode(self):
|
||||
raw = bytes([CmdType.DRIVE, 0x03]) # ERR_ESTOP_ACTIVE
|
||||
frame = self._build_telem_frame(TelType.NACK, raw)
|
||||
_, results = self._parse_all(frame)
|
||||
f = results[0]
|
||||
self.assertIsInstance(f, NackFrame)
|
||||
self.assertEqual(f.cmd_type, CmdType.DRIVE)
|
||||
self.assertEqual(f.error_code, 0x03)
|
||||
|
||||
# ── Parser robustness ───────────────────────────────────────────────────
|
||||
|
||||
def test_bad_crc_discarded(self):
|
||||
frame = bytearray(encode_heartbeat())
|
||||
frame[-1] ^= 0xFF # corrupt CRC
|
||||
parser, results = self._parse_all(bytes(frame))
|
||||
self.assertEqual(results, [])
|
||||
self.assertEqual(parser.frames_error, 1)
|
||||
self.assertEqual(parser.frames_ok, 0)
|
||||
|
||||
def test_garbage_before_sync(self):
|
||||
garbage = bytes([0x00, 0xFF, 0x01, 0x22])
|
||||
frame = encode_heartbeat()
|
||||
parser, results = self._parse_all(garbage + frame)
|
||||
self.assertEqual(len(results), 1)
|
||||
|
||||
def test_two_frames_sequential(self):
|
||||
frames = encode_heartbeat() + encode_drive(100, 50)
|
||||
parser, results = self._parse_all(frames)
|
||||
self.assertEqual(len(results), 2)
|
||||
self.assertEqual(parser.frames_ok, 2)
|
||||
|
||||
def test_oversized_payload_rejected(self):
|
||||
# Craft a frame claiming LEN=63 (> MAX_PAYLOAD_LEN=62)
|
||||
bad = bytes([SYNC_BYTE, 63, CmdType.DRIVE])
|
||||
parser, results = self._parse_all(bad)
|
||||
self.assertEqual(results, [])
|
||||
self.assertEqual(parser.frames_error, 1)
|
||||
|
||||
def test_parser_counter_increments(self):
|
||||
parser = FrameParser()
|
||||
frame = encode_drive(10, 10)
|
||||
for b in frame:
|
||||
parser.feed(b)
|
||||
self.assertEqual(parser.frames_ok, 1)
|
||||
self.assertEqual(parser.frames_error, 0)
|
||||
|
||||
def test_reset_clears_state(self):
|
||||
parser = FrameParser()
|
||||
# Feed partial frame then reset
|
||||
partial = encode_drive(0, 0)[:-2]
|
||||
for b in partial:
|
||||
parser.feed(b)
|
||||
parser.reset()
|
||||
# Now a clean frame should decode correctly
|
||||
for b in encode_heartbeat():
|
||||
r = parser.feed(b)
|
||||
self.assertEqual(parser.frames_ok, 1)
|
||||
|
||||
|
||||
# ── Encode parameter edge cases ───────────────────────────────────────────────
|
||||
|
||||
class TestEncodeEdgeCases(unittest.TestCase):
|
||||
def test_drive_clamp_positive(self):
|
||||
frame = encode_drive(9999, 9999)
|
||||
payload = frame[3:7]
|
||||
speed, steer = struct.unpack(">hh", payload)
|
||||
self.assertEqual(speed, 1000)
|
||||
self.assertEqual(steer, 1000)
|
||||
|
||||
def test_drive_clamp_negative(self):
|
||||
frame = encode_drive(-9999, -9999)
|
||||
payload = frame[3:7]
|
||||
speed, steer = struct.unpack(">hh", payload)
|
||||
self.assertEqual(speed, -1000)
|
||||
self.assertEqual(steer, -1000)
|
||||
|
||||
def test_pid_zero_gains_valid(self):
|
||||
frame = encode_pid(0.0, 0.0, 0.0)
|
||||
self.assertIsNotNone(frame)
|
||||
|
||||
def test_pid_large_gains_valid(self):
|
||||
frame = encode_pid(100.0, 50.0, 25.0)
|
||||
self.assertIsNotNone(frame)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -1,38 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
# ros2-launch.sh — Source ROS2 Humble + workspace overlay, then exec ros2 launch
|
||||
#
|
||||
# Used by saltybot-*.service units so that ExecStart= does not need to inline
|
||||
# bash source chains.
|
||||
#
|
||||
# Usage (from a service file):
|
||||
# ExecStart=/opt/saltybot/scripts/ros2-launch.sh <pkg> <launch_file> [args...]
|
||||
#
|
||||
# Env vars respected:
|
||||
# ROS_DISTRO default: humble
|
||||
# SALTYBOT_WS default: /opt/saltybot/jetson/ros2_ws/install
|
||||
# ROS_DOMAIN_ID default: 42
|
||||
|
||||
set -euo pipefail
|
||||
|
||||
ROS_DISTRO="${ROS_DISTRO:-humble}"
|
||||
SALTYBOT_WS="${SALTYBOT_WS:-/opt/saltybot/jetson/ros2_ws/install}"
|
||||
|
||||
ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
|
||||
WS_SETUP="${SALTYBOT_WS}/setup.bash"
|
||||
|
||||
if [[ ! -f "${ROS_SETUP}" ]]; then
|
||||
echo "[ros2-launch] ERROR: ROS2 setup not found: ${ROS_SETUP}" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# shellcheck disable=SC1090
|
||||
source "${ROS_SETUP}"
|
||||
|
||||
if [[ -f "${WS_SETUP}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${WS_SETUP}"
|
||||
fi
|
||||
|
||||
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}"
|
||||
|
||||
exec ros2 launch "$@"
|
||||
@ -1,194 +1,55 @@
|
||||
#!/usr/bin/env bash
|
||||
# install_systemd.sh — Deploy and enable saltybot systemd services on Orin
|
||||
#
|
||||
# Run as root: sudo ./systemd/install_systemd.sh
|
||||
#
|
||||
# What this does:
|
||||
# 1. Deploy repo to /opt/saltybot/jetson
|
||||
# 2. Build ROS2 workspace (colcon)
|
||||
# 3. Install systemd unit files
|
||||
# 4. Install udev rules (CAN, ESP32)
|
||||
# 5. Enable and optionally start all services
|
||||
#
|
||||
# Services installed (start order):
|
||||
# can-bringup.service CANable2 @ 1 Mbps DroneCAN (Here4 GPS)
|
||||
# saltybot-esp32-serial.service ESP32-S3 BALANCE UART bridge (bd-wim1)
|
||||
# saltybot-here4.service Here4 GPS DroneCAN bridge (bd-p47c)
|
||||
# saltybot-ros2.service Full ROS2 stack (perception + nav)
|
||||
# saltybot-dashboard.service Web dashboard on port 8080
|
||||
# saltybot-social.service Social-bot stack (speech + LLM + face)
|
||||
# tailscale-vpn.service Tailscale VPN for remote access
|
||||
#
|
||||
# Prerequisites:
|
||||
# apt install ros-humble-desktop python3-colcon-common-extensions
|
||||
# pip install dronecan (for Here4 GPS node)
|
||||
# usermod -aG dialout orin (for serial port access)
|
||||
# install_systemd.sh — Install saltybot systemd services on Orin
|
||||
# Run as root: sudo ./systemd/install_systemd.sh
|
||||
set -euo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
REPO_DIR="$(dirname "${SCRIPT_DIR}")"
|
||||
SYSTEMD_DIR="/etc/systemd/system"
|
||||
DEPLOY_DIR="/opt/saltybot/jetson"
|
||||
SCRIPTS_DIR="${DEPLOY_DIR}/scripts"
|
||||
UDEV_DIR="/etc/udev/rules.d"
|
||||
BRINGUP_SYSTEMD="${REPO_DIR}/ros2_ws/src/saltybot_bringup/systemd"
|
||||
BRINGUP_UDEV="${REPO_DIR}/ros2_ws/src/saltybot_bringup/udev"
|
||||
WS_SRC="${REPO_DIR}/ros2_ws/src"
|
||||
WS_BUILD="${DEPLOY_DIR}/ros2_ws"
|
||||
|
||||
ROS_DISTRO="${ROS_DISTRO:-humble}"
|
||||
ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
|
||||
ORIN_USER="${SALTYBOT_USER:-orin}"
|
||||
log() { echo "[install_systemd] $*"; }
|
||||
|
||||
log() { echo "[install_systemd] $*"; }
|
||||
warn() { echo "[install_systemd] WARN: $*" >&2; }
|
||||
die() { echo "[install_systemd] ERROR: $*" >&2; exit 1; }
|
||||
[[ "$(id -u)" == "0" ]] || { echo "Run as root"; exit 1; }
|
||||
|
||||
[[ "$(id -u)" == "0" ]] || die "Run as root (sudo $0)"
|
||||
[[ -f "${ROS_SETUP}" ]] || die "ROS2 ${ROS_DISTRO} not found at ${ROS_SETUP}"
|
||||
|
||||
# ── 1. Deploy repo ─────────────────────────────────────────────────────────────
|
||||
log "Deploying repo to ${DEPLOY_DIR}..."
|
||||
# Deploy repo to /opt/saltybot/jetson
|
||||
log "Deploying to ${DEPLOY_DIR}..."
|
||||
mkdir -p "${DEPLOY_DIR}"
|
||||
rsync -a \
|
||||
--exclude='.git' \
|
||||
--exclude='__pycache__' \
|
||||
--exclude='*.pyc' \
|
||||
--exclude='.pytest_cache' \
|
||||
--exclude='build/' \
|
||||
--exclude='install/' \
|
||||
--exclude='log/' \
|
||||
rsync -a --exclude='.git' --exclude='__pycache__' \
|
||||
"${REPO_DIR}/" "${DEPLOY_DIR}/"
|
||||
|
||||
# Install launch wrapper script
|
||||
log "Installing ros2-launch.sh..."
|
||||
install -m 755 "${SCRIPT_DIR}/../scripts/ros2-launch.sh" "/opt/saltybot/scripts/ros2-launch.sh"
|
||||
|
||||
# ── 2. Build ROS2 workspace ────────────────────────────────────────────────────
|
||||
if command -v colcon &>/dev/null; then
|
||||
log "Building ROS2 workspace (colcon)..."
|
||||
# shellcheck disable=SC1090
|
||||
source "${ROS_SETUP}"
|
||||
(
|
||||
cd "${WS_BUILD}"
|
||||
colcon build \
|
||||
--symlink-install \
|
||||
--cmake-args -DCMAKE_BUILD_TYPE=Release \
|
||||
2>&1 | tee /tmp/colcon-build.log
|
||||
)
|
||||
log "Workspace build complete."
|
||||
else
|
||||
warn "colcon not found — skipping workspace build."
|
||||
warn "Run manually: cd ${WS_BUILD} && colcon build --symlink-install"
|
||||
fi
|
||||
|
||||
# ── 3. Install systemd units ───────────────────────────────────────────────────
|
||||
# Install service files
|
||||
log "Installing systemd units..."
|
||||
cp "${SCRIPT_DIR}/saltybot.target" "${SYSTEMD_DIR}/"
|
||||
cp "${SCRIPT_DIR}/saltybot-social.service" "${SYSTEMD_DIR}/"
|
||||
cp "${SCRIPT_DIR}/tailscale-vpn.service" "${SYSTEMD_DIR}/"
|
||||
cp "${BRINGUP_SYSTEMD}/can-bringup.service" "${SYSTEMD_DIR}/"
|
||||
|
||||
# Units from jetson/systemd/
|
||||
for unit in \
|
||||
saltybot.target \
|
||||
saltybot-ros2.service \
|
||||
saltybot-esp32-serial.service \
|
||||
saltybot-here4.service \
|
||||
saltybot-dashboard.service \
|
||||
saltybot-social.service \
|
||||
tailscale-vpn.service
|
||||
do
|
||||
if [[ -f "${SCRIPT_DIR}/${unit}" ]]; then
|
||||
cp "${SCRIPT_DIR}/${unit}" "${SYSTEMD_DIR}/"
|
||||
log " Installed ${unit}"
|
||||
else
|
||||
warn " ${unit} not found in ${SCRIPT_DIR}/ — skipping"
|
||||
fi
|
||||
done
|
||||
|
||||
# Units from saltybot_bringup/systemd/
|
||||
for unit in \
|
||||
can-bringup.service \
|
||||
chromium-kiosk.service \
|
||||
magedok-display.service \
|
||||
salty-face-server.service
|
||||
do
|
||||
if [[ -f "${BRINGUP_SYSTEMD}/${unit}" ]]; then
|
||||
cp "${BRINGUP_SYSTEMD}/${unit}" "${SYSTEMD_DIR}/"
|
||||
log " Installed ${unit} (from bringup)"
|
||||
else
|
||||
warn " ${unit} not found in bringup systemd/ — skipping"
|
||||
fi
|
||||
done
|
||||
|
||||
# ── 4. Install udev rules ──────────────────────────────────────────────────────
|
||||
# Install udev rules
|
||||
log "Installing udev rules..."
|
||||
mkdir -p "${UDEV_DIR}"
|
||||
|
||||
for rule in \
|
||||
70-canable.rules \
|
||||
80-esp32.rules \
|
||||
90-magedok-touch.rules
|
||||
do
|
||||
if [[ -f "${BRINGUP_UDEV}/${rule}" ]]; then
|
||||
cp "${BRINGUP_UDEV}/${rule}" "${UDEV_DIR}/"
|
||||
log " Installed ${rule}"
|
||||
else
|
||||
warn " ${rule} not found — skipping"
|
||||
fi
|
||||
done
|
||||
|
||||
cp "${BRINGUP_UDEV}/70-canable.rules" "${UDEV_DIR}/"
|
||||
cp "${BRINGUP_UDEV}/90-magedok-touch.rules" "${UDEV_DIR}/"
|
||||
udevadm control --reload
|
||||
udevadm trigger --subsystem-match=net --action=add
|
||||
udevadm trigger --subsystem-match=tty --action=add
|
||||
log "udev rules reloaded."
|
||||
udevadm trigger --subsystem-match=net --action=add
|
||||
|
||||
# ── 5. Set permissions ─────────────────────────────────────────────────────────
|
||||
log "Ensuring '${ORIN_USER}' is in dialout group (serial port access)..."
|
||||
if id "${ORIN_USER}" &>/dev/null; then
|
||||
usermod -aG dialout "${ORIN_USER}" || warn "usermod failed — add ${ORIN_USER} to dialout manually"
|
||||
else
|
||||
warn "User '${ORIN_USER}' not found — skip usermod"
|
||||
fi
|
||||
|
||||
# ── 6. Reload systemd and enable services ─────────────────────────────────────
|
||||
log "Reloading systemd daemon..."
|
||||
# Reload and enable
|
||||
systemctl daemon-reload
|
||||
systemctl enable saltybot.target
|
||||
systemctl enable saltybot-social.service
|
||||
systemctl enable tailscale-vpn.service
|
||||
systemctl enable can-bringup.service
|
||||
|
||||
log "Enabling services..."
|
||||
systemctl enable \
|
||||
saltybot.target \
|
||||
can-bringup.service \
|
||||
saltybot-esp32-serial.service \
|
||||
saltybot-here4.service \
|
||||
saltybot-ros2.service \
|
||||
saltybot-dashboard.service \
|
||||
saltybot-social.service \
|
||||
tailscale-vpn.service
|
||||
|
||||
# Enable mosquitto (MQTT broker) if installed
|
||||
if systemctl list-unit-files mosquitto.service &>/dev/null; then
|
||||
systemctl enable mosquitto.service
|
||||
log " Enabled mosquitto.service"
|
||||
fi
|
||||
|
||||
# ── 7. Summary ────────────────────────────────────────────────────────────────
|
||||
log ""
|
||||
log "Installation complete."
|
||||
log ""
|
||||
log "Services will start automatically on next reboot."
|
||||
log "To start now without rebooting:"
|
||||
log " sudo systemctl start saltybot.target"
|
||||
log ""
|
||||
log "Check status:"
|
||||
log " systemctl status can-bringup saltybot-esp32-serial saltybot-here4 saltybot-ros2 saltybot-dashboard"
|
||||
log ""
|
||||
log "Live logs:"
|
||||
log "Services installed. Start with:"
|
||||
log " systemctl start saltybot-social"
|
||||
log " systemctl start tailscale-vpn"
|
||||
log " systemctl start can-bringup"
|
||||
log " journalctl -fu saltybot-social"
|
||||
log " journalctl -fu tailscale-vpn"
|
||||
log " journalctl -fu can-bringup"
|
||||
log " journalctl -fu saltybot-esp32-serial"
|
||||
log " journalctl -fu saltybot-here4"
|
||||
log " journalctl -fu saltybot-ros2"
|
||||
log " journalctl -fu saltybot-dashboard"
|
||||
log ""
|
||||
log "Dashboard: http://<orin-ip>:8080"
|
||||
log "rosbridge: ws://<orin-ip>:9090"
|
||||
log ""
|
||||
log "Note: saltybot-esp32-serial and saltybot-here4 require packages"
|
||||
log " from bd-wim1 (PR #727) and bd-p47c (PR #728) to be merged."
|
||||
log "Verify CAN bus: candump can0"
|
||||
log " VESC CAN IDs: 61 (0x3D) and 79 (0x4F)"
|
||||
|
||||
@ -1,38 +0,0 @@
|
||||
[Unit]
|
||||
Description=SaltyBot Web Dashboard (saltybot_dashboard — port 8080)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
#
|
||||
# Serves the live monitoring dashboard on http://<orin-ip>:8080
|
||||
# Subscribes to ROS2 topics over the local DomainID (ROS_DOMAIN_ID=42).
|
||||
# Starts after the ROS2 stack but does not hard-depend on it —
|
||||
# the dashboard handles missing topics gracefully and will show data as
|
||||
# nodes come up.
|
||||
After=network-online.target saltybot-ros2.service
|
||||
Wants=saltybot-ros2.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=orin
|
||||
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_dashboard dashboard.launch.py
|
||||
|
||||
Restart=on-failure
|
||||
RestartSec=5s
|
||||
StartLimitInterval=60s
|
||||
StartLimitBurst=5
|
||||
|
||||
TimeoutStartSec=30s
|
||||
TimeoutStopSec=10s
|
||||
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-dashboard
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
@ -1,54 +0,0 @@
|
||||
[Unit]
|
||||
Description=SaltyBot ESP32-S3 BALANCE UART Serial Bridge (bd-wim1)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/bd-wim1
|
||||
# Requires package: saltybot_esp32_serial (merged in bd-wim1 → PR #727)
|
||||
#
|
||||
# Publishes:
|
||||
# /saltybot/attitude String JSON (pitch, motor_cmd, vbat_mv, state)
|
||||
# /saltybot/balance_state String
|
||||
# /can/battery BatteryState
|
||||
# /can/vesc/left/state Float32MultiArray [erpm, voltage_v, current_a, temp_c]
|
||||
# /can/vesc/right/state Float32MultiArray
|
||||
# /can/connection_status String
|
||||
#
|
||||
# Subscribes:
|
||||
# /cmd_vel Twist → DRIVE command to ESP32 BALANCE
|
||||
# /estop Bool → ESTOP command
|
||||
# /saltybot/arm Bool → ARM command
|
||||
# /saltybot/pid_update String → PID gains update
|
||||
After=network-online.target
|
||||
# Wait for /dev/esp32-balance (created by 80-esp32.rules udev rule).
|
||||
# TAG+="systemd" in the udev rule makes systemd track dev-esp32\x2dbalance.device.
|
||||
# If device is not present at boot the node's auto-reconnect loop handles it.
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=dialout
|
||||
|
||||
# ROS2 Humble environment + workspace overlay
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_esp32_serial esp32_balance.launch.py
|
||||
|
||||
# The node auto-reconnects on serial disconnect (2 s retry).
|
||||
# Restart=on-failure covers node crash; RestartSec gives the device time to re-enumerate.
|
||||
Restart=on-failure
|
||||
RestartSec=5s
|
||||
StartLimitInterval=60s
|
||||
StartLimitBurst=5
|
||||
|
||||
TimeoutStartSec=30s
|
||||
TimeoutStopSec=10s
|
||||
|
||||
# Logging
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-esp32-serial
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
@ -1,50 +0,0 @@
|
||||
[Unit]
|
||||
Description=SaltyBot Here4 GPS DroneCAN Bridge (bd-p47c)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/bd-p47c
|
||||
# Requires package: saltybot_dronecan_gps (merged in bd-p47c → PR #728)
|
||||
# Requires: can0 at 1 Mbps (set by can-bringup.service)
|
||||
#
|
||||
# Publishes:
|
||||
# /gps/fix NavSatFix 5 Hz → navsat_transform EKF
|
||||
# /gps/velocity TwistWithCovarianceStamped 5 Hz
|
||||
# /here4/fix NavSatFix 5 Hz (alias)
|
||||
# /here4/imu Imu 50 Hz
|
||||
# /here4/mag MagneticField 10 Hz
|
||||
# /here4/baro FluidPressure 10 Hz
|
||||
# /here4/status String JSON 1 Hz (node health, fix quality)
|
||||
# /here4/node_id Int32 once (auto-discovered DroneCAN ID)
|
||||
#
|
||||
# Subscribes:
|
||||
# /rtcm ByteMultiArray on demand (RTCM corrections)
|
||||
# /rtcm_hex String on demand (hex fallback)
|
||||
After=network-online.target can-bringup.service
|
||||
Requires=can-bringup.service
|
||||
# If can-bringup stops (CANable2 unplugged), stop this service too
|
||||
BindsTo=can-bringup.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=dialout
|
||||
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_dronecan_gps here4.launch.py
|
||||
|
||||
Restart=on-failure
|
||||
RestartSec=5s
|
||||
StartLimitInterval=60s
|
||||
StartLimitBurst=5
|
||||
|
||||
TimeoutStartSec=30s
|
||||
TimeoutStopSec=10s
|
||||
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-here4
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
@ -1,59 +0,0 @@
|
||||
[Unit]
|
||||
Description=SaltyBot ROS2 Full Stack (perception + navigation + SLAM)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
#
|
||||
# Launches: saltybot_bringup full_stack.launch.py (indoor mode by default)
|
||||
# enable_bridge:=false — ESP32-S3 serial bridge is managed by saltybot-esp32-serial.service
|
||||
#
|
||||
# Stack launched (t=0s → t=17s):
|
||||
# t= 0s robot_description (URDF + TF)
|
||||
# t= 2s RPLIDAR A1M8 + RealSense D435i
|
||||
# t= 3s LIDAR obstacle avoidance
|
||||
# t= 4s 4× IMX219 CSI cameras (optional)
|
||||
# t= 4s UWB driver
|
||||
# t= 5s Audio pipeline (wake word + STT + TTS)
|
||||
# t= 6s RTAB-Map SLAM (indoor) / GPS nav (outdoor)
|
||||
# t= 6s YOLOv8n person + object detection (TensorRT)
|
||||
# t= 7s Autonomous docking
|
||||
# t= 9s Person follower
|
||||
# t=14s Nav2 navigation stack
|
||||
# t=17s rosbridge WebSocket (ws://0.0.0.0:9090)
|
||||
#
|
||||
# Hard deps: can-bringup (Here4 GPS topics), esp32-serial (VESC telemetry)
|
||||
After=network-online.target can-bringup.service saltybot-esp32-serial.service saltybot-here4.service
|
||||
Wants=can-bringup.service saltybot-esp32-serial.service saltybot-here4.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=orin
|
||||
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
# enable_bridge:=false — ESP32 serial bridge runs as its own service (saltybot-esp32-serial)
|
||||
# mode can be overridden via environment: Environment="SALTYBOT_MODE=outdoor"
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_bringup full_stack.launch.py \
|
||||
enable_bridge:=false \
|
||||
mode:=${SALTYBOT_MODE:-indoor}
|
||||
|
||||
Restart=on-failure
|
||||
RestartSec=10s
|
||||
StartLimitInterval=120s
|
||||
StartLimitBurst=3
|
||||
|
||||
TimeoutStartSec=120s
|
||||
TimeoutStopSec=30s
|
||||
|
||||
# ROS2 launch spawns many child processes; kill them on stop
|
||||
KillMode=control-group
|
||||
|
||||
# Logging
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-ros2
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
@ -1,22 +1,8 @@
|
||||
[Unit]
|
||||
Description=SaltyBot Full Stack Target
|
||||
Description=Saltybot Full Stack Target
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
#
|
||||
# Start everything: systemctl start saltybot.target
|
||||
# Stop everything: systemctl stop saltybot.target
|
||||
# Check all: systemctl status 'saltybot-*' can-bringup
|
||||
#
|
||||
# Boot order (dependency chain):
|
||||
# network-online.target
|
||||
# → can-bringup.service (CANable2 @ 1 Mbps DroneCAN)
|
||||
# → saltybot-here4.service (Here4 GPS → /gps/fix, /here4/*)
|
||||
# → saltybot-esp32-serial.service (ESP32-S3 UART → /can/vesc/*, /saltybot/attitude)
|
||||
# → saltybot-ros2.service (full_stack.launch.py, perception + nav)
|
||||
# → saltybot-dashboard.service (port 8080)
|
||||
# (independent) saltybot-social.service
|
||||
# (independent) tailscale-vpn.service
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
After=docker.service network-online.target
|
||||
Requires=docker.service
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user