Plane: added GUIDED takeoff for quadplanes

this allows takeoff in GUIDED mode in the same way as copters

ArduPlane/GCS_Mavlink.cpp

@@ -1094,6 +1094,18 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             result = MAV_RESULT_ACCEPTED;
             break;
 
+        case MAV_CMD_NAV_TAKEOFF: {
+            // user takeoff only works with quadplane code for now
+            // param7 : altitude [metres]
+            float takeoff_alt = packet.param7;
+            if (plane.quadplane.available() && plane.quadplane.do_user_takeoff(takeoff_alt)) {
+                result = MAV_RESULT_ACCEPTED;
+            } else {
+                result = MAV_RESULT_FAILED;
+            }
+            break;
+        }
+            
 #if MOUNT == ENABLED
         // Sets the region of interest (ROI) for the camera
         case MAV_CMD_DO_SET_ROI:

ArduPlane/quadplane.cpp

@@ -861,6 +861,9 @@ bool QuadPlane::is_flying(void)
     if (!available()) {
         return false;
     }
+    if (plane.control_mode == GUIDED && guided_takeoff) {
+        return true;
+    }
     if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) {
         return true;
     }
@@ -895,6 +898,9 @@ bool QuadPlane::is_flying_vtol(void)
         // if we are demanding more than 1% throttle then don't consider aircraft landed
         return true;
     }
+    if (plane.control_mode == GUIDED && guided_takeoff) {
+        return true;
+    }
     if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER) {
         // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
         return plane.channel_throttle->get_control_in() > 0;
@@ -980,6 +986,8 @@ void QuadPlane::control_loiter()
         float descent_rate = (poscontrol.state == QPOS_LAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground);
         pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
         check_land_complete();
+    } else if (plane.control_mode == GUIDED && guided_takeoff) {
+        pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, false);
     } else {
         // update altitude target and call position controller
         pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
@@ -1534,6 +1542,9 @@ bool QuadPlane::init_mode(void)
     case QRTL:
         init_qrtl();
         break;
+    case GUIDED:
+        guided_takeoff = false;
+        break;
     default:
         break;
     }
@@ -2287,6 +2298,7 @@ void QuadPlane::guided_start(void)
 {
     poscontrol.state = QPOS_POSITION1;
     poscontrol.speed_scale = 0;
+    guided_takeoff = false;
     setup_target_position();
     poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
 }
@@ -2296,8 +2308,15 @@ void QuadPlane::guided_start(void)
  */
 void QuadPlane::guided_update(void)
 {
-    // run VTOL position controller
-    vtol_position_controller();
+    if (plane.control_mode == GUIDED && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
+        throttle_wait = false;
+        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+        takeoff_controller();
+    } else {
+        guided_takeoff = false;
+        // run VTOL position controller
+        vtol_position_controller();
+    }
 }
 
 void QuadPlane::afs_terminate(void)
@@ -2341,3 +2360,32 @@ void QuadPlane::adjust_alt_target(float altitude_cm)
     float target_cm = constrain_float(altitude_cm, current_alt-50, current_alt+50);
     pos_control->set_alt_target(target_cm);
 }
+
+// user initiated takeoff for guided mode
+bool QuadPlane::do_user_takeoff(float takeoff_altitude)
+{
+    if (plane.control_mode != GUIDED) {
+        gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode");
+        return false;
+    }
+    if (!hal.util->get_soft_armed()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Must be armed for takeoff");
+        return false;
+    }
+    if (guided_mode == 0) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Q_GUIDED_MODE must be set to 1");
+        return false;
+    }
+    if (is_flying()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Already flying - no takeoff");
+        return false;
+    }
+    plane.auto_state.vtol_loiter = true;
+    plane.prev_WP_loc = plane.current_loc;
+    plane.next_WP_loc = plane.current_loc;
+    plane.next_WP_loc.alt += takeoff_altitude*100;
+    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+    guided_start();
+    guided_takeoff = true;
+    return false;
+}

ArduPlane/quadplane.h

@@ -88,6 +88,9 @@ public:
     
     // check if we have completed transition
     bool tailsitter_transition_complete(void);
+
+    // user initiated takeoff for guided mode
+    bool do_user_takeoff(float takeoff_altitude);
     
     struct PACKED log_QControl_Tuning {
         LOG_PACKET_HEADER;
@@ -292,6 +295,9 @@ private:
     // true when in angle assist
     bool in_angle_assist:1;
 
+    // are we in a guided takeoff?
+    bool guided_takeoff:1;
+
     struct {
         // time when motors reached lower limit
         uint32_t lower_limit_start_ms;