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>
This commit is contained in:
2026-05-18 14:18:41 -04:00
parent 65860948b4
commit 295efa2d83
16 changed files with 1477 additions and 6 deletions
+10
View File
@@ -78,6 +78,12 @@ INPUTS: dict[str, Reg] = {
"HEADING_AGE_MS": Reg(addr=26, name="HEADING_AGE_MS", desc='Milliseconds since the last heading update (0..60000)', unit="ms", scale=1.0, offset=0.0),
"BATTERY_VOLTAGE_X100": Reg(addr=32, name="BATTERY_VOLTAGE_X100", desc='System battery voltage, V*100', unit="V", scale=0.01, offset=0.0),
"ACTUATOR_CURRENT_X100": Reg(addr=33, name="ACTUATOR_CURRENT_X100", desc='Actuator current, A*100', unit="A", scale=0.01, offset=0.0),
"PID_INNER_SETPOINT_X100": Reg(addr=40, name="PID_INNER_SETPOINT_X100", desc='Inner-loop rudder setpoint, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_OUTPUT_X100": Reg(addr=41, name="PID_INNER_OUTPUT_X100", desc='Last PID command, %*100 (signed int16, -10000..+10000)', unit="%", scale=0.01, offset=0.0),
"PID_INNER_ERROR_X100": Reg(addr=42, name="PID_INNER_ERROR_X100", desc='Last PID error, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_KP_X1000": Reg(addr=43, name="PID_INNER_KP_X1000", desc='Inner-loop kp * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KI_X1000": Reg(addr=44, name="PID_INNER_KI_X1000", desc='Inner-loop ki * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KD_X1000": Reg(addr=45, name="PID_INNER_KD_X1000", desc='Inner-loop kd * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
}
# HOLDINGS
@@ -87,6 +93,10 @@ HOLDINGS: dict[str, Reg] = {
"BRIGHTNESS_PCT": Reg(addr=2, name="BRIGHTNESS_PCT", desc='Display brightness 0..100', unit="%", scale=1.0, offset=0.0),
"ALARM_VOLUME_PCT": Reg(addr=3, name="ALARM_VOLUME_PCT", desc='Alarm volume 0..100', unit="%", scale=1.0, offset=0.0),
"DODGE_OFFSET_DEG_X100": Reg(addr=8, name="DODGE_OFFSET_DEG_X100", desc='Dodge mode heading offset, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_SETPOINT_REQ_X100": Reg(addr=16, name="PID_INNER_SETPOINT_REQ_X100", desc='Requested inner-loop rudder setpoint, deg*100 (signed int16)', unit="deg", scale=0.01, offset=0.0),
"PID_INNER_KP_REQ_X1000": Reg(addr=17, name="PID_INNER_KP_REQ_X1000", desc='Requested inner-loop kp * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KI_REQ_X1000": Reg(addr=18, name="PID_INNER_KI_REQ_X1000", desc='Requested inner-loop ki * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
"PID_INNER_KD_REQ_X1000": Reg(addr=19, name="PID_INNER_KD_REQ_X1000", desc='Requested inner-loop kd * 1000 (unsigned)', unit="", scale=0.001, offset=0.0),
}
ALL_GROUPS: dict[str, dict[str, Reg]] = {