sprint-5: True Course + Track Keeping + XTE + PGN 129026/129284

- Python: NmeaNavData (COG/SOG/XTE data models with staleness tracking)
- Python: TrueCoursePilot with TRUE_COURSE and TRACK_KEEPING modes
- Python: 26 new tests (test_nmea_data, test_true_course)
- Modbus: COG/SOG/XTE input registers + TC setpoint/XTE-gain holdings
- Firmware: nmea2000_consumer handles PGN 129026 + 129284
- Firmware: pid_outer_task wired for TC + TK modes with live SOG scheduling
- YAML regenerated; 284 tests pass, firmware compiles clean

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

arautopilot/studio/simulator/true_course.py

@@ -0,0 +1,170 @@
+"""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