ArduPlane:add subclass for copter tailsitters

cleanup and add tailsit_motmx
add constexpr and motor mask check
add support for tricopter tailsitter
don't call output_motor_mask unless armed
fix whitespace

ArduPlane/quadplane.cpp

@@ -402,6 +402,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Advanced
     AP_GROUPINFO("TRANS_FAIL", 8, QuadPlane, transition_failure, 0),
 
+    // @Param: TAILSIT_MOTMX
+    // @DisplayName: Tailsiter mask
+    // @Description: Bitmask of motors to remain active in forward flight for a 'copter' tailsitter. Non-zero indicates airframe is a tailsitter which pitches forward 90 degrees in forward flight modes.
+    // @User: Standard
+    // @Bitmask: 0:Motor 1,1:Motor 2,2:Motor 3,3:Motor 4, 4:Motor 5,5:Motor 6,6:Motor 7,7:Motor 8
+    AP_GROUPINFO("TAILSIT_MOTMX", 9, QuadPlane, tailsitter.motor_mask, 0),
+
     AP_GROUPEND
 };
 
@@ -563,21 +570,36 @@ bool QuadPlane::setup(void)
         break;
     }
 
-    switch (motor_class) {
-    case AP_Motors::MOTOR_FRAME_TRI:
-        motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed);
-        motors_var_info = AP_MotorsTri::var_info;
-        break;
-    case AP_Motors::MOTOR_FRAME_TAILSITTER:
-        motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed);
-        motors_var_info = AP_MotorsTailsitter::var_info;
+    if (tailsitter.motor_mask == 0) {
+        // this is a normal quadplane
+        switch (motor_class) {
+        case AP_Motors::MOTOR_FRAME_TRI:
+            motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed);
+            motors_var_info = AP_MotorsTri::var_info;
+            break;
+        case AP_Motors::MOTOR_FRAME_TAILSITTER:
+            // this is a duo-motor tailsitter (vectored thrust if tilt.tilt_mask != 0)
+            motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed);
+            motors_var_info = AP_MotorsTailsitter::var_info;
+            rotation = ROTATION_PITCH_90;
+            break;
+        default:
+            motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed);
+            motors_var_info = AP_MotorsMatrix::var_info;
+            break;
+        }
+    } else {
+        // this is a copter tailsitter with motor layout specified by frame_class and frame_type
+        // tilting motors are not supported (tiltrotor control variables are ignored)
+        if (tilt.tilt_mask != 0) {
+            hal.console->printf("Warning tilting motors not supported, setting tilt_mask to zero\n");
+            tilt.tilt_mask.set(0);
+        }
         rotation = ROTATION_PITCH_90;
-        break;
-    default:
-        motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed);
-        motors_var_info = AP_MotorsMatrix::var_info;
-        break;
+        motors = new AP_MotorsMatrixTS(plane.scheduler.get_loop_rate_hz(), rc_speed);
+        motors_var_info = AP_MotorsMatrixTS::var_info;
     }
+    
     const static char *strUnableToAllocate = "Unable to allocate";
     if (!motors) {
         hal.console->printf("%s motors\n", strUnableToAllocate);
@@ -644,7 +666,7 @@ bool QuadPlane::setup(void)
     setup_defaults();
 
     AP_Param::convert_old_parameters(&q_conversion_table[0], ARRAY_SIZE(q_conversion_table));
-    
+
     gcs().send_text(MAV_SEVERITY_INFO, "QuadPlane initialised");
     initialised = true;
     return true;

ArduPlane/quadplane.h

@@ -432,6 +432,7 @@ private:
         AP_Float vectored_hover_power;
         AP_Float throttle_scale_max;
         AP_Float max_roll_angle;
+        AP_Int16 motor_mask;
     } tailsitter;
 
     // the attitude view of the VTOL attitude controller

ArduPlane/tailsitter.cpp

@@ -14,6 +14,8 @@
  */
 /*
   control code for tailsitters. Enabled by setting Q_FRAME_CLASS=10 
+  or by setting Q_TAILSIT_MOTMX nonzero and Q_FRAME_CLASS and Q_FRAME_TYPE
+  to a configuration supported by AP_MotorsMatrix
  */
 
 #include "Plane.h"
@@ -23,7 +25,8 @@
  */
 bool QuadPlane::is_tailsitter(void) const
 {
-    return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER;
+    return available() && ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || 
+                           (tailsitter.motor_mask != 0));
 }
 
 /*
@@ -55,8 +58,11 @@ void QuadPlane::tailsitter_output(void)
 
     float tilt_left = 0.0f;
     float tilt_right = 0.0f;
+    uint16_t mask = tailsitter.motor_mask;
 
+    // handle forward flight modes and transition to VTOL modes
     if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
+        // in forward flight: set motor tilt servos and throttles using FW controller
         if (tailsitter.vectored_forward_gain > 0) {
             // thrust vectoring in fixed wing flight
             float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
@@ -66,23 +72,38 @@ void QuadPlane::tailsitter_output(void)
         }
         SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
         SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
-        
-        if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) {
+
+        // get FW controller throttle demand and mask of motors enabled during forward flight
+        float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+        if (hal.util->get_soft_armed()) {
+            if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) {
             /*
-              during transitions to vtol mode set the throttle to the
-              hover throttle, and set the altitude controller
+              during transitions to vtol mode set the throttle to
+              hover thrust, center the rudder and set the altitude controller
               integrator to the same throttle level
              */
-            uint8_t throttle = motors->get_throttle_hover() * 100;
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle);
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle);
+            throttle = motors->get_throttle_hover() * 100;
             SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
             pos_control->get_accel_z_pid().set_integrator(throttle*10);
+            
+            if (mask == 0) {
+                // override AP_MotorsTailsitter throttles during back transition
+                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
+                SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle);
+                SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle);            
+            }
+        }
+        if (mask != 0) {
+            // set AP_MotorsMatrix throttles enabled for forward flight
+            motors->output_motor_mask(throttle * 0.01f, mask, plane.rudder_dt);
+            }
         }
         return;
     }
     
+    // handle VTOL modes
+    // the MultiCopter rate controller has already been run in an earlier call 
+    // to motors_output() from quadplane.update()
     motors_output(false);
     plane.pitchController.reset_I();
     plane.rollController.reset_I();