diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index b8d1222d00..929e8683a2 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1368,6 +1368,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
     switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
         case NAV_WP_ACTION_WAYPOINT:
             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
+            posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
+            posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
 
         case NAV_WP_ACTION_RTH:
@@ -1392,7 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
                 else {
-                    setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+                    fpVector3_t tmpWaypoint;
+                    tmpWaypoint.x = posControl.activeWaypoint.pos.x;
+                    tmpWaypoint.y = posControl.activeWaypoint.pos.y;
+                    tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
+                        posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
+                        posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
+                    setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
                     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
                 }
                 break;
@@ -2040,15 +2048,15 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
 {
     // We consider waypoint reached if within specified radius
-    const uint32_t wpDistance = calculateDistanceToDestination(pos);
+    posControl.wpDistance = calculateDistanceToDestination(pos);
 
     if (STATE(FIXED_WING) && isWaypointHome) {
         // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
-        return (wpDistance <= navConfig()->general.waypoint_radius)
-                || (wpDistance <= (navConfig()->fw.loiter_radius * 1.10f));  // 10% margin of desired circular loiter radius
+        return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
+                || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f));  // 10% margin of desired circular loiter radius
     }
     else {
-        return (wpDistance <= navConfig()->general.waypoint_radius);
+        return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
     }
 }
 
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 14eaa21a3a..a11e9a0f62 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -354,6 +354,9 @@ typedef struct {
 
     navWaypointPosition_t       activeWaypoint;     // Local position and initial bearing, filled on waypoint activation
     int8_t                      activeWaypointIndex;
+    float                       wpInitialAltitude; // Altitude at start of WP
+    float                       wpInitialDistance; // Distance when starting flight to WP
+    float                       wpDistance;        // Distance to active WP
 
     /* Internals & statistics */
     int16_t                     rcAdjustment[4];