From 1148bb12344f1d41d87d93642dee653b9235f879 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 19 Sep 2021 00:15:41 +0100 Subject: [PATCH] Plane: Quadplane: add loiter to alt then QLAND mode --- ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/GCS_Plane.cpp | 1 + ArduPlane/Plane.h | 4 +++ ArduPlane/control_modes.cpp | 6 +++++ ArduPlane/events.cpp | 6 +++++ ArduPlane/mode.h | 23 ++++++++++++++++ ArduPlane/mode_LoiterAltQLand.cpp | 45 +++++++++++++++++++++++++++++++ ArduPlane/quadplane.h | 1 + 8 files changed, 87 insertions(+) create mode 100644 ArduPlane/mode_LoiterAltQLand.cpp diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 233f1b576f..00e12223e1 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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; diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index c018ed1dd1..633b93451d 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 0866ab3d2d..43c8d70a16 100644 --- a/ArduPlane/Plane.h +++ b/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 diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index f2a7dfab58..6870509521 100644 --- a/ArduPlane/control_modes.cpp +++ b/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; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 3513ff49d7..87a1757fee 100644 --- a/ArduPlane/events.cpp +++ b/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 diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 3e1b19f3e5..ecbaa36244 100644 --- a/ArduPlane/mode.h +++ b/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: diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp new file mode 100644 index 0000000000..51f102042a --- /dev/null +++ b/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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 61564c2d38..c015457280 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -52,6 +52,7 @@ public: friend class ModeQStabilize; friend class ModeQAutotune; friend class ModeQAcro; + friend class ModeLoiterAltQLand; QuadPlane(AP_AHRS &_ahrs);