diff --git a/radio/src/targets/common/arm/stm32/stm32_hal.cpp b/radio/src/targets/common/arm/stm32/stm32_hal.cpp
index c881223a5..1f40ab9db 100644
--- a/radio/src/targets/common/arm/stm32/stm32_hal.cpp
+++ b/radio/src/targets/common/arm/stm32/stm32_hal.cpp
@@ -20,9 +20,9 @@
  */
 
 #include "stm32_hal.h"
-#include "rtos.h"
+#include "timers_driver.h"
 
 extern "C" uint32_t HAL_GetTick(void)
 {
-    return RTOS_GET_MS();
+    return timersGetMsTick();
 }
diff --git a/radio/src/targets/common/arm/stm32/timers_driver.cpp b/radio/src/targets/common/arm/stm32/timers_driver.cpp
index ef59d0b53..36ae45170 100644
--- a/radio/src/targets/common/arm/stm32/timers_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/timers_driver.cpp
@@ -21,6 +21,8 @@
 
 #include "opentx.h"
 
+static volatile uint32_t msTickCount; // Used to get 1 kHz counter
+
 // Start TIMER at 2000000Hz
 void init2MhzTimer()
 {
@@ -30,10 +32,12 @@ void init2MhzTimer()
   TIMER_2MHz_TIMER->CR1 = TIM_CR1_CEN;
 }
 
-// Start TIMER at 200Hz
-void init5msTimer()
+// Start TIMER at 1000Hz
+void init1msTimer()
 {
-  INTERRUPT_xMS_TIMER->ARR = 4999; // 5mS in uS
+  msTickCount = 0;
+
+  INTERRUPT_xMS_TIMER->ARR = 999; // 5mS in uS
   INTERRUPT_xMS_TIMER->PSC = (PERI1_FREQUENCY * TIMER_MULT_APB1) / 1000000 - 1;  // 1uS
   INTERRUPT_xMS_TIMER->CCER = 0;
   INTERRUPT_xMS_TIMER->CCMR1 = 0;
@@ -44,12 +48,17 @@ void init5msTimer()
   NVIC_SetPriority(INTERRUPT_xMS_IRQn, 4);
 }
 
-void stop5msTimer()
+void stop1msTimer()
 {
   INTERRUPT_xMS_TIMER->CR1 = 0; // stop timer
   NVIC_DisableIRQ(INTERRUPT_xMS_IRQn);
 }
 
+uint32_t timersGetMsTick()
+{
+  return msTickCount;
+}
+
 static uint32_t watchdogTimeout = 0;
 
 void watchdogSuspend(uint32_t timeout)
@@ -57,20 +66,26 @@ void watchdogSuspend(uint32_t timeout)
   watchdogTimeout = timeout;
 }
 
-static void interrupt5ms()
+static void interrupt1ms()
 {
-  static uint8_t pre_scale; // Used to get 10 Hz counter
+  static uint8_t pre_scale = 0;
+
   ++pre_scale;
+  ++msTickCount;
+
 
   // 5ms loop
+  if(pre_scale == 5 || pre_scale == 10) {
 #if defined(HAPTIC)
-  DEBUG_TIMER_START(debugTimerHaptic);
-  HAPTIC_HEARTBEAT();
-  DEBUG_TIMER_STOP(debugTimerHaptic);
+    DEBUG_TIMER_START(debugTimerHaptic);
+    HAPTIC_HEARTBEAT();
+    DEBUG_TIMER_STOP(debugTimerHaptic);
 #endif
+  }
   
   // 10ms loop
-  if (pre_scale & 1) {
+  if (pre_scale == 10) {
+    pre_scale = 0;
     if (watchdogTimeout) {
       watchdogTimeout -= 1;
       WDG_RESET();  // Retrigger hardware watchdog
@@ -86,6 +101,6 @@ static void interrupt5ms()
 extern "C" void INTERRUPT_xMS_IRQHandler()
 {
   INTERRUPT_xMS_TIMER->SR &= ~TIM_SR_UIF;
-  interrupt5ms();
+  interrupt1ms();
   DEBUG_INTERRUPT(INT_5MS);
 }
diff --git a/radio/src/targets/common/arm/stm32/timers_driver.h b/radio/src/targets/common/arm/stm32/timers_driver.h
index c51734826..a8a6e7081 100644
--- a/radio/src/targets/common/arm/stm32/timers_driver.h
+++ b/radio/src/targets/common/arm/stm32/timers_driver.h
@@ -31,8 +31,8 @@ uint16_t getTmr2MHz();
 #include "hal.h"
 
 void init2MhzTimer();
-void init5msTimer();
-void stop5msTimer();
+void init1msTimer();
+void stop1msTimer();
 
 static inline uint16_t getTmr2MHz() { return TIMER_2MHz_TIMER->CNT; }
 
@@ -48,3 +48,5 @@ static inline tmr10ms_t get_tmr10ms()
 {
   return g_tmr10ms;
 }
+
+uint32_t timersGetMsTick();
diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp
index f1a6002fc..42284507d 100644
--- a/radio/src/targets/horus/board.cpp
+++ b/radio/src/targets/horus/board.cpp
@@ -168,7 +168,7 @@ void boardInit()
     TRACE("adcInit failed");
 
   init2MhzTimer();
-  init5msTimer();
+  init1msTimer();
 
   usbInit();
   hapticInit();
diff --git a/radio/src/targets/nv14/board.cpp b/radio/src/targets/nv14/board.cpp
index c612094ae..ee7bb69d5 100644
--- a/radio/src/targets/nv14/board.cpp
+++ b/radio/src/targets/nv14/board.cpp
@@ -185,7 +185,7 @@ void boardInit()
   battery_charge_init();
   globalData.flyskygimbals = flysky_gimbal_init();
   init2MhzTimer();
-  init5msTimer();
+  init1msTimer();
   TouchInit();
   usbInit();
 
diff --git a/radio/src/targets/nv14/hal.h b/radio/src/targets/nv14/hal.h
index 6d20d7eab..db73de640 100644
--- a/radio/src/targets/nv14/hal.h
+++ b/radio/src/targets/nv14/hal.h
@@ -37,7 +37,7 @@
  * TIM7 = 2MHz counter
  *
  *
- * TIM14 = 5ms counter
+ * TIM14 = 1ms counter
  */
 
 /* DMA Allocation:
diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp
index c5d3bd231..f15c16cc0 100644
--- a/radio/src/targets/taranis/board.cpp
+++ b/radio/src/targets/taranis/board.cpp
@@ -218,7 +218,7 @@ void boardInit()
   lcdInit(); // delaysInit() must be called before
   audioInit();
   init2MhzTimer();
-  init5msTimer();
+  init1msTimer();
   __enable_irq();
   usbInit();