Firmware auf dem ESP32-S3

Die Firmware ist das C++-Programm auf dem Seeed XIAO ESP32-S3, das Gelenkwinkelbefehle vom Pi 200-mal pro Sekunde in Servopulse umsetzt, die IMU und den Akkumonitor ausliest und den Roboter sicher anhält, wenn etwas schiefgeht. Sie ist klein, aber sie ist die letzte Softwareschicht zwischen einem Fehler und zwölf Servos.

In Strido

• Projekt: firmware/strido_esp32s3 (PlatformIO, Arduino-ESP32 3.x).
• Einstiegspunkt: src/main.cpp; Pins und Raten in src/board_config.h; LCD-Gesicht in src/face.cpp.
• Portabler Kern: lib/strido_core (Kniekoppelgetriebe, Servozuordnung, Protokoll, Sicherheit), mit Unit-Tests auf Ihrem Computer über pio test -e native.
• Generiert: lib/strido_core/include/strido/geometry_generated.h, aus dem CAD erzeugt von strido_cad.codegen; niemals von Hand bearbeiten.
• Peripherie am I²C (400 kHz): PCA9685-Servotreiber 0x40, LSM6DSOX-IMU 0x6A, INA260-Power-Monitor 0x41; ST7789V-LCD an SPI.
• Host-Seite: mcu_bridge (ROS 2) und strido-cli (Werkbank-Werkzeug) sprechen dasselbe Protokoll über packages/strido-core/src/strido/protocol.py.

Warum überhaupt ein Mikrocontroller

Auf dem Pi läuft Linux, das jeden Prozess für einige zehn Millisekunden anhalten kann. Ein stotterndes Servosignal lässt das Bein ruckeln; eine Sicherheitsprüfung, die „meistens“ läuft, ist keine Sicherheitsprüfung. Auf dem ESP32-S3 läuft nichts außer dieser Firmware, daher ist seine Schleifenperiode vorhersagbar. Er arbeitet auch weiter, wenn der Pi abstürzt, und genau dann brauchen Sie den Watchdog.

Die Arbeitsteilung: Der Pi entscheidet, welche Winkel die Beine haben sollen; der ESP32 entscheidet, wie sie sicher erreicht werden (Koppelgetriebe-Mathematik, Anstiegsbegrenzung, Begrenzungen, Kalibrierung) und ob überhaupt angesteuert wird.

Mit PlatformIO bauen, testen und flashen

PlatformIO ist ein Build-System und Paketmanager für Embedded-Code. platformio.ini legt die Toolchain (espressif32 @ ~8.0.0), das Board (seeed_xiao_esp32s3) und jede Bibliothek (Adafruit PWM Servo Driver, LSM6DS, INA260, AHRS, LovyanGFX) fest, sodass alle dasselbe Binary bauen.

Es definiert zwei Umgebungen:

Umgebung	Baut für	Verwendet von
xiao_esp32s3 (Standard)	Das echte Board	make firmware, Upload
native	Ihren Computer, mit dem Testframework Unity	make test-firmware

cd firmware/strido_esp32s3
pio test -e native                      # 15 unit tests: linkage, protocol, watchdog, e-stop, battery, NaN rejection
pio run -e xiao_esp32s3                 # compile (same as `make firmware` from the repo root)
pio run -e xiao_esp32s3 -t upload       # compile and flash over USB
pio device monitor                      # serial monitor (frames are binary; use strido-cli instead)

-DARDUINO_USB_CDC_ON_BOOT=1 leitet Serial auf den nativen USB-Anschluss des Chips um, sodass das Board unter Linux als CDC-ACM-Gerät erscheint (/dev/ttyACM*, verlinkt auf /dev/strido_mcu). Die Baudrate wird bei USB CDC ignoriert. Wenn ein Upload keine Verbindung aufbauen kann, halten Sie beim Einstecken des USB-Kabels die BOOT-Taste des XIAO gedrückt, um den ROM-Bootloader zu erzwingen.

Warum eine native Testumgebung

lib/strido_core enthält keine Arduino-Aufrufe. Derselbe Kinematik-, Protokoll- und Sicherheitscode kompiliert für den ESP32 und für Ihren Laptop, daher prüft pio test -e native ihn in Sekunden ohne Hardware und schlägt in der CI fehl, wenn jemand den Watchdog kaputt macht.

Programmstruktur

setup() initialisiert die Hardware und startet zwei FreeRTOS-Tasks, die an die beiden Kerne des ESP32-S3 gebunden sind. loop() schläft danach für immer.

Diagramm wird gezeichnet…

Warum zwei Tasks: USB-Verkehr kommt stoßweise, und das LCD braucht Zeit zum Zeichnen; keines von beiden darf die Servoaktualisierung verzögern. Der Regel-Task läuft mit höherer Priorität auf einem eigenen Kern, und die gemeinsamen Daten (command, state) werden in kurzen kritischen Abschnitten unter Spinlock kopiert (portENTER_CRITICAL). Blockierende I²C-Lesevorgänge finden nie innerhalb eines kritischen Abschnitts statt.

Startsequenz

1. Den OE-Pin des PCA9685 (D3) auf High setzen: alle Servoausgänge aus, Servos kraftlos.
2. I²C an D4/D5 mit 400 kHz starten; Kalibrierung aus dem Flash laden.
3. Den internen Gelenkzustand auf die Montage-Referenzpose setzen.
4. PCA9685 mit 200 Hz und allen Kanälen aus starten; IMU mit ±8 g, ±1000 °/s, 416 Hz; INA260 an 0x41.
5. „ready“ auf dem LCD anzeigen (oder „NO PCA9685“), die Tasks starten.

Die Servos bleiben kraftlos, bis der Host seinen ersten Positionsbefehl sendet. Deshalb funktioniert die Startreihenfolge in Elektronik: Der Loop-Key kann sicher eingesteckt werden, und der erste Befehl des Gait-Nodes ist die Ruhepose, in der der Roboter bereits liegt.

Ein Regelzyklus (5 ms)

1. IMU. Den LSM6DSOX auslesen, seine Achsen in das Basiskoordinatensystem drehen (das Board ist mit x zur linken Seite des Roboters, y zum Heck des Roboters montiert), den Mahony-Orientierungsfilter aktualisieren.
2. Leistung. Alle 100 ms Busspannung und Strom vom INA260 lesen und die Spannung an den Sicherheitsüberwacher weitergeben.
3. Sicherheit. safety.update(now) liefert den Ansteuerzustand (siehe unten). Den neuesten Befehl kopieren.
4. Servos, wenn der Zustand Active ist:

   • Ratenbegrenzung: Kein Gelenk bewegt sich schneller als ${\dot{q}}_{\max }=6\mathrm{rad}/\mathrm{s}$, daher wird jeder 5-ms-Schritt auf $|\mathrm{\Delta }q|\le {\dot{q}}_{\max }\mathrm{\Delta }t=0.03\mathrm{rad}$ begrenzt. Die Nenngeschwindigkeit des SPT5435LV beträgt 0,14 s pro 60° bei 6 V, etwa $7.5\mathrm{rad}/\mathrm{s}$; wer darunter bleibt, hält befohlene und tatsächliche Position nahe beieinander.

   • Kniekoppelgetriebe: (Hüfte, Oberschenkel, Unterschenkel) in die drei Aktuator-Winkel (Hüfte, Oberschenkel, Kniekurbel) umrechnen. Der Knieservo treibt den Unterschenkel über ein Parallelogramm und ein Viergelenk an, daher hängt der Kurbelwinkel sowohl vom Oberschenkel als auch vom Unterschenkel ab (Kinematik).

   • Begrenzen von Hüfte, Oberschenkel und Kurbel auf die generierten Grenzen, dann prüfen, ob der resultierende Unterschenkelwinkel in seinem Fenster liegt. Eine unerreichbare Anforderung behält die vorherigen Pulse bei.

   • Puls: Jeden Aktuatorwinkel $\theta$ in eine Pulsbreite umrechnen und diese dann auf [min_us, max_us] begrenzen:

     $$t_{\mathrm{p}}=t_{\mathrm{neutral}}+s\cdot 636.6\mu \mathrm{s}/\mathrm{rad}\cdot (\theta -{\theta }_0)$$

     mit neutral_us $t_{\mathrm{neutral}}$, direction $s=\pm 1$ und zero_rad ${\theta }_0$. Die Steigung beträgt $2000\mu \mathrm{s}/\pi \mathrm{rad}\approx 636.6\mu \mathrm{s}/\mathrm{rad}$: Der SPT5435LV-180W bildet 500–2500 µs auf 180° ab (Servos).

   • Die Pulse auf die PCA9685-Kanäle schreiben (kServoChannel: FL-Hüfte auf 15 … RR-Unterschenkel auf 4, identisch zur Upstream-Verkabelung von Yertle gehalten).

5. Melden der tatsächlich angewendeten Winkel (nach der Begrenzung, und des Unterschenkelwinkels, den die begrenzte Kurbel wirklich erzeugt) sowie der Flags in den gemeinsamen Zustand.

Warum der PCA9685

Der PCA9685 ist ein 16-Kanal-PWM-Generator mit 12 Bit, der über I²C gesteuert wird. Einmal konfiguriert, erzeugt er alle Pulse in Hardware, sodass der ESP32 nur neue Werte sendet. Bei 200 Hz beträgt eine Periode $T=5000\mu \mathrm{s}$, und 12 Bit ergeben $2^{12}=4096$ Schritte, $T/4096\approx 1.2\mu \mathrm{s}$ pro Schritt, also grob 0,1° Servoweg. Die Servofrequenz beträgt 200 Hz (der SPT5435LV akzeptiert bis zu 330 Hz): Eine höhere Bildrate als die klassischen 50 Hz bedeutet weniger Verzögerung zwischen einem neuen Befehl und dem Zeitpunkt, an dem der Servo ihn sieht. Die Abtastperiode begrenzt außerdem, was jeder digitale Controller leisten kann; siehe Regelung.

Sein Low-aktiver OE-Pin (Output Enable) ist mit D3 verbunden. Wird er auf High gesetzt, stoppen alle Pulse sofort, unabhängig von I²C. Das Board selbst zieht OE auf Low (Ausgänge an), daher ergänzt der Kabelbaum einen 2,2-kΩ-Pull-up von OE nach VCC (W20 in Elektronik): Er hält OE auf High, Servos aus, während der ESP32 neu startet oder in seinem Bootloader steht.

Das serielle Protokoll zum Pi

Nachrichten laufen über USB CDC als Frames:

COBS( type | payload | CRC16 little-endian ) 0x00

• COBS (Consistent Overhead Byte Stuffing) entfernt jedes Null-Byte aus den Daten, sodass 0x00 das Ende eines Frames markieren kann. Ein Empfänger, der mitten im Datenstrom einsteigt oder ein Byte verliert, synchronisiert sich bei der nächsten Null neu.
• CRC-16/CCITT-FALSE erkennt beschädigte Frames, die verworfen werden. Firmware und Python teilen sich einen Testvektor, sodass beide Seiten dieselbe CRC berechnen.
• Nutzdaten sind gepackte Little-Endian-Structs, zweimal definiert und gegeneinander getestet: lib/strido_core/include/strido/protocol.h und packages/strido-core/src/strido/protocol.py.

Typ	Name	Richtung	Nutzdaten
0x01	JOINT_COMMAND	Pi → MCU	seq u16, mode u8 (0 relax, 1 position), q[12] f32
0x02	SERVO_PULSE_COMMAND	Pi → MCU	seq u16, pulse_us[12] u16 (0 = aus); nur Kalibrierung
0x03	SET_CALIBRATION	Pi → MCU	joint, direction, neutral_us, zero_rad, min_us, max_us
0x04	SAVE_CALIBRATION	Pi → MCU	keine
0x05	REQUEST_CALIBRATION	Pi → MCU	keine → 12 × CALIBRATION
0x06	ESTOP	Pi → MCU	engaged u8
0x07	DISPLAY	Pi → MCU	expression u8, length u8, text
0x81	STATE	MCU → Pi, 100 Hz	letzte seq, Zeit, Flags, q[12], Quaternion, Gyro, Beschleunigung, Bus-V, Strom
0x82	CALIBRATION	MCU → Pi	wie SET_CALIBRATION
0x83	LOG	MCU → Pi	ASCII

Ein Gelenkbefehl ist auf der Leitung 56 Bytes lang (51 Byte Nutzdaten + Typ + CRC + COBS-Overhead + Trennzeichen); bei den 100 Hz des Gaits sind das etwa 5,6 kB/s, für USB eine Kleinigkeit. Die Firmware verwirft beim Deserialisieren jeden Gelenkbefehl, der NaN oder Unendlich enthält: Ein Vergleich mit NaN ist gegen jede Grenze falsch, und NaN würde sonst durch alle Begrenzungen rutschen.

Kalibrierung im Flash

Keine zwei Servohörner sitzen in exakt demselben Winkel auf ihrer Verzahnung. Jedes Gelenk hat daher eine ServoCalibration:

Feld	Bedeutung
direction	+1 oder −1: Vorzeichen zwischen Wellenwinkel und Pulsbreite (gespiegelte Beine)
neutral_us	Puls, der die Welle auf zero_rad hält (Standard 1500 µs)
zero_rad	Aktuatorwinkel beim Neutralpuls (Standard: die Montage-Referenzpose)
min_us, max_us	Hartes Pulsfenster (Standard 500–2500 µs)

Die Standardwerte stammen aus der generierten Geometrie. Gespeicherte Werte werden mit der ESP32-Bibliothek Preferences abgelegt (Schlüssel-Wert-Speicher im Flash, Namensraum strido, Schlüssel cal0 … cal11), sodass sie das Ausschalten und das erneute Flashen der Anwendung überstehen. Das Werkbank-Werkzeug steuert den Ablauf:

strido-cli center        # 1500 µs on every channel: fit the horns in this reference pose
strido-cli calibrate     # trim each joint's neutral in ±5 / ±25 µs steps, saved to flash
strido-cli monitor       # battery, flags, roll/pitch

strido-cli sendet Befehle mit 20 Hz, damit der Watchdog versorgt bleibt. Rohe Pulse umgehen die Gelenkgrenzen (nur das Pulsfenster des jeweiligen Gelenks gilt), deshalb wird mit nicht montierten Hörnern oder frei beweglichen Beinen kalibriert. Wann das zu tun ist, steht in der Montageanleitung.

Sicherheit und Failsafes, wie implementiert

SafetySupervisor (lib/strido_core/src/safety.cpp) ist ein kleiner Zustandsautomat mit vier Ansteuerzuständen:

Diagramm wird gezeichnet…

Failsafe	Verhalten	Warum
Befehls-Watchdog	Nur Gelenkbefehle versorgen ihn; kein Befehl für 200 ms → halten; weitere 2 s → entspannen	Wenn der Gait-Node oder der Pi ausfällt, erstarrt der Roboter weder für immer unter Last, noch bricht er sofort zusammen
Verriegelter Not-Halt	ESTOP schaltet die Ausgänge ab, bis er freigegeben wird; die Freigabe führt zu Relaxed, daher ist ein neuer Positionsbefehl nötig	Das Freigeben des Halts darf den Roboter nie wieder in Bewegung springen lassen
Harte Ausgangsabschaltung	Relaxed und Estop ziehen OE des PCA9685 auf High und setzen jeden Kanal auf null	Funktioniert auch, wenn I²C hängt
Akku-Abschaltung	Warnung unter 6,8 V (3,4 V/Zelle); Abschaltung nach 5 aufeinanderfolgenden Messungen (0,5 s) unter 6,4 V (3,2 V/Zelle); erholt sich erst über 6,8 V	Schützt den 2S-LiPo; die Entprellung ignoriert die kurzen Spannungseinbrüche eines tretenden Beins
Kein Power-Monitor, keine Ansteuerung	Ohne funktionierenden INA260 werden Positionsbefehle abgelehnt (rohe Kalibrierpulse bleiben erlaubt)	Ohne Spannungsmessung gibt es keinen Unterspannungsschutz
Ratenbegrenzung	6 rad/s pro Gelenk	Kein Sprung, selbst wenn der Host einen sendet
Gelenk- und Kniekurbelgrenzen	Aus dem CAD-Durchlauf des Bewegungsbereichs, in die Firmware generiert	Ein fehlerhafter Befehl kann ein Bein nicht in den Körper fahren oder das Koppelgetriebe blockieren
Ablehnung nicht endlicher Werte	Befehle mit NaN/Inf werden verworfen	NaN umgeht Vergleiche
Wiedereinschalten ab Befehl	Wenn die Ausgänge wieder einschalten, springt der interne Zustand auf die befohlene Pose, statt von einer veralteten aus zu fahren	Die Servopositionen sind nach dem kraftlosen Zustand unbekannt

Der Zustand all dessen erreicht ROS als Flags in /diagnostics (estop, relaxed, watchdog, imu_ok, power_ok, pwm_ok, clamped, calibration_mode).

DANGER

Dies sind Software-Failsafes. Der Servo-Loop-Key ist die Hardware-Abschaltung und muss immer in Reichweite bleiben, wenn der Roboter scharfgeschaltet ist (Sicherheit).

Selbsttest

1. Warum läuft die Servoschleife auf dem ESP32 und nicht auf dem Pi?
2. Was passiert Schritt für Schritt, wenn der Gait-Node abstürzt, während der Roboter steht?
3. Warum macht COBS es sicher, 0x00 als Frame-Trennzeichen zu verwenden?
4. Wie groß ist die maximale Änderung eines Gelenkwinkels in einem 5-ms-Zyklus?
5. Wo liegen die Kalibrierwerte, und warum überstehen sie einen Firmware-Upload?

Antworten

1. Linux kann Prozesse unvorhersehbar anhalten; der ESP32 führt nur diese Schleife aus, daher sind das 200-Hz-Timing und die Sicherheitsprüfungen deterministisch und arbeiten weiter, wenn der Pi ausfällt.
2. Die Gelenkbefehle bleiben aus; nach 200 ms wechselt der Überwacher in Holding und hält die letzte Pose (Watchdog-Flag gesetzt); nach weiteren 2 s entspannt er und zieht OE auf High.
3. Die COBS-Kodierung entfernt alle Null-Bytes aus den Daten, daher ist die einzige Null im Datenstrom das Trennzeichen.
4. $6\mathrm{rad}/\mathrm{s}\times 0.005\mathrm{s}=0.03\mathrm{rad}$ (etwa ${1.7}^{\circ }$).
5. Im Flash des ESP32 über Preferences (Namensraum strido, Schlüssel cal0–cal11); diese Speicherpartition ist von der Anwendung getrennt, daher löscht ein Upload sie nicht.

Weiterführende Literatur

• PlatformIO-Dokumentation
• Seeed Studio XIAO ESP32S3: Erste Schritte
• ESP32-S3-Datenblatt (Espressif)
• PCA9685-Datenblatt (NXP, Kopie bei Adafruit)
• Arduino-ESP32-Bibliothek Preferences
• Theorie: Regelung, State Estimation (der IMU-Filter)