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>
2026-05-19 20:12:57 -04:00
parent 0be60c5161
commit 0f00ad10da
12 changed files with 1051 additions and 101 deletions
@@ -0,0 +1,100 @@
+"""Tests for arautopilot.core.nmea_data -- Sprint 5."""
+
+from __future__ import annotations
+
+import math
+import time
+from unittest.mock import patch
+
+import pytest
+
+from arautopilot.core.nmea_data import CogSogData, HeadingData, NmeaNavData, XteData
+
+
+class TestCogSogData:
+    def test_initial_state_invalid(self):
+        d = CogSogData()
+        assert not d.is_valid
+        assert math.isnan(d.cog_deg)
+        assert math.isnan(d.sog_kn)
+
+    def test_update_marks_valid(self):
+        d = CogSogData()
+        d.update(270.0, 8.5)
+        assert d.is_valid
+        assert d.cog_deg == pytest.approx(270.0)
+        assert d.sog_kn == pytest.approx(8.5)
+
+    def test_cog_wraps_360(self):
+        d = CogSogData()
+        d.update(370.0, 5.0)
+        assert d.cog_deg == pytest.approx(10.0)
+
+    def test_cog_zero_stays_zero(self):
+        d = CogSogData()
+        d.update(0.0, 5.0)
+        assert d.cog_deg == pytest.approx(0.0)
+
+    def test_stale_after_max_age(self):
+        d = CogSogData(max_age_s=1.0)
+        d.update(90.0, 6.0)
+        assert d.is_valid
+        with patch("arautopilot.core.nmea_data.time") as mock_time:
+            mock_time.monotonic.return_value = d.timestamp + 2.0
+            assert not d.is_valid
+
+    def test_age_ms(self):
+        d = CogSogData()
+        d.update(180.0, 10.0)
+        assert d.age_ms >= 0
+
+
+class TestXteData:
+    def test_initial_invalid(self):
+        d = XteData()
+        assert not d.is_valid
+        assert math.isnan(d.xte_m)
+
+    def test_update_positive_xte(self):
+        """Positive XTE = vessel to starboard of track."""
+        d = XteData()
+        d.update(xte_m=5.0, dtw_m=200.0, waypoint_name="WP01")
+        assert d.is_valid
+        assert d.xte_m == pytest.approx(5.0)
+        assert d.dtw_m == pytest.approx(200.0)
+        assert d.waypoint_name == "WP01"
+
+    def test_update_negative_xte(self):
+        d = XteData()
+        d.update(xte_m=-3.5)
+        assert d.xte_m == pytest.approx(-3.5)
+
+    def test_stale_after_max_age(self):
+        d = XteData(max_age_s=2.0)
+        d.update(1.0)
+        with patch("arautopilot.core.nmea_data.time") as mock_time:
+            mock_time.monotonic.return_value = d.timestamp + 3.0
+            assert not d.is_valid
+
+    def test_update_without_optional_fields(self):
+        d = XteData()
+        d.update(xte_m=0.0)
+        assert d.is_valid
+        assert math.isnan(d.dtw_m)
+        assert d.waypoint_name == ""
+
+
+class TestNmeaNavData:
+    def test_aggregate_has_all_fields(self):
+        nav = NmeaNavData()
+        assert isinstance(nav.heading, HeadingData)
+        assert isinstance(nav.cog_sog, CogSogData)
+        assert isinstance(nav.xte, XteData)
+
+    def test_independent_updates(self):
+        nav = NmeaNavData()
+        nav.cog_sog.update(45.0, 7.0)
+        nav.xte.update(2.5)
+        assert nav.cog_sog.is_valid
+        assert nav.xte.is_valid
+        assert not nav.heading.is_valid
@@ -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)