Copter: PILOT_VELZ_MAX and PILOT_ACCEL_Z take effect immediately

ArduCopter/control_althold.cpp

@@ -30,6 +30,10 @@ void Copter::althold_run()
     AltHoldModeState althold_state;
     float takeoff_climb_rate = 0.0f;
 
+    // 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);
+
     // apply SIMPLE mode transform to pilot inputs
     update_simple_mode();
 

ArduCopter/control_autotune.cpp

@@ -262,6 +262,10 @@ void Copter::autotune_run()
     float target_yaw_rate;
     int16_t target_climb_rate;
 
+    // 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);
+
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     // this should not actually be possible because of the autotune_init() checks
     if (!ap.auto_armed || !motors.get_interlock()) {

ArduCopter/control_circle.cpp

@@ -35,6 +35,12 @@ void Copter::circle_run()
     float target_yaw_rate = 0;
     float target_climb_rate = 0;
 
+    // initialize speeds and accelerations
+    pos_control.set_speed_xy(wp_nav.get_speed_xy());
+    pos_control.set_accel_xy(wp_nav.get_wp_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);
+    
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
         // To-Do: add some initialisation of position controllers

ArduCopter/control_loiter.cpp

@@ -14,7 +14,7 @@ bool Copter::loiter_init(bool ignore_checks)
         // set target to current position
         wp_nav.init_loiter_target();
 
-        // initialize vertical speed and accelerationj
+        // 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);
 
@@ -37,6 +37,10 @@ void Copter::loiter_run()
     float target_climb_rate = 0.0f;
     float takeoff_climb_rate = 0.0f;
 
+    // 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);
+
     // process pilot inputs unless we are in radio failsafe
     if (!failsafe.radio) {
         // apply SIMPLE mode transform to pilot inputs

ArduCopter/control_poshold.cpp

@@ -136,6 +136,10 @@ void Copter::poshold_run()
     float vel_fw, vel_right;            // vehicle's current velocity in body-frame forward and right directions
     const Vector3f& vel = inertial_nav.get_velocity();
 
+    // 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);
+
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if(!ap.auto_armed || !motors.get_interlock()) {
         wp_nav.init_loiter_target();

ArduCopter/control_sport.cpp

@@ -9,7 +9,7 @@
 // sport_init - initialise sport controller
 bool Copter::sport_init(bool ignore_checks)
 {
-    // initialize vertical speed and accelerationj
+    // 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);
 
@@ -28,6 +28,10 @@ void Copter::sport_run()
     float target_climb_rate = 0;
     float takeoff_climb_rate = 0.0f;
 
+    // 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);
+
     // if not armed or throttle at zero, set throttle to zero and exit immediately
     if(!motors.armed() || ap.throttle_zero) {
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);