"""Tests for the Python PID outer loop, including the cascade with the inner PID + rudder simulator + vessel-heading simulator.""" from __future__ import annotations import pytest from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig from arautopilot.studio.simulator.pid_outer import ( GainPoint, PidOuter, PidOuterConfig, interpolate_gains, ) from arautopilot.studio.simulator.rudder_dynamics import ( RudderDynamicsConfig, RudderSimulator, ) from arautopilot.studio.simulator.vessel_heading import ( HeadingRunRecorder, VesselHeadingConfig, VesselHeadingSimulator, heading_error_deg, ) # ---------------------------------------------------------------------------- # Gain scheduling # ---------------------------------------------------------------------------- def test_interpolate_empty_schedule_returns_base() -> None: kp, ki, kd = interpolate_gains([], 10.0, 0.9, 0.02, 1.2) assert (kp, ki, kd) == (0.9, 0.02, 1.2) def test_interpolate_endpoint_hold() -> None: sched = [ GainPoint(5.0, 1.2, 0.03, 0.8), GainPoint(28.0, 0.55, 0.01, 1.8), ] assert interpolate_gains(sched, 0.0, 0, 0, 0) == (1.2, 0.03, 0.8) assert interpolate_gains(sched, 100.0, 0, 0, 0) == (0.55, 0.01, 1.8) def test_interpolate_midpoint() -> None: sched = [ GainPoint(5.0, 1.0, 0.04, 0.8), GainPoint(15.0, 0.5, 0.02, 1.2), ] kp, ki, kd = interpolate_gains(sched, 10.0, 0, 0, 0) assert kp == pytest.approx(0.75) assert ki == pytest.approx(0.03) assert kd == pytest.approx(1.0) # ---------------------------------------------------------------------------- # Standalone outer-PID behaviour # ---------------------------------------------------------------------------- def test_zero_error_in_deadband_produces_zero_output() -> None: pid = PidOuter(PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0, deadband_deg=2.0, rot_ff_gain=0.0)) out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=101.0, rate_of_turn_dps=0.0, speed_kn=10.0) assert out == 0.0 def test_positive_error_commands_starboard_rudder() -> None: pid = PidOuter(PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0, deadband_deg=0.0, rot_ff_gain=0.0, rate_limit_dps=10000.0)) out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0, rate_of_turn_dps=0.0, speed_kn=10.0) assert out > 0.0 def test_rot_feed_forward_subtracts_rotation() -> None: """With positive ROT (already turning stbd) the outer PID should ease off the rudder compared to ROT=0 -- the same error commands less rudder.""" cfg_no_ff = PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0, deadband_deg=0.0, rot_ff_gain=0.0, rate_limit_dps=10000.0) cfg_with_ff = PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0, deadband_deg=0.0, rot_ff_gain=3.0, rate_limit_dps=10000.0) pid_a = PidOuter(cfg_no_ff) pid_b = PidOuter(cfg_with_ff) a = pid_a.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0, rate_of_turn_dps=5.0, speed_kn=10.0) b = pid_b.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0, rate_of_turn_dps=5.0, speed_kn=10.0) assert a > b # ROT feed-forward eases rudder when already turning stbd def test_output_saturates_to_max_rudder() -> None: pid = PidOuter(PidOuterConfig(base_kp=50.0, base_ki=0.0, base_kd=0.0, deadband_deg=0.0, rot_ff_gain=0.0, max_rudder_deg=35.0, rate_limit_dps=10000.0)) out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0, rate_of_turn_dps=0.0, speed_kn=10.0) assert out == 35.0 def test_rate_limit_caps_rudder_setpoint_slew() -> None: """With rate_limit_dps=5 and dt=0.1, the rudder setpoint can change at most 0.5 deg per tick.""" pid = PidOuter(PidOuterConfig(base_kp=10.0, base_ki=0.0, base_kd=0.0, deadband_deg=0.0, rot_ff_gain=0.0, rate_limit_dps=5.0, freq_hz=10.0)) out1 = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0, rate_of_turn_dps=0.0, speed_kn=10.0) assert out1 == pytest.approx(0.5, abs=1e-6) out2 = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0, rate_of_turn_dps=0.0, speed_kn=10.0) assert out2 == pytest.approx(1.0, abs=1e-6) def test_allowed_false_bleeds_integrator() -> None: pid = PidOuter() pid.state.integral = 10.0 out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=80.0, rate_of_turn_dps=0.0, speed_kn=10.0, allowed=False) assert out == 0.0 assert abs(pid.state.integral) < 10.0 # ---------------------------------------------------------------------------- # End-to-end cascade: outer + inner + rudder dynamics + vessel heading # ---------------------------------------------------------------------------- def _run_cascade( setpoint_deg: float, *, initial_heading: float = 0.0, seconds: float = 60.0, speed_kn: float = 10.0, ) -> tuple[HeadingRunRecorder, VesselHeadingSimulator, RudderSimulator]: vessel = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=speed_kn)) vessel.reset(heading_deg=initial_heading) rudder = RudderSimulator() rudder.reset() outer = PidOuter(PidOuterConfig( base_kp=1.5, base_ki=0.04, base_kd=2.0, deadband_deg=0.2, rot_ff_gain=2.0, max_rudder_deg=35.0, rate_limit_dps=30.0, freq_hz=10.0, )) inner = PidInner(PidInnerConfig( kp=15.0, ki=2.0, kd=1.0, deadband_deg=0.1, rate_limit_dps=10000.0, deadband_pct=0.0, min_useful_pwm_pct=0.0, integral_clamp=200.0, freq_hz=50.0, )) outer.reset() inner.reset() rec = HeadingRunRecorder() dt_sim = 0.002 # 500 Hz physics dt_inner = inner.config.dt() # 50 Hz dt_outer = outer.config.dt() # 10 Hz rudder_setpoint = 0.0 inner_carry = 0.0 outer_carry = 0.0 cmd = 0.0 t = 0.0 steps = int(seconds / dt_sim) for _ in range(steps): outer_carry += dt_sim if outer_carry + 1e-12 >= dt_outer: rudder_setpoint = outer.step( heading_setpoint_deg=setpoint_deg, heading_measured_deg=vessel.state.heading_deg, rate_of_turn_dps=vessel.state.rate_of_turn_dps, speed_kn=speed_kn, ) outer_carry -= dt_outer inner_carry += dt_sim if inner_carry + 1e-12 >= dt_inner: cmd = inner.step(setpoint_deg=rudder_setpoint, measured_deg=rudder.state.angle_deg) inner_carry -= dt_inner rudder.step(dt=dt_sim, pwm_pct=cmd) vessel.step(dt=dt_sim, rudder_deg=rudder.state.angle_deg, speed_kn=speed_kn) t += dt_sim rec.record(t=t, setpoint_deg=setpoint_deg, heading_sim=vessel, rudder_setpoint_deg=rudder_setpoint, rudder_actual_deg=rudder.state.angle_deg) return rec, vessel, rudder def test_cascade_settles_on_30_deg_step() -> None: """A 30 deg heading change should settle within +-2 deg of target in 60 s using the cascade outer + inner + rudder dynamics + vessel heading.""" rec, _, _ = _run_cascade(30.0, initial_heading=0.0, seconds=60.0) final_err = rec.final_heading_error_deg(30.0) assert final_err <= 2.0, f"final heading error {final_err} deg, want <= 2.0" def test_cascade_settles_on_negative_step() -> None: """Heading change from 90 to 60 deg (turn -30) should also converge.""" rec, _, _ = _run_cascade(60.0, initial_heading=90.0, seconds=60.0) final_err = rec.final_heading_error_deg(60.0) assert final_err <= 2.0, f"final heading error {final_err} deg, want <= 2.0" def test_cascade_crosses_360_correctly() -> None: """From heading 350 to setpoint 10: should turn 20 deg starboard, not 340 deg port. End within +-4 deg of 10 (wrap-around is the harder convergence case because the rudder reverses near the crossing).""" rec, _, _ = _run_cascade(10.0, initial_heading=350.0, seconds=60.0) final_err = rec.final_heading_error_deg(10.0) assert final_err <= 4.0, f"final heading error {final_err} deg, want <= 4.0" # Rotation direction: the recorded ROT samples should be predominantly # positive (starboard) -- mean over the run. mean_rot = sum(s.rot_dps for s in rec.samples) / len(rec.samples) assert mean_rot > 0.0