Plane: add option do do a VTOL approach RTL

ArduPlane/Plane.h

@@ -348,6 +348,7 @@ private:
     } failsafe;
 
     enum Landing_ApproachStage {
+        RTL,
         LOITER_TO_ALT,
         ENSURE_RADIUS,
         WAIT_FOR_BREAKOUT,

ArduPlane/commands_logic.cpp

@@ -953,6 +953,18 @@ void Plane::exit_mission_callback()
 bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
 {
     switch (vtol_approach_s.approach_stage) {
+        case RTL:
+            {
+                // fly home and loiter at RTL alt
+                update_loiter(fabsf(quadplane.fw_land_approach_radius));
+                if (plane.reached_loiter_target()) {
+                    // decend to Q RTL alt
+                    plane.do_RTL(plane.home.alt + plane.quadplane.qrtl_alt*100UL);
+                    plane.loiter_angle_reset();
+                    vtol_approach_s.approach_stage = LOITER_TO_ALT;
+                }
+                break;
+            }
         case LOITER_TO_ALT:
             {
                 update_loiter(fabsf(quadplane.fw_land_approach_radius));

ArduPlane/mode_rtl.cpp

@@ -6,6 +6,7 @@ bool ModeRTL::_enter()
     plane.prev_WP_loc = plane.current_loc;
     plane.do_RTL(plane.get_RTL_altitude());
     plane.rtl.done_climb = false;
+    plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
 
     // 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
     switch_QRTL(false);
@@ -49,9 +50,23 @@ void ModeRTL::update()
 
 void ModeRTL::navigate()
 {
+    if (plane.control_mode->mode_number() != QRTL) {
+        // QRTL shares this navigate function with RTL
 
-    if ((AP_HAL::millis() - plane.last_mode_change_ms > 1000) && switch_QRTL()) {
-        return;
+        if (plane.quadplane.available() && (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL)) {
+            // VTOL approach landing
+            AP_Mission::Mission_Command cmd;
+            cmd.content.location = plane.next_WP_loc;
+            plane.verify_landing_vtol_approach(cmd);
+            if (plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING) {
+                plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
+            }
+            return;
+        }
+
+        if ((AP_HAL::millis() - plane.last_mode_change_ms > 1000) && switch_QRTL()) {
+            return;
+        }
     }
 
     if (plane.g.rtl_autoland == 1 &&
@@ -94,7 +109,7 @@ void ModeRTL::navigate()
 // Switch to QRTL if enabled and within radius
 bool ModeRTL::switch_QRTL(bool check_loiter_target)
 {
-    if (!plane.quadplane.available() || (plane.quadplane.rtl_mode != 1)) {
+    if (!plane.quadplane.available() || (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL)) {
         return false;
     }
 

ArduPlane/navigation.cpp

@@ -286,7 +286,7 @@ void Plane::update_loiter(uint16_t radius)
                 (control_mode == &mode_auto || control_mode == &mode_guided) &&
                 auto_state.crosstrack &&
                 current_loc.get_distance(next_WP_loc) > radius*3) ||
-               (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == 1)) {
+               (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL)) {
         /*
           if never reached loiter point and using crosstrack and somewhat far away from loiter point
           navigate to it like in auto-mode for normal crosstrack behavior

ArduPlane/quadplane.cpp

@@ -194,8 +194,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: RTL_MODE
     // @DisplayName: VTOL RTL mode
-    // @Description: If this is set to 1 then an RTL will change to QRTL when within RTL_RADIUS meters of the RTL destination
-    // @Values: 0:Disabled,1:Enabled
+    // @Description: If this is set to 1 then an RTL will change to QRTL when within RTL_RADIUS meters of the RTL destination, VTOL approach: vehicle will RTL at RTL alt and circle with a radius of Q_FW_LND_APR_RAD down to Q_RLT_ALT and then transission into the wind and QRTL, see 'AUTO VTOL Landing'
+    // @Values: 0:Disabled,1:Enabled,2:VTOL approach
     // @User: Standard
     AP_GROUPINFO("RTL_MODE", 36, QuadPlane, rtl_mode, 0),
 

ArduPlane/quadplane.h

@@ -350,6 +350,11 @@ private:
 
     // control if a VTOL RTL will be used
     AP_Int8 rtl_mode;
+    enum RTL_MODE{
+        NONE,
+        SWITCH_QRTL,
+        VTOL_APPROACH_QRTL,
+    };
 
     // control if a VTOL GUIDED will be used
     AP_Int8 guided_mode;