From cfa0892f9dca3fa3861a1ffdc193a4fbd297b130 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 4 Aug 2024 11:27:40 +0100
Subject: [PATCH 01/10] wp tracking improvement

---
 src/main/navigation/navigation_fixedwing.c | 41 +++++++++++-----------
 1 file changed, 20 insertions(+), 21 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 71a7f99fc34..183c5a85269 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -401,33 +401,32 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
     /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
-        // courseVirtualCorrection initially used to determine current position relative to course line for later use
         int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
         navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
 
-        // tracking only active when certain distance and heading conditions are met
-        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
-            int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);
-
-            // captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
-            // Closing distance threashold based on speed and an assumed 1 second response time.
-            float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
-
-            // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
-            // initial courseCorrectionFactor based on distance to course line
-            float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
-            courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
+        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) {
+            static float crossTrackErrorRate;
+            if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
+                static float previousCrossTrackError = 0.0f;
+                crossTrackErrorRate = 0.5f * (crossTrackErrorRate + (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousTimeMonitoringUpdate));
+                previousCrossTrackError = navCrossTrackError;
+            }
 
-            // course heading alignment factor
-            float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f);
-            courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
+            /* Apply basic adjustment to factor up virtualTargetBearing error based on navCrossTrackError */
+            float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
+            adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f));
 
-            // final courseCorrectionFactor combining distance and heading factors
-            courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
+            /* Apply additional fine adjustment based on speed of convergence to try and achieve arrival time of around 15s */
+            float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f);
+            float rateFactor = limit;
+            if (crossTrackErrorRate > 0.0f) {
+                rateFactor = scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, 30.0f, -limit, limit);
+            }
 
-            // final courseVirtualCorrection value
-            courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
-            virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection);
+            /* Determine final adjusted virtualTargetBearing */
+            uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
+            adjustmentFactor = constrainf(adjustmentFactor + rateFactor * SIGN(adjustmentFactor), -angleLimit, angleLimit);
+            virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
         }
     }
 

From 42322663f644e1f134ec3ad21a48cbfada3a312f Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 4 Aug 2024 11:48:43 +0100
Subject: [PATCH 02/10] Update navigation_fixedwing.c

---
 src/main/navigation/navigation_fixedwing.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 183c5a85269..f762295d8b2 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -420,7 +420,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
             float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f);
             float rateFactor = limit;
             if (crossTrackErrorRate > 0.0f) {
-                rateFactor = scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, 30.0f, -limit, limit);
+                rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0, 30, -limit, limit), -limit, limit);
             }
 
             /* Determine final adjusted virtualTargetBearing */

From b3da693c5a35f58c12b152f64bb3358c520eb3c0 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 8 Aug 2024 09:06:54 +0100
Subject: [PATCH 03/10] Improvements

---
 src/main/navigation/navigation_fixedwing.c | 26 ++++++++++++++--------
 1 file changed, 17 insertions(+), 9 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index f762295d8b2..0c49ad4fda7 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -401,14 +401,21 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
     /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
-        int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
-        navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
-
-        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) {
+        fpVector3_t virtualCoursePoint;
+        virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
+                               posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+        virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
+                               posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+        navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
+
+        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
             static float crossTrackErrorRate;
-            if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
+            static timeUs_t previousCrossTrackErrorUpdateTime;
+
+            if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10)) {
                 static float previousCrossTrackError = 0.0f;
-                crossTrackErrorRate = 0.5f * (crossTrackErrorRate + (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousTimeMonitoringUpdate));
+                crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
+                previousCrossTrackErrorUpdateTime = currentTimeUs;
                 previousCrossTrackError = navCrossTrackError;
             }
 
@@ -416,11 +423,12 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
             float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
             adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f));
 
-            /* Apply additional fine adjustment based on speed of convergence to try and achieve arrival time of around 15s */
-            float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f);
+            /* Apply additional fine adjustment based on speed of convergence to achieve a convergence speed of around 2 m/s */
+            float limit = constrainf((crossTrackErrorRate > 0.0f ? crossTrackErrorRate : 0.0f) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f);
             float rateFactor = limit;
             if (crossTrackErrorRate > 0.0f) {
-                rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0, 30, -limit, limit), -limit, limit);
+                float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f);
+                rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, timeFactor, -limit, limit), -limit, limit);
             }
 
             /* Determine final adjusted virtualTargetBearing */

From b32d14d701c9a670039b2b27b20b7a73a344d330 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 10 Aug 2024 18:25:32 +0100
Subject: [PATCH 04/10] smooth xt error rate

---
 src/main/navigation/navigation_fixedwing.c | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 0c49ad4fda7..3f814513edf 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -408,13 +408,14 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
         navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
 
-        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
+        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) {
             static float crossTrackErrorRate;
             static timeUs_t previousCrossTrackErrorUpdateTime;
+            static float previousCrossTrackError = 0.0f;
 
-            if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10)) {
-                static float previousCrossTrackError = 0.0f;
-                crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
+            if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
+                const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
+                crossTrackErrorRate = 0.5 * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec));
                 previousCrossTrackErrorUpdateTime = currentTimeUs;
                 previousCrossTrackError = navCrossTrackError;
             }
@@ -424,7 +425,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
             adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f));
 
             /* Apply additional fine adjustment based on speed of convergence to achieve a convergence speed of around 2 m/s */
-            float limit = constrainf((crossTrackErrorRate > 0.0f ? crossTrackErrorRate : 0.0f) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f);
+            float limit = constrainf(fabsf(crossTrackErrorRate) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f);
             float rateFactor = limit;
             if (crossTrackErrorRate > 0.0f) {
                 float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f);

From 3efc75e825b443bd3810bb3dd62f04532b544d93 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 10 Aug 2024 21:52:54 +0100
Subject: [PATCH 05/10] Update navigation_fixedwing.c

---
 src/main/navigation/navigation_fixedwing.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 3f814513edf..751c0698b38 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -415,7 +415,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
             if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
                 const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
-                crossTrackErrorRate = 0.5 * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec));
+                crossTrackErrorRate = 0.5f * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec));
                 previousCrossTrackErrorUpdateTime = currentTimeUs;
                 previousCrossTrackError = navCrossTrackError;
             }

From d7142fc4dab3a70ba442d11ae42fb69afee8e006 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 18 Aug 2024 23:01:12 +0100
Subject: [PATCH 06/10] improved method, setting updated

---
 docs/Settings.md                           |  4 +-
 src/main/fc/settings.yaml                  |  9 ++-
 src/main/navigation/navigation_fixedwing.c | 80 +++++++++++++---------
 3 files changed, 55 insertions(+), 38 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 6487b44b6d1..c5965d13361 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3524,11 +3524,11 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within
 
 ### nav_fw_wp_tracking_accuracy
 
-Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable.
+Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0 | 0 | 10 |
+| OFF | OFF | ON |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 6c16ce93b21..d3aa1f481fc 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -83,7 +83,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", 
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
       "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"]
   - name: aux_operator
     values: ["OR", "AND"]
@@ -2545,11 +2545,10 @@ groups:
         min: 1
         max: 9
       - name: nav_fw_wp_tracking_accuracy
-        description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable."
-        default_value: 0
+        description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible."
+        default_value: OFF
         field: fw.wp_tracking_accuracy
-        min: 0
-        max: 10
+        type: bool
       - name: nav_fw_wp_tracking_max_angle
         description: "Sets the maximum allowed alignment convergence angle to the waypoint course line when nav_fw_wp_tracking_accuracy is active [degrees]. Lower values result in smoother alignment with the course line but will take more distance until this is achieved."
         default_value: 60
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 751c0698b38..4d8215c688d 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -399,42 +399,60 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
         virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
     }
 
-    /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
+    // Calculate cross track error
+    fpVector3_t virtualCoursePoint;
+    virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
+                           posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+    virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
+                           posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+    navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
+
+    /* If waypoint tracking enabled force craft toward and ckosely track along waypoint course line */
     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
-        fpVector3_t virtualCoursePoint;
-        virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
-                               posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
-        virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
-                               posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
-        navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
-
-        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) {
-            static float crossTrackErrorRate;
-            static timeUs_t previousCrossTrackErrorUpdateTime;
-            static float previousCrossTrackError = 0.0f;
-
-            if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
-                const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
-                crossTrackErrorRate = 0.5f * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec));
-                previousCrossTrackErrorUpdateTime = currentTimeUs;
-                previousCrossTrackError = navCrossTrackError;
-            }
+        static float crossTrackErrorRate;
+        static timeUs_t previousCrossTrackErrorUpdateTime;
+        static float previousCrossTrackError = 0.0f;
+        static pt1Filter_t fwCrossTrackErrorRateFilterState;
 
-            /* Apply basic adjustment to factor up virtualTargetBearing error based on navCrossTrackError */
-            float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
-            adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f));
-
-            /* Apply additional fine adjustment based on speed of convergence to achieve a convergence speed of around 2 m/s */
-            float limit = constrainf(fabsf(crossTrackErrorRate) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f);
-            float rateFactor = limit;
-            if (crossTrackErrorRate > 0.0f) {
-                float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f);
-                rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, timeFactor, -limit, limit), -limit, limit);
+        if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
+            const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
+
+            if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
+                crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
             }
+            crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
+
+            previousCrossTrackErrorUpdateTime = currentTimeUs;
+            previousCrossTrackError = navCrossTrackError;
+        }
+
+        /* Dynamic tracking deadband set at minimum 2m for baseline speed of 60 km/h */
+        float trackingDeadband = 200.0f * MAX(posControl.actualState.velXY / 1700.0f, 1.0f);
 
-            /* Determine final adjusted virtualTargetBearing */
+        /* cutTurnActive improves convergence with course line when WP turn smoothing CUT used */
+        static bool cutTurnActive = false;
+        if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
+            cutTurnActive = true;
+        }
+
+        if (cutTurnActive && !posControl.flags.wpTurnSmoothingActive) {
+            virtualTargetBearing = wrap_36000(virtualTargetBearing - wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing));
+            cutTurnActive = ABS(wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog)) > 500;
+        } else if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) &&
+                  navCrossTrackError > trackingDeadband) {
+            float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
             uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
-            adjustmentFactor = constrainf(adjustmentFactor + rateFactor * SIGN(adjustmentFactor), -angleLimit, angleLimit);
+
+            /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile
+             * Adjustment limited to navCrossTrackError as course line approached to avoid instability */
+            float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
+            float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
+            float limit = MIN(angleLimit, navCrossTrackError);
+
+            adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
+            adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
+
+            /* Calculate final adjusted virtualTargetBearing */
             virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
         }
     }

From b2aef2524478104baab81235d2da43be7b2eca86 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 20 Aug 2024 17:38:18 +0100
Subject: [PATCH 07/10] bin cut turn and change setting use

---
 docs/Settings.md                           |  4 +--
 src/main/fc/settings.yaml                  |  7 +++--
 src/main/navigation/navigation_fixedwing.c | 33 ++++++----------------
 3 files changed, 15 insertions(+), 29 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index c5965d13361..6ab03e46334 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3524,11 +3524,11 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within
 
 ### nav_fw_wp_tracking_accuracy
 
-Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible.
+Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF | OFF | ON |
+| 0 | 0 | 10 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d3aa1f481fc..36646efb2c0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2545,10 +2545,11 @@ groups:
         min: 1
         max: 9
       - name: nav_fw_wp_tracking_accuracy
-        description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible."
-        default_value: OFF
+        description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy."
+        default_value: 0
         field: fw.wp_tracking_accuracy
-        type: bool
+        min: 0
+        max: 10
       - name: nav_fw_wp_tracking_max_angle
         description: "Sets the maximum allowed alignment convergence angle to the waypoint course line when nav_fw_wp_tracking_accuracy is active [degrees]. Lower values result in smoother alignment with the course line but will take more distance until this is achieved."
         default_value: 60
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 4d8215c688d..e09fbea57a9 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -399,7 +399,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
         virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
     }
 
-    // Calculate cross track error
+    /* Calculate cross track error */
     fpVector3_t virtualCoursePoint;
     virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
                            posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
@@ -407,8 +407,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                            posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
     navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
 
-    /* If waypoint tracking enabled force craft toward and ckosely track along waypoint course line */
-    if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
+    /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */
+     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
         static float crossTrackErrorRate;
         static timeUs_t previousCrossTrackErrorUpdateTime;
         static float previousCrossTrackError = 0.0f;
@@ -416,43 +416,28 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
         if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
             const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
-
             if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
                 crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
             }
             crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
-
             previousCrossTrackErrorUpdateTime = currentTimeUs;
             previousCrossTrackError = navCrossTrackError;
         }
 
-        /* Dynamic tracking deadband set at minimum 2m for baseline speed of 60 km/h */
-        float trackingDeadband = 200.0f * MAX(posControl.actualState.velXY / 1700.0f, 1.0f);
-
-        /* cutTurnActive improves convergence with course line when WP turn smoothing CUT used */
-        static bool cutTurnActive = false;
-        if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
-            cutTurnActive = true;
-        }
+        uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);
 
-        if (cutTurnActive && !posControl.flags.wpTurnSmoothingActive) {
-            virtualTargetBearing = wrap_36000(virtualTargetBearing - wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing));
-            cutTurnActive = ABS(wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog)) > 500;
-        } else if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) &&
-                  navCrossTrackError > trackingDeadband) {
+        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
             float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
             uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
 
-            /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile
-             * Adjustment limited to navCrossTrackError as course line approached to avoid instability */
+            /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
             float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
-            float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
-            float limit = MIN(angleLimit, navCrossTrackError);
-
+            float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed);
             adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
-            adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
 
             /* Calculate final adjusted virtualTargetBearing */
+            uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
+            adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
             virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
         }
     }

From 8c5c91c9b666b370e9c6940aa51b95260e4048f8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 21 Aug 2024 11:12:16 +0100
Subject: [PATCH 08/10] reinstate fiddle factor

---
 src/main/navigation/navigation_fixedwing.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index e09fbea57a9..fc8a5a051bc 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -432,7 +432,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
             /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
             float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
-            float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed);
+            float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
             adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
 
             /* Calculate final adjusted virtualTargetBearing */

From 29244a4e8c4d28fd5c14b90bb9fca76fa46e3cbb Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 25 Aug 2024 14:16:44 +0100
Subject: [PATCH 09/10] minor fix

---
 src/main/navigation/navigation_fixedwing.c | 73 +++++++++++-----------
 1 file changed, 37 insertions(+), 36 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index fc8a5a051bc..68232ed7e0b 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -399,49 +399,50 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
         virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
     }
 
-    /* Calculate cross track error */
-    fpVector3_t virtualCoursePoint;
-    virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
-                           posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
-    virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
-                           posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
-    navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
-
-    /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */
-     if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !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) {
-                crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
+    if (isWaypointNavTrackingActive()) {
+        /* Calculate cross track error */
+        fpVector3_t virtualCoursePoint;
+        virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
+                               posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+        virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
+                               posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
+        navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);
+
+        /* 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) {
+                    crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
+                }
+                crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
+                previousCrossTrackErrorUpdateTime = currentTimeUs;
+                previousCrossTrackError = navCrossTrackError;
             }
-            crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
-            previousCrossTrackErrorUpdateTime = currentTimeUs;
-            previousCrossTrackError = navCrossTrackError;
-        }
 
-        uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);
+            uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);
 
-        if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
-            float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
-            uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
+            if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
+                float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
+                uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
 
-            /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
-            float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
-            float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
-            adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
+                /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
+                float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
+                float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
+                adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
 
-            /* Calculate final adjusted virtualTargetBearing */
-            uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
-            adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
-            virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
+                /* Calculate final adjusted virtualTargetBearing */
+                uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
+                adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
+                virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
+            }
         }
     }
-
     /*
      * Calculate NAV heading error
      * Units are centidegrees

From 570b301d440a59c15bcc7306303963d62b4e7b17 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 2 Sep 2024 21:16:48 +0100
Subject: [PATCH 10/10] Align update rate for WP distance with XT error

---
 src/main/navigation/navigation_fixedwing.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 68232ed7e0b..ff38b2e17e5 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -401,6 +401,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
     if (isWaypointNavTrackingActive()) {
         /* Calculate cross track error */
+        posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
+
         fpVector3_t virtualCoursePoint;
         virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
                                posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));