Copter: fix position ctrl init for guided takeof

ArduCopter/mode.cpp

@@ -544,6 +544,8 @@ void Mode::make_safe_ground_handling(bool force_throttle_unlimited)
         break;
     }
 
+    pos_control->relax_velocity_controller_xy();
+    pos_control->update_xy_controller();
     pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
     pos_control->update_z_controller();
     // we may need to move this out

ArduCopter/mode_auto.cpp

@@ -900,7 +900,6 @@ void ModeAuto::wp_run()
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
-        wp_nav->wp_and_spline_init();
         return;
     }
 
@@ -997,7 +996,6 @@ void ModeAuto::loiter_run()
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
-        wp_nav->wp_and_spline_init();
         return;
     }
 
@@ -1023,7 +1021,7 @@ void ModeAuto::loiter_to_alt_run()
 {
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if (is_disarmed_or_landed() || !motors->get_interlock()) {
-        zero_throttle_and_relax_ac();
+        make_safe_ground_handling();
         return;
     }
 

ArduCopter/mode_brake.cpp

@@ -37,7 +37,6 @@ void ModeBrake::run()
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
-        pos_control->relax_velocity_controller_xy();
         pos_control->relax_z_controller(0.0f);
         return;
     }