diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h
index 698640cf4..8e159c684 100644
--- a/src/main/drivers/gyro_sync.h
+++ b/src/main/drivers/gyro_sync.h
@@ -5,7 +5,7 @@
  *      Author: borisb
  */
 
-#define INTERRUPT_WAIT_TIME 12
+#define INTERRUPT_WAIT_TIME 5
 
 extern uint32_t targetLooptime;
 
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 0b9d3b48e..90f3be37d 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -57,7 +57,6 @@
 #include "config/config.h"
 
 uint8_t motorCount;
-extern float dT;
 
 int16_t motor[MAX_SUPPORTED_MOTORS];
 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@@ -962,7 +961,8 @@ void filterServos(void)
 {
 #ifdef USE_SERVOS
     int16_t servoIdx;
-    static filterStatePt1_t servoFitlerState[MAX_SUPPORTED_SERVOS];
+    static bool servoFilterIsSet;
+    static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS];
 
 #if defined(MIXER_DEBUG)
     uint32_t startTime = micros();
@@ -970,7 +970,12 @@ void filterServos(void)
 
     if (mixerConfig->servo_lowpass_enable) {
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-            servo[servoIdx] = filterApplyPt1(servo[servoIdx], &servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, dT);
+            if (!servoFilterIsSet) {
+                BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetLooptime);
+                servoFilterIsSet = true;
+            }
+
+            servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx]));
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
         }
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 19219dd10..103ece150 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -135,6 +135,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     int axis, deltaCount;
     float horizonLevelStrength = 1;
 
+    float dT = (float)targetLooptime * 0.000001f;
+
     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
         for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
         deltaStateIsSet = true;
diff --git a/src/main/mw.c b/src/main/mw.c
index a66bdc024..6bef70fcc 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -101,7 +101,8 @@ enum {
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 #define IBATINTERVAL (6 * 3500)
 
-#define GYRO_WATCHDOG_DELAY 100 // Watchdog delay for gyro sync
+#define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
+#define JITTER_BUFFER_TIME 20  // cycleTime jitter buffer time
 
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 
@@ -119,7 +120,6 @@ extern uint32_t currentTime;
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 extern bool antiWindupProtection;
 
-static filterStatePt1_t filteredCycleTimeState;
 uint16_t filteredCycleTime;
 static bool isRXDataNew;
 
@@ -182,7 +182,7 @@ void filterRc(void){
     // Set RC refresh rate for sampling and channels to filter
     initRxRefreshRate(&rxRefreshRate);
 
-    rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
+    rcInterpolationFactor = rxRefreshRate / targetLooptime + 1;
 
     if (isRXDataNew) {
         for (int channel=0; channel < 4; channel++) {
@@ -640,15 +640,6 @@ static bool haveProcessedAnnexCodeOnce = false;
 
 void taskMainPidLoop(void)
 {
-    cycleTime = getTaskDeltaTime(TASK_SELF);
-    dT = (float)targetLooptime * 0.000001f;
-
-    // Calculate average cycle time and average jitter
-    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 0.5f, dT);
-
-    //debug[0] = cycleTime;
-    //debug[1] = cycleTime - filteredCycleTime;
-
     imuUpdateGyroAndAttitude();
 
     annexCode();
@@ -740,12 +731,21 @@ void taskMainPidLoop(void)
 
 // Function for loop trigger
 void taskMainPidLoopCheck(void) {
-    // getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
-    // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
-    uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+    static uint32_t previousTime;
+
+    cycleTime = micros() - previousTime;
+    previousTime = micros();
+
+    // Debugging parameters
+    debug[0] = cycleTime;
+    debug[1] = cycleTime - targetLooptime;
+    debug[2] = averageSystemLoadPercent;
 
     while (1) {
-        if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+        if (gyroSyncCheckUpdate() || ((cycleTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+            while (1) {
+                if (micros() >= JITTER_BUFFER_TIME + previousTime) break;
+            }
             break;
         }
     }
diff --git a/src/main/scheduler.c b/src/main/scheduler.c
index c1f258599..3c7155b39 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler.c
@@ -226,8 +226,7 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 
 uint16_t averageSystemLoadPercent = 0;
 
-#define REALTIME_GUARD_INTERVAL_MIN     10
-#define REALTIME_GUARD_INTERVAL_MAX     300
+#define GUARD_INTERVAL 5
 
 void taskSystem(void)
 {
@@ -248,7 +247,7 @@ void taskSystem(void)
         }
     }
 
-    realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, REALTIME_GUARD_INTERVAL_MIN, REALTIME_GUARD_INTERVAL_MAX);
+    realtimeGuardInterval = GUARD_INTERVAL;
 #if defined SCHEDULER_DEBUG
     debug[2] = realtimeGuardInterval;
 #endif