alro65/AR-Autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sprint-2: PID inner loop + Python rudder simulator

Browse Source 
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:
Alvaro Enrique Romero Donado
2026-05-18 14:18:41 -04:00
parent 65860948b4
commit 295efa2d83
16 changed files with 1477 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
arautopilot/tests/test_pid_inner_python.py
+226
View File
@@ -0,0 +1,226 @@
"""Tests for ``arautopilot.studio.simulator.pid_inner`` against the rudder simulator."""
from __future__ import annotations
import pytest
from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig
from arautopilot.studio.simulator.rudder_dynamics import (
RudderDynamicsConfig,
RudderSimulator,
RunRecorder,
)
def _run(pid: PidInner, sim: RudderSimulator, *, setpoint_deg: float,
seconds: float, allowed: bool = True, dt_inner: float | None = None,
dt_sim: float = 0.002) -> RunRecorder:
"""Drive ``sim`` at ``dt_sim`` while ticking ``pid`` at its own rate."""
if dt_inner is None:
dt_inner = pid.config.dt()
rec = RunRecorder()
pid_carry = 0.0
cmd = 0.0
t = 0.0
steps = int(seconds / dt_sim)
for _ in range(steps):
pid_carry += dt_sim
if pid_carry + 1e-12 >= dt_inner:
cmd = pid.step(setpoint_deg=setpoint_deg,
measured_deg=sim.state.angle_deg,
allowed=allowed)
pid_carry -= dt_inner
sim.step(dt=dt_sim, pwm_pct=cmd)
t += dt_sim
rec.record(t=t, setpoint_deg=setpoint_deg, sim=sim, pwm_pct=cmd)
return rec
def _aggressive_pid() -> PidInner:
"""Gains tuned for the marine-realistic simulator (v_max ~5 dps).
With slow rudder dynamics (full deflection in ~7 s) we want the PID
to drive the actuator at max for a couple of seconds, then back off
smoothly. kp dominates; ki small to clean up steady-state bias from
any external torque; kd light because the rudder is heavily damped.
"""
return PidInner(PidInnerConfig(
kp=10.0, ki=0.5, kd=0.5,
deadband_deg=0.2,
rate_limit_dps=10000.0, # disable rate limit in unit tests
deadband_pct=0.0,
min_useful_pwm_pct=0.0,
integral_clamp=200.0,
))
def test_step_response_settles_near_setpoint() -> None:
"""20 deg step with marine-realistic v_max ~5 dps should settle in ~10 s."""
sim = RudderSimulator()
sim.reset()
pid = _aggressive_pid()
pid.reset(measured_deg=0.0, setpoint_deg=0.0)
rec = _run(pid, sim, setpoint_deg=20.0, seconds=12.0)
final = rec.final_angle()
assert final == pytest.approx(20.0, abs=1.0), f"final angle {final}, want ~20"
def test_negative_step_response() -> None:
sim = RudderSimulator()
sim.reset()
pid = _aggressive_pid()
pid.reset(measured_deg=0.0, setpoint_deg=0.0)
rec = _run(pid, sim, setpoint_deg=-15.0, seconds=12.0)
assert rec.final_angle() == pytest.approx(-15.0, abs=1.0)
def test_setpoint_deadband_holds_when_close() -> None:
"""If the working setpoint stays within the deadband of the measurement,
no command is produced and the rudder doesn't move."""
sim = RudderSimulator()
sim.reset(angle_deg=10.0)
cfg = PidInnerConfig(kp=10.0, ki=0.5, kd=0.5,
deadband_deg=2.0,
rate_limit_dps=10000.0,
deadband_pct=0.0,
min_useful_pwm_pct=0.0)
pid = PidInner(cfg)
# Critically: initialise the working setpoint to the current measurement
# so the rate limiter doesn't pull it through the deadband from 0.
pid.reset(measured_deg=10.0, setpoint_deg=10.0)
rec = _run(pid, sim, setpoint_deg=11.0, seconds=2.0)
final = rec.final_angle()
assert final == pytest.approx(10.0, abs=0.5), f"deadband not holding, final={final}"
def test_anti_windup_keeps_integrator_bounded_under_saturation() -> None:
"""Park the rudder at the end-stop with a far setpoint -> output saturates
indefinitely. The integrator must not run away."""
sim = RudderSimulator()
sim.reset(angle_deg=35.0)
pid = PidInner(PidInnerConfig(
kp=8.0, ki=10.0, kd=0.5,
deadband_deg=0.0,
rate_limit_dps=200.0,
deadband_pct=0.0,
min_useful_pwm_pct=0.0,
integral_clamp=20.0,
))
pid.reset(measured_deg=35.0)
_ = _run(pid, sim, setpoint_deg=80.0, seconds=2.0)
assert abs(pid.state.integral) <= pid.config.integral_clamp + 1e-6
def test_allowed_false_zeros_output_and_bleeds_integrator() -> None:
pid = _aggressive_pid()
pid.reset(measured_deg=0.0)
pid.state.integral = 5.0
out = pid.step(setpoint_deg=20.0, measured_deg=0.0, allowed=False)
assert out == 0.0
assert abs(pid.state.integral) < 5.0
def test_actuator_deadband_compensation_skips_band() -> None:
"""Below deadband_pct=10 the command should be zero; just above, snapped to 15."""
pid = PidInner(PidInnerConfig(
kp=1.0, ki=0.0, kd=0.0,
deadband_deg=0.0,
rate_limit_dps=10000.0, # disable -- testing single tick
deadband_pct=10.0,
min_useful_pwm_pct=15.0,
))
pid.reset(measured_deg=0.0)
# error=5 -> raw_output=5 -> below deadband -> 0
assert pid.step(setpoint_deg=5.0, measured_deg=0.0) == 0.0
pid.reset(measured_deg=0.0)
# error=11 -> raw_output=11 -> snapped to 15
out = pid.step(setpoint_deg=11.0, measured_deg=0.0)
assert out == pytest.approx(15.0, abs=1e-6)
pid.reset(measured_deg=0.0)
# error=-11 -> snapped to -15
out = pid.step(setpoint_deg=-11.0, measured_deg=0.0)
assert out == pytest.approx(-15.0, abs=1e-6)
def test_asymmetry_divides_stbd_command() -> None:
"""If the pump is intrinsically faster to stbd (asymmetry=1.5), the PID
should divide its starboard command by 1.5 to compensate."""
pid = PidInner(PidInnerConfig(
kp=1.0, ki=0.0, kd=0.0,
deadband_deg=0.0,
rate_limit_dps=10000.0, # disable -- testing single tick
deadband_pct=0.0,
min_useful_pwm_pct=0.0,
asymmetry_stbd_over_port=1.5,
))
pid.reset(measured_deg=0.0)
out = pid.step(setpoint_deg=30.0, measured_deg=0.0)
# raw_output = 30 -> divided by 1.5 -> 20
assert out == pytest.approx(20.0, abs=1e-6)
pid.reset(measured_deg=0.0)
out = pid.step(setpoint_deg=-30.0, measured_deg=0.0)
# raw_output = -30 -> port multiplied -> -45 (within saturation)
assert out == pytest.approx(-45.0, abs=1e-6)
def test_output_saturation_at_plus_minus_100() -> None:
pid = PidInner(PidInnerConfig(
kp=50.0, ki=0.0, kd=0.0,
deadband_deg=0.0,
rate_limit_dps=10000.0,
deadband_pct=0.0,
min_useful_pwm_pct=0.0,
))
pid.reset(measured_deg=0.0)
out = pid.step(setpoint_deg=10.0, measured_deg=0.0)
assert out == 100.0
pid.reset(measured_deg=0.0)
out = pid.step(setpoint_deg=-10.0, measured_deg=0.0)
assert out == -100.0
def test_rate_limit_caps_setpoint_slew() -> None:
"""With rate_limit_dps=5 and dt=0.02, the working setpoint should advance
by no more than 0.1 deg per tick toward a step of 50 deg."""
pid = PidInner(PidInnerConfig(
kp=1.0, ki=0.0, kd=0.0,
deadband_deg=0.0,
rate_limit_dps=5.0,
deadband_pct=0.0,
min_useful_pwm_pct=0.0,
freq_hz=50.0,
))
pid.reset(measured_deg=0.0, setpoint_deg=0.0)
# First tick: working setpoint can grow by 5 * 0.02 = 0.1 deg.
out1 = pid.step(setpoint_deg=50.0, measured_deg=0.0)
assert out1 == pytest.approx(0.1, abs=1e-6)
out2 = pid.step(setpoint_deg=50.0, measured_deg=0.0)
assert out2 == pytest.approx(0.2, abs=1e-6)
def test_disturbance_rejection() -> None:
"""Apply a constant external torque; the controller should still settle
near the setpoint, with the integrator absorbing the bias.
External torque +1.0 with actuator_gain=0.2 means the controller needs
a steady-state command of ~-5 % to keep position. The integrator gets
there by integrating the residual error -- with ki=2.0 it converges
within ~25 s.
"""
sim = RudderSimulator(RudderDynamicsConfig(external_torque=1.0))
sim.reset()
# Use higher integral gain than the default _aggressive_pid() for faster
# disturbance rejection in the test window.
pid = PidInner(PidInnerConfig(
kp=10.0, ki=2.0, kd=0.5,
deadband_deg=0.2,
rate_limit_dps=10000.0,
deadband_pct=0.0,
min_useful_pwm_pct=0.0,
integral_clamp=200.0,
))
pid.reset(measured_deg=0.0, setpoint_deg=0.0)
rec = _run(pid, sim, setpoint_deg=10.0, seconds=25.0)
assert rec.final_angle() == pytest.approx(10.0, abs=1.0), (
f"final angle {rec.final_angle()}, want ~10"
)
arautopilot/tests/test_rudder_simulator.py
+117
View File
@@ -0,0 +1,117 @@
"""Tests for ``arautopilot.studio.simulator.rudder_dynamics``."""
from __future__ import annotations
import pytest
from arautopilot.studio.simulator.rudder_dynamics import (
RudderDynamicsConfig,
RudderSimulator,
RunRecorder,
)
def test_zero_input_stays_at_rest() -> None:
sim = RudderSimulator()
sim.reset(angle_deg=0.0)
for _ in range(1000):
sim.step(dt=0.001, pwm_pct=0.0)
assert sim.state.angle_deg == pytest.approx(0.0, abs=1e-9)
assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-9)
def test_full_starboard_command_drives_to_stbd_endstop() -> None:
"""With marine-realistic gain (v_max ~5 dps) reaching 35 deg takes ~8 s."""
sim = RudderSimulator()
sim.reset(angle_deg=0.0)
for _ in range(15000):
sim.step(dt=0.001, pwm_pct=100.0)
assert sim.state.angle_deg == pytest.approx(sim.config.max_angle_deg, abs=1e-6)
assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-6)
def test_full_port_command_drives_to_port_endstop() -> None:
sim = RudderSimulator()
sim.reset(angle_deg=0.0)
for _ in range(15000):
sim.step(dt=0.001, pwm_pct=-100.0)
assert sim.state.angle_deg == pytest.approx(-sim.config.max_angle_deg, abs=1e-6)
assert sim.state.angular_vel_dps == pytest.approx(0.0, abs=1e-6)
def test_command_inside_deadband_produces_no_motion() -> None:
sim = RudderSimulator(RudderDynamicsConfig(deadband_pct=10.0,
min_useful_pwm_pct=15.0))
sim.reset(angle_deg=0.0)
for _ in range(2000):
sim.step(dt=0.001, pwm_pct=5.0) # under deadband
assert sim.state.angle_deg == pytest.approx(0.0, abs=1e-9)
def test_command_just_above_deadband_snaps_to_min_useful() -> None:
"""A command of 11% with deadband=10/min_useful=15 should act like 15%."""
cfg = RudderDynamicsConfig(deadband_pct=10.0, min_useful_pwm_pct=15.0)
sim_a = RudderSimulator(cfg)
sim_b = RudderSimulator(cfg)
sim_a.reset()
sim_b.reset()
for _ in range(3000):
sim_a.step(dt=0.001, pwm_pct=11.0)
sim_b.step(dt=0.001, pwm_pct=15.0)
assert sim_a.state.angle_deg == pytest.approx(sim_b.state.angle_deg, abs=1e-9)
def test_asymmetry_makes_starboard_faster() -> None:
cfg = RudderDynamicsConfig(asymmetry_stbd_over_port=1.5,
deadband_pct=0.0, min_useful_pwm_pct=0.0)
sim_s = RudderSimulator(cfg)
sim_p = RudderSimulator(cfg)
sim_s.reset()
sim_p.reset()
for _ in range(2000):
sim_s.step(dt=0.001, pwm_pct=+50.0)
sim_p.step(dt=0.001, pwm_pct=-50.0)
# Starboard angle should be larger in magnitude than port at the same
# effort, because the pump pushes harder to starboard.
assert abs(sim_s.state.angle_deg) > abs(sim_p.state.angle_deg)
def test_recorder_captures_trajectory() -> None:
"""Sanity check the recorder: capture a few seconds of constant-command
motion and verify the samples reflect the rudder moving in the commanded
direction with strictly monotonic position (no oscillation under constant
drive)."""
sim = RudderSimulator()
sim.reset()
rec = RunRecorder()
for k in range(2000):
t = k * 0.001
sim.step(dt=0.001, pwm_pct=80.0)
rec.record(t=t, setpoint_deg=25.0, sim=sim, pwm_pct=80.0)
assert len(rec.samples) == 2000
# Monotonically increasing angle (constant +ve command + viscous damping
# means no overshoot during the run).
assert rec.samples[-1].angle_deg > rec.samples[0].angle_deg
assert all(rec.samples[i + 1].angle_deg >= rec.samples[i].angle_deg
for i in range(len(rec.samples) - 1))
def test_invalid_dt_rejected() -> None:
sim = RudderSimulator()
with pytest.raises(ValueError):
sim.step(dt=0.0, pwm_pct=50.0)
with pytest.raises(ValueError):
sim.step(dt=-0.001, pwm_pct=50.0)
def test_endstop_zeros_outward_velocity_only() -> None:
"""Hitting the stbd end-stop should kill +velocity but allow reverse."""
sim = RudderSimulator()
sim.reset(angle_deg=sim.config.max_angle_deg, angular_vel_dps=20.0)
sim.step(dt=0.001, pwm_pct=0.0)
assert sim.state.angle_deg == pytest.approx(sim.config.max_angle_deg, abs=1e-9)
assert sim.state.angular_vel_dps == 0.0
# Now reverse: should move away from the stop.
for _ in range(200):
sim.step(dt=0.001, pwm_pct=-80.0)
assert sim.state.angle_deg < sim.config.max_angle_deg