From 7547ad53d34985f09cf3e69bd2a7710be6e6fdf8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 17 Sep 2021 08:46:03 +1000
Subject: [PATCH] Plane: allow for new guided destination during guided takeoff

if you set a very high alt for takeoff this is useful
---
 ArduPlane/commands.cpp  | 5 +++++
 ArduPlane/quadplane.cpp | 4 ++++
 2 files changed, 9 insertions(+)

diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp
index 623e275e23..f410cd0a0d 100644
--- a/ArduPlane/commands.cpp
+++ b/ArduPlane/commands.cpp
@@ -99,6 +99,11 @@ void Plane::set_guided_WP(void)
     auto_state.vtol_loiter = false;
     
     loiter_angle_reset();
+
+#if HAL_QUADPLANE_ENABLED
+    // cancel pending takeoff
+    quadplane.guided_takeoff = false;
+#endif
 }
 
 /*
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 97ce4fd578..d5f6d81dcc 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2011,6 +2011,10 @@ bool QuadPlane::in_vtol_mode(void) const
         poscontrol.get_state() > QPOS_APPROACH) {
         return true;
     }
+    if (plane.control_mode == &plane.mode_guided &&
+        guided_takeoff) {
+        return true;
+    }
     if (in_vtol_auto()) {
         if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_APPROACH) {
             return true;