"""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" )