From f09c715d7aa6d3a54862d1e04dade33d84cf99de Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 5 Mar 2022 08:23:04 +1100
Subject: [PATCH] Plane: wait till motors are fully up before takeoff in guided
 mode

this allows for guided mode takeoff in tilt-rotors. Otherwise motors
till be pointing forward and takeoff will go very badly
---
 ArduPlane/quadplane.cpp |  9 ++++++++-
 ArduPlane/tiltrotor.cpp | 11 +++++++++++
 ArduPlane/tiltrotor.h   |  1 +
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index dc52d1d842..1a3a6eb697 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2697,6 +2697,13 @@ void QuadPlane::takeoff_controller(void)
     if (!hal.util->get_soft_armed()) {
         return;
     }
+
+    if (plane.control_mode == &plane.mode_guided && guided_takeoff
+        && tiltrotor.enabled() && !tiltrotor.fully_up()) {
+        // waiting for motors to tilt up
+        return;
+    }
+
     /*
       for takeoff we use the position controller
     */
@@ -2724,7 +2731,7 @@ void QuadPlane::takeoff_controller(void)
                                                                   get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
 
     float vel_z = wp_nav->get_default_speed_up();
-    if (guided_takeoff) {
+    if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
         // for guided takeoff we aim for a specific height with zero
         // velocity at that height
         Location origin;
diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp
index 5a48b3b4da..3f0660d166 100644
--- a/ArduPlane/tiltrotor.cpp
+++ b/ArduPlane/tiltrotor.cpp
@@ -479,6 +479,17 @@ bool Tiltrotor::fully_fwd(void) const
     return (current_tilt >= get_fully_forward_tilt());
 }
 
+/*
+  return true if the rotors are fully tilted up
+ */
+bool Tiltrotor::fully_up(void) const
+{
+    if (!enabled() || (tilt_mask == 0)) {
+        return false;
+    }
+    return (current_tilt <= 0);
+}
+
 /*
   control vectoring for tilt multicopters
  */
diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h
index e6953ebf46..2467fc28cc 100644
--- a/ArduPlane/tiltrotor.h
+++ b/ArduPlane/tiltrotor.h
@@ -49,6 +49,7 @@ public:
     }
 
     bool fully_fwd() const;
+    bool fully_up() const;
     float tilt_max_change(bool up, bool in_flap_range = false) const;
     float get_fully_forward_tilt() const;
     float get_forward_flight_tilt() const;