Copter: switch to pos control when takeoff finish

ArduCopter/mode_auto.cpp

@@ -729,6 +729,10 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
 void Copter::ModeAuto::takeoff_run()
 {
     auto_takeoff_run();
+    if (wp_nav->reached_wp_destination()) {
+        const Vector3f target = wp_nav->get_wp_destination();
+        wp_start(target);
+    }
 }
 
 // auto_wp_run - runs the auto waypoint controller

ArduCopter/mode_guided.cpp

@@ -366,6 +366,10 @@ void Copter::ModeGuided::run()
 void Copter::ModeGuided::takeoff_run()
 {
     auto_takeoff_run();
+    if (wp_nav->reached_wp_destination()) {
+        const Vector3f target = wp_nav->get_wp_destination();
+        set_destination(target);
+    }
 }
 
 void Copter::Mode::auto_takeoff_run()