Plane: wait till motors are fully up before takeoff in guided mode

this allows for guided mode takeoff in tilt-rotors. Otherwise motors
till be pointing forward and takeoff will go very badly

ArduPlane/quadplane.cpp

@@ -2697,6 +2697,13 @@ void QuadPlane::takeoff_controller(void)
     if (!hal.util->get_soft_armed()) {
         return;
     }
+
+    if (plane.control_mode == &plane.mode_guided && guided_takeoff
+        && tiltrotor.enabled() && !tiltrotor.fully_up()) {
+        // waiting for motors to tilt up
+        return;
+    }
+
     /*
       for takeoff we use the position controller
     */
@@ -2724,7 +2731,7 @@ void QuadPlane::takeoff_controller(void)
                                                                   get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
 
     float vel_z = wp_nav->get_default_speed_up();
-    if (guided_takeoff) {
+    if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
         // for guided takeoff we aim for a specific height with zero
         // velocity at that height
         Location origin;

ArduPlane/tiltrotor.cpp

@@ -479,6 +479,17 @@ bool Tiltrotor::fully_fwd(void) const
     return (current_tilt >= get_fully_forward_tilt());
 }
 
+/*
+  return true if the rotors are fully tilted up
+ */
+bool Tiltrotor::fully_up(void) const
+{
+    if (!enabled() || (tilt_mask == 0)) {
+        return false;
+    }
+    return (current_tilt <= 0);
+}
+
 /*
   control vectoring for tilt multicopters
  */

ArduPlane/tiltrotor.h

@@ -49,6 +49,7 @@ public:
     }
 
     bool fully_fwd() const;
+    bool fully_up() const;
     float tilt_max_change(bool up, bool in_flap_range = false) const;
     float get_fully_forward_tilt() const;
     float get_forward_flight_tilt() const;