Plane: support thrust vectoring in tailsitters

adds Q_TAILSIT_VHGAIN and Q_TAILSIT_VFGAIN, allowing for thust
vectoring in both fixed wing and hover

ArduPlane/quadplane.cpp

@@ -367,6 +367,20 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @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
 };
 

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

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) {