From 35de0855f3d0beb4f98dd6425ea7a349595fea90 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  | 3 +++
 ArduPlane/quadplane.cpp | 4 ++++
 2 files changed, 7 insertions(+)

diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp
index 623e275e23..dc746ccfd0 100644
--- a/ArduPlane/commands.cpp
+++ b/ArduPlane/commands.cpp
@@ -99,6 +99,9 @@ void Plane::set_guided_WP(void)
     auto_state.vtol_loiter = false;
     
     loiter_angle_reset();
+
+    // cancel pending takeoff
+    quadplane.guided_takeoff = false;
 }
 
 /*
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 757f0b8882..dfedf12c37 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2508,6 +2508,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;