From 9c40920ed787b0b47e118f9124ae163cc04a7081 Mon Sep 17 00:00:00 2001
From: b14ckyy <33039058+b14ckyy@users.noreply.github.com>
Date: Tue, 9 Jun 2026 12:39:48 +0200
Subject: [PATCH] Fixed wing nav: reset WP cross-track tracking state on
 re-engagement

The fixed-wing waypoint cross-track controller (nav_fw_wp_tracking_accuracy) kept
its rate/error state in function-local statics that were never reset. After a turn
or loiter, where the controller is disengaged (needToCalculateCircularLoiter), the
first sample on the next leg was computed from stale data from the previous leg,
causing a small heading twitch when rejoining the course line.

Re-seed the controller state to the current cross-track error while it is not
actively steering, so it re-engages cleanly on the next leg.
---
 src/main/navigation/navigation_fixedwing.c | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 2051f64232f..90b3a9ebd0b 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -454,6 +454,13 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     }
 
     if (isWaypointNavTrackingActive()) {
+        /* Cross-track controller state. Scoped here, not in the control branch, so the
+         * else branch can re-seed it while the controller is disengaged. */
+        static float crossTrackErrorRate;
+        static timeUs_t previousCrossTrackErrorUpdateTime;
+        static float previousCrossTrackError = 0.0f;
+        static pt1Filter_t fwCrossTrackErrorRateFilterState;
+
         /* Calculate cross track error */
         posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
 
@@ -466,11 +473,6 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
         /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */
         if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) {
-            static float crossTrackErrorRate;
-            static timeUs_t previousCrossTrackErrorUpdateTime;
-            static float previousCrossTrackError = 0.0f;
-            static pt1Filter_t fwCrossTrackErrorRateFilterState;
-
             if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
                 const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
                 if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
@@ -497,6 +499,13 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                 adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
                 virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
             }
+        } else {
+            /* Keep state synced to the current error while not steering, so the
+             * controller re-engages cleanly on the next leg (no stale-data kick). */
+            previousCrossTrackError = navCrossTrackError;
+            previousCrossTrackErrorUpdateTime = currentTimeUs;
+            crossTrackErrorRate = 0.0f;
+            pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f);
         }
     }
     /*