diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index c42dd7777e..6ba3d193b7 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -134,7 +134,7 @@ static void calc_altitude_error()
         control_mode == CRUISE) {
         return;
     }
-    if (nav_controller->reached_loiter_target()) {
+    if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
         // once we reach a loiter target then lock to the final
         // altitude target
         target_altitude_cm = next_WP_loc.alt;