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/core/nmea_data.py

@@ -0,0 +1,117 @@
+"""NMEA 2000 navigation data models.
+
+Holds the live data received from PGN 127250/127251 (heading/ROT -- Sprint 3)
+and the new PGN 129026/129284 (COG+SOG / XTE+waypoint -- Sprint 5).
+
+All timestamps are float seconds (time.monotonic()).
+NaN means the value has never been received.
+"""
+
+from __future__ import annotations
+
+import math
+import time
+from dataclasses import dataclass, field
+
+
+@dataclass
+class HeadingData:
+    """PGN 127250 + 127251 -- magnetic heading and rate of turn."""
+
+    heading_deg: float = math.nan
+    rot_dps: float = 0.0
+    timestamp: float = field(default_factory=lambda: 0.0)
+    max_age_s: float = 5.0
+
+    def update(self, heading_deg: float, rot_dps: float) -> None:
+        self.heading_deg = heading_deg
+        self.rot_dps = rot_dps
+        self.timestamp = time.monotonic()
+
+    @property
+    def is_valid(self) -> bool:
+        if math.isnan(self.heading_deg):
+            return False
+        return (time.monotonic() - self.timestamp) < self.max_age_s
+
+    @property
+    def age_ms(self) -> int:
+        return int((time.monotonic() - self.timestamp) * 1000)
+
+
+@dataclass
+class CogSogData:
+    """PGN 129026 -- Course Over Ground (true) and Speed Over Ground.
+
+    COG is in degrees true (0..360).
+    SOG is in knots.
+    """
+
+    cog_deg: float = math.nan
+    sog_kn: float = math.nan
+    timestamp: float = field(default_factory=lambda: 0.0)
+    max_age_s: float = 5.0
+
+    def update(self, cog_deg: float, sog_kn: float) -> None:
+        self.cog_deg = cog_deg % 360.0
+        self.sog_kn = sog_kn
+        self.timestamp = time.monotonic()
+
+    @property
+    def is_valid(self) -> bool:
+        if math.isnan(self.cog_deg) or math.isnan(self.sog_kn):
+            return False
+        return (time.monotonic() - self.timestamp) < self.max_age_s
+
+    @property
+    def age_ms(self) -> int:
+        return int((time.monotonic() - self.timestamp) * 1000)
+
+
+@dataclass
+class XteData:
+    """PGN 129284 -- Cross Track Error and active waypoint.
+
+    xte_m: distance off track in metres.
+      Positive  → vessel is to starboard of the track line (steer port).
+      Negative  → vessel is to port  of the track line (steer starboard).
+
+    waypoint_name: free text from the chart plotter, empty if not available.
+    dtw_m: distance to waypoint in metres (NaN if unknown).
+    """
+
+    xte_m: float = math.nan
+    dtw_m: float = math.nan
+    waypoint_name: str = ""
+    timestamp: float = field(default_factory=lambda: 0.0)
+    max_age_s: float = 5.0
+
+    def update(
+        self,
+        xte_m: float,
+        dtw_m: float = math.nan,
+        waypoint_name: str = "",
+    ) -> None:
+        self.xte_m = xte_m
+        self.dtw_m = dtw_m
+        self.waypoint_name = waypoint_name
+        self.timestamp = time.monotonic()
+
+    @property
+    def is_valid(self) -> bool:
+        if math.isnan(self.xte_m):
+            return False
+        return (time.monotonic() - self.timestamp) < self.max_age_s
+
+    @property
+    def age_ms(self) -> int:
+        return int((time.monotonic() - self.timestamp) * 1000)
+
+
+@dataclass
+class NmeaNavData:
+    """Aggregated live navigation data used by the autopilot control loop."""
+
+    heading: HeadingData = field(default_factory=HeadingData)
+    cog_sog: CogSogData = field(default_factory=CogSogData)
+    xte: XteData = field(default_factory=XteData)

arautopilot/shared/modbus_register_map.py

@@ -91,6 +91,12 @@ INPUTS: dict[str, Reg] = {
     "PID_OUTER_KP_X1000": Reg(addr=54, name="PID_OUTER_KP_X1000", desc='Outer-loop active kp * 1000', unit="", scale=0.001, offset=0.0),
     "PID_OUTER_KI_X1000": Reg(addr=55, name="PID_OUTER_KI_X1000", desc='Outer-loop active ki * 1000', unit="", scale=0.001, offset=0.0),
     "PID_OUTER_KD_X1000": Reg(addr=56, name="PID_OUTER_KD_X1000", desc='Outer-loop active kd * 1000', unit="", scale=0.001, offset=0.0),
+    "COG_DEG_X100": Reg(addr=60, name="COG_DEG_X100", desc='Course Over Ground (true) from PGN 129026, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
+    "SOG_KN_X10": Reg(addr=61, name="SOG_KN_X10", desc='Speed Over Ground from PGN 129026, knots*10 (unsigned)', unit="kn", scale=0.1, offset=0.0),
+    "COG_AGE_MS": Reg(addr=62, name="COG_AGE_MS", desc='Milliseconds since last COG/SOG update (0..60000)', unit="ms", scale=1.0, offset=0.0),
+    "XTE_DM_SIGNED": Reg(addr=63, name="XTE_DM_SIGNED", desc='Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)', unit="dm", scale=0.1, offset=0.0),
+    "XTE_AGE_MS": Reg(addr=64, name="XTE_AGE_MS", desc='Milliseconds since last XTE update (0..60000)', unit="ms", scale=1.0, offset=0.0),
+    "DTW_M": Reg(addr=65, name="DTW_M", desc='Distance to next waypoint, metres (unsigned int16, 0..65535)', unit="m", scale=1.0, offset=0.0),
 }
 
 # HOLDINGS
@@ -109,6 +115,9 @@ HOLDINGS: dict[str, Reg] = {
     "PID_OUTER_KP_REQ_X1000": Reg(addr=26, name="PID_OUTER_KP_REQ_X1000", desc='Requested outer-loop base kp * 1000', unit="", scale=0.001, offset=0.0),
     "PID_OUTER_KI_REQ_X1000": Reg(addr=27, name="PID_OUTER_KI_REQ_X1000", desc='Requested outer-loop base ki * 1000', unit="", scale=0.001, offset=0.0),
     "PID_OUTER_KD_REQ_X1000": Reg(addr=28, name="PID_OUTER_KD_REQ_X1000", desc='Requested outer-loop base kd * 1000', unit="", scale=0.001, offset=0.0),
+    "TRUE_COURSE_SP_X100": Reg(addr=32, name="TRUE_COURSE_SP_X100", desc='Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)', unit="deg", scale=0.01, offset=0.0),
+    "XTE_GAIN_X1000": Reg(addr=33, name="XTE_GAIN_X1000", desc='XTE correction gain * 1000 (deg of heading correction per metre of XTE)', unit="", scale=0.001, offset=0.0),
+    "XTE_MAX_CORRECTION_X100": Reg(addr=34, name="XTE_MAX_CORRECTION_X100", desc='Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)', unit="deg", scale=0.01, offset=0.0),
 }
 
 ALL_GROUPS: dict[str, dict[str, Reg]] = {

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

arautopilot/tests/test_nmea_data.py

@@ -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

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)

firmware/ar_autopilot_v1/modbus_registers.yaml

@@ -112,6 +112,14 @@ inputs:
   - { addr: 55, name: PID_OUTER_KI_X1000,          desc: "Outer-loop active ki * 1000", scale: 0.001 }
   - { addr: 56, name: PID_OUTER_KD_X1000,          desc: "Outer-loop active kd * 1000", scale: 0.001 }
 
+  # ----- Sprint 5: True Course + Track Keeping (PGN 129026 / 129284) -----
+  - { addr: 60, name: COG_DEG_X100,   desc: "Course Over Ground (true) from PGN 129026, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
+  - { addr: 61, name: SOG_KN_X10,     desc: "Speed Over Ground from PGN 129026, knots*10 (unsigned)", unit: "kn", scale: 0.1 }
+  - { addr: 62, name: COG_AGE_MS,     desc: "Milliseconds since last COG/SOG update (0..60000)", unit: "ms" }
+  - { addr: 63, name: XTE_DM_SIGNED,  desc: "Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)", unit: "dm", scale: 0.1 }
+  - { addr: 64, name: XTE_AGE_MS,     desc: "Milliseconds since last XTE update (0..60000)", unit: "ms" }
+  - { addr: 65, name: DTW_M,          desc: "Distance to next waypoint, metres (unsigned int16, 0..65535)", unit: "m" }
+
 # -----------------------------------------------------------------------------
 # Holding registers (read-write 16-bit words) -- setpoints and config
 # -----------------------------------------------------------------------------
@@ -134,3 +142,8 @@ holdings:
   - { addr: 26, name: PID_OUTER_KP_REQ_X1000,        desc: "Requested outer-loop base kp * 1000", scale: 0.001 }
   - { addr: 27, name: PID_OUTER_KI_REQ_X1000,        desc: "Requested outer-loop base ki * 1000", scale: 0.001 }
   - { addr: 28, name: PID_OUTER_KD_REQ_X1000,        desc: "Requested outer-loop base kd * 1000", scale: 0.001 }
+
+  # ----- Sprint 5: True Course + Track Keeping -----
+  - { addr: 32, name: TRUE_COURSE_SP_X100,        desc: "Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)", unit: "deg", scale: 0.01 }
+  - { addr: 33, name: XTE_GAIN_X1000,             desc: "XTE correction gain * 1000 (deg of heading correction per metre of XTE)", scale: 0.001 }
+  - { addr: 34, name: XTE_MAX_CORRECTION_X100,    desc: "Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)", unit: "deg", scale: 0.01 }

firmware/ar_autopilot_v1/src/pid/pid_outer_task.cpp

@@ -1,10 +1,11 @@
 // =============================================================================
-//  pid_outer_task.cpp -- 10 Hz outer-loop (heading control) task
+//  pid_outer_task.cpp -- 10 Hz outer-loop task (Sprint 3 / Sprint 5)
 // =============================================================================
 
 #include "pid_outer_task.h"
 
 #include <Arduino.h>
+#include <cmath>
 
 #include "../modes/standby.h"
 #include "../protocols/nmea2000_consumer.h"
@@ -20,10 +21,28 @@ constexpr const char* TAG = "AR/PID";
 
 portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
 PidOuter g_outer{};
-float g_heading_setpoint_deg = 0.0f;
+
+float g_heading_setpoint_deg  = 0.0f;
+float g_cog_setpoint_deg      = 0.0f;
+float g_xte_gain              = 0.5f;   // deg / metre
+float g_xte_max_correction    = 20.0f;  // deg
+
 float g_last_rudder_setpoint_deg = 0.0f;
-float g_last_error_deg = 0.0f;
-float g_speed_kn = 15.0f;  // Sprint 3 default until PGN 129026 wiring (Sprint 5)
+float g_last_error_deg           = 0.0f;
+float g_last_xte_correction_deg  = 0.0f;
+float g_speed_kn                 = 15.0f;
+
+// ---------------------------------------------------------------------------
+//  XTE correction helper (TRACK_KEEPING)
+//    xte_m > 0 → vessel to starboard → steer port → negative correction
+// ---------------------------------------------------------------------------
+static float _xte_correction(float xte_m, float gain, float max_deg) {
+    if (std::isnan(xte_m)) return 0.0f;
+    float c = -gain * xte_m;
+    if (c >  max_deg) c =  max_deg;
+    if (c < -max_deg) c = -max_deg;
+    return c;
+}
 
 void OuterLoopTask(void* /*pv*/) {
     AR_LOGI(TAG, "pid_outer task started on core %d (10 Hz)", xPortGetCoreID());
@@ -31,46 +50,93 @@ void OuterLoopTask(void* /*pv*/) {
 
     TickType_t last_wake = xTaskGetTickCount();
     for (;;) {
-        // Snapshot inputs we need atomically.
-        float setpoint;
-        float speed_kn;
+        float heading_sp, cog_sp, xte_gain, xte_max, speed_kn;
         portENTER_CRITICAL(&g_mux);
-        setpoint = g_heading_setpoint_deg;
-        speed_kn = g_speed_kn;
+        heading_sp = g_heading_setpoint_deg;
+        cog_sp     = g_cog_setpoint_deg;
+        xte_gain   = g_xte_gain;
+        xte_max    = g_xte_max_correction;
+        speed_kn   = g_speed_kn;
         portEXIT_CRITICAL(&g_mux);
 
-        const auto n2k = protocols::nmea2000::nmea2000_latest();
+        const auto n2k       = protocols::nmea2000::nmea2000_latest();
+        const auto cog_sog   = protocols::nmea2000::nmea2000_cog_sog();
+        const auto nav_data  = protocols::nmea2000::nmea2000_nav_data();
+        const auto mode      = modes::current_mode();
 
-        // Only active in HEADING_HOLD with valid heading sensor.
-        const bool in_hh =
-            modes::current_mode() == modes::Mode::HEADING_HOLD;
-        const bool allowed = in_hh && n2k.heading_valid;
+        const bool in_hh  = (mode == modes::Mode::HEADING_HOLD);
+        const bool in_tc  = (mode == modes::Mode::TRUE_COURSE);
+        const bool in_tk  = (mode == modes::Mode::TRACK_KEEPING);
 
-        const float rudder_sp = g_outer.step(
-            setpoint,
-            n2k.heading_deg,
-            n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
-            speed_kn,
-            allowed
-        );
+        float rudder_sp     = 0.0f;
+        float error         = 0.0f;
+        float xte_corr      = 0.0f;
 
-        // Always push the outer-loop output downstream. If `allowed` is
-        // false the output is zero, which corresponds to "rudder centred";
-        // the inner loop will pursue that or its own externally-supplied
-        // setpoint depending on the cascade configuration. In HH mode,
-        // the outer loop owns the inner setpoint.
         if (in_hh) {
+            // ---------------------------------------------------------------
+            //  HEADING_HOLD: feedback = magnetic/true heading (PGN 127250)
+            // ---------------------------------------------------------------
+            const bool allowed = n2k.heading_valid;
+            rudder_sp = g_outer.step(
+                heading_sp,
+                n2k.heading_deg,
+                n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
+                speed_kn,
+                allowed
+            );
+            error = allowed ? (heading_sp - n2k.heading_deg) : 0.0f;
             pid_inner_set_setpoint_deg(rudder_sp);
-        }
 
-        const float err =
-            n2k.heading_valid
-                ? (setpoint - n2k.heading_deg)
-                : 0.0f;
+        } else if (in_tc) {
+            // ---------------------------------------------------------------
+            //  TRUE_COURSE: feedback = COG (PGN 129026)
+            // ---------------------------------------------------------------
+            const bool allowed = cog_sog.valid;
+            const float sog    = cog_sog.valid ? cog_sog.sog_kn : speed_kn;
+            rudder_sp = g_outer.step(
+                cog_sp,
+                cog_sog.cog_deg,
+                n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
+                sog,
+                allowed
+            );
+            error = allowed ? (cog_sp - cog_sog.cog_deg) : 0.0f;
+            pid_inner_set_setpoint_deg(rudder_sp);
+
+        } else if (in_tk) {
+            // ---------------------------------------------------------------
+            //  TRACK_KEEPING: COG hold + XTE proportional correction
+            // ---------------------------------------------------------------
+            const bool cog_ok = cog_sog.valid;
+            const bool xte_ok = nav_data.valid;
+            xte_corr = xte_ok ? _xte_correction(nav_data.xte_m, xte_gain, xte_max) : 0.0f;
+            const float eff_sp = fmodf(cog_sp + xte_corr + 360.0f, 360.0f);
+            const float sog    = cog_ok ? cog_sog.sog_kn : speed_kn;
+            rudder_sp = g_outer.step(
+                eff_sp,
+                cog_sog.cog_deg,
+                n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
+                sog,
+                cog_ok
+            );
+            error = cog_ok ? (cog_sp - cog_sog.cog_deg) : 0.0f;
+            pid_inner_set_setpoint_deg(rudder_sp);
+
+        } else {
+            // ---------------------------------------------------------------
+            //  STANDBY / DODGE: idle tick — bleed integrator, don't push
+            // ---------------------------------------------------------------
+            g_outer.step(0.0f, 0.0f, 0.0f, speed_kn, false);
+        }
 
         portENTER_CRITICAL(&g_mux);
         g_last_rudder_setpoint_deg = rudder_sp;
-        g_last_error_deg = err;
+        g_last_error_deg           = error;
+        g_last_xte_correction_deg  = xte_corr;
+        if (in_tc || in_tk) {
+            // Update SOG from live data
+            if (cog_sog.valid) g_speed_kn = cog_sog.sog_kn;
+        }
         portEXIT_CRITICAL(&g_mux);
 
         safety::watchdog_feed();
@@ -80,15 +146,17 @@ void OuterLoopTask(void* /*pv*/) {
 
 }  // namespace
 
+// ---------------------------------------------------------------------------
+//  Init / start
+// ---------------------------------------------------------------------------
+
 void pid_outer_task_init() {
     PidOuterConfig cfg;
-    // Seed the 3-point gain schedule from the 30 m yacht profile so the
-    // firmware has sensible defaults out of the box.
-    cfg.schedule_size = 3;
-    cfg.schedule[0] = {5.0f,  1.20f, 0.03f, 0.80f};
-    cfg.schedule[1] = {15.0f, 0.90f, 0.02f, 1.20f};
-    cfg.schedule[2] = {28.0f, 0.55f, 0.01f, 1.80f};
-    cfg.rot_ff_gain = 1.5f;
+    cfg.schedule_size   = 3;
+    cfg.schedule[0]     = {5.0f,  1.20f, 0.03f, 0.80f};
+    cfg.schedule[1]     = {15.0f, 0.90f, 0.02f, 1.20f};
+    cfg.schedule[2]     = {28.0f, 0.55f, 0.01f, 1.80f};
+    cfg.rot_ff_gain     = 1.5f;
     g_outer.update_config(cfg);
     g_outer.reset();
     AR_LOGI(TAG,
@@ -102,10 +170,12 @@ void pid_outer_task_start() {
                             AR_TASK_CORE_REALTIME);
 }
 
-void pid_outer_set_heading_setpoint_deg(float setpoint_deg) {
-    // Normalise to [0, 360).
-    float sp = setpoint_deg;
-    while (sp < 0.0f) sp += 360.0f;
+// ---------------------------------------------------------------------------
+//  Setpoints
+// ---------------------------------------------------------------------------
+
+void pid_outer_set_heading_setpoint_deg(float sp) {
+    while (sp <   0.0f) sp += 360.0f;
     while (sp >= 360.0f) sp -= 360.0f;
     portENTER_CRITICAL(&g_mux);
     g_heading_setpoint_deg = sp;
@@ -119,6 +189,58 @@ float pid_outer_heading_setpoint_deg() {
     return v;
 }
 
+void pid_outer_set_cog_setpoint_deg(float sp) {
+    while (sp <   0.0f) sp += 360.0f;
+    while (sp >= 360.0f) sp -= 360.0f;
+    portENTER_CRITICAL(&g_mux);
+    g_cog_setpoint_deg = sp;
+    portEXIT_CRITICAL(&g_mux);
+}
+
+float pid_outer_cog_setpoint_deg() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_cog_setpoint_deg;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+// ---------------------------------------------------------------------------
+//  XTE parameters
+// ---------------------------------------------------------------------------
+
+void pid_outer_set_xte_gain(float gain) {
+    if (gain < 0.0f) gain = 0.0f;
+    portENTER_CRITICAL(&g_mux);
+    g_xte_gain = gain;
+    portEXIT_CRITICAL(&g_mux);
+}
+
+float pid_outer_xte_gain() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_xte_gain;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+void pid_outer_set_xte_max_correction(float deg) {
+    if (deg < 0.0f) deg = 0.0f;
+    if (deg > 90.0f) deg = 90.0f;
+    portENTER_CRITICAL(&g_mux);
+    g_xte_max_correction = deg;
+    portEXIT_CRITICAL(&g_mux);
+}
+
+float pid_outer_xte_max_correction() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_xte_max_correction;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+// ---------------------------------------------------------------------------
+//  Telemetry
+// ---------------------------------------------------------------------------
+
 float pid_outer_last_rudder_setpoint_deg() {
     portENTER_CRITICAL(&g_mux);
     float v = g_last_rudder_setpoint_deg;
@@ -133,8 +255,19 @@ float pid_outer_last_error_deg() {
     return v;
 }
 
+float pid_outer_last_xte_correction_deg() {
+    portENTER_CRITICAL(&g_mux);
+    float v = g_last_xte_correction_deg;
+    portEXIT_CRITICAL(&g_mux);
+    return v;
+}
+
+// ---------------------------------------------------------------------------
+//  SOG / gain scheduling
+// ---------------------------------------------------------------------------
+
 void pid_outer_set_speed_kn(float speed_kn) {
-    if (speed_kn < 0.0f) speed_kn = 0.0f;
+    if (speed_kn < 0.0f)  speed_kn = 0.0f;
     if (speed_kn > 80.0f) speed_kn = 80.0f;
     portENTER_CRITICAL(&g_mux);
     g_speed_kn = speed_kn;
@@ -150,15 +283,14 @@ float pid_outer_speed_kn() {
 
 void pid_outer_update_gains(float kp, float ki, float kd) {
     PidOuterConfig cfg = g_outer.config();
-    cfg.base_kp = kp;
-    cfg.base_ki = ki;
-    cfg.base_kd = kd;
-    cfg.schedule_size = 0;  // explicit gains override the schedule
+    cfg.base_kp      = kp;
+    cfg.base_ki      = ki;
+    cfg.base_kd      = kd;
+    cfg.schedule_size = 0;  // explicit gains disable the schedule
     portENTER_CRITICAL(&g_mux);
     g_outer.update_config(cfg);
     portEXIT_CRITICAL(&g_mux);
-    AR_LOGI(TAG, "pid_outer base gains updated: kp=%.3f ki=%.3f kd=%.3f "
-                  "(schedule disabled)", kp, ki, kd);
+    AR_LOGI(TAG, "pid_outer gains updated: kp=%.3f ki=%.3f kd=%.3f", kp, ki, kd);
 }
 
 void pid_outer_get_gains(float& kp, float& ki, float& kd) {

firmware/ar_autopilot_v1/src/pid/pid_outer_task.h

@@ -1,11 +1,14 @@
 // =============================================================================
-//  pid_outer_task.h -- 10 Hz outer-loop (heading control) task (Sprint 3)
+//  pid_outer_task.h -- 10 Hz outer-loop task (Sprint 3 / Sprint 5)
 // =============================================================================
 //
-//  Reads heading + ROT from the NMEA 2000 snapshot, computes a rudder
-//  setpoint, hands it off to the inner loop. Active only in HEADING_HOLD
-//  mode; in any other mode the task ticks idle (allowed=false) which
-//  bleeds the integrator.
+//  Sprint 3: HEADING_HOLD -- holds a magnetic/true heading setpoint.
+//  Sprint 5: TRUE_COURSE  -- holds a COG setpoint (compensates current/wind).
+//            TRACK_KEEPING -- COG hold + XTE proportional correction.
+//
+//  In all modes the output is a rudder setpoint handed to the inner loop.
+//  In any other mode (STANDBY, DODGE) the task ticks idle (allowed=false),
+//  bleeding the integrator so the transition back to active is smooth.
 // =============================================================================
 
 #pragma once
@@ -17,24 +20,30 @@ namespace arautopilot::pid {
 void pid_outer_task_init();
 void pid_outer_task_start();
 
-/// Update the heading the outer loop pursues (degrees, 0..360).
-void pid_outer_set_heading_setpoint_deg(float setpoint_deg);
-
-/// Read the active heading setpoint (thread-safe).
+// ----- HEADING_HOLD setpoint -----
+void  pid_outer_set_heading_setpoint_deg(float setpoint_deg);
 float pid_outer_heading_setpoint_deg();
 
-/// Read the last rudder setpoint the outer loop produced.
+// ----- TRUE_COURSE / TRACK_KEEPING setpoint -----
+void  pid_outer_set_cog_setpoint_deg(float setpoint_deg);
+float pid_outer_cog_setpoint_deg();
+
+// ----- XTE parameters (TRACK_KEEPING) -----
+void  pid_outer_set_xte_gain(float gain);         ///< deg correction / metre
+void  pid_outer_set_xte_max_correction(float deg); ///< hard clamp
+float pid_outer_xte_gain();
+float pid_outer_xte_max_correction();
+
+// ----- Telemetry -----
 float pid_outer_last_rudder_setpoint_deg();
-
-/// Read the last heading error the controller saw.
 float pid_outer_last_error_deg();
+float pid_outer_last_xte_correction_deg();  ///< 0 in non-TK modes
 
-/// Override the SOG used for gain scheduling (Sprint 3 keeps a default of
-/// 15 kn until PGN 129026 wiring lands in Sprint 5).
-void pid_outer_set_speed_kn(float speed_kn);
+// ----- SOG / gain scheduling -----
+void  pid_outer_set_speed_kn(float speed_kn);
 float pid_outer_speed_kn();
 
-void pid_outer_update_gains(float kp, float ki, float kd);
-void pid_outer_get_gains(float& kp, float& ki, float& kd);
+void  pid_outer_update_gains(float kp, float ki, float kd);
+void  pid_outer_get_gains(float& kp, float& ki, float& kd);
 
 }  // namespace arautopilot::pid

firmware/ar_autopilot_v1/src/protocols/modbus_registers.h

@@ -77,8 +77,8 @@ constexpr uint16_t COIL_CMD_ACK_ALL_ALARMS = 2;
 constexpr uint16_t COIL_CMD_KNOB_ARM = 3;
 
 // ----- Input registers (read-only words) -----
-constexpr uint16_t INPUT_COUNT = 30;
-constexpr uint16_t INPUT_MAX_ADDR = 56;
+constexpr uint16_t INPUT_COUNT = 36;
+constexpr uint16_t INPUT_MAX_ADDR = 65;
 
 // Firmware major version
 constexpr uint16_t INPUT_FW_VERSION_MAJOR = 0;
@@ -164,10 +164,28 @@ constexpr uint16_t INPUT_PID_OUTER_KI_X1000 = 55;
 // Outer-loop active kd * 1000
 // scale=0.001
 constexpr uint16_t INPUT_PID_OUTER_KD_X1000 = 56;
+// Course Over Ground (true) from PGN 129026, deg*100 (0..35999)
+// unit=deg, scale=0.01
+constexpr uint16_t INPUT_COG_DEG_X100 = 60;
+// Speed Over Ground from PGN 129026, knots*10 (unsigned)
+// unit=kn, scale=0.1
+constexpr uint16_t INPUT_SOG_KN_X10 = 61;
+// Milliseconds since last COG/SOG update (0..60000)
+// unit=ms
+constexpr uint16_t INPUT_COG_AGE_MS = 62;
+// Cross Track Error from PGN 129284, decimetres (signed int16, +stbd/-port)
+// unit=dm, scale=0.1
+constexpr uint16_t INPUT_XTE_DM_SIGNED = 63;
+// Milliseconds since last XTE update (0..60000)
+// unit=ms
+constexpr uint16_t INPUT_XTE_AGE_MS = 64;
+// Distance to next waypoint, metres (unsigned int16, 0..65535)
+// unit=m
+constexpr uint16_t INPUT_DTW_M = 65;
 
 // ----- Holding registers (read-write words) -----
-constexpr uint16_t HOLDING_COUNT = 14;
-constexpr uint16_t HOLDING_MAX_ADDR = 28;
+constexpr uint16_t HOLDING_COUNT = 17;
+constexpr uint16_t HOLDING_MAX_ADDR = 34;
 
 // Mode requested by operator (0=STANDBY,1=HH,2=TC,3=TK,4=DODGE)
 constexpr uint16_t HOLDING_MODE_REQUEST = 0;
@@ -210,5 +228,14 @@ constexpr uint16_t HOLDING_PID_OUTER_KI_REQ_X1000 = 27;
 // Requested outer-loop base kd * 1000
 // scale=0.001
 constexpr uint16_t HOLDING_PID_OUTER_KD_REQ_X1000 = 28;
+// Desired COG setpoint for TRUE_COURSE / TRACK_KEEPING, deg*100 (0..35999)
+// unit=deg, scale=0.01
+constexpr uint16_t HOLDING_TRUE_COURSE_SP_X100 = 32;
+// XTE correction gain * 1000 (deg of heading correction per metre of XTE)
+// scale=0.001
+constexpr uint16_t HOLDING_XTE_GAIN_X1000 = 33;
+// Maximum XTE heading correction, deg*100 (default 2000 = 20 deg)
+// unit=deg, scale=0.01
+constexpr uint16_t HOLDING_XTE_MAX_CORRECTION_X100 = 34;
 
 }  // namespace arautopilot::protocols::modbus

firmware/ar_autopilot_v1/src/protocols/modbus_slave.cpp

@@ -66,6 +66,10 @@ struct HoldingStorage {
     uint16_t pid_outer_kp_req_x1000 = 0;
     uint16_t pid_outer_ki_req_x1000 = 0;
     uint16_t pid_outer_kd_req_x1000 = 0;
+    // Sprint 5: True Course + Track Keeping
+    uint16_t true_course_sp_x100 = 0;
+    uint16_t xte_gain_x1000 = 500;      // 0.5 deg/m default
+    uint16_t xte_max_correction_x100 = 2000;  // 20.0 deg default
 };
 HoldingStorage g_holding;
 
@@ -209,6 +213,51 @@ uint16_t read_input_register(uint16_t addr) {
             return (uint16_t)scaled;
         }
 
+        // ----- Sprint 5: COG / SOG / XTE telemetry -----
+        case INPUT_COG_DEG_X100: {
+            auto cs = nmea2000::nmea2000_cog_sog();
+            if (!cs.valid) return 0;
+            int v = (int)(cs.cog_deg * 100.0f);
+            if (v < 0) v = 0;
+            if (v > 35999) v = 35999;
+            return (uint16_t)v;
+        }
+        case INPUT_SOG_KN_X10: {
+            auto cs = nmea2000::nmea2000_cog_sog();
+            if (!cs.valid) return 0;
+            int v = (int)(cs.sog_kn * 10.0f);
+            if (v < 0) v = 0;
+            if (v > 65535) v = 65535;
+            return (uint16_t)v;
+        }
+        case INPUT_COG_AGE_MS: {
+            auto cs = nmea2000::nmea2000_cog_sog();
+            uint32_t age = cs.valid ? (millis() - cs.age_ms) : 60000;
+            if (age > 60000) age = 60000;
+            return (uint16_t)age;
+        }
+        case INPUT_XTE_DM_SIGNED: {
+            auto nd = nmea2000::nmea2000_nav_data();
+            if (!nd.valid) return 0;
+            int v = (int)(nd.xte_m * 10.0f);  // metres → decimetres
+            if (v < -32768) v = -32768;
+            if (v >  32767) v =  32767;
+            return (uint16_t)(int16_t)v;
+        }
+        case INPUT_XTE_AGE_MS: {
+            auto nd = nmea2000::nmea2000_nav_data();
+            uint32_t age = nd.valid ? (millis() - nd.age_ms) : 60000;
+            if (age > 60000) age = 60000;
+            return (uint16_t)age;
+        }
+        case INPUT_DTW_M: {
+            auto nd = nmea2000::nmea2000_nav_data();
+            if (!nd.valid) return 0;
+            uint32_t dtw = (uint32_t)nd.dtw_m;
+            if (dtw > 65535) dtw = 65535;
+            return (uint16_t)dtw;
+        }
+
         default:
             return 0;
     }
@@ -258,6 +307,9 @@ uint16_t read_holding(uint16_t addr) {
         case HOLDING_PID_OUTER_KP_REQ_X1000:        return g_holding.pid_outer_kp_req_x1000;
         case HOLDING_PID_OUTER_KI_REQ_X1000:        return g_holding.pid_outer_ki_req_x1000;
         case HOLDING_PID_OUTER_KD_REQ_X1000:        return g_holding.pid_outer_kd_req_x1000;
+        case HOLDING_TRUE_COURSE_SP_X100:            return g_holding.true_course_sp_x100;
+        case HOLDING_XTE_GAIN_X1000:                 return g_holding.xte_gain_x1000;
+        case HOLDING_XTE_MAX_CORRECTION_X100:        return g_holding.xte_max_correction_x100;
         default:                             return 0;
     }
 }
@@ -348,6 +400,25 @@ Modbus::Error write_holding(uint16_t addr, uint16_t value) {
             pid::pid_inner_update_gains(kp, ki, kd);
             return Modbus::Error::SUCCESS;
         }
+        // ----- Sprint 5: True Course + XTE parameters -----
+        case HOLDING_TRUE_COURSE_SP_X100: {
+            if (value > 35999) return Modbus::Error::ILLEGAL_DATA_VALUE;
+            g_holding.true_course_sp_x100 = value;
+            pid::pid_outer_set_cog_setpoint_deg((float)value * 0.01f);
+            return Modbus::Error::SUCCESS;
+        }
+        case HOLDING_XTE_GAIN_X1000: {
+            g_holding.xte_gain_x1000 = value;
+            pid::pid_outer_set_xte_gain((float)value * 0.001f);
+            return Modbus::Error::SUCCESS;
+        }
+        case HOLDING_XTE_MAX_CORRECTION_X100: {
+            if (value > 9000) return Modbus::Error::ILLEGAL_DATA_VALUE;  // 90 deg cap
+            g_holding.xte_max_correction_x100 = value;
+            pid::pid_outer_set_xte_max_correction((float)value * 0.01f);
+            return Modbus::Error::SUCCESS;
+        }
+
         default:
             return Modbus::Error::ILLEGAL_DATA_ADDRESS;
     }

firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.cpp

@@ -25,6 +25,7 @@ constexpr const char* TAG = "AR/N2K";
 constexpr uint32_t STALE_THRESHOLD_MS = 5000;
 
 portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
+
 HeadingSnapshot g_snap{
     .heading_deg = 0.0f,
     .is_true = false,
@@ -35,6 +36,21 @@ HeadingSnapshot g_snap{
     .rot_valid = false,
 };
 
+CogSogSnapshot g_cog_sog{
+    .cog_deg = NAN,
+    .sog_kn  = NAN,
+    .age_ms  = 0,
+    .valid   = false,
+};
+
+NavDataSnapshot g_nav_data{
+    .xte_m    = NAN,
+    .dtw_m    = NAN,
+    .wp_name  = {},
+    .age_ms   = 0,
+    .valid    = false,
+};
+
 float rad_to_deg_pos(float rad) {
     float d = rad * (180.0f / (float)M_PI);
     // Normalise to 0..360.
@@ -90,19 +106,62 @@ void HandleROT(const tN2kMsg& msg) {
     AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
 }
 
-// One global dispatcher because NMEA2000.SetMsgHandler() takes a single
-// callback that we have to filter by PGN ourselves.
+void HandleCogSog(const tN2kMsg& msg) {
+    unsigned char sid;
+    tN2kHeadingReference ref;
+    double cog = 0.0, sog = 0.0;
+    if (!ParseN2kCOGSOGRapid(msg, sid, ref, cog, sog)) return;
+    if (cog > 1e8 || sog > 1e8) return;
+    const float cog_deg = rad_to_deg_pos((float)cog);
+    const float sog_kn  = (float)(sog * 1.94384);  // m/s → knots
+    const uint32_t now  = millis();
+    portENTER_CRITICAL(&g_mux);
+    g_cog_sog.cog_deg = cog_deg;
+    g_cog_sog.sog_kn  = sog_kn;
+    g_cog_sog.age_ms  = now;
+    g_cog_sog.valid   = true;
+    portEXIT_CRITICAL(&g_mux);
+    AR_LOGV(TAG, "PGN 129026 COG=%.2f deg SOG=%.2f kn", cog_deg, sog_kn);
+}
+
+void HandleNavData(const tN2kMsg& msg) {
+    unsigned char sid;
+    double dist = 0.0, xte = 0.0;
+    double origLat = 0.0, origLon = 0.0, destLat = 0.0, destLon = 0.0;
+    double closingVel = 0.0;
+    tN2kHeadingReference bearingRef = N2khr_Unavailable;
+    tN2kDistanceCalculationType calcType = N2kdct_GreatCircle;
+    bool perpCrossed = false, arrivalAlarm = false;
+    int16_t origWpNum = 0;
+    uint32_t destWpNum = 0, eta = 0;
+
+    if (!ParseN2kNavigationInfo(
+            msg, sid, dist, bearingRef,
+            perpCrossed, arrivalAlarm, calcType, xte,
+            origWpNum, origLat, origLon,
+            destWpNum, eta, destLat, destLon, closingVel)) {
+        return;
+    }
+    if (xte > 1e8 || xte < -1e8) return;
+    const float xte_m = (float)xte;
+    const float dtw_m = (dist < 1e8) ? (float)dist : NAN;
+    const uint32_t now = millis();
+    portENTER_CRITICAL(&g_mux);
+    g_nav_data.xte_m  = xte_m;
+    g_nav_data.dtw_m  = dtw_m;
+    g_nav_data.age_ms = now;
+    g_nav_data.valid  = true;
+    portEXIT_CRITICAL(&g_mux);
+    AR_LOGV(TAG, "PGN 129284 XTE=%.2f m DTW=%.0f m", xte_m, dtw_m);
+}
+
 void MessageHandler(const tN2kMsg& msg) {
     switch (msg.PGN) {
-        case 127250L:
-            HandleHeading(msg);
-            break;
-        case 127251L:
-            HandleROT(msg);
-            break;
-        default:
-            // Sprint 1: ignore everything else.
-            break;
+        case 127250L: HandleHeading(msg); break;
+        case 127251L: HandleROT(msg);     break;
+        case 129026L: HandleCogSog(msg);  break;
+        case 129284L: HandleNavData(msg); break;
+        default: break;
     }
 }
 
@@ -110,7 +169,6 @@ void RxTask(void* /*pv*/) {
     AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
     for (;;) {
         NMEA2000.ParseMessages();
-        // Update validity flags based on age.
         const uint32_t now = millis();
         portENTER_CRITICAL(&g_mux);
         if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
@@ -120,6 +178,15 @@ void RxTask(void* /*pv*/) {
             g_snap.rot_valid = false;
         }
         portEXIT_CRITICAL(&g_mux);
+        // Update COG/SOG and nav-data stale flags.
+        portENTER_CRITICAL(&g_mux);
+        if (g_cog_sog.valid && (now - g_cog_sog.age_ms) > STALE_THRESHOLD_MS) {
+            g_cog_sog.valid = false;
+        }
+        if (g_nav_data.valid && (now - g_nav_data.age_ms) > STALE_THRESHOLD_MS) {
+            g_nav_data.valid = false;
+        }
+        portEXIT_CRITICAL(&g_mux);
         // 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
         vTaskDelay(pdMS_TO_TICKS(10));
     }
@@ -170,9 +237,29 @@ HeadingSnapshot nmea2000_latest() {
     return copy;
 }
 
+CogSogSnapshot nmea2000_cog_sog() {
+    CogSogSnapshot copy;
+    portENTER_CRITICAL(&g_mux);
+    copy = g_cog_sog;
+    portEXIT_CRITICAL(&g_mux);
+    return copy;
+}
+
+NavDataSnapshot nmea2000_nav_data() {
+    NavDataSnapshot copy;
+    portENTER_CRITICAL(&g_mux);
+    copy = g_nav_data;
+    portEXIT_CRITICAL(&g_mux);
+    return copy;
+}
+
 bool nmea2000_is_stale() {
     const auto s = nmea2000_latest();
     return !s.heading_valid;
 }
 
+bool nmea2000_cog_is_stale() {
+    return !nmea2000_cog_sog().valid;
+}
+
 }  // namespace arautopilot::protocols::nmea2000

firmware/ar_autopilot_v1/src/protocols/nmea2000_consumer.h

@@ -2,34 +2,61 @@
 //  nmea2000_consumer.h -- NMEA 2000 backbone consumer
 // =============================================================================
 //
-//  Sprint 1 subscribes to PGN 127250 (Vessel Heading) and PGN 127251
-//  (Rate of Turn) from the boat's NMEA 2000 backbone. The latest values
-//  are stashed in a thread-safe snapshot that the Modbus slave (and later
-//  the PID outer loop) read from.
+//  Sprint 1: PGN 127250 (Heading) + 127251 (ROT).
+//  Sprint 5: PGN 129026 (COG/SOG) + 129284 (Navigation Data / XTE).
 //
-//  Later sprints will also subscribe to PGN 129025/129029 (Position),
-//  129026 (COG/SOG), 129284 (Navigation Data), 127257 (Attitude).
-//
-//  This module owns the NMEA2000 instance and its dedicated FreeRTOS task
-//  pinned to Core 0.
+//  All snapshots are updated from the NMEA 2000 task (Core 0) and
+//  read from the PID outer-loop task (Core 1) via a portMUX spinlock.
 // =============================================================================
 
 #pragma once
 
 #include <cstdint>
+#include <cmath>
 
 namespace arautopilot::protocols::nmea2000 {
 
+// ---------------------------------------------------------------------------
+//  PGN 127250 / 127251 -- heading + ROT
+// ---------------------------------------------------------------------------
+
 struct HeadingSnapshot {
-    float heading_deg;        ///< 0..360, magnetic or true depending on source
-    bool is_true;             ///< true if reference is "true north", false if magnetic
-    float rate_of_turn_dps;   ///< signed deg/s; positive = turning to starboard
-    uint32_t heading_age_ms;  ///< millis() at the last 127250 update
-    uint32_t rot_age_ms;      ///< millis() at the last 127251 update
-    bool heading_valid;       ///< true if the heading is fresh (<5 s old)
-    bool rot_valid;           ///< true if the ROT is fresh (<5 s old)
+    float    heading_deg;       ///< 0..360, magnetic or true per is_true flag
+    bool     is_true;           ///< true = true north reference
+    float    rate_of_turn_dps;  ///< signed deg/s; positive = turning to stbd
+    uint32_t heading_age_ms;    ///< millis() at last 127250 update
+    uint32_t rot_age_ms;        ///< millis() at last 127251 update
+    bool     heading_valid;     ///< fresh (<5 s)
+    bool     rot_valid;         ///< fresh (<5 s)
 };
 
+// ---------------------------------------------------------------------------
+//  PGN 129026 -- Course Over Ground (true) + Speed Over Ground
+// ---------------------------------------------------------------------------
+
+struct CogSogSnapshot {
+    float    cog_deg;      ///< 0..360, degrees true; NAN until first message
+    float    sog_kn;       ///< knots; NAN until first message
+    uint32_t age_ms;       ///< millis() at last update
+    bool     valid;        ///< fresh (<5 s) and non-NaN
+};
+
+// ---------------------------------------------------------------------------
+//  PGN 129284 -- Navigation Data (XTE + waypoint)
+// ---------------------------------------------------------------------------
+
+struct NavDataSnapshot {
+    float    xte_m;        ///< cross-track error, metres; +stbd / -port; NAN if unknown
+    float    dtw_m;        ///< distance to waypoint, metres; NAN if unknown
+    char     wp_name[16];  ///< null-terminated waypoint name (truncated at 15 chars)
+    uint32_t age_ms;       ///< millis() at last update
+    bool     valid;        ///< fresh (<5 s) and non-NaN
+};
+
+// ---------------------------------------------------------------------------
+//  Public API
+// ---------------------------------------------------------------------------
+
 /// Initialise the NMEA2000 stack with our PGN handlers.
 void nmea2000_consumer_init();
 
@@ -39,8 +66,16 @@ void nmea2000_consumer_start_task();
 /// Thread-safe read of the latest heading + ROT snapshot.
 HeadingSnapshot nmea2000_latest();
 
-/// True if either heading_age_ms or rot_age_ms exceeds the stale-threshold
-/// (default 5 s, brief section 7).
+/// Thread-safe read of the latest COG/SOG snapshot (PGN 129026).
+CogSogSnapshot  nmea2000_cog_sog();
+
+/// Thread-safe read of the latest navigation data snapshot (PGN 129284).
+NavDataSnapshot nmea2000_nav_data();
+
+/// True if heading age or ROT age exceeds 5 s.
 bool nmea2000_is_stale();
 
+/// True if COG/SOG age exceeds 5 s or data was never received.
+bool nmea2000_cog_is_stale();
+
 }  // namespace arautopilot::protocols::nmea2000