From 1c89b7f3a22ed1889b3b6da9f53d79cbc2d32693 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Dec 2021 07:26:39 +1100 Subject: [PATCH] Plane: check for VTOL takeoff in AUTO if the "only arm in Q modes" bit is set in Q_OPTIONS then check that in AUTO mode we are in a VTOL takeoff WP --- ArduPlane/AP_Arming.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 97bb652265..1abe179c28 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -163,10 +163,15 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure) ret = false; } - if (((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) && - !plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) { - check_failed(display_failure,"not in Q mode"); - ret = false; + if ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) { + if (!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) { + check_failed(display_failure,"not in Q mode"); + ret = false; + } + if ((plane.control_mode == &plane.mode_auto) && !plane.quadplane.is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) { + check_failed(display_failure,"not in VTOL takeoff"); + ret = false; + } } return ret;