AR-Autopilot/arautopilot/tests/test_pid_inner_python.py

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