From 7547ad53d34985f09cf3e69bd2a7710be6e6fdf8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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;