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

"""Vessel heading dynamics: simplified yaw-rate model.

Used by Sprint 3 to validate the outer (heading-control) PID without a
real boat. Combine with ``RudderSimulator`` (Sprint 2) to get a full
two-stage cascade: outer PID -> rudder setpoint -> inner PID -> PWM ->
rudder dynamics -> rudder angle -> vessel yaw -> heading -> loop.

Model
-----

The simplest physically-honest yaw model for a displacement / planing
vessel under way is a first-order response of rate-of-turn (ROT) to
rudder angle, with the gain proportional to forward speed:

    yaw_response_dps  = rudder_response_gain * speed_kn * rudder_deg
    accel_yaw         = (yaw_response_dps - yaw_damping * rot) / yaw_inertia
    rot              += accel_yaw * dt
    heading          += rot * dt          (mod 360)

This captures the qualitative behaviour the outer PID needs to handle:

- More rudder -> faster turn (linear at low angles, saturates at large
  angles -- this simple model is linear so the outer PID's rate limit
  on the rudder setpoint must keep it inside the linear region).
- More speed -> more turn per degree of rudder (basis for SOG-based
  gain scheduling).
- The turn rate decays toward zero when the rudder is centred (damped
  first-order response).

Defaults are tuned for a 30 m motor yacht at cruise speed:

    rudder_response_gain * speed_kn = 1.0 * 10 = 10 dps per +-1 rad of
    rudder per second^2 of acceleration

which yields ~3 dps steady-state turn for a 5 deg rudder at 10 kn --
comparable to real yachts.
"""

from __future__ import annotations

from dataclasses import dataclass, field


@dataclass
class VesselHeadingConfig:
    yaw_inertia: float = 1.0
    """Effective rotational inertia of the vessel (dimensionless scaling)."""

    yaw_damping: float = 0.8
    """Viscous damping on yaw (higher = less coasting after the rudder centres)."""

    rudder_response_gain: float = 0.18
    """Yaw torque per (rudder_deg * speed_kn). Tuned so that 5 deg of rudder
    at 10 kn produces ~3 dps steady-state ROT."""

    speed_kn: float = 10.0
    """Default forward speed over ground in knots. Can be set per-step."""

    external_yaw_torque: float = 0.0
    """Constant external yaw torque (wind / current). 0 by default."""


@dataclass
class VesselHeadingState:
    heading_deg: float = 0.0          # 0..360
    rate_of_turn_dps: float = 0.0


class VesselHeadingSimulator:
    """Discrete-time integrator of the vessel yaw model."""

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

    def reset(self, *, heading_deg: float = 0.0,
              rate_of_turn_dps: float = 0.0) -> None:
        self.state = VesselHeadingState(
            heading_deg=_wrap_deg(heading_deg),
            rate_of_turn_dps=rate_of_turn_dps,
        )

    def step(self, *, dt: float, rudder_deg: float,
             speed_kn: float | None = None) -> VesselHeadingState:
        if dt <= 0.0:
            raise ValueError(f"dt must be > 0, got {dt}")
        cfg = self.config
        st = self.state
        v_kn = cfg.speed_kn if speed_kn is None else speed_kn
        yaw_response = cfg.rudder_response_gain * v_kn * rudder_deg
        accel = (yaw_response + cfg.external_yaw_torque
                 - cfg.yaw_damping * st.rate_of_turn_dps) / cfg.yaw_inertia
        st.rate_of_turn_dps += accel * dt
        st.heading_deg = _wrap_deg(st.heading_deg + st.rate_of_turn_dps * dt)
        return st


def _wrap_deg(deg: float) -> float:
    """Wrap a heading into [0, 360)."""
    return deg % 360.0


def heading_error_deg(setpoint_deg: float, measured_deg: float) -> float:
    """Signed shortest-arc error between two compass headings.

    Result is in (-180, +180]. Positive means "we should turn starboard
    (clockwise)" to reduce the error -- this matches the marine
    convention of positive ROT = turning starboard.
    """
    delta = (setpoint_deg - measured_deg) % 360.0
    if delta > 180.0:
        delta -= 360.0
    return delta


@dataclass
class HeadingTrajectorySample:
    t: float
    setpoint_deg: float
    heading_deg: float
    rot_dps: float
    rudder_setpoint_deg: float
    rudder_actual_deg: float


@dataclass
class HeadingRunRecorder:
    samples: list[HeadingTrajectorySample] = field(default_factory=list)

    def record(
        self,
        *,
        t: float,
        setpoint_deg: float,
        heading_sim: VesselHeadingSimulator,
        rudder_setpoint_deg: float,
        rudder_actual_deg: float,
    ) -> None:
        self.samples.append(
            HeadingTrajectorySample(
                t=t,
                setpoint_deg=setpoint_deg,
                heading_deg=heading_sim.state.heading_deg,
                rot_dps=heading_sim.state.rate_of_turn_dps,
                rudder_setpoint_deg=rudder_setpoint_deg,
                rudder_actual_deg=rudder_actual_deg,
            )
        )

    def final_heading_error_deg(self, setpoint: float) -> float:
        if not self.samples:
            return 0.0
        return abs(heading_error_deg(setpoint, self.samples[-1].heading_deg))