Plane: Quadplane: add loiter to alt then QLAND mode

ArduPlane/GCS_Mavlink.cpp

@@ -58,6 +58,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case Mode::Number::TAKEOFF:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
+    case Mode::Number::LOITER_ALT_QLAND:
 #endif
         _base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
                      MAV_MODE_FLAG_STABILIZE_ENABLED;

ArduPlane/GCS_Plane.cpp

@@ -84,6 +84,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
     case Mode::Number::TAKEOFF:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
+    case Mode::Number::LOITER_ALT_QLAND:
 #endif
     case Mode::Number::THERMAL:
         rate_controlled = true;

ArduPlane/Plane.h

@@ -166,6 +166,7 @@ public:
     friend class ModeQAutotune;
     friend class ModeTakeoff;
     friend class ModeThermal;
+    friend class ModeLoiterAltQLand;
 
     Plane(void);
 
@@ -299,6 +300,9 @@ private:
 #if HAL_SOARING_ENABLED
     ModeThermal mode_thermal;
 #endif
+#if HAL_QUADPLANE_ENABLED
+    ModeLoiterAltQLand mode_lotier_qland;
+#endif
 
     // This is the state of the flight control system
     // There are multiple states defined such as MANUAL, FBW-A, AUTO

ArduPlane/control_modes.cpp

@@ -88,6 +88,12 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
         ret = &mode_thermal;
 #endif
         break;
+#if HAL_QUADPLANE_ENABLED
+    case Mode::Number::LOITER_ALT_QLAND:
+        ret = &mode_lotier_qland;
+        break;
+#endif  // HAL_QUADPLANE_ENABLED
+
     }
     return ret;
 }

ArduPlane/events.cpp

@@ -68,6 +68,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
     case Mode::Number::AVOID_ADSB:
     case Mode::Number::GUIDED:
     case Mode::Number::LOITER:
+#if HAL_QUADPLANE_ENABLED
+    case Mode::Number::LOITER_ALT_QLAND:
+#endif
     case Mode::Number::THERMAL:
         if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) {
             failsafe.saved_mode_number = control_mode->mode_number();
@@ -112,6 +115,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::TRAINING:
     case Mode::Number::CIRCLE:
     case Mode::Number::LOITER:
+#if HAL_QUADPLANE_ENABLED
+    case Mode::Number::LOITER_ALT_QLAND:
+#endif
     case Mode::Number::THERMAL:
         if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
 #if PARACHUTE == ENABLED

ArduPlane/mode.h

@@ -51,6 +51,9 @@ public:
         QACRO         = 23,
 #endif
         THERMAL       = 24,
+#if HAL_QUADPLANE_ENABLED
+        LOITER_ALT_QLAND = 25,
+#endif
     };
 
     // Constructor
@@ -269,6 +272,26 @@ protected:
     bool _enter() override;
 };
 
+#if HAL_QUADPLANE_ENABLED
+class ModeLoiterAltQLand : public ModeLoiter
+{
+public:
+
+    Number mode_number() const override { return Number::LOITER_ALT_QLAND; }
+    const char *name() const override { return "Loiter to QLAND"; }
+    const char *name4() const override { return "L2QL"; }
+
+protected:
+    bool _enter() override;
+
+    void navigate() override;
+
+private:
+    void switch_qland();
+
+};
+#endif // HAL_QUADPLANE_ENABLED
+
 class ModeManual : public Mode
 {
 public:

ArduPlane/mode_LoiterAltQLand.cpp

@@ -0,0 +1,45 @@
+#include "mode.h"
+#include "Plane.h"
+
+#if HAL_QUADPLANE_ENABLED
+
+bool ModeLoiterAltQLand::_enter()
+{
+    if (plane.previous_mode->is_vtol_mode() || plane.quadplane.in_vtol_mode()) {
+        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_IN_VTOL);
+        return true;
+    }
+
+    ModeLoiter::_enter();
+
+#if AP_TERRAIN_AVAILABLE
+    if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
+        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
+    } else {
+        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
+    }
+#else
+    plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
+#endif
+
+    switch_qland();
+
+    return true;
+}
+
+void ModeLoiterAltQLand::navigate()
+{
+    switch_qland();
+
+    ModeLoiter::navigate();
+}
+
+void ModeLoiterAltQLand::switch_qland()
+{
+    ftype dist;
+    if (!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) {
+        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
+    }
+}
+
+#endif

ArduPlane/quadplane.h

@@ -52,6 +52,7 @@ public:
     friend class ModeQStabilize;
     friend class ModeQAutotune;
     friend class ModeQAcro;
+    friend class ModeLoiterAltQLand;
     
     QuadPlane(AP_AHRS &_ahrs);