Plane: Quadplane: add a loiter stage in VTOL land approach with a loiter time of Q_RTL_PAUSE_TIME

ArduPlane/quadplane.cpp

@@ -572,6 +572,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @Bitmask: 1: Disable thrust loss detection in transtions and fixed wing modes. Thrust loss detection will only run in VTOL modes.
     AP_GROUPINFO("THRST_LOSS_OPT", 42, QuadPlane, thrust_loss.options, 0),
 
+    // @Param: RTL_PAUSE_TIME
+    // @DisplayName: Q RTL pause time.
+    // @Description: Time (in seconds) to pause in a VTOL loiter above landing point before starting final descent. Zero disables. This applies in VTOL landing in auto mode and QRTL mode.
+    // @Units: s
+    // @Range: 0 10
+    AP_GROUPINFO("RTL_PAUSE_TIME", 43, QuadPlane, qrtl_pause_time, 0),
+
     AP_GROUPEND
 };
 
@@ -2671,6 +2678,7 @@ void QuadPlane::vtol_position_controller(void)
     }
 
     case QPOS_POSITION2:
+    case QPOS_PAUSE:
     case QPOS_LAND_ABORT:
     case QPOS_LAND_DESCEND: {
         setup_target_position();
@@ -2821,6 +2829,12 @@ void QuadPlane::vtol_position_controller(void)
         break;
     }
 
+    case QPOS_PAUSE: {
+        // Hold zero climb rate for the duration of the pause
+        set_climb_rate_ms(0);
+        break;
+    }
+
     case QPOS_LAND_DESCEND:
     case QPOS_LAND_ABORT:
     case QPOS_LAND_FINAL: {
@@ -3602,6 +3616,9 @@ bool QuadPlane::verify_vtol_land(void)
         return true;
     }
 
+    // True if land descent should be started
+    bool start_descend = false;
+
     if (poscontrol.get_state() == QPOS_POSITION2) {
         // see if we should move onto the descend stage of landing
         const float descend_dist_threshold_m = 2.0;
@@ -3622,27 +3639,43 @@ bool QuadPlane::verify_vtol_land(void)
         
         if (reached_position &&
             (vel_ned_ms.xy() - approach_vel_ne_ms).length() < descend_speed_threshold_ms) {
-            poscontrol.set_state(QPOS_LAND_DESCEND);
-            poscontrol.pilot_correction_done = false;
             pos_control->set_lean_angle_max_cd(0);
             poscontrol.correction_ne_m.zero();
 #if AP_LANDINGGEAR_ENABLED
             plane.g2.landing_gear.deploy_for_landing();
 #endif
-            last_land_final_agl_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
-            gcs().send_text(MAV_SEVERITY_INFO,"Land descend started");
-            if (plane.control_mode == &plane.mode_auto) {
-                // set height to mission height, so we can use the mission
-                // WP height for triggering land final if no rangefinder
-                // available
-                plane.set_next_WP(plane.mission.get_current_nav_cmd().content.location);
+            // Start loitering if loiter time is set else go directly to descent
+            if (is_positive(qrtl_pause_time.get())) {
+                poscontrol.set_state(QPOS_PAUSE);
+                gcs().send_text(MAV_SEVERITY_INFO,"Land loiter started");
             } else {
-                plane.set_next_WP(plane.next_WP_loc);
-                plane.next_WP_loc.copy_alt_from(ahrs.get_home());
+                start_descend = true;
             }
         }
     }
 
+    // Check if loiter time has passed
+    if ((poscontrol.get_state() == QPOS_PAUSE) &&
+        (poscontrol.time_since_state_start_ms() > (qrtl_pause_time.get() * 1000)))  {
+        start_descend = true;
+    }
+
+    // Move onto land descend state
+    if (start_descend) {
+        poscontrol.set_state(QPOS_LAND_DESCEND);
+        last_land_final_agl_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
+        gcs().send_text(MAV_SEVERITY_INFO,"Land descend started");
+        if (plane.control_mode == &plane.mode_auto) {
+            // set height to mission height, so we can use the mission
+            // WP height for triggering land final if no rangefinder
+            // available
+            plane.set_next_WP(plane.mission.get_current_nav_cmd().content.location);
+        } else {
+            plane.set_next_WP(plane.next_WP_loc);
+            plane.next_WP_loc.copy_alt_from(ahrs.get_home());
+        }
+    }
+
     // at land_final_alt_m begin final landing
     if (poscontrol.get_state() == QPOS_LAND_DESCEND && check_land_final()) {
         poscontrol.set_state(QPOS_LAND_FINAL);
@@ -4221,16 +4254,24 @@ void QuadPlane::update_throttle_mix(void)
 bool QuadPlane::in_vtol_land_approach(void) const
 {
     if (plane.control_mode == &plane.mode_qrtl &&
-        poscontrol.get_state() <= QPOS_POSITION2) {
+        poscontrol.get_state() <= QPOS_PAUSE) {
         return true;
     }
-    if (in_vtol_auto()) {
-        if (is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
-            (poscontrol.get_state() == QPOS_APPROACH ||
-             poscontrol.get_state() == QPOS_AIRBRAKE ||
-             poscontrol.get_state() == QPOS_POSITION1 ||
-             poscontrol.get_state() == QPOS_POSITION2)) {
-            return true;
+    if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id)) {
+        switch (poscontrol.get_state()) {
+            case QPOS_APPROACH:
+            case QPOS_AIRBRAKE:
+            case QPOS_POSITION1:
+            case QPOS_POSITION2:
+            case QPOS_PAUSE:
+                return true;
+
+            case QPOS_NONE:
+            case QPOS_LAND_DESCEND:
+            case QPOS_LAND_ABORT:
+            case QPOS_LAND_FINAL:
+            case QPOS_LAND_COMPLETE:
+                break;
         }
     }
     return false;

ArduPlane/quadplane.h

@@ -345,7 +345,10 @@ private:
     // QRTL start altitude, meters
     AP_Int16 qrtl_alt_m;
     AP_Int16 qrtl_alt_min_m;
-    
+
+    // QRTL pause time in seconds
+    AP_Float qrtl_pause_time;
+
     // alt to switch to QLAND_FINAL
     AP_Float land_final_alt_m;
     AP_Float vel_forward_alt_cutoff_m;
@@ -495,6 +498,7 @@ private:
         QPOS_AIRBRAKE,
         QPOS_POSITION1,
         QPOS_POSITION2,
+        QPOS_PAUSE,
         QPOS_LAND_DESCEND,
         QPOS_LAND_ABORT,
         QPOS_LAND_FINAL,