alro65
295efa2d83
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>
75 lines
3.4 KiB
Markdown
75 lines
3.4 KiB
Markdown
# 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:
|
|
|
|
1. **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).
|
|
2. **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.
|
|
3. **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.
|
|
4. **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.
|
|
5. **Tests Unity host-side** del PID C++ (corren cuando hay g++/clang).
|
|
6. **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)`.
|
|
7. **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.
|