AR-Autopilot/arautopilot/studio/simulator/rudder_dynamics.py

"""Minimal physical model of a hydraulic rudder actuator.

The state is the rudder angle in degrees and its angular velocity in deg/s.
The driver is a signed PWM command in percent (-100..+100):

    pwm > 0  -> drive starboard (positive angular acceleration)
    pwm < 0  -> drive port      (negative angular acceleration)
    pwm == 0 -> no torque from the actuator (still subject to friction)

The model is intentionally small: it must capture the qualitative behaviour
that makes a PID interesting (inertia, friction, deadband, asymmetry,
end-stop saturation) but it does NOT pretend to be a faithful CFD of any
real pump. The integrator's affinated tunings live elsewhere.

Equations (per integration step ``dt``):

    effective_cmd = compensate_deadband_and_asymmetry(pwm)
    torque        = actuator_gain * effective_cmd
    accel         = (torque - friction * angular_vel) / inertia
    angular_vel  += accel * dt
    angle        += angular_vel * dt
    # then clamp to [-max_angle_deg, +max_angle_deg], zeroing velocity at the stop

Units throughout:
    angle      [deg]
    velocity   [deg / s]
    accel      [deg / s^2]
    torque     [arbitrary scaled units; absorbed into actuator_gain]
    friction   [(deg/s) -> (deg/s^2)], i.e. viscous coefficient
"""

from __future__ import annotations

from dataclasses import dataclass, field


@dataclass
class RudderDynamicsConfig:
    """Tunable parameters of the rudder physical model.

    Defaults are reasonable for a 30 m motor yacht with a reversible
    hydraulic pump (calibration profile shipped in the seed library).
    """

    # --- Mechanical ---------------------------------------------------------
    max_angle_deg: float = 35.0
    """Mechanical end-stop on either side, degrees."""

    inertia: float = 1.0
    """Rotational inertia (arbitrary units; absorbed with actuator_gain)."""

    friction: float = 4.0
    """Viscous friction coefficient (higher = more damping)."""

    # --- Actuator -----------------------------------------------------------
    # Tuned so that 100 % PWM produces a steady-state angular velocity of
    # ~5 deg/s, which matches the brief's "typical 3-6 dps" max rate for a
    # 30 m yacht hydraulic pump:
    #   v_steady = (actuator_gain * 100) / friction = (0.2 * 100) / 4 = 5
    actuator_gain: float = 0.2
    """Torque-equivalent per percent of PWM. Marine-realistic for a 30 m yacht."""

    deadband_pct: float = 7.0
    """Below this absolute command, the pump produces no torque (static friction)."""

    min_useful_pwm_pct: float = 12.0
    """Once above the deadband, the effective PWM is snapped up to this minimum."""

    asymmetry_stbd_over_port: float = 1.0
    """Ratio of starboard speed to port speed; 1.0 = symmetric."""

    # --- Optional disturbance -----------------------------------------------
    external_torque: float = 0.0
    """Constant external torque (e.g. simulating wave action). 0 by default."""


@dataclass
class RudderState:
    angle_deg: float = 0.0
    angular_vel_dps: float = 0.0


class RudderSimulator:
    """Discrete-time integrator of the rudder model.

    Typical usage::

        sim = RudderSimulator()
        sim.reset(angle_deg=0.0)
        for k in range(1000):
            sim.step(dt=0.001, pwm_pct=cmd)
            print(sim.state.angle_deg)
    """

    def __init__(self, config: RudderDynamicsConfig | None = None) -> None:
        self.config: RudderDynamicsConfig = config or RudderDynamicsConfig()
        self.state: RudderState = RudderState()

    # -- Lifecycle -----------------------------------------------------------
    def reset(self, *, angle_deg: float = 0.0, angular_vel_dps: float = 0.0) -> None:
        self.state = RudderState(angle_deg=angle_deg, angular_vel_dps=angular_vel_dps)

    # -- Integration ---------------------------------------------------------
    def step(self, *, dt: float, pwm_pct: float) -> RudderState:
        """Integrate the model forward by ``dt`` seconds with constant ``pwm_pct``."""
        if dt <= 0.0:
            raise ValueError(f"dt must be > 0, got {dt}")
        cfg = self.config
        st = self.state

        effective = self._compensate(pwm_pct)
        torque = cfg.actuator_gain * effective + cfg.external_torque
        accel = (torque - cfg.friction * st.angular_vel_dps) / cfg.inertia

        st.angular_vel_dps += accel * dt
        st.angle_deg += st.angular_vel_dps * dt

        # End-stop saturation.
        if st.angle_deg > cfg.max_angle_deg:
            st.angle_deg = cfg.max_angle_deg
            if st.angular_vel_dps > 0.0:
                st.angular_vel_dps = 0.0
        elif st.angle_deg < -cfg.max_angle_deg:
            st.angle_deg = -cfg.max_angle_deg
            if st.angular_vel_dps < 0.0:
                st.angular_vel_dps = 0.0

        return st

    # -- Actuator non-linearity ---------------------------------------------
    def _compensate(self, pwm_pct: float) -> float:
        """Apply deadband + min-useful-PWM + asymmetry.

        Returned units: signed effective PWM in percent. Sign convention
        matches the input.
        """
        cfg = self.config
        if pwm_pct == 0.0:
            return 0.0
        magnitude = abs(pwm_pct)
        if magnitude <= cfg.deadband_pct:
            return 0.0
        if magnitude < cfg.min_useful_pwm_pct:
            magnitude = cfg.min_useful_pwm_pct
        sign = 1.0 if pwm_pct > 0.0 else -1.0
        effective = sign * magnitude
        # Asymmetry: starboard (positive) is multiplied; port is divided so
        # the symmetric case asymmetry=1.0 leaves both sides untouched.
        if effective > 0.0:
            effective *= cfg.asymmetry_stbd_over_port
        elif cfg.asymmetry_stbd_over_port != 0.0:
            effective /= cfg.asymmetry_stbd_over_port
        return effective


@dataclass
class TrajectorySample:
    """One row of a recorded run."""

    t: float
    setpoint_deg: float
    angle_deg: float
    angular_vel_dps: float
    pwm_pct: float


@dataclass
class RunRecorder:
    """Capture a (time, setpoint, angle, vel, cmd) trace for plotting/tests."""

    samples: list[TrajectorySample] = field(default_factory=list)

    def record(self, *, t: float, setpoint_deg: float, sim: RudderSimulator,
               pwm_pct: float) -> None:
        self.samples.append(
            TrajectorySample(
                t=t,
                setpoint_deg=setpoint_deg,
                angle_deg=sim.state.angle_deg,
                angular_vel_dps=sim.state.angular_vel_dps,
                pwm_pct=pwm_pct,
            )
        )

    def final_angle(self) -> float:
        return self.samples[-1].angle_deg if self.samples else 0.0

    def settling_time_s(self, target: float, tolerance_deg: float = 0.5) -> float | None:
        """Return time at which ``|angle - target| <= tolerance`` and stays there
        until the end of the run. ``None`` if it never settles."""
        if not self.samples:
            return None
        # Walk backwards: find the last time the error exceeded tolerance.
        for i in range(len(self.samples) - 1, -1, -1):
            if abs(self.samples[i].angle_deg - target) > tolerance_deg:
                if i + 1 < len(self.samples):
                    return self.samples[i + 1].t
                return None
        return self.samples[0].t

    def max_overshoot_deg(self, target: float) -> float:
        """Maximum overshoot past ``target`` in the direction of the step."""
        if not self.samples:
            return 0.0
        start = self.samples[0].angle_deg
        going_up = target > start
        peak = max((s.angle_deg for s in self.samples), default=start) if going_up \
            else min((s.angle_deg for s in self.samples), default=start)
        overshoot = (peak - target) if going_up else (target - peak)
        return max(0.0, overshoot)