From fd7c87e6294a4cd3da21034c9d68809df850bbd2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 22 Apr 2016 13:24:04 +1000
Subject: [PATCH] AP_Motors: allow enabling oneshot on a subset of motors

---
 libraries/AP_Motors/AP_Motors_Class.cpp | 13 +++++++++----
 libraries/AP_Motors/AP_Motors_Class.h   |  1 +
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
index d1a242a1cb..c14a5f6226 100644
--- a/libraries/AP_Motors/AP_Motors_Class.cpp
+++ b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -79,7 +79,7 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
         // we have a mapped motor number for this channel
         chan = _motor_map[chan];
     }
-    if (_pwm_type == PWM_TYPE_ONESHOT125) {
+    if (_pwm_type == PWM_TYPE_ONESHOT125 && (_motor_fast_mask & (1U<<chan))) {
         // OneShot125 uses a PWM range from 125 to 250 usec
         pwm /= 8;
         /*
@@ -101,9 +101,14 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
  */
 void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
 {
-    hal.rcout->set_freq(rc_map_mask(mask), freq_hz);
-    if (_pwm_type == PWM_TYPE_ONESHOT ||
-        _pwm_type == PWM_TYPE_ONESHOT125) {
+    mask = rc_map_mask(mask);
+    if (freq_hz > 50) {
+        _motor_fast_mask |= mask;
+    }
+    hal.rcout->set_freq(mask, freq_hz);
+    if ((_pwm_type == PWM_TYPE_ONESHOT ||
+         _pwm_type == PWM_TYPE_ONESHOT125) &&
+        freq_hz > 50) {
         // tell HAL to do immediate output
         hal.rcout->set_output_mode(AP_HAL::RCOutput::MODE_PWM_ONESHOT);
     }
diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
index 631134e3a7..97f1c05083 100644
--- a/libraries/AP_Motors/AP_Motors_Class.h
+++ b/libraries/AP_Motors/AP_Motors_Class.h
@@ -176,6 +176,7 @@ protected:
     // mapping to output channels
     uint8_t             _motor_map[AP_MOTORS_MAX_NUM_MOTORS];
     uint16_t            _motor_map_mask;
+    uint16_t            _motor_fast_mask;
 
     // pass through variables
     float _roll_radio_passthrough = 0.0f;     // roll input from pilot in -1 ~ +1 range.  used for setup and providing servo feedback while landed