AP_Motors: added rc_map_mask() function

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -316,7 +316,7 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
 uint16_t AP_MotorsHeli_Single::get_motor_mask()
 {
     // heli uses channels 1,2,3,4,7 and 8
-    return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
+    return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
 }
 
 // update_motor_controls - sends commands to motor controllers

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -114,7 +114,7 @@ uint16_t AP_MotorsMatrix::get_motor_mask()
             mask |= 1U << i;
         }
     }
-    return mask;
+    return rc_map_mask(mask);
 }
 
 void AP_MotorsMatrix::output_armed_not_stabilizing()

libraries/AP_Motors/AP_MotorsSingle.cpp

@@ -130,7 +130,7 @@ void AP_MotorsSingle::output_min()
 uint16_t AP_MotorsSingle::get_motor_mask()
 {
     // single copter uses channels 1,2,3,4 and 7
-    return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
+    return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
 }
 
 void AP_MotorsSingle::output_armed_not_stabilizing()

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -130,7 +130,7 @@ void AP_MotorsTri::output_min()
 uint16_t AP_MotorsTri::get_motor_mask()
 {
     // tri copter uses channels 1,2,4 and 7
-    return (1U << AP_MOTORS_MOT_1) |
+    return rc_map_mask(1U << AP_MOTORS_MOT_1) |
         (1U << AP_MOTORS_MOT_2) |
         (1U << AP_MOTORS_MOT_4) |
         (1U << AP_MOTORS_CH_TRI_YAW);

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -81,6 +81,23 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
   set frequency of a set of channels
  */
 void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
+{
+    hal.rcout->set_freq(rc_map_mask(mask), freq_hz);
+}
+
+void AP_Motors::rc_enable_ch(uint8_t chan)
+{
+    if (_motor_map_mask & (1U<<chan)) {
+        // we have a mapped motor number for this channel
+        chan = _motor_map[chan];
+    }
+    hal.rcout->enable_ch(chan);
+}
+
+/*
+  map an internal motor mask to real motor mask
+ */
+uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
 {
     uint32_t mask2 = 0;
     for (uint8_t i=0; i<32; i++) {
@@ -94,14 +111,5 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
             }
         }
     }
-    hal.rcout->set_freq(mask2, freq_hz);
-}
-
-void AP_Motors::rc_enable_ch(uint8_t chan)
-{
-    if (_motor_map_mask & (1U<<chan)) {
-        // we have a mapped motor number for this channel
-        chan = _motor_map[chan];
-    }
-    hal.rcout->enable_ch(chan);
+    return mask2;
 }

libraries/AP_Motors/AP_Motors_Class.h

@@ -132,6 +132,7 @@ protected:
     virtual void        rc_write(uint8_t chan, uint16_t pwm);
     virtual void        rc_set_freq(uint32_t mask, uint16_t freq_hz);
     virtual void        rc_enable_ch(uint8_t chan);
+    virtual uint32_t    rc_map_mask(uint32_t mask) const;
     
     // update the throttle input filter
     virtual void        update_throttle_filter() = 0;