Plane: Quadplane: only advance to QPOS_POSITION2 once tilts have finished slewing

ArduPlane/quadplane.cpp

@@ -2365,7 +2365,8 @@ void QuadPlane::vtol_position_controller(void)
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
                                                                       plane.nav_pitch_cd,
                                                                       desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
-        if (plane.auto_state.wp_distance < position2_dist_threshold) {
+        if ((plane.auto_state.wp_distance < position2_dist_threshold) && tiltrotor.tilt_angle_achieved()) {
+            // if continuous tiltrotor only advance to position 2 once tilts have finished moving
             poscontrol.set_state(QPOS_POSITION2);
             poscontrol.pilot_correction_done = false;
             gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f",

ArduPlane/tiltrotor.cpp

@@ -186,6 +186,8 @@ void Tiltrotor::slew(float newtilt)
     float max_change = tilt_max_change(newtilt<current_tilt, newtilt > get_fully_forward_tilt());
     current_tilt = constrain_float(newtilt, current_tilt-max_change, current_tilt+max_change);
 
+    angle_achieved = is_equal(newtilt, current_tilt);
+
     // translate to 0..1000 range and output
     SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt);
 }

ArduPlane/tiltrotor.h

@@ -63,6 +63,10 @@ public:
 
     bool motors_active() const { return enabled() && _motors_active; }
 
+    // true if the tilts have completed slewing
+    // always return true if not enabled or not a continuous type
+    bool tilt_angle_achieved() const { return !enabled() || (type != TILT_TYPE_CONTINUOUS) || angle_achieved; }
+
     AP_Int8 enable;
     AP_Int16 tilt_mask;
     AP_Int16 max_rate_up_dps;
@@ -100,6 +104,10 @@ private:
     // true if all motors tilt with no fixed VTOL motor
     bool _have_vtol_motor;
 
+    // true if the current tilt angle is equal to the desired
+    // with slow tilt rates the tilt angle can lag
+    bool angle_achieved;
+
     // refences for convenience
     QuadPlane& quadplane;
     AP_MotorsMulticopter*& motors;