bin cut turn and change setting use

breadoven · Aug 20, 2024 · b2aef25 · b2aef25
1 parent d7142fc
commit b2aef25
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 29 deletions.
diff --git 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
@@ -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
@@ -399,60 +399,45 @@ 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));
     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) {
+    /* 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;
             }
             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);
         }
     }