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

"""True Course and Track Keeping pilot -- Sprint 5.

TRUE_COURSE mode
----------------
Holds a fixed Course Over Ground (COG) instead of compass heading.
This compensates for current/wind drift: if the vessel is pushed sideways,
COG drifts and the outer PID corrects heading to restore the intended track.

Input:  cog_setpoint_deg, measured_cog_deg, sog_kn, rot_dps
Output: rudder_setpoint_deg  (passed to inner PID)

TRACK_KEEPING mode
------------------
Follows an ECDIS/chart-plotter route using XTE feedback.
An XTE proportional term adds a heading correction to the COG setpoint:

    xte_correction_deg = -xte_gain * xte_m          (clamped ± max_correction)
    effective_setpoint  = cog_setpoint_deg + xte_correction_deg

Sign convention (NMEA 0183 / IEC 61162-1):
    XTE > 0  → vessel is to starboard of track → steer port  → negative correction
    XTE < 0  → vessel is to port of track      → steer stbd  → positive correction

The correction is clamped so the autopilot cannot be tricked into dangerous
large heading swings by a stale or erroneous XTE feed.
"""

from __future__ import annotations

import math
from dataclasses import dataclass, field

from arautopilot.studio.simulator.pid_outer import (
    PidOuter,
    PidOuterConfig,
    PidOuterState,
)
from arautopilot.studio.simulator.vessel_heading import heading_error_deg


@dataclass
class TrueCourseConfig:
    """Configuration for both TRUE_COURSE and TRACK_KEEPING modes."""

    # Inner PID outer loop config (reused — same dynamics, different input)
    pid: PidOuterConfig = field(default_factory=PidOuterConfig)

    # XTE correction parameters (TRACK_KEEPING only)
    xte_gain: float = 0.5          # deg of heading correction per metre of XTE
    xte_max_correction_deg: float = 20.0   # hard clamp on XTE term
    xte_max_age_s: float = 5.0     # treat XTE as stale beyond this


@dataclass
class TrueCourseState:
    xte_correction_deg: float = 0.0
    effective_setpoint_deg: float = 0.0


class TrueCoursePilot:
    """Outer-loop pilot for TRUE_COURSE and TRACK_KEEPING modes.

    Wraps PidOuter, substituting COG for heading and optionally adding
    an XTE proportional correction to the setpoint.
    """

    def __init__(self, config: TrueCourseConfig | None = None) -> None:
        self.config = config or TrueCourseConfig()
        self._pid = PidOuter(self.config.pid)
        self.tc_state = TrueCourseState()

    def reset(self) -> None:
        self._pid.reset()
        self.tc_state = TrueCourseState()

    def update_config(self, config: TrueCourseConfig) -> None:
        self.config = config
        self._pid.update_config(config.pid)

    # ------------------------------------------------------------------
    #  TRUE_COURSE step
    # ------------------------------------------------------------------

    def step_true_course(
        self,
        *,
        cog_setpoint_deg: float,
        measured_cog_deg: float,
        sog_kn: float,
        rot_dps: float = 0.0,
        allowed: bool = True,
    ) -> float:
        """Pure COG-hold step (no XTE).  Returns rudder setpoint (deg)."""
        self.tc_state.effective_setpoint_deg = cog_setpoint_deg
        self.tc_state.xte_correction_deg = 0.0
        return self._pid.step(
            heading_setpoint_deg=cog_setpoint_deg,
            heading_measured_deg=measured_cog_deg,
            rate_of_turn_dps=rot_dps,
            speed_kn=sog_kn,
            allowed=allowed,
        )

    # ------------------------------------------------------------------
    #  TRACK_KEEPING step
    # ------------------------------------------------------------------

    def step_track_keeping(
        self,
        *,
        cog_setpoint_deg: float,
        measured_cog_deg: float,
        sog_kn: float,
        rot_dps: float = 0.0,
        xte_m: float = math.nan,
        xte_age_s: float = 0.0,
        allowed: bool = True,
    ) -> float:
        """COG-hold with XTE correction.  Returns rudder setpoint (deg).

        If XTE is NaN or stale the correction is zeroed and the pilot
        falls back to pure COG hold — safer than going to STANDBY.
        """
        cfg = self.config
        xte_valid = (
            not math.isnan(xte_m)
            and xte_age_s < cfg.xte_max_age_s
        )

        if xte_valid:
            raw_correction = -cfg.xte_gain * xte_m
            correction = _clamp(
                raw_correction,
                -cfg.xte_max_correction_deg,
                cfg.xte_max_correction_deg,
            )
        else:
            correction = 0.0

        self.tc_state.xte_correction_deg = correction
        effective_sp = (cog_setpoint_deg + correction) % 360.0
        self.tc_state.effective_setpoint_deg = effective_sp

        return self._pid.step(
            heading_setpoint_deg=effective_sp,
            heading_measured_deg=measured_cog_deg,
            rate_of_turn_dps=rot_dps,
            speed_kn=sog_kn,
            allowed=allowed,
        )

    # ------------------------------------------------------------------
    #  Accessors
    # ------------------------------------------------------------------

    @property
    def pid_state(self) -> PidOuterState:
        return self._pid.state

    @property
    def last_rudder_setpoint_deg(self) -> float:
        return self._pid.state.last_output_deg


def _clamp(x: float, lo: float, hi: float) -> float:
    if x < lo:
        return lo
    if x > hi:
        return hi
    return x