From a3036fc4438cd278e148960473430ad1398ae429 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Apr 2014 16:29:32 +0900
Subject: [PATCH] Copter: init pos_control z-axis after setting speed and accel

---
 ArduCopter/control_althold.pde  | 6 +++---
 ArduCopter/control_autotune.pde | 6 +++---
 ArduCopter/control_hybrid.pde   | 6 +++---
 ArduCopter/control_land.pde     | 7 ++++---
 ArduCopter/control_ofloiter.pde | 5 +++--
 5 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde
index f15818056..5be0461d4 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 d57f40f53..751785814 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 a0b309328..7ccc0df9a 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 97f7dee16..fb7fd699f 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 32ddd011d..a226bfba2 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;
-- 
GitLab