Merge pull request #11640 from b14ckyy/fix/fw-wp-tracking-state-reset

Fixed wing nav: reset WP cross-track tracking state on re-engagement

Author: b14ckyy

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);
         }
     }
     /*