From a07330a4b20ea589567d5fc94cb4f2de1c4b54ca Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 12 Oct 2023 14:59:12 -0700 Subject: [PATCH] VTOL: Mode 25 to land at the guided location --- ArduPlane/mode.h | 2 ++ ArduPlane/mode_LoiterAltQLand.cpp | 23 +++++++++++++---------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index a88dbec5b6e3c1..066a0150346c7f 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -362,6 +362,8 @@ class ModeLoiterAltQLand : public ModeLoiter // handle a guided target request from GCS bool handle_guided_request(Location target_loc) override; + Location loiter_wp; + protected: bool _enter() override; diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 0cc8b9457c11e7..76318d7698e8e8 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -10,17 +10,12 @@ bool ModeLoiterAltQLand::_enter() return true; } + const Location previous_wp = plane.next_WP_loc; + 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 + const Location target_loc = (plane.previous_mode->is_guided_mode() && !plane.any_failsafe_triggered()) ? previous_wp : plane.next_WP_loc; + handle_guided_request(target_loc); switch_qland(); @@ -38,7 +33,13 @@ void ModeLoiterAltQLand::switch_qland() { ftype dist; if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) { - plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND); + if (plane.previous_mode->is_guided_mode() && !plane.any_failsafe_triggered()) { + plane.set_mode(plane.mode_qrtl, ModeReason::LOITER_ALT_REACHED_QLAND); + plane.set_next_WP(loiter_wp); + } else + { + plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND); + } } } @@ -57,6 +58,8 @@ bool ModeLoiterAltQLand::handle_guided_request(Location target_loc) plane.set_guided_WP(target_loc); + loiter_wp = target_loc; + return true; }