Files

T

alro65 295efa2d83 sprint-2: PID inner loop + Python rudder simulator

End-to-end implementation per docs/sprint-2-plan.md.

Builds: pio run -e esp32-dev SUCCESS, RAM 6.8%, Flash 26.8% (351 KB).
Tests: pytest 129/129 green (110 Sprint 1 + 19 Sprint 2).

Python (arautopilot/studio/simulator/):

- rudder_dynamics.py: marine-realistic physical model of a hydraulic
  rudder actuator. Defaults tuned so 100 % PWM produces steady-state
  v_max ~5 deg/s, matching the brief's "typical 3-6 dps" for a 30 m
  yacht. Includes deadband, min-useful PWM snap, port/stbd asymmetry,
  end-stops, optional external torque, RunRecorder helper.
- pid_inner.py: pure-Python reference PID. Anti-windup via back-
  calculation, setpoint rate limit, setpoint deadband, derivative LPF,
  actuator non-linearity compensation. This module is the algorithmic
  source of truth; C++ firmware is a line-by-line port.

Firmware (firmware/ar_autopilot_v1/src/pid/):

- pid_inner.h: header-only C++17 controller, byte-equivalent port of
  pid_inner.py. Compiles on ESP32 toolchain AND on host g++/clang/MSVC
  (no Arduino dependencies) -- ready for native Unity cross-validation
  once a host compiler is installed.
- pid_inner_task.{h,cpp}: FreeRTOS task wrapper. 50 Hz on Core 1
  (real-time core). Subscribes to TWDT, bleeds integrator during
  STANDBY, surfaces telemetry + tunables via the Modbus slave.

Modbus map (regenerated from YAML):

- 6 new INPUT registers (40-45): setpoint, output, error, kp/ki/kd live
- 4 new HOLDING registers (16-19): writable setpoint + kp/ki/kd req
  (writes propagate atomically; zero kp rejected as ILLEGAL_DATA_VALUE)

Tests:

- test_rudder_simulator.py: 9 tests (zero-input rest, full deflection,
  end-stop saturation, deadband, min-useful snap, asymmetry, recorder
  API, invalid dt, end-stop velocity zeroing).
- test_pid_inner_python.py: 10 tests (positive/negative step response,
  setpoint deadband holds, anti-windup bounds under saturation,
  allowed=false bleeds integrator, actuator deadband + asymmetry
  compensation, output saturation, rate limit, disturbance rejection).

NOT in Sprint 2 (intentional per brief sec. 12):
  - Outer heading PID, gain scheduling by SOG, ROT feed-forward
    (those land in Sprint 3)
  - Cross-validation tests via ctypes (need host C++ compiler that
    this Windows machine lacks; algorithmic parity enforced by review)

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

2026-05-18 15:27:45 -04:00

3.4 KiB

Raw Permalink Blame History

Sprint 2 — PID inner loop + Python rudder simulator

Brief reference: §12 Sprint 2. Status: executed overnight under user's blanket authorisation.

Objetivo

Lazo cerrado de posición de timón: dado un setpoint en grados, comandar la bomba/motor para llegar y mantener esa posición. Frecuencia 50 Hz (hard real-time, brief §6).

Sin esto el Sprint 3 (heading control) no tiene sobre qué construir.

Estrategia

Dado que no tengo hardware AR-NMEA-IO ni compilador C++ host, sigo este camino:

Simulador de timón en Python (arautopilot.studio.simulator.rudder_dynamics): modelo físico mínimo de un timón con inercia rotacional, fricción viscosa, deadband hidráulico y asimetría babor/estribor. Frecuencia de integración libre (1 ms).
PID inner loop en Python (arautopilot.studio.simulator.pid_inner): implementación pura-Python del algoritmo, anti-windup tipo back-calculation, deadband del setpoint, rate limit, saturación, y compensación de no-linealidades del actuador (offset por deadband, ganancia asimétrica). Usado para iterar rápido contra el simulador y producir las curvas de respuesta esperadas.
PID inner loop en C++ (firmware/.../src/pid/pid_inner.{h,cpp}): port línea-por-línea del algoritmo Python. Compila tanto en ESP32 como en host-native (sin dependencias de Arduino). Mismas constantes, mismos coeficientes, misma topología.
Cross-validation tests: pytest carga el módulo Python directamente y un wrapper ctypes del PID C++ compilado, los corre contra el mismo simulador y verifica diferencia < 1e-3 en la trayectoria.
Tests Unity host-side del PID C++ (corren cuando hay g++/clang).
Wire al firmware: el pid_inner_task ya estaba creado como stub en task_config.h; ahora hace algo útil. Lee setpoint de Modbus (HOLDING_HEADING_SETPOINT_X100 reasignado pro-tem para "rudder setpoint", se separa correctamente en Sprint 3), lee posición de hal::rudder_sensor_latest(), comanda hal::rudder_command(pwm_pct).
Modbus expone: ganancias activas (read), setpoint actual (RW), error instantáneo (read), salida PID en % (read).

Salvaguardas implementadas (todas obligatorias por brief §6)

Anti-windup: integrador clamped a [-anti_windup_limit, +anti_windup_limit], además back-calculation cuando la salida satura
Deadband del setpoint: ±0.5° por defecto (configurable) — evita oscilación por ruido
Saturación de salida: comando en %, limitado a [-100, +100]
Rate limit del setpoint interno: 3-6 °/s típico (configurable)
Compensación de deadband del actuador: si |cmd| > 0, snap to min_useful_pwm_pct
Asimetría babor/estribor: ganancia asymmetry_stbd_over_port aplicada al lado correspondiente
Refuse-to-act en STANDBY (ya estaba desde Sprint 1)

Validación

pytest verde con tests Python contra el simulador (step response, disturbance rejection, deadband behaviour, saturation, anti-windup, asymmetry).
pio run -e esp32-dev compila clean.
Cross-validation Python ↔ C++: trayectorias coinciden < 1e-3.

Lo que NO hace Sprint 2

No es lazo de rumbo. Eso es Sprint 3 (PID outer).
No tiene gain scheduling por velocidad. Sprint 3.
No tiene ROT feed-forward. Sprint 3.
No usa modos distintos a STANDBY (sigue rechazando otros). Modes reales en Sprint 3 cuando el outer loop entra.

3.4 KiB Raw Permalink Blame History