diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde
index f15818056fa49c8689ea209a4d97c85a22656197..5be0461d4af1fe2b7774f5be58335f4ec16b6349 100644
--- a/ArduCopter/control_althold.pde
+++ b/ArduCopter/control_althold.pde
@@ -7,13 +7,13 @@
 // althold_init - initialise althold controller
 static bool althold_init(bool ignore_checks)
 {
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
-
     // initialize vertical speeds and leash lengths
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
+    // initialise altitude target to stopping point
+    pos_control.set_target_to_stopping_point_z();
+
     return true;
 }
 
diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde
index d57f40f53f19e0fc05be460c9474995e3985d353..751785814cd704a90fc93346be59cbfb49d083d7 100644
--- a/ArduCopter/control_autotune.pde
+++ b/ArduCopter/control_autotune.pde
@@ -192,13 +192,13 @@ static bool autotune_init(bool ignore_checks)
         return false;
     }
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
-
     // initialize vertical speeds and leash lengths
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
+    // initialise altitude target to stopping point
+    pos_control.set_target_to_stopping_point_z();
+
     return true;
 }
 
diff --git a/ArduCopter/control_hybrid.pde b/ArduCopter/control_hybrid.pde
index a0b309328bd5c3d8bdfad497dfc5950bd87ccd70..7ccc0df9aebb94a9192e103ef381468a4575b136 100644
--- a/ArduCopter/control_hybrid.pde
+++ b/ArduCopter/control_hybrid.pde
@@ -100,14 +100,14 @@ static bool hybrid_init(bool ignore_checks)
     if (!GPS_ok() && !ignore_checks) {
         return false;
     }
-
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
     
     // initialize vertical speeds and leash lengths
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
 
+    // initialise altitude target to stopping point
+    pos_control.set_target_to_stopping_point_z();
+
     // initialise lean angles to current attitude
     hybrid.pilot_roll = 0;
     hybrid.pilot_pitch = 0;
diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde
index 97f7dee1612a92d7fc29ee657663d25b526846d7..fb7fd699f65651e0e206fa9c7f4188f6b573a068 100644
--- a/ArduCopter/control_land.pde
+++ b/ArduCopter/control_land.pde
@@ -16,12 +16,13 @@ static bool land_init(bool ignore_checks)
         wp_nav.set_loiter_target(stopping_point);
     }
 
-    // initialise altitude target to stopping point
-    pos_control.set_target_to_stopping_point_z();
-
     // initialize vertical speeds and leash lengths
     pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
     pos_control.set_accel_z(wp_nav.get_accel_z());
+
+    // initialise altitude target to stopping point
+    pos_control.set_target_to_stopping_point_z();
+
     return true;
 }
 
diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde
index 32ddd011dbdc02ddb828d905dc81081618acc77e..a226bfba2d75065604489e5eff7ce1169bd43f20 100644
--- a/ArduCopter/control_ofloiter.pde
+++ b/ArduCopter/control_ofloiter.pde
@@ -9,13 +9,14 @@ static bool ofloiter_init(bool ignore_checks)
 {
 #if OPTFLOW == ENABLED
     if (g.optflow_enabled || ignore_checks) {
-        // initialise altitude target to stopping point
-        pos_control.set_target_to_stopping_point_z();
 
         // initialize vertical speed and acceleration
         pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
         pos_control.set_accel_z(g.pilot_accel_z);
 
+        // initialise altitude target to stopping point
+        pos_control.set_target_to_stopping_point_z();
+
         return true;
     }else{
         return false;