Align update rate for WP distance with XT error

iNavFlight · Sep 2, 2024 · 570b301 · 570b301
1 parent 29244a4
commit 570b301
Showing 1 changed file with 2 additions and 0 deletions.
diff --git 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));