Plane: increased safety of guided -> auto quadplane takeoff

when we arm in guided mode then enter a special guided_wait_takeoff
state. We keep motors suppressed until one of the following happens

  1) disarm
  2) guided takeoff command
  3) change to AUTO with a takeoff waypoint as first nav waypoint
  4) change to another mode

while in this state we don't go to throttle unlimited, and will refuse
a change to AUTO mode if the first waypoint is not a takeoff. If we
try to switch to RTL then we will instead use QLAND

This state is needed to cope with the takeoff sequence used by QGC on
common controllers such as the MX16, which do this on a "takeoff"
swipe:

  - changes mode to GUIDED
  - arms
  - changes mode to AUTO

ArduPlane/AP_Arming.cpp

@@ -179,6 +179,16 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure)
         }
     }
 
+    if ((plane.control_mode == &plane.mode_auto) && !plane.mission.starts_with_takeoff_cmd()) {
+        check_failed(display_failure,"missing takeoff waypoint");
+        ret = false;
+    }
+
+    if (plane.control_mode == &plane.mode_rtl) {
+        check_failed(display_failure,"in RTL mode");
+        ret = false;
+    }
+    
     /*
       Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
      */

ArduPlane/mode_auto.cpp

@@ -4,6 +4,16 @@
 bool ModeAuto::_enter()
 {
 #if HAL_QUADPLANE_ENABLED
+    // check if we should refuse auto mode due to a missing takeoff in
+    // guided_wait_takeoff state
+    if (plane.previous_mode == &plane.mode_guided &&
+        quadplane.guided_wait_takeoff_on_mode_enter) {
+        if (!plane.mission.starts_with_takeoff_cmd()) {
+            gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required");
+            return false;
+        }
+    }
+    
     if (plane.quadplane.available() && plane.quadplane.enable == 2) {
         plane.auto_state.vtol_mode = true;
     } else {

ArduPlane/mode_qrtl.cpp

@@ -5,6 +5,13 @@
 
 bool ModeQRTL::_enter()
 {
+    // treat QRTL as QLAND if we are in guided wait takeoff state, to cope
+    // with failsafes during GUIDED->AUTO takeoff sequence
+    if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
+       plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
+       return true;
+    }
+
     // use do_RTL() to setup next_WP_loc
     plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL);
     plane.prev_WP_loc = plane.current_loc;

ArduPlane/mode_rtl.cpp

@@ -8,10 +8,15 @@ bool ModeRTL::_enter()
     plane.rtl.done_climb = false;
 #if HAL_QUADPLANE_ENABLED
     plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
-#endif
+
+    // treat RTL as QLAND if we are in guided wait takeoff state, to cope
+    // with failsafes during GUIDED->AUTO takeoff sequence
+    if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
+       plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
+       return true;
+    }
 
     // do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination
-#if HAL_QUADPLANE_ENABLED
     switch_QRTL(false);
 #endif
 

ArduPlane/quadplane.cpp

@@ -1266,6 +1266,10 @@ void QuadPlane::set_armed(bool armed)
         return;
     }
     motors->armed(armed);
+
+    if (plane.control_mode == &plane.mode_guided) {
+        guided_wait_takeoff = armed;
+    }
 }
 
 
@@ -1793,6 +1797,10 @@ void QuadPlane::update_throttle_suppression(void)
         return;
     }
 
+    if (guided_wait_takeoff) {
+        goto idle_state;
+    }
+
     /* if the users throttle is above zero then allow motors to run
 
        if the user has unset the "check throttle zero when arming"
@@ -1833,6 +1841,7 @@ void QuadPlane::update_throttle_suppression(void)
         return;
     }
 
+idle_state:
     // motors should be in the spin when armed state to warn user they could become active
     set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
     motors->set_throttle(0);
@@ -3506,6 +3515,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude)
     set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     guided_start();
     guided_takeoff = true;
+    guided_wait_takeoff = false;
     if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
         ahrs.set_takeoff_expected(true);
     }
@@ -4082,6 +4092,11 @@ void QuadPlane::mode_enter(void)
     poscontrol.xy_correction.zero();
     poscontrol.velocity_match.zero();
     poscontrol.last_velocity_match_ms = 0;
+
+    // clear guided takeoff wait on any mode change, but remember the
+    // state for special behaviour
+    guided_wait_takeoff_on_mode_enter = guided_wait_takeoff;
+    guided_wait_takeoff = false;
 }
 
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -403,6 +403,30 @@ private:
     // are we in a guided takeoff?
     bool guided_takeoff:1;
 
+    /* if we arm in guided mode when we arm then go into a "waiting
+       for takeoff command" state. In this state we are waiting for
+       one of the following:
+
+       1) disarm
+       2) guided takeoff command
+       3) change to AUTO with a takeoff waypoint as first nav waypoint
+       4) change to another mode
+
+       while in this state we don't go to throttle unlimited, and will
+       refuse a change to AUTO mode if the first waypoint is not a
+       takeoff. If we try to switch to RTL then we will instead use
+       QLAND
+
+       This state is needed to cope with the takeoff sequence used
+       by QGC on common controllers such as the MX16, which do this on a "takeoff" swipe:
+
+          - changes mode to GUIDED
+          - arms
+          - changes mode to AUTO
+    */
+    bool guided_wait_takeoff;
+    bool guided_wait_takeoff_on_mode_enter;
+
     struct {
         // time when motors reached lower limit
         uint32_t lower_limit_start_ms;