diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index fd7c8ba2d..ea12fc531 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1270,9 +1270,9 @@ static bool blackboxWriteSysinfo(void)
             }
             );
 
-        BLACKBOX_PRINT_HEADER_LINE("looptime", "%d",                        gyro.targetLooptime);
-        BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d",                 gyroConfig()->gyro_sync_denom);
-        BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d",               pidConfig()->pid_process_denom);
+        BLACKBOX_PRINT_HEADER_LINE("looptime", "%d",                        gyro.sampleLooptime);
+        BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d",                 1);
+        BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d",               activePidLoopDenom);
         BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d",                         currentControlRateProfile->thrMid8);
         BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d",                        currentControlRateProfile->thrExpo8);
         BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d",                        currentControlRateProfile->dynThrPID);
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 7fa205f00..c47726ef3 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -94,4 +94,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "FF_LIMIT",
     "FF_INTERPOLATED",
     "BLACKBOX_OUTPUT",
+    "GYRO_SAMPLE",
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index e80c16006..afcf284c4 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -110,6 +110,7 @@ typedef enum {
     DEBUG_FF_LIMIT,
     DEBUG_FF_INTERPOLATED,
     DEBUG_BLACKBOX_OUTPUT,
+    DEBUG_GYRO_SAMPLE,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 9e7de7fa4..f4d277728 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -54,6 +54,7 @@ bool cliMode = false;
 #include "common/typeconversion.h"
 #include "common/utils.h"
 
+#include "config/config.h"
 #include "config/config_eeprom.h"
 #include "config/feature.h"
 
@@ -94,7 +95,6 @@ bool cliMode = false;
 #include "drivers/vtx_table.h"
 
 #include "fc/board_info.h"
-#include "config/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
 #include "fc/rc.h"
@@ -4680,11 +4680,11 @@ static void cliStatus(char *cmdline)
 
     // Run status
 
-    const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID)));
+    const int gyroRate = getTaskDeltaTime(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYRO)));
     const int rxRate = currentRxRefreshRate == 0 ? 0 : (int)(1000000.0f / ((float)currentRxRefreshRate));
     const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
     cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
-            constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
+            constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYRO), gyroRate, rxRate, systemRate);
 
     // Battery meter
 
@@ -4731,21 +4731,8 @@ static void cliTasks(char *cmdline)
         cfTaskInfo_t taskInfo;
         getTaskInfo(taskId, &taskInfo);
         if (taskInfo.isEnabled) {
-            int taskFrequency;
-            int subTaskFrequency = 0;
-            if (taskId == TASK_GYROPID) {
-                subTaskFrequency = taskInfo.movingAverageCycleTime == 0.0f ? 0.0f : (int)(1000000.0f / (taskInfo.movingAverageCycleTime));
-                taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
-                if (pidConfig()->pid_process_denom > 1) {
-                    cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
-                } else {
-                    taskFrequency = subTaskFrequency;
-                    cliPrintf("%02d - (%11s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
-                }
-            } else {
-                taskFrequency = taskInfo.averageDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)taskInfo.averageDeltaTime));
-                cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
-            }
+            int taskFrequency = taskInfo.averageDeltaTime == 0 ? 0 : lrintf(1e6f / taskInfo.averageDeltaTime);
+            cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
             const int maxLoad = taskInfo.maxExecutionTime == 0 ? 0 :(taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
             const int averageLoad = taskInfo.averageExecutionTime == 0 ? 0 : (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
             if (taskId != TASK_SERIAL) {
@@ -4759,9 +4746,6 @@ static void cliTasks(char *cmdline)
             } else {
                 cliPrintLinef("%6d", taskFrequency);
             }
-            if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) {
-                cliPrintLinef("   - (%15s) %6d", taskInfo.subTaskName, subTaskFrequency);
-            }
 
             schedulerResetTaskMaxExecutionTime(taskId);
         }
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index d89b17fdc..fa15d708a 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -258,7 +258,6 @@ static const char * const lookupTableRxSpi[] = {
 
 static const char * const lookupTableGyroHardwareLpf[] = {
     "NORMAL",
-    "1KHZ_SAMPLING",
 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
     "EXPERIMENTAL"
 #endif
@@ -621,7 +620,6 @@ const clivalue_t valueTable[] = {
 #if defined(USE_GYRO_SPI_ICM20649)
     { "gyro_high_range",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
 #endif
-    { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) },
 
     { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
     { "gyro_lowpass_hz",            VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 5f3d6d8a4..7ecb3c6b5 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -113,7 +113,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
     .debug_mode = DEBUG_MODE,
     .task_statistics = true,
     .rateProfile6PosSwitch = false,
-    .cpu_overclock = 0,
+    .cpu_overclock = DEFAULT_CPU_OVERCLOCK,
     .powerOnArmingGraceTime = 5,
     .boardIdentifier = TARGET_BOARD_IDENTIFIER,
     .hseMhz = SYSTEM_HSE_VALUE,  // Not used for non-F4 targets
@@ -559,13 +559,6 @@ static void validateAndFixConfig(void)
 
 void validateAndFixGyroConfig(void)
 {
-#ifdef USE_GYRO_DATA_ANALYSE
-    // Disable dynamic filter if gyro loop is less than 2KHz
-    if (gyro.targetLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) {
-        featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
-    }
-#endif
-
     // Fix gyro filter settings to handle cases where an older configurator was used that
     // allowed higher cutoff limits from previous firmware versions.
     adjustFilterLimit(&gyroConfigMutable()->gyro_lowpass_hz, FILTER_FREQUENCY_MAX);
@@ -589,15 +582,6 @@ void validateAndFixGyroConfig(void)
     }
 #endif
 
-    if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
-        pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
-        gyroConfigMutable()->gyro_sync_denom = 1;
-    }
-
-#if defined(STM32F1)
-    gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
-#endif
-
     float samplingTime;
     switch (gyroMpuDetectionResult()->sensor) {
     case ICM_20649_SPI:
@@ -610,16 +594,6 @@ void validateAndFixGyroConfig(void)
         samplingTime = 0.000125f;
         break;
     }
-    if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
-        switch (gyroMpuDetectionResult()->sensor) {
-        case ICM_20649_SPI:
-            samplingTime = 1.0f / 1100.0f;
-            break;
-        default:
-            samplingTime = 0.001f;
-            break;
-        }
-    }
 
 
     // check for looptime restrictions based on motor protocol. Motor times have safety margin
@@ -654,13 +628,21 @@ void validateAndFixGyroConfig(void)
             motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
         }
     } else {
-        const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
+        const float pidLooptime = samplingTime * pidConfig()->pid_process_denom;
         if (pidLooptime < motorUpdateRestriction) {
-            const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
+            const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / samplingTime, 1, MAX_PID_PROCESS_DENOM);
             pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
         }
     }
 
+#ifdef USE_GYRO_DATA_ANALYSE
+    // Disable dynamic filter if gyro loop is less than 2KHz
+    const uint32_t configuredLooptime = (gyro.sampleRateHz > 0) ? (pidConfig()->pid_process_denom * 1e6 / gyro.sampleRateHz) : 0;
+    if (configuredLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) {
+        featureDisableImmediate(FEATURE_DYNAMIC_FILTER);
+    }
+#endif
+
 #ifdef USE_BLACKBOX
 #ifndef USE_FLASHFS
     if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h
index 27a92ec68..e9d7f44d7 100644
--- a/src/main/config/config_unittest.h
+++ b/src/main/config/config_unittest.h
@@ -26,14 +26,12 @@
 cfTask_t *unittest_scheduler_selectedTask;
 uint8_t unittest_scheduler_selectedTaskDynamicPriority;
 uint16_t unittest_scheduler_waitingTasks;
-bool unittest_outsideRealtimeGuardInterval;
 
 #define GET_SCHEDULER_LOCALS() \
     { \
     unittest_scheduler_selectedTask = selectedTask; \
     unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \
     unittest_scheduler_waitingTasks = waitingTasks; \
-    unittest_outsideRealtimeGuardInterval = outsideRealtimeGuardInterval; \
     }
 
 #else
diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
index 73f45ed44..4b206cff8 100644
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/src/main/drivers/accgyro/accgyro.h
@@ -59,17 +59,11 @@ typedef enum {
 
 typedef enum {
     GYRO_HARDWARE_LPF_NORMAL,
-    GYRO_HARDWARE_LPF_1KHZ_SAMPLE,
 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
     GYRO_HARDWARE_LPF_EXPERIMENTAL
 #endif
 } gyroHardwareLpf_e;
 
-typedef enum {
-    GYRO_32KHZ_HARDWARE_LPF_NORMAL,
-    GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL
-} gyro32khzHardwareLpf;
-
 typedef enum {
     GYRO_RATE_1_kHz,
     GYRO_RATE_1100_Hz,
@@ -106,6 +100,8 @@ typedef struct gyroDev_s {
     uint8_t gyroHasOverflowProtection;
     gyroHardware_e gyroHardware;
     fp_rotationMatrix_t rotationMatrix;
+    uint16_t gyroSampleRateHz;
+    uint16_t accSampleRateHz;
 } gyroDev_t;
 
 typedef struct accDev_s {
diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c
index 6adaf159a..9d8c1ef75 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -350,10 +350,6 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro)
                 break;
 #endif
 
-            case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
-                ret = 1;
-                break;
-
             case GYRO_HARDWARE_LPF_NORMAL:
             default:
                 ret = 0;
diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c
index 51291cc04..1127fe3c5 100644
--- a/src/main/drivers/accgyro/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu3050.c
@@ -53,17 +53,6 @@
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
-static uint8_t mpu3050GetDLPF(uint8_t lpf)
-{
-    uint8_t ret;
-    if (lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
-        ret = MPU3050_DLPF_188HZ;
-    } else {
-        ret = MPU3050_DLPF_256HZ;
-    }
-    return ret;
-}
-
 static void mpu3050Init(gyroDev_t *gyro)
 {
     delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
@@ -73,7 +62,7 @@ static void mpu3050Init(gyroDev_t *gyro)
         failureMode(FAILURE_ACC_INIT);
     }
 
-    busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpu3050GetDLPF(gyro->hardware_lpf));
+    busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | MPU3050_DLPF_256HZ);
     busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0);
     busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
     busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c
index e0603bf5e..0f66bbe5e 100644
--- a/src/main/drivers/accgyro/gyro_sync.c
+++ b/src/main/drivers/accgyro/gyro_sync.c
@@ -47,41 +47,30 @@ bool gyroSyncCheckUpdate(gyroDev_t *gyro)
     return ret;
 }
 
-uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator)
+uint16_t gyroSetSampleRate(gyroDev_t *gyro)
 {
-    float gyroSamplePeriod;
+    uint16_t gyroSampleRateHz;
+    uint16_t accSampleRateHz;
 
-    if (lpf != GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
-        switch (gyro->mpuDetectionResult.sensor) {
-            case BMI_160_SPI:
-                gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
-                gyroSamplePeriod = 312.0f;
-                break;
-            case ICM_20649_SPI:
-                gyro->gyroRateKHz = GYRO_RATE_9_kHz;
-                gyroSamplePeriod = 1000000.0f / 9000.0f;
-                break;
-            default:
-                gyro->gyroRateKHz = GYRO_RATE_8_kHz;
-                gyroSamplePeriod = 125.0f;
-                break;
-        }
-    } else {
-        switch (gyro->mpuDetectionResult.sensor) {
+    switch (gyro->mpuDetectionResult.sensor) {
+        case BMI_160_SPI:
+            gyro->gyroRateKHz = GYRO_RATE_3200_Hz;
+            gyroSampleRateHz = 3200;
+            accSampleRateHz = 800;
+            break;
         case ICM_20649_SPI:
-            gyro->gyroRateKHz = GYRO_RATE_1100_Hz;
-            gyroSamplePeriod = 1000000.0f / 1100.0f;
+            gyro->gyroRateKHz = GYRO_RATE_9_kHz;
+            gyroSampleRateHz = 9000;
+            accSampleRateHz = 1125;
             break;
         default:
-            gyro->gyroRateKHz = GYRO_RATE_1_kHz;
-            gyroSamplePeriod = 1000.0f;
+            gyro->gyroRateKHz = GYRO_RATE_8_kHz;
+            gyroSampleRateHz = 8000;
+            accSampleRateHz = 1000;
             break;
-        }
-        gyroSyncDenominator = 1; // Always full Sampling 1khz
     }
 
-    // calculate gyro divider and targetLooptime (expected cycleTime)
-    gyro->mpuDividerDrops  = gyroSyncDenominator - 1;
-    const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod);
-    return targetLooptime;
+    gyro->mpuDividerDrops  = 0; // we no longer use the gyro's sample divider
+    gyro->accSampleRateHz = accSampleRateHz;
+    return gyroSampleRateHz;
 }
diff --git a/src/main/drivers/accgyro/gyro_sync.h b/src/main/drivers/accgyro/gyro_sync.h
index ab05deefb..e0aac884c 100644
--- a/src/main/drivers/accgyro/gyro_sync.h
+++ b/src/main/drivers/accgyro/gyro_sync.h
@@ -30,4 +30,4 @@
 #include "drivers/accgyro/accgyro.h"
 
 bool gyroSyncCheckUpdate(gyroDev_t *gyro);
-uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator);
+uint16_t gyroSetSampleRate(gyroDev_t *gyro);
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 42f1d8f32..1cb1ef2d4 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -58,6 +58,7 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 #include "fc/stats.h"
+#include "fc/tasks.h"
 
 #include "flight/failsafe.h"
 #include "flight/gps_rescue.h"
@@ -142,6 +143,8 @@ enum {
 int16_t magHold;
 #endif
 
+static FAST_RAM_ZERO_INIT uint8_t pidUpdateCounter;
+
 static bool flipOverAfterCrashActive = false;
 
 static timeUs_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
@@ -1214,10 +1217,42 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
     processRcCommand();
 }
 
+FAST_CODE void taskGyroSample(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    gyroUpdate();
+    if (pidUpdateCounter % activePidLoopDenom == 0) {
+        pidUpdateCounter = 0;
+    }
+    pidUpdateCounter++;
+}
+
+FAST_CODE bool gyroFilterReady(void)
+{
+    if (pidUpdateCounter % activePidLoopDenom == 0) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+FAST_CODE bool pidLoopReady(void)
+{
+    if ((pidUpdateCounter % activePidLoopDenom) == (activePidLoopDenom / 2)) {
+        return true;
+    }
+    return false;
+}
+
+FAST_CODE void taskFiltering(timeUs_t currentTimeUs)
+{
+    gyroFiltering(currentTimeUs);
+
+}
+
 // Function for loop trigger
 FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
 {
-    static uint32_t pidUpdateCounter = 0;
 
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
     if (lockMainPID() != 0) return;
@@ -1228,15 +1263,12 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
     // 1 - subTaskPidController()
     // 2 - subTaskMotorUpdate()
     // 3 - subTaskPidSubprocesses()
-    gyroUpdate(currentTimeUs);
     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
 
-    if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) {
-        subTaskRcCommand(currentTimeUs);
-        subTaskPidController(currentTimeUs);
-        subTaskMotorUpdate(currentTimeUs);
-        subTaskPidSubprocesses(currentTimeUs);
-    }
+    subTaskRcCommand(currentTimeUs);
+    subTaskPidController(currentTimeUs);
+    subTaskMotorUpdate(currentTimeUs);
+    subTaskPidSubprocesses(currentTimeUs);
 
     if (debugMode == DEBUG_CYCLETIME) {
         debug[0] = getTaskDeltaTime(TASK_SELF);
diff --git a/src/main/fc/core.h b/src/main/fc/core.h
index c316a2c33..6afd626c4 100644
--- a/src/main/fc/core.h
+++ b/src/main/fc/core.h
@@ -80,6 +80,10 @@ void tryArm(void);
 bool processRx(timeUs_t currentTimeUs);
 void updateArmingStatus(void);
 
+void taskGyroSample(timeUs_t currentTimeUs);
+bool gyroFilterReady(void);
+bool pidLoopReady(void);
+void taskFiltering(timeUs_t currentTimeUs);
 void taskMainPidLoop(timeUs_t currentTimeUs);
 
 bool isFlipOverAfterCrashActive(void);
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index d6dd430e8..2391f280f 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -724,13 +724,19 @@ void init(void)
 
     systemState |= SYSTEM_STATE_SENSORS_READY;
 
-    // gyro.targetLooptime set in sensorsAutodetect(),
-    // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
+    // Set the targetLooptime based on the detected gyro sampleRateHz and pid_process_denom
+    gyroSetTargetLooptime(pidConfig()->pid_process_denom);
+
+    // Validate and correct the gyro config or PID loop time if needed
     validateAndFixGyroConfig();
+
+    // Now reset the targetLooptime as it's possible for the validation to change the pid_process_denom
+    gyroSetTargetLooptime(pidConfig()->pid_process_denom);
+
+    // Finally initialize the gyro filtering
+    gyroInitFilters();
+
     pidInit(currentPidProfile);
-#ifdef USE_ACC
-    accInitFilters();
-#endif
 
 #ifdef USE_PID_AUDIO
     pidAudioInit();
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 3d1580607..6541563e3 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -249,14 +249,19 @@ void tasksInit(void)
 #endif
 
     if (sensors(SENSOR_GYRO)) {
-        rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
-        setTaskEnabled(TASK_GYROPID, true);
+        rescheduleTask(TASK_GYRO, gyro.sampleLooptime);
+        rescheduleTask(TASK_FILTER, gyro.targetLooptime);
+        rescheduleTask(TASK_PID, gyro.targetLooptime);
+        setTaskEnabled(TASK_GYRO, true);
+        setTaskEnabled(TASK_FILTER, true);
+        setTaskEnabled(TASK_PID, true);
+        schedulerEnableGyro();
     }
 
 #if defined(USE_ACC)
-    if (sensors(SENSOR_ACC)) {
+    if (sensors(SENSOR_ACC) && acc.sampleRateHz) {
         setTaskEnabled(TASK_ACCEL, true);
-        rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
+        rescheduleTask(TASK_ACCEL, TASK_PERIOD_HZ(acc.sampleRateHz));
         setTaskEnabled(TASK_ATTITUDE, true);
     }
 #endif
@@ -394,7 +399,9 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_STACK_CHECK] = DEFINE_TASK("STACKCHECK", NULL, NULL, taskStackCheck, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
 #endif
 
-    [TASK_GYROPID] = DEFINE_TASK("PID", "GYRO", NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
+    [TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
+    [TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
+    [TASK_PID] = DEFINE_TASK("PID", NULL, NULL, taskMainPidLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 #ifdef USE_ACC
     [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
     [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
diff --git a/src/main/fc/tasks.h b/src/main/fc/tasks.h
index 109dd75ff..903f10e04 100644
--- a/src/main/fc/tasks.h
+++ b/src/main/fc/tasks.h
@@ -20,6 +20,4 @@
 
 #pragma once
 
-#define LOOPTIME_SUSPEND_TIME 3  // Prevent too long busy wait times
-
 void tasksInit(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b06edd6c8..6cfad9306 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -755,7 +755,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
 
 void pidInit(const pidProfile_t *pidProfile)
 {
-    pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
+    pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
     pidInitFilters(pidProfile);
     pidInitConfig(pidProfile);
 #ifdef USE_RPM_FILTER
@@ -857,7 +857,10 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
     return constrainf(horizonLevelStrength, 0, 1);
 }
 
-STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
+// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
+// The impact is possibly slightly slower performance on F7/H7 but they have more than enough
+// processing power that it should be a non-issue.
+STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
     // calculate error angle and limit the angle to the max inclination
     // rcDeflection is in range [-1.0, 1.0]
     float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
@@ -1215,7 +1218,10 @@ float pidGetAirmodeThrottleOffset()
 #define LAUNCH_CONTROL_MIN_RATE 5.0f
 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f  // The remaining angle degrees where rate dampening starts
 
-static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
+// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
+// The impact is possibly slightly slower performance on F7/H7 but they have more than enough
+// processing power that it should be a non-issue.
+static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
 {
     float ret = 0.0f;
 
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
index f96cf699e..9e46162ec 100644
--- a/src/main/flight/rpm_filter.c
+++ b/src/main/flight/rpm_filter.c
@@ -124,7 +124,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
         return;
     }
 
-    pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom;
+    pidLooptime = gyro.targetLooptime;
     if (config->gyro_rpm_notch_harmonics) {
         gyroFilter = &filters[numberRpmNotchFilters++];
         rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics,
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index bc9001084..ff76ee35c 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -655,6 +655,9 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
         // Added in API version 1.42
         sbufWriteU8(dst, systemConfig()->configurationState);
 
+        //Added in API version 1.43
+        sbufWriteU16(dst, gyro.sampleRateHz); // informational so the configurator can display the correct gyro/pid frequencies in the drop-down
+
         break;
     }
 
@@ -971,7 +974,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
             boxBitmask_t flightModeFlags;
             const int flagBits = packFlightModeFlags(&flightModeFlags);
 
-            sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID));
+            sbufWriteU16(dst, getTaskDeltaTime(TASK_PID));
 #ifdef USE_I2C
             sbufWriteU16(dst, i2cGetErrorCounter());
 #else
@@ -1634,7 +1637,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         break;
     }
     case MSP_ADVANCED_CONFIG:
-        sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
+        sbufWriteU8(dst, 1);  // was gyroConfig()->gyro_sync_denom - removed in API 1.43
         sbufWriteU8(dst, pidConfig()->pid_process_denom);
         sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
         sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
@@ -2424,7 +2427,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
     }
 
     case MSP_SET_ADVANCED_CONFIG:
-        gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
+        sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43
         pidConfigMutable()->pid_process_denom = sbufReadU8(src);
         motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
 #ifdef USE_DSHOT
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index 9dbecb2e7..0fd8b7fac 100644
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -39,6 +39,11 @@
 
 #include "drivers/time.h"
 
+#include "fc/core.h"
+
+#define TASK_AVERAGE_EXECUTE_FALLBACK_US 30 // Default task average time if USE_TASK_STATISTICS is not defined
+#define TASK_AVERAGE_EXECUTE_PADDING_US 5   // Add a little padding to the average execution time
+
 // DEBUG_SCHEDULER, timings for:
 // 0 - gyroUpdate()
 // 1 - pidController()
@@ -57,6 +62,7 @@ static FAST_RAM_ZERO_INIT int taskQueuePos = 0;
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0;
 
 static FAST_RAM int periodCalculationBasisOffset = offsetof(cfTask_t, lastExecutedAt);
+static FAST_RAM_ZERO_INIT bool gyroEnabled;
 
 // No need for a linked list for the queue, since items are only inserted at startup
 
@@ -140,7 +146,6 @@ void taskSystemLoad(timeUs_t currentTimeUs)
 }
 
 #if defined(USE_TASK_STATISTICS)
-#define MOVING_SUM_COUNT 32
 timeUs_t checkFuncMaxExecutionTime;
 timeUs_t checkFuncTotalExecutionTime;
 timeUs_t checkFuncMovingSumExecutionTime;
@@ -150,8 +155,8 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
 {
     checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
     checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
-    checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
-    checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / MOVING_SUM_COUNT;
+    checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
+    checkFuncInfo->averageDeltaTime = checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
 }
 #endif
 
@@ -165,8 +170,8 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
     taskInfo->subTaskName = cfTasks[taskId].subTaskName;
     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
-    taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / MOVING_SUM_COUNT;
-    taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / MOVING_SUM_COUNT;
+    taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
+    taskInfo->averageDeltaTime = cfTasks[taskId].movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
     taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
     taskInfo->movingAverageCycleTime = cfTasks[taskId].movingAverageCycleTime;
 #endif
@@ -271,89 +276,12 @@ inline static timeUs_t getPeriodCalculationBasis(const cfTask_t* task)
     }
 }
 
-FAST_CODE void scheduler(void)
+FAST_CODE timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs)
 {
-    // Cache currentTime
-    const timeUs_t currentTimeUs = micros();
-
-    // Check for realtime tasks
-    bool outsideRealtimeGuardInterval = true;
-    for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
-        const timeUs_t nextExecuteAt = getPeriodCalculationBasis(task) + task->desiredPeriod;
-        if ((timeDelta_t)(currentTimeUs - nextExecuteAt) >= 0) {
-            outsideRealtimeGuardInterval = false;
-            break;
-        }
-    }
-
-    // The task to be invoked
-    cfTask_t *selectedTask = NULL;
-    uint16_t selectedTaskDynamicPriority = 0;
-
-    // Update task dynamic priorities
-    uint16_t waitingTasks = 0;
-    for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
-        // Task has checkFunc - event driven
-        if (task->checkFunc) {
-#if defined(SCHEDULER_DEBUG)
-            const timeUs_t currentTimeBeforeCheckFuncCall = micros();
-#else
-            const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
-#endif
-            // Increase priority for event driven tasks
-            if (task->dynamicPriority > 0) {
-                task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
-                task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
-                waitingTasks++;
-            } else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
-#if defined(SCHEDULER_DEBUG)
-                DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
-#endif
-#if defined(USE_TASK_STATISTICS)
-                if (calculateTaskStatistics) {
-                    const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
-                    checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT;
-                    checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / MOVING_SUM_COUNT;
-                    checkFuncTotalExecutionTime += checkFuncExecutionTime;   // time consumed by scheduler + task
-                    checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
-                }
-#endif
-                task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
-                task->taskAgeCycles = 1;
-                task->dynamicPriority = 1 + task->staticPriority;
-                waitingTasks++;
-            } else {
-                task->taskAgeCycles = 0;
-            }
-        } else {
-            // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
-            // Task age is calculated from last execution
-            task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod);
-            if (task->taskAgeCycles > 0) {
-                task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
-                waitingTasks++;
-            }
-        }
-
-        if (task->dynamicPriority > selectedTaskDynamicPriority) {
-            const bool taskCanBeChosenForScheduling =
-                (outsideRealtimeGuardInterval) ||
-                (task->taskAgeCycles > 1) ||
-                (task->staticPriority == TASK_PRIORITY_REALTIME);
-            if (taskCanBeChosenForScheduling) {
-                selectedTaskDynamicPriority = task->dynamicPriority;
-                selectedTask = task;
-            }
-        }
-    }
-
-    totalWaitingTasksSamples++;
-    totalWaitingTasks += waitingTasks;
-
-    currentTask = selectedTask;
+    timeUs_t taskExecutionTime = 0;
 
     if (selectedTask) {
-        // Found a task that should be run
+        currentTask = selectedTask;
         selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
 #if defined(USE_TASK_STATISTICS)
         float period = currentTimeUs - selectedTask->lastExecutedAt;
@@ -367,9 +295,9 @@ FAST_CODE void scheduler(void)
         if (calculateTaskStatistics) {
             const timeUs_t currentTimeBeforeTaskCall = micros();
             selectedTask->taskFunc(currentTimeBeforeTaskCall);
-            const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
-            selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT;
-            selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / MOVING_SUM_COUNT;
+            taskExecutionTime = micros() - currentTimeBeforeTaskCall;
+            selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
+            selectedTask->movingSumDeltaTime += selectedTask->taskLatestDeltaTime - selectedTask->movingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
             selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
             selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
             selectedTask->movingAverageCycleTime += 0.05f * (period - selectedTask->movingAverageCycleTime);
@@ -378,13 +306,128 @@ FAST_CODE void scheduler(void)
         {
             selectedTask->taskFunc(currentTimeUs);
         }
+    }
+
+    return taskExecutionTime;
+}
+
+FAST_CODE void scheduler(void)
+{
+    // Cache currentTime
+    const timeUs_t schedulerStartTimeUs = micros();
+    timeUs_t currentTimeUs = schedulerStartTimeUs;
+    timeUs_t taskExecutionTime = 0;
+    cfTask_t *selectedTask = NULL;
+    uint16_t selectedTaskDynamicPriority = 0;
+    uint16_t waitingTasks = 0;
+    bool realtimeTaskRan = false;
+    timeDelta_t gyroTaskDelayUs;
+
+    if (gyroEnabled) {
+        // Realtime gyro/filtering/PID tasks get complete priority
+        cfTask_t *gyroTask = &cfTasks[TASK_GYRO];
+        const timeUs_t gyroExecuteTime = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriod;
+        gyroTaskDelayUs = cmpTimeUs(gyroExecuteTime, currentTimeUs);  // time until the next expected gyro sample
+        if (cmpTimeUs(currentTimeUs, gyroExecuteTime) >= 0) {
+            taskExecutionTime = schedulerExecuteTask(gyroTask, currentTimeUs);
+            if (gyroFilterReady()) {
+                taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_FILTER], currentTimeUs);
+            }
+            if (pidLoopReady()) {
+                taskExecutionTime += schedulerExecuteTask(&cfTasks[TASK_PID], currentTimeUs);
+            }
+            currentTimeUs = micros();
+            realtimeTaskRan = true;
+        }
+    }
+
+    if (!gyroEnabled || realtimeTaskRan || (gyroTaskDelayUs > GYRO_TASK_GUARD_INTERVAL_US)) {
+        // The task to be invoked
+
+        // Update task dynamic priorities
+        for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
+            if (task->staticPriority != TASK_PRIORITY_REALTIME) {
+                // Task has checkFunc - event driven
+                if (task->checkFunc) {
+#if defined(SCHEDULER_DEBUG)
+                    const timeUs_t currentTimeBeforeCheckFuncCall = micros();
+#else
+                    const timeUs_t currentTimeBeforeCheckFuncCall = currentTimeUs;
+#endif
+                    // Increase priority for event driven tasks
+                    if (task->dynamicPriority > 0) {
+                        task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
+                        task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
+                        waitingTasks++;
+                    } else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
+#if defined(SCHEDULER_DEBUG)
+                        DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCall);
+#endif
+#if defined(USE_TASK_STATISTICS)
+                        if (calculateTaskStatistics) {
+                            const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall;
+                            checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT;
+                            checkFuncMovingSumDeltaTime += task->taskLatestDeltaTime - checkFuncMovingSumDeltaTime / TASK_STATS_MOVING_SUM_COUNT;
+                            checkFuncTotalExecutionTime += checkFuncExecutionTime;   // time consumed by scheduler + task
+                            checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
+                        }
+#endif
+                        task->lastSignaledAt = currentTimeBeforeCheckFuncCall;
+                        task->taskAgeCycles = 1;
+                        task->dynamicPriority = 1 + task->staticPriority;
+                        waitingTasks++;
+                    } else {
+                        task->taskAgeCycles = 0;
+                    }
+                } else {
+                    // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
+                    // Task age is calculated from last execution
+                    task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriod);
+                    if (task->taskAgeCycles > 0) {
+                        task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
+                        waitingTasks++;
+                    }
+                }
+
+                if (task->dynamicPriority > selectedTaskDynamicPriority) {
+                    selectedTaskDynamicPriority = task->dynamicPriority;
+                    selectedTask = task;
+                }
+            }
+        }
+
+        totalWaitingTasksSamples++;
+        totalWaitingTasks += waitingTasks;
+
+        if (selectedTask) {
+            timeDelta_t taskRequiredTimeUs = TASK_AVERAGE_EXECUTE_FALLBACK_US;  // default average time if task statistics are not available
+#if defined(USE_TASK_STATISTICS)
+            if (calculateTaskStatistics) {
+                taskRequiredTimeUs = selectedTask->movingSumExecutionTime / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US;
+            }
+#endif
+            // Add in the time spent so far in check functions and the scheduler logic
+            taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs);
+            if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) {
+                taskExecutionTime += schedulerExecuteTask(selectedTask, currentTimeUs);
+            } else {
+                selectedTask = NULL;
+            }
+        }
+    }
+
 
 #if defined(SCHEDULER_DEBUG)
-        DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
-    } else {
-        DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
+    DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTime); // time spent in scheduler
+#else
+    UNUSED(taskExecutionTime);
 #endif
-    }
 
     GET_SCHEDULER_LOCALS();
 }
+
+void schedulerEnableGyro(void)
+{
+    gyroEnabled = true;
+}
+
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 02325ceab..51759f21e 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -27,14 +27,19 @@
 #define TASK_PERIOD_MS(ms) ((ms) * 1000)
 #define TASK_PERIOD_US(us) (us)
 
+#define GYRO_TASK_GUARD_INTERVAL_US 10  // Don't run any other tasks if gyro task will be run soon
+
+#if defined(USE_TASK_STATISTICS)
+#define TASK_STATS_MOVING_SUM_COUNT 32
+#endif
 
 typedef enum {
-    TASK_PRIORITY_IDLE = 0,     // Disables dynamic scheduling, task is executed only if no other task is active this cycle
+    TASK_PRIORITY_REALTIME = -1, // Task will be run outside the scheduler logic
+    TASK_PRIORITY_IDLE = 0,      // Disables dynamic scheduling, task is executed only if no other task is active this cycle
     TASK_PRIORITY_LOW = 1,
     TASK_PRIORITY_MEDIUM = 3,
     TASK_PRIORITY_MEDIUM_HIGH = 4,
     TASK_PRIORITY_HIGH = 5,
-    TASK_PRIORITY_REALTIME = 6,
     TASK_PRIORITY_MAX = 255
 } cfTaskPriority_e;
 
@@ -49,7 +54,7 @@ typedef struct {
     const char * taskName;
     const char * subTaskName;
     bool         isEnabled;
-    uint8_t      staticPriority;
+    int8_t       staticPriority;
     timeDelta_t  desiredPeriod;
     timeDelta_t  latestDeltaTime;
     timeUs_t     maxExecutionTime;
@@ -63,7 +68,9 @@ typedef enum {
     /* Actual tasks */
     TASK_SYSTEM = 0,
     TASK_MAIN,
-    TASK_GYROPID,
+    TASK_GYRO,
+    TASK_FILTER,
+    TASK_PID,
     TASK_ACCEL,
     TASK_ATTITUDE,
     TASK_RX,
@@ -153,7 +160,7 @@ typedef struct {
     bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
     void (*taskFunc)(timeUs_t currentTimeUs);
     timeDelta_t desiredPeriod;      // target period of execution
-    const uint8_t staticPriority;   // dynamicPriority grows in steps of this size, shouldn't be zero
+    const int8_t staticPriority;    // dynamicPriority grows in steps of this size
 
     // Scheduling
     uint16_t dynamicPriority;       // measurement of how old task was last executed, used to avoid task starvation
@@ -188,8 +195,10 @@ void schedulerResetCheckFunctionMaxExecutionTime(void);
 
 void schedulerInit(void);
 void scheduler(void);
+timeUs_t schedulerExecuteTask(cfTask_t *selectedTask, timeUs_t currentTimeUs);
 void taskSystemLoad(timeUs_t currentTime);
 void schedulerOptimizeRate(bool optimizeRate);
+void schedulerEnableGyro(void);
 
 #define LOAD_PERCENTAGE_ONE 100
 
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index a6270b780..206c3d9fc 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -323,7 +323,20 @@ retry:
     return true;
 }
 
-bool accInit(uint32_t gyroSamplingInverval)
+void accInitFilters(void)
+{
+    // Only set the lowpass cutoff if the ACC sample rate is detected otherwise
+    // the filter initialization is not defined (sample rate = 0)
+    accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
+    if (accLpfCutHz) {
+        const uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSampleTimeUs);
+        }
+    }
+}
+
+bool accInit(uint16_t accSampleRateHz)
 {
     memset(&acc, 0, sizeof(acc));
     // copy over the common gyro mpu settings
@@ -353,23 +366,9 @@ bool accInit(uint32_t gyroSamplingInverval)
     acc.dev.acc_1G = 256; // set default
     acc.dev.initFn(&acc.dev); // driver initialisation
     acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
-    // set the acc sampling interval according to the gyro sampling interval
-    switch (gyroSamplingInverval) {  // Switch statement kept in place to change acc sampling interval in the future
-    case 500:
-    case 375:
-    case 250:
-    case 125:
-        acc.accSamplingInterval = 1000;
-        break;
-    case 1000:
-    default:
-        acc.accSamplingInterval = 1000;
-    }
-    if (accLpfCutHz) {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
-        }
-    }
+
+    acc.sampleRateHz = accSampleRateHz;
+    accInitFilters();
     return true;
 }
 
@@ -552,16 +551,6 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
     accelerationTrims = accelerationTrimsToUse;
 }
 
-void accInitFilters(void)
-{
-    accLpfCutHz = accelerometerConfig()->acc_lpf_hz;
-    if (acc.accSamplingInterval) {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
-        }
-    }
-}
-
 void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 {
     accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index cff3caf16..b060b7626 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -49,7 +49,7 @@ typedef enum {
 
 typedef struct acc_s {
     accDev_t dev;
-    uint32_t accSamplingInterval;
+    uint16_t sampleRateHz;
     float accADC[XYZ_AXIS_COUNT];
     bool isAccelUpdatedAtLeastOnce;
 } acc_t;
@@ -78,7 +78,7 @@ typedef struct accelerometerConfig_s {
 PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
 #endif
 
-bool accInit(uint32_t gyroTargetLooptime);
+bool accInit(uint16_t accSampleRateHz);
 bool accIsCalibrationComplete(void);
 bool accHasBeenCalibrated(void);
 void accStartCalibration(void);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 83746bca1..0145dc78a 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -114,6 +114,7 @@ static bool gyroHasOverflowProtection = true;
 
 static FAST_RAM_ZERO_INIT bool useDualGyroDebugging;
 static FAST_RAM_ZERO_INIT flight_dynamics_index_t gyroDebugAxis;
+FAST_RAM uint8_t activePidLoopDenom = 1;
 
 typedef struct gyroCalibration_s {
     float sum[XYZ_AXIS_COUNT];
@@ -141,19 +142,10 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
 #endif
 
 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
-static void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz);
+static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime);
 
 #define DEBUG_GYRO_CALIBRATION 3
 
-#ifdef STM32F10X
-#define GYRO_SYNC_DENOM_DEFAULT 8
-#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
-   || defined(USE_GYRO_SPI_ICM20689)
-#define GYRO_SYNC_DENOM_DEFAULT 1
-#else
-#define GYRO_SYNC_DENOM_DEFAULT 3
-#endif
-
 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 
@@ -172,7 +164,6 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
 { 
     gyroConfig->gyroCalibrationDuration = 125;        // 1.25 seconds
     gyroConfig->gyroMovementCalibrationThreshold = 48;
-    gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
     gyroConfig->gyro_lowpass_type = FILTER_PT1;
     gyroConfig->gyro_lowpass_hz = 200;  // NOTE: dynamic lpf is enabled by default so this setting is actually
@@ -421,8 +412,8 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
     buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
     gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
 
-    // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
-    gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
+    // The targetLooptime gets set later based on the active sensor's gyroSampleRateHz and pid_process_denom
+    gyroSensor->gyroDev.gyroSampleRateHz = gyroSetSampleRate(&gyroSensor->gyroDev);
     gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
 
@@ -490,6 +481,7 @@ bool gyroInit(void)
     case DEBUG_GYRO_SCALED:
     case DEBUG_GYRO_FILTERED:
     case DEBUG_DYN_LPF:
+    case DEBUG_GYRO_SAMPLE:
         gyroDebugMode = debugMode;
         break;
     case DEBUG_DUAL_GYRO_DIFF:
@@ -567,7 +559,14 @@ bool gyroInit(void)
     }
 #endif
 
-    gyroInitFilters();
+    if (gyro.rawSensorDev) {
+        gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz;
+        gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz;
+    } else {
+        gyro.sampleRateHz = 0;
+        gyro.accSampleRateHz = 0;
+    }
+
     return true;
 }
 
@@ -603,7 +602,7 @@ static void dynLpfFilterInit()
 }
 #endif
 
-void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
+bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime)
 {
     filterApplyFnPtr *lowpassFilterApplyFn;
     gyroLowpassFilter_t *lowpassFilter = NULL;
@@ -620,12 +619,14 @@ void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
         break;
 
     default:
-        return;
+        return false;
     }
 
+    bool ret = false;
+
     // Establish some common constants
-    const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
-    const float gyroDt = gyro.targetLooptime * 1e-6f;
+    const uint32_t gyroFrequencyNyquist = 1000000 / 2 / looptime;
+    const float gyroDt = looptime * 1e-6f;
 
     // Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
     const float gain = pt1FilterGain(lpfHz, gyroDt);
@@ -642,6 +643,7 @@ void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
             }
+            ret = true;
             break;
         case FILTER_BIQUAD:
 #ifdef USE_DYN_LPF
@@ -650,11 +652,13 @@ void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 #endif
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
+                biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
             }
+            ret = true;
             break;
         }
     }
+    return ret;
 }
 
 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
@@ -744,6 +748,18 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 #endif
 }
 
+void gyroSetTargetLooptime(uint8_t pidDenom)
+{
+    activePidLoopDenom = pidDenom;
+    if (gyro.sampleRateHz) {
+        gyro.sampleLooptime = 1e6 / gyro.sampleRateHz;
+        gyro.targetLooptime = activePidLoopDenom * 1e6 / gyro.sampleRateHz;
+    } else {
+        gyro.sampleLooptime = 0;
+        gyro.targetLooptime = 0;
+    }
+}
+
 void gyroInitFilters(void)
 {
     uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
@@ -757,13 +773,15 @@ void gyroInitFilters(void)
     gyroInitLowpassFilterLpf(
       FILTER_LOWPASS,
       gyroConfig()->gyro_lowpass_type,
-      gyro_lowpass_hz
+      gyro_lowpass_hz,
+      gyro.targetLooptime
     );
 
-    gyroInitLowpassFilterLpf(
+    gyro.downsampleFilterEnabled = gyroInitLowpassFilterLpf(
       FILTER_LOWPASS2,
       gyroConfig()->gyro_lowpass2_type,
-      gyroConfig()->gyro_lowpass2_hz
+      gyroConfig()->gyro_lowpass2_hz,
+      gyro.sampleLooptime
     );
 
     gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
@@ -809,7 +827,7 @@ static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibrati
 
 static int32_t gyroCalculateCalibratingCycles(void)
 {
-    return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.targetLooptime;
+    return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.sampleLooptime;
 }
 
 static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
@@ -1046,21 +1064,8 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
     }
 }
 
-#define GYRO_FILTER_FUNCTION_NAME filterGyro
-#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
-#include "gyro_filter_impl.c"
-#undef GYRO_FILTER_FUNCTION_NAME
-#undef GYRO_FILTER_DEBUG_SET
-
-#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
-#define GYRO_FILTER_DEBUG_SET DEBUG_SET
-#include "gyro_filter_impl.c"
-#undef GYRO_FILTER_FUNCTION_NAME
-#undef GYRO_FILTER_DEBUG_SET
-
-FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
+FAST_CODE void gyroUpdate(void)
 {
-
     switch (gyroToUse) {
     case GYRO_CONFIG_USE_GYRO_1:
         gyroUpdateSensor(&gyroSensor1);
@@ -1091,6 +1096,38 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
 #endif
     }
 
+    if (gyro.downsampleFilterEnabled) {
+        // using gyro lowpass 2 filter for downsampling
+        gyro.sampleSum[X] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[X], gyro.gyroADC[X]);
+        gyro.sampleSum[Y] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[Y], gyro.gyroADC[Y]);
+        gyro.sampleSum[Z] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[Z], gyro.gyroADC[Z]);
+    } else {
+        // using simple averaging for downsampling
+        gyro.sampleSum[X] += gyro.gyroADC[X];
+        gyro.sampleSum[Y] += gyro.gyroADC[Y];
+        gyro.sampleSum[Z] += gyro.gyroADC[Z];
+        gyro.sampleCount++;
+    }
+}
+
+#define GYRO_FILTER_FUNCTION_NAME filterGyro
+#define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
+#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) do { UNUSED(axis); UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
+#include "gyro_filter_impl.c"
+#undef GYRO_FILTER_FUNCTION_NAME
+#undef GYRO_FILTER_DEBUG_SET
+#undef GYRO_FILTER_AXIS_DEBUG_SET
+
+#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
+#define GYRO_FILTER_DEBUG_SET DEBUG_SET
+#define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) if (axis == (int)gyroDebugAxis) DEBUG_SET(mode, index, value)
+#include "gyro_filter_impl.c"
+#undef GYRO_FILTER_FUNCTION_NAME
+#undef GYRO_FILTER_DEBUG_SET
+#undef GYRO_FILTER_AXIS_DEBUG_SET
+
+FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
+{
     if (gyroDebugMode == DEBUG_NONE) {
         filterGyro();
     } else {
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 081c6538d..dece4639d 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -32,6 +32,8 @@
 #include "flight/gyroanalyse.h"
 #endif
 
+#include "flight/pid.h"
+
 #include "pg/pg.h"
 
 #define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
@@ -42,10 +44,15 @@ typedef union gyroLowpassFilter_u {
 } gyroLowpassFilter_t;
 
 typedef struct gyro_s {
+    uint16_t sampleRateHz;
     uint32_t targetLooptime;
+    uint32_t sampleLooptime;
     float scale;
     float gyroADC[XYZ_AXIS_COUNT];     // aligned, calibrated, scaled, but unfiltered data from the sensor(s)
     float gyroADCf[XYZ_AXIS_COUNT];    // filtered gyro data
+    uint8_t sampleCount;               // gyro sensor sample counter
+    float sampleSum[XYZ_AXIS_COUNT];   // summed samples used for downsampling
+    bool downsampleFilterEnabled;      // if true then downsample using gyro lowpass 2, otherwise use averaging
 
     gyroDev_t *rawSensorDev;           // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW
 
@@ -72,9 +79,12 @@ typedef struct gyro_s {
 #ifdef USE_GYRO_DATA_ANALYSE
     gyroAnalyseState_t gyroAnalyseState;
 #endif
+
+    uint16_t accSampleRateHz;
 } gyro_t;
 
 extern gyro_t gyro;
+extern uint8_t activePidLoopDenom;
 
 enum {
     GYRO_OVERFLOW_CHECK_NONE = 0,
@@ -109,7 +119,6 @@ typedef enum gyroDetectionFlags_e {
 
 typedef struct gyroConfig_s {
     uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
-    uint8_t  gyro_sync_denom;                  // Gyro sample divider
     uint8_t  gyro_hardware_lpf;                // gyro DLPF setting
 
     uint8_t  gyro_high_fsr;
@@ -149,9 +158,10 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
 
 void gyroPreInit(void);
 bool gyroInit(void);
-
+void gyroSetTargetLooptime(uint8_t pidDenom);
 void gyroInitFilters(void);
-void gyroUpdate(timeUs_t currentTimeUs);
+void gyroUpdate(void);
+void gyroFiltering(timeUs_t currentTimeUs);
 bool gyroGetAccumulationAverage(float *accumulation);
 const busDevice_t *gyroSensorBus(void);
 struct mpuDetectionResult_s;
diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index 8f7ab80f6..e26209ee9 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -23,11 +23,31 @@
 static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
 {
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        // DEBUG_GYRO_RAW records the raw value read from the sensor (not zero offset, not scaled)
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyro.rawSensorDev->gyroADCRaw[axis]);
-        // scale gyro output to degrees per second
-        float gyroADCf = gyro.gyroADC[axis];
+
         // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output
-        GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
+        // If downsampling than the last value in the sample group will be output
+        GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyro.gyroADC[axis]));
+
+        // DEBUG_GYRO_SAMPLE(0) Record the pre-downsample value for the selected debug axis (same as DEBUG_GYRO_SCALED)
+        GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 0, lrintf(gyro.gyroADC[axis]));
+
+        // downsample the individual gyro samples
+        float gyroADCf = 0;
+        if (gyro.downsampleFilterEnabled) {
+            // using gyro lowpass 2 filter for downsampling
+            gyroADCf = gyro.sampleSum[axis];
+        } else {
+            // using simple average for downsampling
+            if (gyro.sampleCount) {
+                gyroADCf = gyro.sampleSum[axis] / gyro.sampleCount;
+            }
+            gyro.sampleSum[axis] = 0;
+        }
+
+        // DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
+        GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
 
 #ifdef USE_GYRO_DATA_ANALYSE
         if (isDynamicFilterActive()) {
@@ -43,12 +63,16 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         gyroADCf = rpmFilterGyro(axis, gyroADCf);
 #endif
 
+        // DEBUG_GYRO_SAMPLE(2) Record the post-RPM Filter value for the selected debug axis
+        GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 2, lrintf(gyroADCf));
 
         // apply static notch filters and software lowpass filters
         gyroADCf = gyro.notchFilter1ApplyFn((filter_t *)&gyro.notchFilter1[axis], gyroADCf);
         gyroADCf = gyro.notchFilter2ApplyFn((filter_t *)&gyro.notchFilter2[axis], gyroADCf);
         gyroADCf = gyro.lowpassFilterApplyFn((filter_t *)&gyro.lowpassFilter[axis], gyroADCf);
-        gyroADCf = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[axis], gyroADCf);
+
+        // DEBUG_GYRO_SAMPLE(3) Record the post-static notch and lowpass filter value for the selected debug axis
+        GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 3, lrintf(gyroADCf));
 
 #ifdef USE_GYRO_DATA_ANALYSE
         if (isDynamicFilterActive()) {
@@ -68,4 +92,5 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
 
         gyro.gyroADCf[axis] = gyroADCf;
     }
+    gyro.sampleCount = 0;
 }
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 3ace81ec1..57c81e2f0 100644
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -33,6 +33,8 @@
 #include "config/config.h"
 #include "fc/runtime_config.h"
 
+#include "flight/pid.h"
+
 #include "sensors/sensors.h"
 #include "sensors/adcinternal.h"
 #include "sensors/acceleration.h"
@@ -68,7 +70,7 @@ bool sensorsAutodetect(void)
 
 #ifdef USE_ACC
     if (gyroDetected) {
-        accInit(gyro.targetLooptime);
+        accInit(gyro.accSampleRateHz);
     }
 #endif
 
diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c
index 1a949c085..8840b128e 100644
--- a/src/main/target/ALIENFLIGHTF3/config.c
+++ b/src/main/target/ALIENFLIGHTF3/config.c
@@ -90,8 +90,7 @@ void targetConfiguration(void)
         statusLedConfigMutable()->ioTags[2] = IO_TAG(LED2_A);
 #endif
     } else {
-        gyroConfigMutable()->gyro_sync_denom = 2;
-        pidConfigMutable()->pid_process_denom = 2;
+        pidConfigMutable()->pid_process_denom = 4;
     }
 
     if (!haveFrSkyRX) {
diff --git a/src/main/target/CLRACINGF4/config.c b/src/main/target/CLRACINGF4/config.c
index 229195579..4394760d0 100644
--- a/src/main/target/CLRACINGF4/config.c
+++ b/src/main/target/CLRACINGF4/config.c
@@ -48,7 +48,6 @@ void targetConfiguration(void)
 {
     pinioBoxConfigMutable()->permanentId[0] = 40;
     motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT600;
-    gyroConfigMutable()->gyro_sync_denom = 1;  // 8kHz gyro
     pidConfigMutable()->pid_process_denom = 1; // 8kHz PID
 }
 #endif
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index 58db831c5..325701739 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -279,7 +279,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             }
             break;
         case BST_STATUS:
-            bstWrite16(getTaskDeltaTime(TASK_GYROPID));
+            bstWrite16(getTaskDeltaTime(TASK_PID));
 #ifdef USE_I2C
             bstWrite16(i2cGetErrorCounter());
 #else
@@ -314,7 +314,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             bstWrite8(getCurrentPidProfileIndex());
             break;
         case BST_LOOP_TIME:
-            bstWrite16(getTaskDeltaTime(TASK_GYROPID));
+            bstWrite16(getTaskDeltaTime(TASK_PID));
             break;
         case BST_RC_TUNING:
             bstWrite8(currentControlRateProfile->rcRates[FD_ROLL]);
@@ -420,14 +420,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
             bstWrite8(rcControlsConfig()->yaw_deadband);
             break;
         case BST_FC_FILTERS:
-            switch (gyroConfig()->gyro_hardware_lpf) { // Extra safety to prevent OSD setting corrupt values
-                case GYRO_HARDWARE_LPF_1KHZ_SAMPLE:
-                    bstWrite16(1);
-                    break;
-                default:
-                    bstWrite16(0);
-                    break;
-            }
+            bstWrite16(0);
             break;
         default:
             // we do not know how to handle the (valid) message, indicate error BST
@@ -628,14 +621,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             rcControlsConfigMutable()->yaw_deadband = bstRead8();
             break;
         case BST_SET_FC_FILTERS:
-            switch (bstRead16()) {
-                case 1:
-                    gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
-                    break;
-                default:
-                    gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
-                    break;
-            }
+            bstRead16(); // 1KHz sampling no longer supported
             break;
 
         default:
diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c
index f0e53b17a..fd0f36acf 100644
--- a/src/main/target/MOTOLAB/config.c
+++ b/src/main/target/MOTOLAB/config.c
@@ -34,7 +34,6 @@
 // Motolab target supports 2 different type of boards Tornado / Cyclone.
 void targetConfiguration(void)
 {
-    gyroConfigMutable()->gyro_sync_denom = 4;
-    pidConfigMutable()->pid_process_denom = 1;
+    pidConfigMutable()->pid_process_denom = 4;
 }
 #endif
diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c
index d2230b8ff..320275e3c 100644
--- a/src/main/target/MULTIFLITEPICO/config.c
+++ b/src/main/target/MULTIFLITEPICO/config.c
@@ -81,8 +81,7 @@ void targetConfiguration(void)
 
     motorConfigMutable()->dev.motorPwmRate = 17000;
 
-    gyroConfigMutable()->gyro_sync_denom = 4;
-    pidConfigMutable()->pid_process_denom = 1;
+    pidConfigMutable()->pid_process_denom = 4;
 
     for (uint8_t pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {
         pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c
index 80edf5418..5c668475c 100644
--- a/src/main/target/NAZE/config.c
+++ b/src/main/target/NAZE/config.c
@@ -63,7 +63,6 @@ void targetConfiguration(void)
 
     gyroDeviceConfigMutable()->extiTag = selectMPUIntExtiConfigByHardwareRevision();
 
-    gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
     gyroConfigMutable()->gyro_soft_lpf_hz = 100;
     gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
     gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
diff --git a/src/main/target/OMNIBUSF4/config.c b/src/main/target/OMNIBUSF4/config.c
index 047615322..f09d280bf 100644
--- a/src/main/target/OMNIBUSF4/config.c
+++ b/src/main/target/OMNIBUSF4/config.c
@@ -75,7 +75,6 @@ void targetConfiguration(void)
     ledStripStatusModeConfigMutable()->ledConfigs[0] = DEFINE_LED(0, 0, 0, 0, LF(COLOR), LO(VTX), 0);
     targetSerialPortFunctionConfig(targetSerialPortFunction, ARRAYLEN(targetSerialPortFunction));
     motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT600;
-    gyroConfigMutable()->gyro_sync_denom = 1;  // 8kHz gyro
     pidConfigMutable()->pid_process_denom = 1; // 8kHz PID
 #endif
 }
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index b7f7b729c..08548f33e 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -137,6 +137,14 @@
 #define DEFAULT_AUX_CHANNEL_COUNT       6
 #endif
 
+// Set the default cpu_overclock to the first level (108MHz) for F411
+// Helps with looptime stability as the CPU is borderline when running native gyro sampling
+#if defined(USE_OVERCLOCK) && defined(STM32F411xE)
+#define DEFAULT_CPU_OVERCLOCK 1
+#else
+#define DEFAULT_CPU_OVERCLOCK 0
+#endif
+
 
 #ifdef USE_ITCM_RAM
 #define FAST_CODE                   __attribute__((section(".tcm_code")))
diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc
index 832adaff9..a4ff1daca 100644
--- a/src/test/unit/arming_prevention_unittest.cc
+++ b/src/test/unit/arming_prevention_unittest.cc
@@ -74,6 +74,7 @@ extern "C" {
     int16_t GPS_directionToHome = 0;
     acc_t acc = {};
     bool mockIsUpright = false;
+    uint8_t activePidLoopDenom = 1;
 }
 
 uint32_t simulationFeatureFlags = 0;
@@ -1049,7 +1050,7 @@ extern "C" {
     void writeServos(void) {};
     bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
     bool isMixerUsingServos(void) { return false; }
-    void gyroUpdate(timeUs_t) {}
+    void gyroUpdate(void) {}
     timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
     void updateRSSI(timeUs_t) {}
     bool failsafeIsMonitoring(void) { return false; }
@@ -1099,4 +1100,5 @@ extern "C" {
     bool compassIsCalibrationComplete(void) { return true; }
     bool isUpright(void) { return mockIsUpright; }
     void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
+    void gyroFiltering(timeUs_t) {};
 }
diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc
index 1cc19e64a..cb16fc24b 100644
--- a/src/test/unit/blackbox_unittest.cc
+++ b/src/test/unit/blackbox_unittest.cc
@@ -75,14 +75,6 @@ TEST(BlackboxTest, TestInitIntervals)
     EXPECT_EQ(0, blackboxPInterval);
     EXPECT_EQ(4096, blackboxSInterval);
 
-    // 1kHz PIDloop
-    gyro.targetLooptime = gyroSetSampleRate(&gyroDev, GYRO_HARDWARE_LPF_1KHZ_SAMPLE, 1);
-    targetPidLooptime = gyro.targetLooptime * 1;
-    blackboxInit();
-    EXPECT_EQ(32, blackboxIInterval);
-    EXPECT_EQ(1, blackboxPInterval);
-    EXPECT_EQ(8192, blackboxSInterval);
-
     // 2kHz PIDloop
     targetPidLooptime = 500;
     blackboxInit();
diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
index fd39b1fdf..9fde0016c 100644
--- a/src/test/unit/pid_unittest.cc
+++ b/src/test/unit/pid_unittest.cc
@@ -144,7 +144,7 @@ void setDefaultTestSettings(void) {
     pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
     pidProfile->launchControlGain = 40,
 
-    gyro.targetLooptime = 4000;
+    gyro.targetLooptime = 8000;
 }
 
 timeUs_t currentTestTime(void) {
diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc
index 1515f18c4..823582807 100644
--- a/src/test/unit/scheduler_unittest.cc
+++ b/src/test/unit/scheduler_unittest.cc
@@ -25,8 +25,12 @@ extern "C" {
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
 
-const int TEST_PID_LOOP_TIME = 650;
-const int TEST_UPDATE_ACCEL_TIME = 192;
+const int TEST_GYRO_SAMPLE_HZ = 8000;
+const int TEST_GYRO_SAMPLE_TIME = 10;
+const int TEST_FILTERING_TIME = 40;
+const int TEST_PID_LOOP_TIME = 58;
+const int TEST_UPDATE_ACCEL_TIME = 32;
+const int TEST_UPDATE_ATTITUDE_TIME = 28;
 const int TEST_HANDLE_SERIAL_TIME = 30;
 const int TEST_UPDATE_BATTERY_TIME = 1;
 const int TEST_UPDATE_RX_CHECK_TIME = 34;
@@ -41,13 +45,23 @@ extern "C" {
     cfTask_t * unittest_scheduler_selectedTask;
     uint8_t unittest_scheduler_selectedTaskDynPrio;
     uint16_t unittest_scheduler_waitingTasks;
+    timeDelta_t unittest_scheduler_taskRequiredTimeUs;
+    bool taskGyroRan = false;
+    bool taskFilterRan = false;
+    bool taskPidRan = false;
+    bool taskFilterReady = false;
+    bool taskPidReady = false;
 
     // set up micros() to simulate time
     uint32_t simulatedTime = 0;
     uint32_t micros(void) { return simulatedTime; }
 
     // set up tasks to take a simulated representative time to execute
-    void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; }
+    bool gyroFilterReady(void) { return taskFilterReady; }
+    bool pidLoopReady(void) { return taskPidReady; }
+    void taskGyroSample(timeUs_t) { simulatedTime += TEST_GYRO_SAMPLE_TIME; taskGyroRan = true; }
+    void taskFiltering(timeUs_t) { simulatedTime += TEST_FILTERING_TIME; taskFilterRan = true; }
+    void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; taskPidRan = true; }
     void taskUpdateAccelerometer(timeUs_t) { simulatedTime += TEST_UPDATE_ACCEL_TIME; }
     void taskHandleSerial(timeUs_t) { simulatedTime += TEST_HANDLE_SERIAL_TIME; }
     void taskUpdateBatteryVoltage(timeUs_t) { simulatedTime += TEST_UPDATE_BATTERY_TIME; }
@@ -56,6 +70,14 @@ extern "C" {
     void imuUpdateAttitude(timeUs_t) { simulatedTime += TEST_IMU_UPDATE_TIME; }
     void dispatchProcess(timeUs_t) { simulatedTime += TEST_DISPATCH_TIME; }
 
+    void resetGyroTaskTestFlags(void) {
+        taskGyroRan = false;
+        taskFilterRan = false;
+        taskPidRan = false;
+        taskFilterReady = false;
+        taskPidReady = false;
+    }
+
     extern int taskQueueSize;
     extern cfTask_t* taskQueueArray[];
 
@@ -73,17 +95,28 @@ extern "C" {
             .desiredPeriod = TASK_PERIOD_HZ(10),
             .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,
         },
-        [TASK_GYROPID] = {
+        [TASK_GYRO] = {
+            .taskName = "GYRO",
+            .taskFunc = taskGyroSample,
+            .desiredPeriod = TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ),
+            .staticPriority = TASK_PRIORITY_REALTIME,
+        },
+        [TASK_FILTER] = {
+            .taskName = "FILTER",
+            .taskFunc = taskFiltering,
+            .desiredPeriod = TASK_PERIOD_HZ(4000),
+            .staticPriority = TASK_PRIORITY_REALTIME,
+        },
+        [TASK_PID] = {
             .taskName = "PID",
-            .subTaskName = "GYRO",
             .taskFunc = taskMainPidLoop,
-            .desiredPeriod = 1000,
+            .desiredPeriod = TASK_PERIOD_HZ(4000),
             .staticPriority = TASK_PRIORITY_REALTIME,
         },
         [TASK_ACCEL] = {
             .taskName = "ACCEL",
             .taskFunc = taskUpdateAccelerometer,
-            .desiredPeriod = 10000,
+            .desiredPeriod = TASK_PERIOD_HZ(1000),
             .staticPriority = TASK_PRIORITY_MEDIUM,
         },
         [TASK_ATTITUDE] = {
@@ -123,7 +156,7 @@ extern "C" {
 TEST(SchedulerUnittest, TestPriorites)
 {
     EXPECT_EQ(TASK_PRIORITY_MEDIUM_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
-    EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority);
+    EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYRO].staticPriority);
     EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority);
     EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority);
     EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY_VOLTAGE].staticPriority);
@@ -152,34 +185,24 @@ TEST(SchedulerUnittest, TestQueue)
     EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 
-    queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME
-    EXPECT_EQ(2, taskQueueSize);
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
-    EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
-    EXPECT_EQ(NULL, queueNext());
-    EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
-
     queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW
-    EXPECT_EQ(3, taskQueueSize);
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
-    EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
+    EXPECT_EQ(2, taskQueueSize);
+    EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
     EXPECT_EQ(NULL, queueNext());
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 
     queueAdd(&cfTasks[TASK_BATTERY_VOLTAGE]); // TASK_PRIORITY_MEDIUM
-    EXPECT_EQ(4, taskQueueSize);
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
-    EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
+    EXPECT_EQ(3, taskQueueSize);
+    EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
     EXPECT_EQ(NULL, queueNext());
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 
     queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH
-    EXPECT_EQ(5, taskQueueSize);
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
-    EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
+    EXPECT_EQ(4, taskQueueSize);
+    EXPECT_EQ(&cfTasks[TASK_RX], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
     EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
@@ -187,9 +210,8 @@ TEST(SchedulerUnittest, TestQueue)
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 
     queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH
-    EXPECT_EQ(4, taskQueueSize);
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
-    EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
+    EXPECT_EQ(3, taskQueueSize);
+    EXPECT_EQ(&cfTasks[TASK_RX], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_BATTERY_VOLTAGE], queueNext());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
     EXPECT_EQ(NULL, queueNext());
@@ -320,47 +342,47 @@ TEST(SchedulerUnittest, TestScheduleEmptyQueue)
 TEST(SchedulerUnittest, TestSingleTask)
 {
     schedulerInit();
-    // disable all tasks except TASK_GYROPID
-    for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
-        setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
-    }
-    setTaskEnabled(TASK_GYROPID, true);
-    cfTasks[TASK_GYROPID].lastExecutedAt = 1000;
-    simulatedTime = 4000;
-    // run the scheduler and check the task has executed
-    scheduler();
-    EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
-    EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime);
-    EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt);
-    EXPECT_EQ(TEST_PID_LOOP_TIME, cfTasks[TASK_GYROPID].totalExecutionTime);
-    // task has run, so its dynamic priority should have been set to zero
-    EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority);
-}
-
-TEST(SchedulerUnittest, TestTwoTasks)
-{
-    // disable all tasks except TASK_GYROPID  and TASK_ACCEL
+    // disable all tasks except TASK_ACCEL
     for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
         setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
     }
     setTaskEnabled(TASK_ACCEL, true);
-    setTaskEnabled(TASK_GYROPID, true);
+    cfTasks[TASK_ACCEL].lastExecutedAt = 1000;
+    simulatedTime = 2050;
+    // run the scheduler and check the task has executed
+    scheduler();
+    EXPECT_NE(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+    EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
+    EXPECT_EQ(1050, cfTasks[TASK_ACCEL].taskLatestDeltaTime);
+    EXPECT_EQ(2050, cfTasks[TASK_ACCEL].lastExecutedAt);
+    EXPECT_EQ(TEST_UPDATE_ACCEL_TIME, cfTasks[TASK_ACCEL].totalExecutionTime);
+    // task has run, so its dynamic priority should have been set to zero
+    EXPECT_EQ(0, cfTasks[TASK_GYRO].dynamicPriority);
+}
 
-    // set it up so that TASK_ACCEL ran just before TASK_GYROPID
+TEST(SchedulerUnittest, TestTwoTasks)
+{
+    // disable all tasks except TASK_ACCEL and TASK_ATTITUDE
+    for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
+        setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
+    }
+    setTaskEnabled(TASK_ACCEL, true);
+    setTaskEnabled(TASK_ATTITUDE, true);
+
+    // set it up so that TASK_ACCEL ran just before TASK_ATTITUDE
     static const uint32_t startTime = 4000;
     simulatedTime = startTime;
-    cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime;
-    cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - TEST_UPDATE_ACCEL_TIME;
-    EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles);
+    cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime;
+    cfTasks[TASK_ATTITUDE].lastExecutedAt = cfTasks[TASK_ACCEL].lastExecutedAt - TEST_UPDATE_ATTITUDE_TIME;
+    EXPECT_EQ(0, cfTasks[TASK_ATTITUDE].taskAgeCycles);
     // run the scheduler
     scheduler();
     // no tasks should have run, since neither task's desired time has elapsed
     EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
 
     // NOTE:
-    // TASK_GYROPID desiredPeriod is  1000 microseconds
-    // TASK_ACCEL   desiredPeriod is 10000 microseconds
+    // TASK_ACCEL    desiredPeriod is 1000 microseconds
+    // TASK_ATTITUDE desiredPeriod is 10000 microseconds
     // 500 microseconds later
     simulatedTime += 500;
     // no tasks should run, since neither task's desired time has elapsed
@@ -368,28 +390,213 @@ TEST(SchedulerUnittest, TestTwoTasks)
     EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
     EXPECT_EQ(0, unittest_scheduler_waitingTasks);
 
-    // 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed
+    // 500 microseconds later, TASK_ACCEL desiredPeriod has elapsed
     simulatedTime += 500;
-    // TASK_GYROPID should now run
+    // TASK_ACCEL should now run
     scheduler();
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
+    EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
     EXPECT_EQ(1, unittest_scheduler_waitingTasks);
-    EXPECT_EQ(5000 + TEST_PID_LOOP_TIME, simulatedTime);
+    EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 
-    simulatedTime += 1000 - TEST_PID_LOOP_TIME;
+    simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
     scheduler();
-    // TASK_GYROPID should run again
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
+    // TASK_ACCEL should run again
+    EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 
     scheduler();
+    // No task should have run
     EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
     EXPECT_EQ(0, unittest_scheduler_waitingTasks);
 
-    simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed
-    // of the two TASK_GYROPID should run first
-    scheduler();
-    EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask);
-    // and finally TASK_ACCEL should now run
+    simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriods have elapsed
+    // of the two TASK_ACCEL should run first
     scheduler();
     EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
+    // and finally TASK_ATTITUDE should now run
+    scheduler();
+    EXPECT_EQ(&cfTasks[TASK_ATTITUDE], unittest_scheduler_selectedTask);
+}
+
+TEST(SchedulerUnittest, TestGyroTask)
+{
+    static const uint32_t startTime = 4000;
+
+    // enable the gyro
+    schedulerEnableGyro();
+
+    // disable all tasks except TASK_GYRO, TASK_FILTER and TASK_PID
+    for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
+        setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
+    }
+    setTaskEnabled(TASK_GYRO, true);
+    setTaskEnabled(TASK_FILTER, true);
+    setTaskEnabled(TASK_PID, true);
+
+    // First set it up so TASK_GYRO just ran
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
+    // reset the flags
+    resetGyroTaskTestFlags();
+
+    // run the scheduler
+    scheduler();
+    // no tasks should have run
+    EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+    // also the gyro, filter and PID task indicators should be false
+    EXPECT_FALSE(taskGyroRan);
+    EXPECT_FALSE(taskFilterRan);
+    EXPECT_FALSE(taskPidRan);
+
+    /* Test the gyro task running but not triggering the filtering or PID */
+    // set the TASK_GYRO last executed time to be one period earlier
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
+
+    // reset the flags
+    resetGyroTaskTestFlags();
+
+    // run the scheduler
+    scheduler();
+    // the gyro task indicator should be true and the TASK_FILTER and TASK_PID indicators should be false
+    EXPECT_TRUE(taskGyroRan);
+    EXPECT_FALSE(taskFilterRan);
+    EXPECT_FALSE(taskPidRan);
+    // expect that no other tasks other than TASK_GYRO should have run
+    EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+
+    /* Test the gyro task running and triggering the filtering task */
+    // set the TASK_GYRO last executed time to be one period earlier
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
+
+    // reset the flags
+    resetGyroTaskTestFlags();
+    taskFilterReady = true;
+
+    // run the scheduler
+    scheduler();
+    // the gyro and filter task indicators should be true and TASK_PID indicator should be false
+    EXPECT_TRUE(taskGyroRan);
+    EXPECT_TRUE(taskFilterRan);
+    EXPECT_FALSE(taskPidRan);
+    // expect that no other tasks other tasks should have run
+    EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+
+    /* Test the gyro task running and triggering the PID task */
+    // set the TASK_GYRO last executed time to be one period earlier
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
+
+    // reset the flags
+    resetGyroTaskTestFlags();
+    taskPidReady = true;
+
+    // run the scheduler
+    scheduler();
+    // the gyro and PID task indicators should be true and TASK_FILTER indicator should be false
+    EXPECT_TRUE(taskGyroRan);
+    EXPECT_FALSE(taskFilterRan);
+    EXPECT_TRUE(taskPidRan);
+    // expect that no other tasks other tasks should have run
+    EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+}
+
+// Test the scheduling logic that prevents other tasks from running if they
+// might interfere with the timing of the next gyro task.
+TEST(SchedulerUnittest, TestGyroLookahead)
+{
+    static const uint32_t startTime = 4000;
+
+    // enable task statistics
+    schedulerSetCalulateTaskStatistics(true);
+
+    // disable scheduler optimize rate
+    schedulerOptimizeRate(false);
+
+    // enable the gyro
+    schedulerEnableGyro();
+
+    // disable all tasks except TASK_GYRO, TASK_ACCEL
+    for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
+        setTaskEnabled(static_cast<cfTaskId_e>(taskId), false);
+    }
+    setTaskEnabled(TASK_GYRO, true);
+    setTaskEnabled(TASK_ACCEL, true);
+
+#if defined(USE_TASK_STATISTICS)
+    // set the average run time for TASK_ACCEL
+    cfTasks[TASK_ACCEL].movingSumExecutionTime = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT;
+#endif
+
+    /* Test that another task will run if there's plenty of time till the next gyro sample time */
+    // set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime;
+    cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
+    // reset the flags
+    resetGyroTaskTestFlags();
+
+    // run the scheduler
+    scheduler();
+    // the gyro, filter and PID task indicators should be false
+    EXPECT_FALSE(taskGyroRan);
+    EXPECT_FALSE(taskFilterRan);
+    EXPECT_FALSE(taskPidRan);
+    // TASK_ACCEL should have run
+    EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
+
+    /* Test that another task won't run if the time till the gyro task is less than the guard interval */
+    // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
+    cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
+    // reset the flags
+    resetGyroTaskTestFlags();
+
+    // run the scheduler
+    scheduler();
+    // the gyro, filter and PID task indicators should be false
+    EXPECT_FALSE(taskGyroRan);
+    EXPECT_FALSE(taskFilterRan);
+    EXPECT_FALSE(taskPidRan);
+    // TASK_ACCEL should not have run
+    EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+
+    /* Test that another task won't run if the time till the gyro task is less than the average task interval */
+    // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
+    cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
+    // reset the flags
+    resetGyroTaskTestFlags();
+
+    // run the scheduler
+    scheduler();
+    // the gyro, filter and PID task indicators should be false
+    EXPECT_FALSE(taskGyroRan);
+    EXPECT_FALSE(taskFilterRan);
+    EXPECT_FALSE(taskPidRan);
+    // TASK_ACCEL should not have run
+    EXPECT_EQ(static_cast<cfTask_t*>(0), unittest_scheduler_selectedTask);
+
+    /* Test that another task will run if the gyro task gets executed */
+    // set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run
+    simulatedTime = startTime;
+    cfTasks[TASK_GYRO].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
+    cfTasks[TASK_ACCEL].lastExecutedAt = simulatedTime - TASK_PERIOD_HZ(1000);
+    // reset the flags
+    resetGyroTaskTestFlags();
+
+    // make the TASK_FILTER and TASK_PID ready to run
+    taskFilterReady = true;
+    taskPidReady = true;
+
+    // run the scheduler
+    scheduler();
+    // TASK_GYRO, TASK_FILTER, and TASK_PID should all run
+    EXPECT_TRUE(taskGyroRan);
+    EXPECT_TRUE(taskFilterRan);
+    EXPECT_TRUE(taskPidRan);
+    // TASK_ACCEL should have run
+    EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask);
 }
diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc
index 95286c716..d9e7c7c04 100644
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@@ -85,6 +85,7 @@ TEST(SensorGyro, Calibrate)
 {
     pgResetAll();
     gyroInit();
+    gyroSetTargetLooptime(1);
     fakeGyroSet(gyroDevPtr, 5, 6, 7);
     const bool read = gyroDevPtr->readFn(gyroDevPtr);
     EXPECT_TRUE(read);
@@ -119,16 +120,16 @@ TEST(SensorGyro, Update)
     gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
     gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
     gyroInit();
+    gyroSetTargetLooptime(1);
     gyroDevPtr->readFn = fakeGyroRead;
     gyroStartCalibration(false);
     EXPECT_FALSE(gyroIsCalibrationComplete());
 
-    timeUs_t currentTimeUs = 0;
     fakeGyroSet(gyroDevPtr, 5, 6, 7);
-    gyroUpdate(currentTimeUs);
+    gyroUpdate();
     while (!gyroIsCalibrationComplete()) {
         fakeGyroSet(gyroDevPtr, 5, 6, 7);
-        gyroUpdate(currentTimeUs);
+        gyroUpdate();
     }
     EXPECT_TRUE(gyroIsCalibrationComplete());
     EXPECT_EQ(5, gyroDevPtr->gyroZero[X]);
@@ -137,16 +138,16 @@ TEST(SensorGyro, Update)
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
-    gyroUpdate(currentTimeUs);
+    gyroUpdate();
     // expect zero values since gyro is calibrated
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
     fakeGyroSet(gyroDevPtr, 15, 26, 97);
-    gyroUpdate(currentTimeUs);
-    EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADCf[X], 1e-3); // gyroADCf values are scaled
-    EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADCf[Y], 1e-3);
-    EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADCf[Z], 1e-3);
+    gyroUpdate();
+    EXPECT_NEAR(10 * gyroDevPtr->scale, gyro.gyroADC[X], 1e-3); // gyro.gyroADC values are scaled
+    EXPECT_NEAR(20 * gyroDevPtr->scale, gyro.gyroADC[Y], 1e-3);
+    EXPECT_NEAR(90 * gyroDevPtr->scale, gyro.gyroADC[Z], 1e-3);
 }
 
 // STUBS
diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc
index 0e05316d0..d25df081e 100644
--- a/src/test/unit/vtx_unittest.cc
+++ b/src/test/unit/vtx_unittest.cc
@@ -113,6 +113,7 @@ TEST(VtxTest, PitMode)
 
 // STUBS
 extern "C" {
+    uint8_t activePidLoopDenom = 1;
     uint32_t micros(void) { return simulationTime; }
     uint32_t millis(void) { return micros() / 1000; }
     bool rxIsReceivingSignal(void) { return simulationHaveRx; }
@@ -141,7 +142,7 @@ extern "C" {
     void writeServos(void) {};
     bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
     bool isMixerUsingServos(void) { return false; }
-    void gyroUpdate(timeUs_t) {}
+    void gyroUpdate() {}
     timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
     void updateRSSI(timeUs_t) {}
     bool failsafeIsMonitoring(void) { return false; }
@@ -184,4 +185,5 @@ extern "C" {
     bool compassIsCalibrationComplete(void) { return true; }
     bool isUpright(void) { return true; }
     void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
+    void gyroFiltering(timeUs_t) {};
 }