AP_Motors: added output_motor_mask()

this will be used for tiltrotors in forward flight

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -128,6 +128,7 @@ void AP_MotorsMatrix::output_to_motors()
     hal.rcout->push();
 }
 
+
 // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
 //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
 uint16_t AP_MotorsMatrix::get_motor_mask()

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -499,3 +499,23 @@ void AP_MotorsMulticopter::throttle_pass_through(int16_t pwm)
         hal.rcout->push();
     }
 }
+
+// output a thrust to all motors that match a given motor mask. This
+// is used to control tiltrotor motors in forward flight. Thrust is in
+// the range 0 to 1
+void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
+{
+    hal.rcout->cork();
+    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            int16_t motor_out;
+            if (mask & (1U<<i)) {
+                motor_out = calc_thrust_to_pwm(thrust);
+            } else {
+                motor_out = _throttle_radio_min;
+            }
+            rc_write(i, motor_out);
+        }
+    }
+    hal.rcout->push();
+}

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -100,6 +100,11 @@ public:
     // return true if spool up is complete
     bool spool_up_complete() const { return _multicopter_flags.spool_mode == THROTTLE_UNLIMITED; }
 
+    // output a thrust to all motors that match a given motor
+    // mask. This is used to control tiltrotor motors in forward
+    // flight. Thrust is in the range 0 to 1
+    void                output_motor_mask(float thrust, uint8_t mask);
+        
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];