revert back to original tracking accuracy method

src/main/fc/settings.yaml

@@ -2421,7 +2421,7 @@ groups:
         min: 0
         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 5 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."
+        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
         field: fw.waypoint_tracking_accuracy
         min: 0

src/main/navigation/navigation_fixedwing.c

@@ -73,6 +73,7 @@ static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
 static int8_t loiterDirYaw = 1;
+bool needToCalculateCircularLoiter;
 
 // Calculates the cutoff frequency for smoothing out roll/pitch commands
 // control_smoothness valid range from 0 to 9
@@ -276,37 +277,35 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
     uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
     fpVector3_t loiterCenterPos = posControl.desiredState.pos;
     int8_t loiterTurnDirection = loiterDirection();
-    bool isWpTrackingActive = isWaypointNavTrackingActive();
 
     // Detemine if a circular loiter is required.
     // For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
     #define TAN_15DEG    0.26795f
-    bool needToCalculateCircularLoiter = isNavHoldPositionActive() &&
+    needToCalculateCircularLoiter = isNavHoldPositionActive() &&
-                                         (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
+                                     (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
-                                         (distanceToActualTarget > 50.0f);
+                                     (distanceToActualTarget > 50.0f);
 
     /* WP turn smoothing option - switches to loiter path around waypoint using navLoiterRadius.
      * Loiter centered on point inside turn at required distance from waypoint and
      * on a bearing midway between current and next waypoint course bearings.
      * Works for turns > 30 degs and < 120 degs.
      * 2 options, 1 = pass through WP, 2 = cut inside turn missing WP */
-
     int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle);
     posControl.flags.wpTurnSmoothingActive = false;
-    if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWpTrackingActive && !needToCalculateCircularLoiter) {
+    if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
         // turnFactor adjusts start of loiter based on turn angle
         float turnFactor = 0.0f;
         if (navConfig()->fw.waypoint_turn_smoothing == 1) {     // passes through WP
             turnFactor = waypointTurnAngle / 6000.0f;
         } else {
-            turnFactor = 1.0f / tan_approx(CENTIDEGREES_TO_RADIANS(9000.0f - waypointTurnAngle / 2.0f));    // cut inside turn missing WP
+            turnFactor = tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f));    // cut inside turn missing WP
         }
-        constrainf(turnFactor, 0.5f, 1.5f);
+        constrainf(turnFactor, 0.5f, 2.0f);
         if (posControl.wpDistance < navLoiterRadius * turnFactor) {
             int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
             float distToTurnCentre = navLoiterRadius;
             if (navConfig()->fw.waypoint_turn_smoothing == 2) {
-                distToTurnCentre = navLoiterRadius / sin_approx(CENTIDEGREES_TO_RADIANS(9000.0f - waypointTurnAngle / 2.0f));
+                distToTurnCentre = navLoiterRadius / cos_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f));
             }
             loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
             loiterCenterPos.y = posControl.activeWaypoint.pos.y + distToTurnCentre * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
@@ -331,36 +330,6 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
         posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
         posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
         distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
-    } else if (navConfig()->fw.waypoint_tracking_accuracy && isWpTrackingActive) {
-        // track along waypoint course line if tracking accuracy used
-        float tempPosErrorX;
-        float tempPosErrorY;
-        fpVector3_t currentWPPos = posControl.activeWaypoint.pos;
-        uint32_t currentCourse = posControl.activeWaypoint.yaw;
-
-        if (currentCourse == 9000 || currentCourse == 27000) {
-            tempPosErrorX = currentWPPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
-            tempPosErrorY = navGetCurrentActualPositionAndVelocity()->pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
-        } else if (currentCourse == 0 || currentCourse == 18000) {
-            tempPosErrorX = navGetCurrentActualPositionAndVelocity()->pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
-            tempPosErrorY = currentWPPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
-        } else {
-            // using y = mx + c
-            // constant2 = y axis intercept of line normal to course line passing through actual position
-            // used to calculate position on course line normal to actual position
-            float gradient = tan_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw));
-            float constant = currentWPPos.y - gradient * currentWPPos.x;
-            float constant2 = navGetCurrentActualPositionAndVelocity()->pos.y + navGetCurrentActualPositionAndVelocity()->pos.x / gradient;
-            float tempPosX = (constant2 - constant) / (gradient + 1 / gradient);
-            tempPosErrorX = tempPosX - navGetCurrentActualPositionAndVelocity()->pos.x;
-            tempPosErrorY = (gradient * tempPosX + constant) - navGetCurrentActualPositionAndVelocity()->pos.y;
-        }
-        // uint32_t distanceline = calc_length_pythagorean_2D(tempPosErrorX, tempPosErrorY);
-        float distanceRatio = constrainf(posControl.wpDistance > 0 ? trackingDistance * navConfig()->fw.waypoint_tracking_accuracy / posControl.wpDistance :
-        0, 0.0f, 1.0f);
-        posErrorX = distanceRatio * (posErrorX - tempPosErrorX) + tempPosErrorX;
-        posErrorY = distanceRatio * (posErrorY - tempPosErrorY) + tempPosErrorY;
-        distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
     }
 
     // Calculate virtual waypoint
@@ -425,6 +394,34 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     // We have virtual position target, calculate heading error
     int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
 
+    /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
+    if (navConfig()->fw.waypoint_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
+        // only apply course tracking correction if target bearing error < 90 degs or when close to waypoint (within 10m)
+        if (ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) {
+            // courseVirtualCorrection initially used to determine current position relative to course line for later use
+            int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
+
+            float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
+
+            // 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(distToCourseLine / (1000.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f);
+            courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
+
+            // course heading alignment factor
+            int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
+            float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f);
+            courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
+
+            // final courseCorrectionFactor combining distance and heading factors
+            courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
+
+            // final courseVirtualCorrection using max 80 deg heading adjustment toward course line
+            courseVirtualCorrection = 8000 * courseCorrectionFactor;
+            virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
+        }
+    }
+
     /*
      * Calculate NAV heading error
      * Units are centidegrees