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>

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