diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index d486f6fba2..d566a0081b 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -50,9 +50,6 @@
 
 #include "config/runtime_config.h"
 
-//#define DEBUG_IMU
-//#define DEBUG_IMU_SPEED
-
 int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
 
@@ -411,19 +408,10 @@ static void imuCalculateEstimatedAttitude(void)
 
 void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors)
 {
-#ifdef DEBUG_IMU_SPEED
-	uint32_t time = micros();
-#endif
 	if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) {
         gyroUpdate();
-#ifdef DEBUG_IMU_SPEED
-    debug[0] = micros() - time; // gyro read time
-#endif
     }
     if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
-#ifdef DEBUG_IMU_SPEED
-        time = micros();
-#endif
         updateAccelerationReadings(accelerometerTrims);
         imuCalculateEstimatedAttitude();
     } else {
@@ -431,12 +419,6 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors
         accADC[Y] = 0;
         accADC[Z] = 0;
     }
-#ifdef DEBUG_IMU_SPEED
-    debug[1] = micros() - time;     // acc read time
-	if (imuUpdateSensors == ACC_AND_GYRO) {
-        debug[2] = debug[0] + debug[1]; // gyro + acc read time
-	}
-#endif
 }
 
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
diff --git a/src/main/mw.c b/src/main/mw.c
index 60832969f5..a322d33d46 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -90,8 +90,6 @@ enum {
     ALIGN_MAG = 2
 };
 
-//#define DEBUG_JITTER 3
-
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)       
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
@@ -102,7 +100,7 @@ enum {
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 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
-float dT;
+float dT = 0.000001f;  // dT set for 1khz mode
 
 int16_t magHold;
 int16_t headFreeModeHold;
@@ -166,6 +164,41 @@ bool isCalibrating()
     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 }
 
+void filterRc(void){
+    static int16_t lastCommand[4] = { 0, 0, 0, 0 };
+    static int16_t deltaRC[4] = { 0, 0, 0, 0 };
+    static int16_t factor, rcInterpolationFactor;
+    static filterStatePt1_t filteredCycleTimeState;
+    uint16_t rxRefreshRate, filteredCycleTime;
+
+    // Set RC refresh rate for sampling and channels to filter
+   	initRxRefreshRate(&rxRefreshRate);
+
+   	filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
+    rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
+
+    if (isRXDataNew) {
+        for (int channel=0; channel < 4; channel++) {
+        	deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+            lastCommand[channel] = rcCommand[channel];
+        }
+
+        isRXDataNew = false;
+        factor = rcInterpolationFactor - 1;
+    } else {
+        factor--;
+    }
+
+    // Interpolate steps of rcCommand
+    if (factor > 0) {
+        for (int channel=0; channel < 4; channel++) {
+            rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
+         }
+    } else {
+        factor = 0;
+    }
+}
+
 void annexCode(void)
 {
     int32_t tmp, tmp2;
@@ -242,6 +275,8 @@ void annexCode(void)
         rcCommand[PITCH] = rcCommand_PITCH;
     }
 
+    filterRc();  // rcCommand smoothing function
+
     if (feature(FEATURE_VBAT)) {
         if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
             vbatLastServiced = currentTime;
@@ -673,42 +708,6 @@ void processRx(void)
 
 }
 
-void filterRc(void){
-    static int16_t lastCommand[4] = { 0, 0, 0, 0 };
-    static int16_t deltaRC[4] = { 0, 0, 0, 0 };
-    static int16_t factor, rcInterpolationFactor;
-    static filterStatePt1_t filteredCycleTimeState;
-    uint16_t rxRefreshRate, filteredCycleTime;
-
-    // Set RC refresh rate for sampling and channels to filter
-   	initRxRefreshRate(&rxRefreshRate);
-
-   	filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
-    rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
-
-    if (isRXDataNew) {
-        for (int channel=0; channel < 4; channel++) {
-        	deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
-            lastCommand[channel] = rcCommand[channel];
-        }
-
-        isRXDataNew = false;
-        factor = rcInterpolationFactor - 1;
-    } else {
-        factor--;
-    }
-
-    // Interpolate steps of rcCommand
-    if (factor > 0) {
-        for (int channel=0; channel < 4; channel++) {
-            rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
-         }
-    } else {
-        factor = 0;
-    }
-}
-
-
 void loop(void)
 {
     static uint32_t loopTime;
@@ -776,26 +775,8 @@ void loop(void)
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
 
-#ifdef DEBUG_JITTER
-        static uint32_t previousCycleTime;
-
-		if (previousCycleTime > cycleTime) {
-		    debug[DEBUG_JITTER] = previousCycleTime - cycleTime;
-		} else {
-	        debug[DEBUG_JITTER] = cycleTime - previousCycleTime;
-		}
-		previousCycleTime = cycleTime;
-#endif
-
-
-        dT = (float)targetLooptime * 0.000001f;
-
-        filterApply7TapFIR(gyroADC);
-
         annexCode();
 
-        filterRc();
-
 #if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;
 #endif
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 4f703b676d..a6bfdaf2d6 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -22,6 +22,7 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
@@ -122,6 +123,8 @@ void gyroUpdate(void)
     }
     alignSensors(gyroADC, gyroADC, gyroAlign);
 
+    filterApply7TapFIR(gyroADC); // Apply filter to gyro
+
     if (!isGyroCalibrationComplete()) {
         performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
     }