Copter: althold uses current alt target if active

Previously we always reset the altitude target to the current altitude but this causes a jump if the vehicle is already in an alt-hold flight mode but has an altitude error

ArduCopter/control_althold.cpp

@@ -21,8 +21,10 @@ bool Copter::althold_init(bool ignore_checks)
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     // stop takeoff if running
     takeoff_stop();

ArduCopter/control_auto.cpp

@@ -361,8 +361,10 @@ void Copter::auto_land_start(const Vector3f& destination)
     wp_nav.init_loiter_target(destination);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target(inertial_nav.get_altitude());
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     // initialise yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);

ArduCopter/control_autotune.cpp

@@ -246,8 +246,10 @@ bool Copter::autotune_start(bool ignore_checks)
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     return true;
 }

ArduCopter/control_brake.cpp

@@ -22,8 +22,10 @@ bool Copter::brake_init(bool ignore_checks)
         pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
 
         // initialise position and desired velocity
-        pos_control.set_alt_target(inertial_nav.get_altitude());
-        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+        if (!pos_control.is_active_z()) {
+            pos_control.set_alt_target_to_current_alt();
+            pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+        }
 
         brake_timeout_ms = 0;
 

ArduCopter/control_guided.cpp

@@ -156,8 +156,10 @@ void Copter::guided_angle_control_start()
     pos_control.set_accel_z(wp_nav.get_accel_z());
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     // initialise targets
     guided_angle_state.update_time_ms = millis();

ArduCopter/control_land.cpp

@@ -24,8 +24,10 @@ bool Copter::land_init(bool ignore_checks)
     pos_control.set_accel_z(wp_nav.get_accel_z());
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
     
     land_start_time = millis();
 

ArduCopter/control_loiter.cpp

@@ -26,8 +26,10 @@ bool Copter::loiter_init(bool ignore_checks)
         pos_control.set_accel_z(g.pilot_accel_z);
 
         // initialise position and desired velocity
-        pos_control.set_alt_target(inertial_nav.get_altitude());
-        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+        if (!pos_control.is_active_z()) {
+            pos_control.set_alt_target_to_current_alt();
+            pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+        }
 
         return true;
     }else{

ArduCopter/control_poshold.cpp

@@ -87,8 +87,10 @@ bool Copter::poshold_init(bool ignore_checks)
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     // initialise lean angles to current attitude
     poshold.pilot_roll = 0;

ArduCopter/control_rtl.cpp

@@ -350,8 +350,10 @@ void Copter::rtl_land_start()
     wp_nav.init_loiter_target(wp_nav.get_wp_destination());
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     // initialise yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);

ArduCopter/control_sport.cpp

@@ -14,8 +14,10 @@ bool Copter::sport_init(bool ignore_checks)
     pos_control.set_accel_z(g.pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    if (!pos_control.is_active_z()) {
+        pos_control.set_alt_target_to_current_alt();
+        pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    }
 
     return true;
 }