diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index eab056be7e..8e53243a7c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2693,6 +2693,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 */ @@ -2720,7 +2727,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;