Copter: init pos_control z-axis after setting speed and accel

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;
 }
 

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;
 }
 

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;

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;
 }
 

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;