diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h
index 88282ca59..974c19c65 100644
--- a/src/main/drivers/light_ws2811strip.h
+++ b/src/main/drivers/light_ws2811strip.h
@@ -25,8 +25,13 @@
 
 #define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)   // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
 
+#if defined(STM32F40_41xxx)
+#define BIT_COMPARE_1 67 // timer compare value for logical 1
+#define BIT_COMPARE_0 33  // timer compare value for logical 0
+#else
 #define BIT_COMPARE_1 17 // timer compare value for logical 1
 #define BIT_COMPARE_0 9  // timer compare value for logical 0
+#endif
 
 void ws2811LedStripInit(void);
 
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index 40aa5d4d2..ffb554099 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -34,10 +34,20 @@
 #define MAX_INPUTS  8
 #define PWM_TIMER_MHZ 1
 
+#if defined(STM32F40_41xxx) // must be multiples of timer clock
+#define ONESHOT42_TIMER_MHZ 21
+#define ONESHOT125_TIMER_MHZ 12
+#define PWM_BRUSHED_TIMER_MHZ 21
+#define MULTISHOT_TIMER_MHZ 84
+#else
 #define PWM_BRUSHED_TIMER_MHZ 24
 #define MULTISHOT_TIMER_MHZ   72
 #define ONESHOT42_TIMER_MHZ   24
 #define ONESHOT125_TIMER_MHZ  8
+#endif
+
+#define MULTISHOT_5US_PW    (MULTISHOT_TIMER_MHZ * 5)
+#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
 
 typedef struct sonarIOConfig_s {
     ioTag_t triggerTag;
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index d75ff21c5..fd8446584 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -131,9 +131,19 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
     *motors[index]->ccr = value;
 }
 
+static void pwmWriteOneShot42(uint8_t index, uint16_t value)
+{
+    *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
+}
+
+static void pwmWriteOneShot125(uint8_t index, uint16_t value)
+{
+    *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
+}
+
 static void pwmWriteMultiShot(uint8_t index, uint16_t value)
 {
-    *motors[index]->ccr = lrintf(((float)(value-1000) / 0.69444f) + 360);
+    *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
 }
 
 void pwmWriteMotor(uint8_t index, uint16_t value)
@@ -218,7 +228,9 @@ void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn
         motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
     }
 
-    motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : pwmWriteStandard;
+    motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot :
+            ((fastPwmProtocolType == PWM_TYPE_ONESHOT125) ? pwmWriteOneShot125 :
+            pwmWriteOneShot42);
 }
 
 #ifdef USE_SERVOS
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index 7938b9a5b..a3b362437 100755
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -88,9 +88,36 @@ static uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
 #if USED_TIMERS & TIM_N(4)
         _CASE(4);
 #endif
+#if USED_TIMERS & TIM_N(5)
+        _CASE(5);
+#endif
+#if USED_TIMERS & TIM_N(6)
+        _CASE(6);
+#endif
+#if USED_TIMERS & TIM_N(7)
+        _CASE(7);
+#endif
 #if USED_TIMERS & TIM_N(8)
         _CASE(8);
 #endif
+#if USED_TIMERS & TIM_N(9)
+        _CASE(9);
+#endif
+#if USED_TIMERS & TIM_N(10)
+        _CASE(10);
+#endif
+#if USED_TIMERS & TIM_N(11)
+        _CASE(11);
+#endif
+#if USED_TIMERS & TIM_N(12)
+        _CASE(12);
+#endif
+#if USED_TIMERS & TIM_N(13)
+        _CASE(13);
+#endif
+#if USED_TIMERS & TIM_N(14)
+        _CASE(14);
+#endif
 #if USED_TIMERS & TIM_N(15)
         _CASE(15);
 #endif
@@ -121,9 +148,36 @@ TIM_TypeDef * const usedTimers[USED_TIMER_COUNT] = {
 #if USED_TIMERS & TIM_N(4)
     _DEF(4),
 #endif
+#if USED_TIMERS & TIM_N(5)
+    _DEF(5),
+#endif
+#if USED_TIMERS & TIM_N(6)
+    _DEF(6),
+#endif
+#if USED_TIMERS & TIM_N(7)
+    _DEF(7),
+#endif
 #if USED_TIMERS & TIM_N(8)
     _DEF(8),
 #endif
+#if USED_TIMERS & TIM_N(9)
+    _DEF(9),
+#endif
+#if USED_TIMERS & TIM_N(10)
+    _DEF(10),
+#endif
+#if USED_TIMERS & TIM_N(11)
+    _DEF(11),
+#endif
+#if USED_TIMERS & TIM_N(12)
+    _DEF(12),
+#endif
+#if USED_TIMERS & TIM_N(13)
+    _DEF(13),
+#endif
+#if USED_TIMERS & TIM_N(14)
+    _DEF(14),
+#endif
 #if USED_TIMERS & TIM_N(15)
     _DEF(15),
 #endif