From 21547384218e84c7a9bf3d13039bd59769702db4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 26 Nov 2021 22:53:24 +0000 Subject: [PATCH] Plane: Quadplane: only advance to QPOS_POSITION2 once tilts have finished slewing --- ArduPlane/quadplane.cpp | 3 ++- ArduPlane/tiltrotor.cpp | 2 ++ ArduPlane/tiltrotor.h | 8 ++++++++ 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7aa203919e..9ae9773272 100644 --- a/ArduPlane/quadplane.cpp +++ b/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", diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index c9bb89dc92..a8132abb16 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -186,6 +186,8 @@ void Tiltrotor::slew(float newtilt) float max_change = tilt_max_change(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); } diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h index 3d63367e4c..6680f13d66 100644 --- a/ArduPlane/tiltrotor.h +++ b/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;