From e0c1f842064deb92f2f64848e00b4d53b43989cb Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Tue, 23 Feb 2016 23:42:26 +0100
Subject: [PATCH] FastPwm for all oneshot42/125 and multishot // Cleanup

---
 src/main/drivers/pwm_mapping.c |  4 ++--
 src/main/drivers/pwm_mapping.h |  2 +-
 src/main/drivers/pwm_output.c  | 23 ++++++++++++++++-------
 src/main/main.c                |  4 ++--
 4 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 4a09df092b..02eb9dcb52 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -31,7 +31,7 @@
 
 void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
 void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
-void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42);
+void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42, uint8_t useMultiShot);
 void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42);
 void pwmMultiShotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex);
 void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
@@ -806,7 +806,7 @@ if (init->useBuzzerP6) {
 #endif
             if (init->useOneshot) {
                 if (init->useFastPWM) {
-                    fastPWMMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->useOneshot42);
+                    fastPWMMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->useOneshot42, init->useMultiShot);
                 } else if (init->useMultiShot) {
                     pwmMultiShotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount);
                 } else {
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 67c24a1431..f77cbd0694 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -36,7 +36,7 @@
 #define MAX_INPUTS  8
 
 #define PWM_TIMER_MHZ 1
-#define ONESHOT125_TIMER_MHZ 24
+#define ONESHOT_TIMER_MHZ 24
 #define PWM_BRUSHED_TIMER_MHZ 8
 #define MULTISHOT_TIMER_MHZ 12
 
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 58e878f289..aaeeed5a61 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -135,7 +135,7 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
     *motors[index]->ccr = value;
 }
 
-static void pwmWriteOneshot(uint8_t index, uint16_t value)
+static void pwmWriteOneshot125(uint8_t index, uint16_t value)
 {
     *motors[index]->ccr = value * 3;  // 24Mhz -> 8Mhz
 }
@@ -205,24 +205,33 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor
     motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
 }
 
-void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42)
+void fastPWMMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t useOneshot42, uint8_t useMultiShot)
 {
-    uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
-    motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, hz / motorPwmRate, idlePulse);
+    uint32_t hz;
+    if (useMultiShot) {
+        hz = MULTISHOT_TIMER_MHZ * 1000000;
+        motors[motorIndex] = pwmOutConfig(timerHardware, MULTISHOT_TIMER_MHZ, hz / motorPwmRate, idlePulse);
+    } else {
+        hz = ONESHOT_TIMER_MHZ * 1000000;
+        motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT_TIMER_MHZ, hz / motorPwmRate, idlePulse);
+    }
+
     if (useOneshot42) {
         motors[motorIndex]->pwmWritePtr = pwmWriteOneshot42;
+    } else if (useMultiShot) {
+        motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot;
     } else {
-        motors[motorIndex]->pwmWritePtr = pwmWriteOneshot;
+        motors[motorIndex]->pwmWritePtr = pwmWriteOneshot125;
     }
 }
 
 void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t useOneshot42)
 {
-    motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0);
+    motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT_TIMER_MHZ, 0xFFFF, 0);
     if (useOneshot42) {
         motors[motorIndex]->pwmWritePtr = pwmWriteOneshot42;
     } else {
-        motors[motorIndex]->pwmWritePtr = pwmWriteOneshot;
+        motors[motorIndex]->pwmWritePtr = pwmWriteOneshot125;
     }
 }
 
diff --git a/src/main/main.c b/src/main/main.c
index c4a394c07e..bd06185f7a 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -308,8 +308,8 @@ void init(void)
 #endif
 
     pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
-    if (masterConfig.use_fast_pwm || masterConfig.use_oneshot42) {
-        pwm_params.useFastPWM = masterConfig.use_fast_pwm ? true : false;
+    pwm_params.useFastPWM = masterConfig.use_fast_pwm ? true : false;
+    if (masterConfig.use_oneshot42) {
         pwm_params.useOneshot42 = masterConfig.use_oneshot42 ? true : false;
         masterConfig.use_multiShot = false;
     } else {