diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 6c801bd2f4..60ab61b2a5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -366,6 +366,20 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: This controls what input channel will activate the Q_TAILSIT_MASK mask. When this channel goes above 1700 then the pilot will have direct manual control of the output channels specified in Q_TAILSIT_MASK. Set to zero to disable. // @Values: 0:Disabled,1:Channel1,2:Channel2,3:Channel3,4:Channel4,5:Channel5,6:Channel6,7:Channel7,8:Channel8 AP_GROUPINFO("TAILSIT_MASKCH", 52, QuadPlane, tailsitter.input_mask_chan, 0), + + // @Param: TAILSIT_VFGAIN + // @DisplayName: Tailsitter vector thrust gain in forward flight + // @Description: This controls the amount of vectored thrust control used in forward flight for a vectored tailsitter + // @Range: 0 1 + // @Increment: 0.01 + AP_GROUPINFO("TAILSIT_VFGAIN", 53, QuadPlane, tailsitter.vectored_forward_gain, 0), + + // @Param: TAILSIT_VHGAIN + // @DisplayName: Tailsitter vector thrust gain in hover + // @Description: This controls the amount of vectored thrust control used in hover for a vectored tailsitter + // @Range: 0 1 + // @Increment: 0.01 + AP_GROUPINFO("TAILSIT_VHGAIN", 54, QuadPlane, tailsitter.vectored_hover_gain, 0.5), AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index bd7ae37315..ae7ef11848 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -358,6 +358,8 @@ private: AP_Int8 input_type; AP_Int8 input_mask; AP_Int8 input_mask_chan; + AP_Float vectored_forward_gain; + AP_Float vectored_hover_gain; } tailsitter; // the attitude view of the VTOL attitude controller diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 647bf82e36..dca9cf981b 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -40,6 +40,18 @@ bool QuadPlane::tailsitter_active(void) void QuadPlane::tailsitter_output(void) { if (!tailsitter_active()) { + if (tailsitter.vectored_forward_gain > 0) { + // thrust vectoring in fixed wing flight + 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_forward_gain; + float tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain; + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); + } else { + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 0); + } return; } @@ -47,6 +59,17 @@ void QuadPlane::tailsitter_output(void) plane.pitchController.reset_I(); plane.rollController.reset_I(); + if (tailsitter.vectored_hover_gain > 0) { + // 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; + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); + } + + if (tailsitter.input_mask_chan > 0 && tailsitter.input_mask > 0 && hal.rcin->read(tailsitter.input_mask_chan-1) > 1700) {