Copter: Add guided_takeoff_start and guided_takeoff_run functions

ArduCopter/control_guided.pde

@@ -24,6 +24,24 @@ static bool guided_init(bool ignore_checks)
     }
 }
 
+
+// guided_takeoff_start - initialises waypoint controller to implement take-off
+static void guided_takeoff_start(float final_alt)
+{
+    guided_mode = Guided_TakeOff;
+    
+    // initialise wpnav destination
+    Vector3f target_pos = inertial_nav.get_position();
+    target_pos.z = final_alt;
+    wp_nav.set_wp_destination(target_pos);
+
+    // initialise yaw
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
+
+    // tell motors to do a slow start
+    motors.slow_start(true);
+}
+
 // initialise guided mode's position controller
 void guided_pos_control_start()
 {
@@ -97,15 +115,58 @@ static void guided_run()
         return;
     }
 
-    // run position or velocity controller
-    if (guided_mode == Guided_WP) {
+    // call the correct auto controller
+    switch (guided_mode) {
+
+    case Guided_TakeOff:
+        // run takeoff controller
+        guided_takeoff_run();
+        break;
+
+    case Guided_WP:
+        // run position controller
         guided_pos_control_run();
-    } else {
-        // must be velocity controller
+        break;
+
+    case Guided_Velocity:
+        // run velocity controller
         guided_vel_control_run();
     }
  }
 
+// guided_takeoff_run - takeoff in guided mode
+//      called by guided_run at 100hz or more
+static void guided_takeoff_run()
+{
+    // if not auto armed set throttle to zero and exit immediately
+    if(!ap.auto_armed) {
+        // reset attitude control targets
+        attitude_control.relax_bf_rate_controller();
+        attitude_control.set_yaw_target_to_current_heading();
+        attitude_control.set_throttle_out(0, false);
+        // tell motors to do a slow start
+        motors.slow_start(true);
+        // To-Do: re-initialise wpnav targets
+        return;
+    }
+
+    // process pilot's yaw input
+    float target_yaw_rate = 0;
+    if (!failsafe.radio) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+    }
+
+    // run waypoint controller
+    wp_nav.update_wpnav();
+
+    // call z-axis position controller (wpnav should have already updated it's alt target)
+    pos_control.update_z_controller();
+
+    // roll & pitch from waypoint controller, yaw rate from pilot
+    attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+}
+
 // guided_pos_control_run - runs the guided position controller
 // called from guided_run
 static void guided_pos_control_run()