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/tests/test_true_course.py

@@ -0,0 +1,180 @@
+"""Tests for TrueCoursePilot -- Sprint 5."""
+
+from __future__ import annotations
+
+import math
+import pytest
+
+from arautopilot.studio.simulator.true_course import TrueCoursePilot, TrueCourseConfig
+from arautopilot.studio.simulator.pid_outer import PidOuterConfig
+
+
+def _pilot(xte_gain: float = 0.5, max_corr: float = 20.0) -> TrueCoursePilot:
+    cfg = TrueCourseConfig(
+        pid=PidOuterConfig(base_kp=1.0, base_ki=0.0, base_kd=0.0,
+                           rot_ff_gain=0.0, rate_limit_dps=360.0),
+        xte_gain=xte_gain,
+        xte_max_correction_deg=max_corr,
+    )
+    return TrueCoursePilot(cfg)
+
+
+# ---------------------------------------------------------------------------
+#  TRUE_COURSE mode
+# ---------------------------------------------------------------------------
+
+class TestTrueCoursePilot:
+    def test_zero_error_zero_output(self):
+        p = _pilot()
+        out = p.step_true_course(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=90.0,
+            sog_kn=8.0,
+        )
+        assert out == pytest.approx(0.0, abs=0.6)
+
+    def test_positive_error_steer_starboard(self):
+        """COG is west of setpoint → steer to starboard (positive rudder)."""
+        p = _pilot()
+        out = p.step_true_course(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=80.0,   # 10° behind
+            sog_kn=8.0,
+        )
+        assert out > 0.0
+
+    def test_negative_error_steer_port(self):
+        p = _pilot()
+        out = p.step_true_course(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=100.0,
+            sog_kn=8.0,
+        )
+        assert out < 0.0
+
+    def test_360_boundary_crossing(self):
+        """Setpoint=5°, measured=355° → shortest arc = +10° → steer stbd."""
+        p = _pilot()
+        out = p.step_true_course(
+            cog_setpoint_deg=5.0,
+            measured_cog_deg=355.0,
+            sog_kn=8.0,
+        )
+        assert out > 0.0
+
+    def test_not_allowed_returns_zero(self):
+        p = _pilot()
+        out = p.step_true_course(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=80.0,
+            sog_kn=8.0,
+            allowed=False,
+        )
+        assert out == pytest.approx(0.0)
+
+    def test_xte_correction_zero_in_true_course_mode(self):
+        p = _pilot()
+        p.step_true_course(cog_setpoint_deg=90.0, measured_cog_deg=90.0, sog_kn=8.0)
+        assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
+
+
+# ---------------------------------------------------------------------------
+#  TRACK_KEEPING mode
+# ---------------------------------------------------------------------------
+
+class TestTrackKeeping:
+    def test_no_xte_correction_when_nan(self):
+        p = _pilot()
+        p.step_track_keeping(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=90.0,
+            sog_kn=8.0,
+            xte_m=math.nan,
+        )
+        assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
+        assert p.tc_state.effective_setpoint_deg == pytest.approx(90.0)
+
+    def test_positive_xte_steers_port(self):
+        """XTE>0 = vessel to starboard → correction negative → setpoint shifted port."""
+        p = _pilot(xte_gain=1.0)
+        p.step_track_keeping(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=90.0,
+            sog_kn=8.0,
+            xte_m=5.0,   # 5 m to starboard
+        )
+        assert p.tc_state.xte_correction_deg == pytest.approx(-5.0)
+        assert p.tc_state.effective_setpoint_deg == pytest.approx(85.0)
+
+    def test_negative_xte_steers_starboard(self):
+        p = _pilot(xte_gain=1.0)
+        p.step_track_keeping(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=90.0,
+            sog_kn=8.0,
+            xte_m=-5.0,
+        )
+        assert p.tc_state.xte_correction_deg == pytest.approx(5.0)
+        assert p.tc_state.effective_setpoint_deg == pytest.approx(95.0)
+
+    def test_xte_correction_clamped(self):
+        p = _pilot(xte_gain=1.0, max_corr=10.0)
+        p.step_track_keeping(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=90.0,
+            sog_kn=8.0,
+            xte_m=50.0,   # huge XTE → clamp to 10°
+        )
+        assert p.tc_state.xte_correction_deg == pytest.approx(-10.0)
+
+    def test_stale_xte_ignored(self):
+        """If xte_age_s > max_age_s, correction must be zero."""
+        p = _pilot(xte_gain=1.0)
+        p.step_track_keeping(
+            cog_setpoint_deg=90.0,
+            measured_cog_deg=90.0,
+            sog_kn=8.0,
+            xte_m=10.0,
+            xte_age_s=10.0,   # stale (default max_age_s=5)
+        )
+        assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
+
+    def test_convergence_to_track(self):
+        """Simulate 30 steps: XTE should shrink (very basic closed-loop sanity)."""
+        from arautopilot.studio.simulator.vessel_heading import heading_error_deg
+
+        p = TrueCoursePilot(TrueCourseConfig(
+            pid=PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
+                               rot_ff_gain=0.0, rate_limit_dps=360.0,
+                               max_rudder_deg=35.0),
+            xte_gain=0.5,
+            xte_max_correction_deg=20.0,
+        ))
+
+        xte = 20.0   # 20 m to starboard
+        cog = 90.0   # vessel COG
+        dt = 0.1     # 10 Hz
+        sog = 5.0    # kn ≈ 2.57 m/s
+
+        for _ in range(200):
+            rudder = p.step_track_keeping(
+                cog_setpoint_deg=90.0,
+                measured_cog_deg=cog,
+                sog_kn=sog,
+                xte_m=xte,
+            )
+            cog_change = rudder * 0.5 * dt
+            cog = (cog + cog_change) % 360.0
+            # Cross-track rate: -SOG * cos(COG) for east-going track (COG=90 nominal)
+            xte -= sog * 0.5144 * math.cos(math.radians(cog)) * dt
+
+        assert abs(xte) < 18.0   # must have improved from initial 20 m
+
+    def test_reset_clears_state(self):
+        p = _pilot()
+        p.step_track_keeping(
+            cog_setpoint_deg=90.0, measured_cog_deg=80.0,
+            sog_kn=8.0, xte_m=5.0,
+        )
+        p.reset()
+        assert p.pid_state.integral == pytest.approx(0.0)