diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 312cd95d39..44f0aeb480 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -223,6 +223,8 @@ void Tailsitter::setup() _have_elevator = SRV_Channels::function_assigned(SRV_Channel::k_elevator); _have_aileron = SRV_Channels::function_assigned(SRV_Channel::k_aileron); _have_rudder = SRV_Channels::function_assigned(SRV_Channel::k_rudder); + _have_elevon = SRV_Channels::function_assigned(SRV_Channel::k_elevon_left) || SRV_Channels::function_assigned(SRV_Channel::k_elevon_right); + _have_v_tail = SRV_Channels::function_assigned(SRV_Channel::k_vtail_left) || SRV_Channels::function_assigned(SRV_Channel::k_vtail_right); // set defaults for dual/single motor tailsitter if (quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) { @@ -362,9 +364,24 @@ void Tailsitter::output(void) quadplane.motors_output(true); if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) { - // only use motors for Q assist, control surfaces remain under plane control - // zero copter I terms and use plane - quadplane.attitude_control->reset_rate_controller_I_terms(); + // only use motors for Q assist, control surfaces remain under plane control. Zero copter I terms and use plane. + // Smoothly relax to zero so there is no step change in output, must also set limit flags so integrator cannot build faster than the relax. + // Assume there is always roll and pitch control surfaces, otherwise motors only assist should not be set. + const float dt = quadplane.attitude_control->get_dt(); + quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); + motors->limit.pitch = true; + quadplane.attitude_control->get_rate_yaw_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); + motors->limit.yaw = true; + + if (_have_rudder || _have_v_tail) { + // there are yaw control surfaces, zero motor I term + quadplane.attitude_control->get_rate_roll_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC); + motors->limit.roll = true; + } else { + // no yaw control surfaces, zero plane I terms and use motors + // We skip the outputing to surfaces for this axis from the copter controller but there are none setup + plane.yawController.reset_I(); + } // output tilt motors tilt_left = 0.0f; @@ -447,18 +464,18 @@ void Tailsitter::output(void) if (is_positive(headroom)) { if (fabsf(aileron_mix) > headroom) { aileron_mix *= headroom / fabsf(aileron_mix); - yaw_lim = true; + yaw_lim |= _have_elevon; } if (fabsf(rudder_mix) > headroom) { rudder_mix *= headroom / fabsf(rudder_mix); - roll_lim = true; + roll_lim |= _have_v_tail; } } else { aileron_mix = 0.0; rudder_mix = 0.0; - roll_lim = true; - pitch_lim = true; - yaw_lim = true; + yaw_lim |= _have_elevon; + pitch_lim |= _have_elevon || _have_v_tail; + roll_lim |= _have_v_tail; } SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, elevator_mix - aileron_mix); SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, elevator_mix + aileron_mix); diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index b8e0a0380c..1d91fa49e5 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -121,6 +121,8 @@ private: bool _have_elevator; bool _have_aileron; bool _have_rudder; + bool _have_elevon; + bool _have_v_tail; // refences for convenience QuadPlane& quadplane;