From 1e62b5d5c7facf83c465b655def92515f18f0b62 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 17 Apr 2017 18:15:55 +1000
Subject: [PATCH] AP_Motors: many to one mapping of motors

this allows multiple outputs for one motor number
---
 libraries/AP_Motors/AP_Motors_Class.cpp | 30 ++++++++++---------------
 libraries/AP_Motors/AP_Motors_Class.h   |  4 +---
 2 files changed, 13 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
index 8f37f5a9ca..d917b926a0 100644
--- a/libraries/AP_Motors/AP_Motors_Class.cpp
+++ b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -22,6 +22,7 @@
 #include "AP_Motors_Class.h"
 #include <AP_HAL/AP_HAL.h>
 #include <SRV_Channel/SRV_Channel.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -39,7 +40,6 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
     _batt_voltage(0.0f),
     _batt_current(0.0f),
     _air_density_ratio(1.0f),
-    _motor_map_mask(0),
     _motor_fast_mask(0)
 {
     // init other flags
@@ -83,10 +83,6 @@ void AP_Motors::set_radio_passthrough(float roll_input, float pitch_input, float
  */
 void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
 {
-    if (_motor_map_mask & (1U<<chan)) {
-        // we have a mapped motor number for this channel
-        chan = _motor_map[chan];
-    }
     if (_pwm_type == PWM_TYPE_ONESHOT125 && (_motor_fast_mask & (1U<<chan))) {
         // OneShot125 uses a PWM range from 125 to 250 usec
         pwm /= 8;
@@ -101,7 +97,8 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
             pwm = 250;
         }
     }
-    hal.rcout->write(chan, pwm);
+    SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)((uint8_t)SRV_Channel::k_motor1 + chan);
+    SRV_Channels::set_output_pwm(function, pwm);
 }
 
 /*
@@ -109,10 +106,10 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
  */
 void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
 {
-    mask = rc_map_mask(mask);
     if (freq_hz > 50) {
         _motor_fast_mask |= mask;
     }
+    mask = rc_map_mask(mask);
     hal.rcout->set_freq(mask, freq_hz);
     if ((_pwm_type == PWM_TYPE_ONESHOT ||
          _pwm_type == PWM_TYPE_ONESHOT125) &&
@@ -126,7 +123,9 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
 }
 
 /*
-  map an internal motor mask to real motor mask
+  map an internal motor mask to real motor mask, accounting for
+  SERVOn_FUNCTION mappings, and allowing for multiple outputs per
+  motor number
  */
 uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
 {
@@ -134,12 +133,8 @@ uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
     for (uint8_t i=0; i<32; i++) {
         uint32_t bit = 1UL<<i;
         if (mask & bit) {
-            if ((i < AP_MOTORS_MAX_NUM_MOTORS) && (_motor_map_mask & bit)) {
-                // we have a mapped motor number for this channel
-                mask2 |= (1UL << _motor_map[i]);
-            } else {
-                mask2 |= bit;
-            }
+            SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)((uint8_t)SRV_Channel::k_motor1 + i);
+            mask2 |= SRV_Channels::get_output_channel_mask(function);
         }
     }
     return mask2;
@@ -182,7 +177,7 @@ int16_t AP_Motors::calc_pwm_output_0to1(float input, const SRV_Channel *servo)
 }
 
 /*
-  add a motor, setting up _motor_map and _motor_map_mask as needed
+  add a motor, setting up default output function as needed
  */
 void AP_Motors::add_motor_num(int8_t motor_num)
 {
@@ -196,9 +191,8 @@ void AP_Motors::add_motor_num(int8_t motor_num)
             function = (SRV_Channel::Aux_servo_function_t)(SRV_Channel::k_motor9+(motor_num-8));
         }
         SRV_Channels::set_aux_channel_default(function, motor_num);
-        if (SRV_Channels::find_channel(function, chan) && chan != motor_num) {
-            _motor_map[motor_num] = chan;
-            _motor_map_mask |= 1U<<motor_num;
+        if (!SRV_Channels::find_channel(function, chan)) {
+            gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
         }
     }
 }
diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
index 3ed5dffb38..1989a7c226 100644
--- a/libraries/AP_Motors/AP_Motors_Class.h
+++ b/libraries/AP_Motors/AP_Motors_Class.h
@@ -214,9 +214,7 @@ protected:
     float               _batt_resistance;       // latest battery resistance estimate in ohms
     float               _air_density_ratio;     // air density / sea level density - decreases in altitude
 
-    // mapping to output channels
-    uint8_t             _motor_map[AP_MOTORS_MAX_NUM_MOTORS];
-    uint16_t            _motor_map_mask;
+    // mask of what channels need fast output
     uint16_t            _motor_fast_mask;
 
     // pass through variables