cr132

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 |
 
 ---
 

src/main/fc/settings.yaml

@@ -2556,10 +2556,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
         # // CR132
       - 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."

src/main/navigation/navigation.c

@@ -4766,9 +4766,9 @@ void navigationUsePIDs(void)
                                         0.0f
     );
 
-    navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
+    navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 50.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 50.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 50.0f,
                                         // (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,  // poss new FF, original was 0.0f
                                         0.0f,
                                         NAV_DTERM_CUT_HZ,

src/main/navigation/navigation_fixedwing.c

@@ -433,47 +433,26 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
         DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing);
         DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate);
 
-        /* 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);
+        uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);
 
-        /* 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;
-        }
-        DEBUG_SET(DEBUG_ALWAYS, 5, cutTurnActive);
-        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);
+            DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed);
+            DEBUG_SET(DEBUG_ALWAYS, 5, 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);
 
-            // old code
-            // float limit = constrainf(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);
-                // rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, timeFactor, -limit, limit), -limit, limit);
-            // }
-
             DEBUG_SET(DEBUG_ALWAYS, 7, limit);
             DEBUG_SET(DEBUG_ALWAYS, 6, adjustmentFactor);
-            DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed);
         }
         DEBUG_SET(DEBUG_ALWAYS, 4, virtualTargetBearing);
     }