diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde
index 843b175ca7..dbc0f7fdf6 100644
--- a/ArduCopter/control_althold.pde
+++ b/ArduCopter/control_althold.pde
@@ -27,10 +27,10 @@ static void althold_run()
 
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
-        // To-Do: reset altitude target if we're somehow not landed?
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
@@ -61,6 +61,7 @@ static void althold_run()
         attitude_control.set_yaw_target_to_current_heading();
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
+        pos_control.set_alt_target_to_current_alt();
     }else{
         // call attitude controller
         attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde
index 298b3fcb43..572ce1d45c 100644
--- a/ArduCopter/control_autotune.pde
+++ b/ArduCopter/control_autotune.pde
@@ -216,6 +216,7 @@ static void autotune_run()
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
@@ -245,6 +246,7 @@ static void autotune_run()
         attitude_control.set_yaw_target_to_current_heading();
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
+        pos_control.set_alt_target_to_current_alt();
     }else{
         // check if pilot is overriding the controls
         if (target_roll != 0 || target_pitch != 0 || target_yaw_rate != 0.0f || target_climb_rate != 0) {
diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde
index fb2b52ee84..f336c3f3ea 100644
--- a/ArduCopter/control_circle.pde
+++ b/ArduCopter/control_circle.pde
@@ -38,6 +38,7 @@ static void circle_run()
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde
index c5d46d47c7..3883342e6f 100644
--- a/ArduCopter/control_loiter.pde
+++ b/ArduCopter/control_loiter.pde
@@ -38,6 +38,7 @@ static void loiter_run()
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
@@ -74,6 +75,7 @@ static void loiter_run()
         attitude_control.set_yaw_target_to_current_heading();
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
+        pos_control.set_alt_target_to_current_alt();
     }else{
         // run loiter controller
         wp_nav.update_loiter();
diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde
index 0558d39f05..a54a7b3243 100644
--- a/ArduCopter/control_ofloiter.pde
+++ b/ArduCopter/control_ofloiter.pde
@@ -37,6 +37,7 @@ static void ofloiter_run()
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         reset_optflow_I();
         return;
     }
@@ -70,6 +71,7 @@ static void ofloiter_run()
         attitude_control.set_yaw_target_to_current_heading();
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
+        pos_control.set_alt_target_to_current_alt();
         reset_optflow_I();
     }else{
         // mix in user control with optical flow
diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde
index 7134dd537b..beec114b5b 100644
--- a/ArduCopter/control_poshold.pde
+++ b/ArduCopter/control_poshold.pde
@@ -122,7 +122,6 @@ static bool poshold_init(bool ignore_checks)
         // set target to current position
         // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
         wp_nav.init_loiter_target();
-
     }else{
         // if not landed start in pilot override to avoid hard twitch
         poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
@@ -160,6 +159,7 @@ static void poshold_run()
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
@@ -190,6 +190,7 @@ static void poshold_run()
         attitude_control.set_yaw_target_to_current_heading();
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }else{
         // convert pilot input to lean angles
diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde
index 180a976cdd..7626320848 100644
--- a/ArduCopter/control_sport.pde
+++ b/ArduCopter/control_sport.pde
@@ -29,6 +29,7 @@ static void sport_run()
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
+        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
@@ -80,6 +81,7 @@ static void sport_run()
         attitude_control.set_yaw_target_to_current_heading();
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
+        pos_control.set_alt_target_to_current_alt();
     }else{
 
         // call attitude controller