From 6eaad591151c6e009511ed463d9a93d8f98c9a97 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 5 May 2017 07:34:27 +1000 Subject: [PATCH] Plane: improve vectored tailsitter takeoff this gives extra pitch input when taking off a vectored tailsitter, allowing the motors to point straight up for easy takeoff thanks to Leonard for the idea! --- ArduPlane/quadplane.cpp | 7 +++++++ ArduPlane/quadplane.h | 1 + ArduPlane/tailsitter.cpp | 18 ++++++++++++++++-- 3 files changed, 24 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 28a07439ae..4a9a7a7df4 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -386,6 +386,13 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes. // @Range: 0 30 AP_GROUPINFO("TILT_YAW_ANGLE", 55, QuadPlane, tilt.tilt_yaw_angle, 0), + + // @Param: TAILSIT_VHPOW + // @DisplayName: Tailsitter vector thrust gain power + // @Description: This controls the amount of extra pitch given to the vectored control when at high pitch errors + // @Range: 0 4 + // @Increment: 0.1 + AP_GROUPINFO("TAILSIT_VHPOW", 56, QuadPlane, tailsitter.vectored_hover_power, 2.5), AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index dbae5ee546..3ca4166bf4 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -369,6 +369,7 @@ private: AP_Int8 input_mask_chan; AP_Float vectored_forward_gain; AP_Float vectored_hover_gain; + AP_Float vectored_hover_power; } tailsitter; // the attitude view of the VTOL attitude controller diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index b7cdf9735e..8ed79d33e8 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -66,8 +66,22 @@ void QuadPlane::tailsitter_output(void) // thrust vectoring VTOL modes float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); - float tilt_left = (elevator + aileron) * tailsitter.vectored_hover_gain; - float tilt_right = (elevator - aileron) * tailsitter.vectored_hover_gain; + /* + apply extra elevator when at high pitch errors, using a + power law. This allows the motors to point straight up for + takeoff without integrator windup + */ + int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5; + float extra_pitch = constrain_float(pitch_error_cd, -4500, 4500) / 4500.0; + float extra_sign = extra_pitch > 0?1:-1; + float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * 4500; + float tilt_left = extra_elevator + (elevator + aileron) * tailsitter.vectored_hover_gain; + float tilt_right = extra_elevator + (elevator - aileron) * tailsitter.vectored_hover_gain; + if (fabsf(tilt_left) >= 4500 || fabsf(tilt_right) >= 4500) { + // prevent integrator windup + motors->limit.roll_pitch = 1; + motors->limit.yaw = 1; + } SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); }