Trigger gyro SPI DMA reads in EXTI handler if supported and lock gyroTask loop to gyro to eliminate missed updates and jitter

src/main/build/debug.c

@@ -99,4 +99,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "D_LPF",
     "VTX_TRAMP",
     "GHST",
+    "SCHEDULER_DETERMINISM"
 };

src/main/build/debug.h

@@ -115,6 +115,7 @@ typedef enum {
     DEBUG_D_LPF,
     DEBUG_VTX_TRAMP,
     DEBUG_GHST,
+    DEBUG_SCHEDULER_DETERMINISM,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/cli.c

@@ -4782,6 +4782,19 @@ static void cliStatus(const char *cmdName, char *cmdline)
             cliPrintf(" gyro %d", pos + 1);
         }
     }
+#ifdef USE_SPI
+#ifdef USE_GYRO_EXTI
+    if (gyroActiveDev()->gyroModeSPI != GYRO_EXTI_NO_INT) {
+        cliPrintf(" locked");
+    }
+    if (gyroActiveDev()->gyroModeSPI == GYRO_EXTI_INT_DMA) {
+        cliPrintf(" dma");
+    }
+#endif
+    if (spiGetExtDeviceCount(&gyroActiveDev()->dev) > 1) {
+        cliPrintf(" shared");
+    }
+#endif
     cliPrintLinefeed();
 
 #if defined(USE_SENSOR_NAMES)
@@ -4867,7 +4880,6 @@ static void cliStatus(const char *cmdName, char *cmdline)
     cliPrintLinefeed();
 }
 
-#if defined(USE_TASK_STATISTICS)
 static void cliTasks(const char *cmdName, char *cmdline)
 {
     UNUSED(cmdName);
@@ -4923,10 +4935,14 @@ static void cliTasks(const char *cmdName, char *cmdline)
         getCheckFuncInfo(&checkFuncInfo);
         cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTimeUs, checkFuncInfo.averageExecutionTimeUs, checkFuncInfo.totalExecutionTimeUs / 1000);
         cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
+        if (debugMode == DEBUG_SCHEDULER_DETERMINISM) {
+            extern int32_t schedLoopStartCycles, taskGuardCycles;
+
+            cliPrintLinef("Scheduler start cycles %d guard cycles %d", schedLoopStartCycles, taskGuardCycles);
+        }
         schedulerResetCheckFunctionMaxExecutionTime();
     }
 }
-#endif
 
 static void printVersion(const char *cmdName, bool printBoardInfo)
 {
@@ -6584,9 +6600,7 @@ const clicmd_t cmdTable[] = {
         "\treverse <servo> <source> r|n", cliServoMix),
 #endif
     CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
-#if defined(USE_TASK_STATISTICS)
     CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
-#endif
 #ifdef USE_TIMER_MGMT
     CLI_COMMAND_DEF("timer", "show/set timers", "<> | <pin> list | <pin> [af<alternate function>|none|<option(deprecated)>] | list | show", cliTimer),
 #endif
@@ -6832,8 +6846,6 @@ void cliEnter(serialPort_t *serialPort)
     cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
     cliErrorWriter = cliWriter;
 
-    schedulerSetCalulateTaskStatistics(systemConfig()->task_statistics);
-
 #ifndef MINIMAL_CLI
     cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'");
 #else

src/main/cli/settings.c

@@ -694,7 +694,7 @@ const clivalue_t valueTable[] = {
     { "mag_align_roll",             VAR_INT16  | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.roll) },
     { "mag_align_pitch",            VAR_INT16  | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.pitch) },
     { "mag_align_yaw",              VAR_INT16  | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.yaw) },
-    { "mag_bustype",                VAR_UINT8  | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_bustype) },
+    { "mag_bustype",                VAR_UINT8  | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_busType) },
     { "mag_i2c_device",             VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_device) },
     { "mag_i2c_address",            VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_address) },
     { "mag_spi_device",             VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_device) },
@@ -1469,9 +1469,7 @@ const clivalue_t valueTable[] = {
 #if defined(STM32F4) || defined(STM32G4)
     { "system_hse_mhz",             VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, hseMhz) },
 #endif
-#if defined(USE_TASK_STATISTICS)
     { "task_statistics",            VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) },
-#endif
     { "debug_mode",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) },
     { "rate_6pos_switch",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, rateProfile6PosSwitch) },
 #ifdef USE_OVERCLOCK

src/main/common/time.h

@@ -44,6 +44,7 @@ typedef uint32_t timeUs_t;
 #define TIMEZONE_OFFSET_MINUTES_MAX 780   // +13 hours
 
 static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
+static inline int32_t cmpTimeCycles(uint32_t a, uint32_t b) { return (int32_t)(a - b); }
 
 #define FORMATTED_DATE_TIME_BUFSIZE 30
 

src/main/drivers/accgyro/accgyro.h

@@ -25,6 +25,7 @@
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/sensor_alignment.h"
+#include "common/time.h"
 #include "drivers/exti.h"
 #include "drivers/bus.h"
 #include "drivers/sensor.h"
@@ -79,6 +80,13 @@ typedef enum {
     GYRO_RATE_32_kHz,
 } gyroRateKHz_e;
 
+typedef enum {
+    GYRO_EXTI_INIT = 0,
+    GYRO_EXTI_INT_DMA,
+    GYRO_EXTI_INT,
+    GYRO_EXTI_NO_INT
+} gyroModeSPI_e;
+
 typedef struct gyroDev_s {
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
     pthread_mutex_t lock;
@@ -97,7 +105,15 @@ typedef struct gyroDev_s {
     mpuDetectionResult_t mpuDetectionResult;
     sensor_align_e gyroAlign;
     gyroRateKHz_e gyroRateKHz;
-    bool dataReady;
+    gyroModeSPI_e gyroModeSPI;
+#ifdef USE_GYRO_EXTI
+    uint32_t detectedEXTI;
+    uint32_t gyroLastEXTI;
+    uint32_t gyroSyncEXTI;
+    int32_t gyroShortPeriod;
+    int32_t gyroDmaMaxDuration;
+#endif
+    volatile bool dataReady;
     bool gyro_high_fsr;
     uint8_t hardware_lpf;
     uint8_t hardware_32khz_lpf;

src/main/drivers/accgyro/accgyro_mpu.c

@@ -67,6 +67,9 @@
 
 #define MPU_INQUIRY_MASK   0x7E
 
+// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
+#define GYRO_EXTI_DETECT_THRESHOLD 1000
+
 #ifdef USE_I2C_GYRO
 static void mpu6050FindRevision(gyroDev_t *gyro)
 {
@@ -107,21 +110,60 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
  * Gyro interrupt service routine
  */
 #ifdef USE_GYRO_EXTI
+#ifdef USE_SPI_GYRO
+// Called in ISR context
+// Gyro read has just completed
+busStatus_e mpuIntcallback(uint32_t arg)
+{
+    volatile gyroDev_t *gyro = (gyroDev_t *)arg;
+    int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
+
+    if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
+        gyro->gyroDmaMaxDuration = gyroDmaDuration;
+    }
+
+    gyro->dataReady = true;
+
+    return BUS_READY;
+}
+
+static void mpuIntExtiHandler(extiCallbackRec_t *cb)
+{
+    // Non-blocking, so this needs to be static
+    static busSegment_t segments[] = {
+            {NULL, NULL, 15, true, mpuIntcallback},
+            {NULL, NULL, 0, true, NULL},
+    };
+    gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
+
+    // Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does
+    // not have an associated timer
+    uint32_t nowCycles = getCycleCounter();
+    int32_t gyroLastPeriod = cmpTimeCycles(nowCycles, gyro->gyroLastEXTI);
+    // This detects the short (~79us) EXTI interval of an MPU6xxx gyro
+    if ((gyro->gyroShortPeriod == 0) || (gyroLastPeriod < gyro->gyroShortPeriod)) {
+        gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
+    }
+    gyro->gyroLastEXTI = nowCycles;
+
+    if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
+        segments[0].txData = gyro->dev.txBuf;;
+        segments[0].rxData = &gyro->dev.rxBuf[1];
+
+        if (!spiIsBusy(&gyro->dev)) {
+            spiSequence(&gyro->dev, &segments[0]);
+        }
+    }
+
+    gyro->detectedEXTI++;
+}
+#else
 static void mpuIntExtiHandler(extiCallbackRec_t *cb)
 {
-#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
-    static uint32_t lastCalledAtUs = 0;
-    const uint32_t nowUs = micros();
-    debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
-    lastCalledAtUs = nowUs;
-#endif
     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
     gyro->dataReady = true;
-#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
-    const uint32_t now2Us = micros();
-    debug[1] = (uint16_t)(now2Us - nowUs);
-#endif
 }
+#endif
 
 static void mpuIntExtiInit(gyroDev_t *gyro)
 {
@@ -177,37 +219,128 @@ bool mpuGyroRead(gyroDev_t *gyro)
     return true;
 }
 
+
 #ifdef USE_SPI_GYRO
 bool mpuAccReadSPI(accDev_t *acc)
 {
-    STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_ACCEL_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    switch (acc->gyro->gyroModeSPI) {
-    STATIC_DMA_DATA_AUTO uint8_t data[7];
+    case GYRO_EXTI_INT:
+    case GYRO_EXTI_NO_INT:
+    {
+        // Ensure any prior DMA has completed before continuing
+        spiWaitClaim(&acc->gyro->dev);
 
-    const bool ack = spiReadWriteBufRB(&acc->gyro->dev, dataToSend, data, 7);
+        acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
-    if (!ack) {
+
-        return false;
+        busSegment_t segments[] = {
+                {NULL, NULL, 7, true, NULL},
+                {NULL, NULL, 0, true, NULL},
+        };
+        segments[0].txData = acc->gyro->dev.txBuf;
+        segments[0].rxData = &acc->gyro->dev.rxBuf[1];
+
+        spiSequence(&acc->gyro->dev, &segments[0]);
+
+        // Wait for completion
+        spiWait(&acc->gyro->dev);
+
+        // Fall through
+        FALLTHROUGH;
     }
 
-    acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
+    case GYRO_EXTI_INT_DMA:
-    acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
+    {
-    acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
+        // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
+        // up an old value.
+
+        // This data was read from the gyro, which is the same SPI device as the acc
+        uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
+        acc->ADCRaw[X] = __builtin_bswap16(accData[1]);
+        acc->ADCRaw[Y] = __builtin_bswap16(accData[2]);
+        acc->ADCRaw[Z] = __builtin_bswap16(accData[3]);
+        break;
+    }
+
+    case GYRO_EXTI_INIT:
+    default:
+        break;
+    }
 
     return true;
 }
 
 bool mpuGyroReadSPI(gyroDev_t *gyro)
 {
-    STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
-    STATIC_DMA_DATA_AUTO uint8_t data[7];
+    switch (gyro->gyroModeSPI) {
+    case GYRO_EXTI_INIT:
+    {
+        // Initialise the tx buffer to all 0xff
+        memset(gyro->dev.txBuf, 0xff, 16);
+#ifdef USE_GYRO_EXTI
+        // Check that minimum number of interrupts have been detected
 
-    const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
+        // We need some offset from the gyro interrupts to ensure sampling after the interrupt
-    if (!ack) {
+        gyro->gyroDmaMaxDuration = 5;
-        return false;
+        // Using DMA for gyro access upsets the scheduler on the F4
+        if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
+            if (spiUseDMA(&gyro->dev)) {
+                // Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
+                spiSetAtomicWait(&gyro->dev);
+                gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
+                gyro->dev.callbackArg = (uint32_t)gyro;
+                gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
+            } else {
+                // Interrupts are present, but no DMA
+                gyro->gyroModeSPI = GYRO_EXTI_INT;
+            }
+        } else
+#endif
+        {
+            gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
+        }
+        break;
     }
 
-    gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
+    case GYRO_EXTI_INT:
-    gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
+    case GYRO_EXTI_NO_INT:
-    gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
+    {
+        // Ensure any prior DMA has completed before continuing
+        spiWaitClaim(&gyro->dev);
+
+        gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80;
+
+        busSegment_t segments[] = {
+                {NULL, NULL, 7, true, NULL},
+                {NULL, NULL, 0, true, NULL},
+        };
+        segments[0].txData = gyro->dev.txBuf;
+        segments[0].rxData = &gyro->dev.rxBuf[1];
+
+        spiSequence(&gyro->dev, &segments[0]);
+
+        // Wait for completion
+        spiWait(&gyro->dev);
+
+        gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[1]);
+        gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[2]);
+        gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[3]);
+        break;
+    }
+
+    case GYRO_EXTI_INT_DMA:
+    {
+        // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
+        // up an old value.
+        gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[5]);
+        gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[6]);
+        gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[7]);
+        break;
+    }
+
+    default:
+        break;
+    }
 
     return true;
 }

src/main/drivers/accgyro/accgyro_mpu.h

@@ -24,8 +24,6 @@
 #include "drivers/exti.h"
 #include "drivers/sensor.h"
 
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
-
 #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \
  || defined(USE_GYRO_SPI_ICM20689)
 #define GYRO_USES_SPI

src/main/drivers/accgyro/accgyro_mpu6050.c

@@ -41,8 +41,6 @@
 #include "accgyro_mpu.h"
 #include "accgyro_mpu6050.h"
 
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
-
 // MPU6050, Standard address 0x68
 // MPU_INT on PB13 on rev4 Naze32 hardware
 #define MPU6050_ADDRESS         0x68

src/main/drivers/accgyro/accgyro_spi_bmi270.c

@@ -20,6 +20,8 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
 
 #include "platform.h"
 
@@ -33,6 +35,7 @@
 #include "drivers/io_impl.h"
 #include "drivers/nvic.h"
 #include "drivers/sensor.h"
+#include "drivers/system.h"
 #include "drivers/time.h"
 
 // 10 MHz max SPI frequency
@@ -123,6 +126,9 @@ typedef enum {
     BMI270_VAL_FIFO_WTM_1 = 0x00,            // FIFO watermark MSB
 } bmi270ConfigValues_e;
 
+// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
+#define GYRO_EXTI_DETECT_THRESHOLD 1000
+
 // BMI270 register reads are 16bits with the first byte a "dummy" value 0
 // that must be ignored. The result is in the second byte.
 static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId)
@@ -246,11 +252,52 @@ static void bmi270Config(gyroDev_t *gyro)
 
 extiCallbackRec_t bmi270IntCallbackRec;
 
-#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)
+/*
+ * Gyro interrupt service routine
+ */
+#ifdef USE_GYRO_EXTI
+// Called in ISR context
+// Gyro read has just completed
+busStatus_e bmi270Intcallback(uint32_t arg)
+{
+    volatile gyroDev_t *gyro = (gyroDev_t *)arg;
+    int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
+
+    if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
+        gyro->gyroDmaMaxDuration = gyroDmaDuration;
+    }
+
+    gyro->dataReady = true;
+
+    return BUS_READY;
+}
+
 void bmi270ExtiHandler(extiCallbackRec_t *cb)
 {
+    // Non-blocking, so this needs to be static
+    static busSegment_t segments[] = {
+            {NULL, NULL, 14, true, bmi270Intcallback},
+            {NULL, NULL, 0, true, NULL},
+    };
+
     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
-    gyro->dataReady = true;
+    // Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does
+    // not have an associated timer
+    uint32_t nowCycles = getCycleCounter();
+    gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
+    gyro->gyroLastEXTI = nowCycles;
+
+    if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
+        segments[0].txData = gyro->dev.txBuf;
+        segments[0].rxData = gyro->dev.rxBuf;
+
+        if (!spiIsBusy(&gyro->dev)) {
+            spiSequence(&gyro->dev, &segments[0]);
+        }
+    }
+
+    gyro->detectedEXTI++;
+
 }
 
 static void bmi270IntExtiInit(gyroDev_t *gyro)
@@ -266,56 +313,135 @@ static void bmi270IntExtiInit(gyroDev_t *gyro)
     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
     EXTIEnable(mpuIntIO, true);
 }
+#else
+void bmi270ExtiHandler(extiCallbackRec_t *cb)
+{
+    gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
+    gyro->dataReady = true;
+}
 #endif
 
 static bool bmi270AccRead(accDev_t *acc)
 {
-    enum {
+    switch (acc->gyro->gyroModeSPI) {
-        IDX_REG = 0,
+    case GYRO_EXTI_INT:
-        IDX_SKIP,
+    case GYRO_EXTI_NO_INT:
-        IDX_ACCEL_XOUT_L,
+    {
-        IDX_ACCEL_XOUT_H,
+        // Ensure any prior DMA has completed before continuing
-        IDX_ACCEL_YOUT_L,
+        spiWaitClaim(&acc->gyro->dev);
-        IDX_ACCEL_YOUT_H,
-        IDX_ACCEL_ZOUT_L,
-        IDX_ACCEL_ZOUT_H,
-        BUFFER_SIZE,
-    };
 
-    STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
+        acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
-    STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
 
-    spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE);   // receive response
+        busSegment_t segments[] = {
+                {NULL, NULL, 8, true, NULL},
+                {NULL, NULL, 0, true, NULL},
+        };
+        segments[0].txData = acc->gyro->dev.txBuf;
+        segments[0].rxData = acc->gyro->dev.rxBuf;
 
-    acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]);
+        spiSequence(&acc->gyro->dev, &segments[0]);
-    acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]);
+
-    acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]);
+        // Wait for completion
+        spiWait(&acc->gyro->dev);
+
+        // Fall through
+        FALLTHROUGH;
+    }
+
+    case GYRO_EXTI_INT_DMA:
+    {
+        // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
+        // up an old value.
+
+        // This data was read from the gyro, which is the same SPI device as the acc
+        uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
+        acc->ADCRaw[X] = accData[1];
+        acc->ADCRaw[Y] = accData[2];
+        acc->ADCRaw[Z] = accData[3];
+        break;
+    }
+
+    case GYRO_EXTI_INIT:
+    default:
+        break;
+    }
 
     return true;
 }
 
 static bool bmi270GyroReadRegister(gyroDev_t *gyro)
 {
-    enum {
+    uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
-        IDX_REG = 0,
+    switch (gyro->gyroModeSPI) {
-        IDX_SKIP,
+    case GYRO_EXTI_INIT:
-        IDX_GYRO_XOUT_L,
+    {
-        IDX_GYRO_XOUT_H,
+        // Initialise the tx buffer to all 0x00
-        IDX_GYRO_YOUT_L,
+        memset(gyro->dev.txBuf, 0x00, 14);
-        IDX_GYRO_YOUT_H,
+#ifdef USE_GYRO_EXTI
-        IDX_GYRO_ZOUT_L,
+        // Check that minimum number of interrupts have been detected
-        IDX_GYRO_ZOUT_H,
-        BUFFER_SIZE,
-    };
 
-    STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
+        // We need some offset from the gyro interrupts to ensure sampling after the interrupt
-    STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
+        gyro->gyroDmaMaxDuration = 5;
+        // Using DMA for gyro access upsets the scheduler on the F4
+        if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
+            if (spiUseDMA(&gyro->dev)) {
+                // Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
+                spiSetAtomicWait(&gyro->dev);
+                gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
+                gyro->dev.callbackArg = (uint32_t)gyro;
+                gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
+            } else {
+                // Interrupts are present, but no DMA
+                gyro->gyroModeSPI = GYRO_EXTI_INT;
+            }
+        } else
+#endif
+        {
+            gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
+        }
+        break;
+    }
 
-    spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE);   // receive response
+    case GYRO_EXTI_INT:
+    case GYRO_EXTI_NO_INT:
+    {
+        // Ensure any prior DMA has completed before continuing
+        spiWaitClaim(&gyro->dev);
 
-    gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]);
+        gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80;
-    gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]);
+
-    gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]);
+        busSegment_t segments[] = {
+                {NULL, NULL, 8, true, NULL},
+                {NULL, NULL, 0, true, NULL},
+        };
+        segments[0].txData = gyro->dev.txBuf;
+        segments[0].rxData = gyro->dev.rxBuf;
+
+        spiSequence(&gyro->dev, &segments[0]);
+
+        // Wait for completion
+        spiWait(&gyro->dev);
+
+        gyro->gyroADCRaw[X] = gyroData[1];
+        gyro->gyroADCRaw[Y] = gyroData[2];
+        gyro->gyroADCRaw[Z] = gyroData[3];
+
+        break;
+    }
+
+    case GYRO_EXTI_INT_DMA:
+    {
+        // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
+        // up an old value.
+        gyro->gyroADCRaw[X] = gyroData[4];
+        gyro->gyroADCRaw[Y] = gyroData[5];
+        gyro->gyroADCRaw[Z] = gyroData[6];
+        break;
+    }
+
+    default:
+        break;
+    }
 
     return true;
 }
@@ -401,7 +527,7 @@ static void bmi270SpiGyroInit(gyroDev_t *gyro)
 {
     bmi270Config(gyro);
 
-#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)
+#if defined(USE_GYRO_EXTI)
     bmi270IntExtiInit(gyro);
 #endif
 }

src/main/drivers/accgyro/accgyro_spi_mpu6000.c

@@ -52,6 +52,8 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
 // 20 MHz max SPI frequency
 #define MPU6000_MAX_SPI_CLK_HZ 20000000
 
+#define MPU6000_SHORT_THRESHOLD         82  // Any interrupt interval less than this will be recognised as the short interval of ~79us
+
 // Bits
 #define BIT_SLEEP                   0x40
 #define BIT_H_RESET                 0x80
@@ -235,7 +237,9 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
     gyro->initFn = mpu6000SpiGyroInit;
     gyro->readFn = mpuGyroReadSPI;
     gyro->scale = GYRO_SCALE_2000DPS;
-
+#ifdef USE_GYRO_EXTI
+    gyro->gyroShortPeriod = clockMicrosToCycles(MPU6000_SHORT_THRESHOLD);
+#endif
     return true;
 }
 

src/main/drivers/dshot_dpwm.c

@@ -53,7 +53,7 @@ FAST_DATA_ZERO_INIT bool useDshotTelemetry = false;
 
 FAST_DATA_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
 
-FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
+FAST_CODE_NOINLINE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
 {
     int i;
     for (i = 0; i < 16; i++) {
@@ -66,7 +66,7 @@ FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t p
     return DSHOT_DMA_BUFFER_SIZE;
 }
 
-FAST_CODE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
+FAST_CODE_NOINLINE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
 {
     int i;
     for (i = 0; i < 4; i++) {

src/main/drivers/light_ws2811strip.c

@@ -44,6 +44,8 @@
 
 #include "light_ws2811strip.h"
 
+#include "scheduler/scheduler.h"
+
 #ifdef USE_LEDSTRIP_CACHE_MGMT
 // WS2811_DMA_BUFFER_SIZE is multiples of uint32_t
 // Number of bytes required for buffer
@@ -179,6 +181,7 @@ void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat)
 {
     // don't wait - risk of infinite block, just get an update next time round
     if (!ws2811Initialised || ws2811LedDataTransferInProgress) {
+        ignoreTaskStateTime();
         return;
     }
 

src/main/drivers/nvic.h

@@ -70,7 +70,7 @@
 #define NVIC_PRIO_USB                      NVIC_BUILD_PRIORITY(2, 0)
 #define NVIC_PRIO_USB_WUP                  NVIC_BUILD_PRIORITY(1, 0)
 #define NVIC_PRIO_SONAR_ECHO               NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MPU_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
+#define NVIC_PRIO_MPU_DATA_READY           NVIC_BUILD_PRIORITY(0, 1)
 #define NVIC_PRIO_MAG_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 #define NVIC_PRIO_CALLBACK                 NVIC_BUILD_PRIORITY(0x0f, 0x0f)
 #define NVIC_PRIO_MAX7456_DMA              NVIC_BUILD_PRIORITY(3, 0)

src/main/drivers/system.c

@@ -68,7 +68,7 @@ void cycleCounterInit(void)
 
 #if defined(DWT_LAR_UNLOCK_VALUE)
 #if defined(STM32F7) || defined(STM32H7)
-    DWT->LAR = DWT_LAR_UNLOCK_VALUE;
+    ITM->LAR = DWT_LAR_UNLOCK_VALUE;
 #elif defined(STM32F3) || defined(STM32F4)
     // Note: DWT_Type does not contain LAR member.
 #define DWT_LAR
@@ -146,7 +146,7 @@ uint32_t micros(void)
     return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
 }
 
-inline uint32_t getCycleCounter(void)
+uint32_t getCycleCounter(void)
 {
     return DWT->CYCCNT;
 }
@@ -156,6 +156,17 @@ uint32_t clockCyclesToMicros(uint32_t clockCycles)
     return clockCycles / usTicks;
 }
 
+// Note that this conversion is signed as this is used for periods rather than absolute timestamps
+int32_t clockCyclesTo10thMicros(int32_t clockCycles)
+{
+    return 10 * clockCycles / (int32_t)usTicks;
+}
+
+uint32_t clockMicrosToCycles(uint32_t micros)
+{
+    return micros * usTicks;
+}
+
 // Return system uptime in milliseconds (rollover in 49 days)
 uint32_t millis(void)
 {

src/main/drivers/system.h

@@ -66,6 +66,8 @@ void systemResetToBootloader(bootloaderRequestType_e requestType);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
 uint32_t clockCyclesToMicros(uint32_t clockCycles);
+int32_t clockCyclesTo10thMicros(int32_t clockCycles);
+uint32_t clockMicrosToCycles(uint32_t micros);
 uint32_t getCycleCounter(void);
 #if defined(STM32H7) || defined(STM32G4)
 void systemCheckResetReason(void);

src/main/fc/tasks.c

@@ -81,6 +81,8 @@
 
 #include "rx/rx.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/acceleration.h"
 #include "sensors/adcinternal.h"
 #include "sensors/barometer.h"
@@ -91,8 +93,6 @@
 #include "sensors/sensors.h"
 #include "sensors/rangefinder.h"
 
-#include "scheduler/scheduler.h"
-
 #include "telemetry/telemetry.h"
 #include "telemetry/crsf.h"
 
@@ -170,9 +170,9 @@ bool taskUpdateRxMainInProgress()
 
 static void taskUpdateRxMain(timeUs_t currentTimeUs)
 {
-    // Where we are using a state machine call ignoreTaskTime() for all states bar one
+    // Where we are using a state machine call ignoreTaskStateTime() for all states bar one
     if (rxState != MODES) {
-        ignoreTaskTime();
+        ignoreTaskStateTime();
     }
 
     switch (rxState) {
@@ -418,7 +418,6 @@ void tasksInit(void)
 #endif
 }
 
-#if defined(USE_TASK_STATISTICS)
 #define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) {  \
     .taskName = taskNameParam, \
     .subTaskName = subTaskNameParam, \
@@ -427,15 +426,6 @@ void tasksInit(void)
     .desiredPeriodUs = desiredPeriodParam, \
     .staticPriority = staticPriorityParam \
 }
-#else
-#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) {  \
-    .checkFunc = checkFuncParam, \
-    .taskFunc = taskFuncParam, \
-    .desiredPeriodUs = desiredPeriodParam, \
-    .staticPriority = staticPriorityParam \
-}
-#endif
-
 
 task_t tasks[TASK_COUNT] = {
     [TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH),

src/main/flight/imu.c

@@ -45,6 +45,8 @@
 
 #include "io/gps.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
@@ -591,6 +593,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         acc.accADC[X] = 0;
         acc.accADC[Y] = 0;
         acc.accADC[Z] = 0;
+        ignoreTaskStateTime();
     }
 }
 #endif // USE_ACC

src/main/io/dashboard.c

@@ -543,7 +543,6 @@ static void showSensorsPage(void)
 
 }
 
-#if defined(USE_TASK_STATISTICS)
 static void showTasksPage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
@@ -568,7 +567,6 @@ static void showTasksPage(void)
         }
     }
 }
-#endif
 
 #ifdef USE_BLACKBOX
 static void showBBPage(void)
@@ -624,9 +622,7 @@ static const pageEntry_t pages[PAGE_COUNT] = {
     { PAGE_RX,      "RX",              showRxPage,         PAGE_FLAGS_NONE },
     { PAGE_BATTERY, "BATTERY",         showBatteryPage,    PAGE_FLAGS_NONE },
     { PAGE_SENSORS, "SENSORS",         showSensorsPage,    PAGE_FLAGS_NONE },
-#if defined(USE_TASK_STATISTICS)
     { PAGE_TASKS,   "TASKS",           showTasksPage,      PAGE_FLAGS_NONE },
-#endif
 #ifdef USE_BLACKBOX
     { PAGE_BB,      "BLACK BOX",       showBBPage,         PAGE_FLAGS_NONE },
 #endif

src/main/io/dashboard.h

@@ -44,9 +44,7 @@ typedef enum {
     PAGE_RX,
     PAGE_PROFILE,
     PAGE_RPROF,
-#if defined(USE_TASK_STATISTICS)
     PAGE_TASKS,
-#endif
 #ifdef USE_GPS
     PAGE_GPS,
 #endif

src/main/io/gps.c

@@ -755,14 +755,14 @@ void gpsUpdate(timeUs_t currentTimeUs)
         updateGPSRescueState();
     }
 #endif
-    // Call ignoreTaskTime() unless this took appreciable time
+    // Call ignoreTaskShortExecTime() unless this took appreciable time
     // Note that this will mess up the rate/Hz display under tasks, but the code
     // takes widely varying time to complete
     endTimeUs = micros();
     if ((endTimeUs - currentTimeUs) > maxTimeUs) {
         maxTimeUs = endTimeUs - currentTimeUs;
     } else {
-        ignoreTaskTime();
+        ignoreTaskShortExecTime();
         // Decay max time
         maxTimeUs--;
     }

src/main/io/ledstrip.c

@@ -1066,8 +1066,8 @@ static void applyStatusProfile(timeUs_t now) {
     }
 
     if (!timActive) {
-        // Call ignoreTaskTime() unless data is being processed
+        // Call ignoreTaskShortExecTime() unless data is being processed
-        ignoreTaskTime();
+        ignoreTaskShortExecTime();
         return;          // no change this update, keep old state
     }
 
@@ -1253,8 +1253,8 @@ void ledStripUpdate(timeUs_t currentTimeUs)
 #endif
 
     if (!isWS2811LedStripReady()) {
-        // Call ignoreTaskTime() unless data is being processed
+        // Call ignoreTaskShortExecTime() unless data is being processed
-        ignoreTaskTime();
+        ignoreTaskShortExecTime();
         return;
     }
 
@@ -1282,8 +1282,8 @@ void ledStripUpdate(timeUs_t currentTimeUs)
                 break;
         }
     } else {
-        // Call ignoreTaskTime() unless data is being processed
+        // Call ignoreTaskShortExecTime() unless data is being processed
-        ignoreTaskTime();
+        ignoreTaskShortExecTime();
     }
 }
 

src/main/osd/osd.c

@@ -87,6 +87,8 @@
 #include "rx/crsf.h"
 #include "rx/rx.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/esc_sensor.h"
@@ -1049,6 +1051,8 @@ void osdUpdate(timeUs_t currentTimeUs)
             // Frsky osd need a display redraw after search for MAX7456 devices
             if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
                 displayRedraw(osdDisplayPort);
+            } else {
+                ignoreTaskShortExecTime();
             }
             return;
         }
@@ -1062,6 +1066,7 @@ void osdUpdate(timeUs_t currentTimeUs)
 
     // don't touch buffers if DMA transaction is in progress
     if (displayIsTransferInProgress(osdDisplayPort)) {
+        ignoreTaskShortExecTime();
         return;
     }
 
@@ -1069,6 +1074,7 @@ void osdUpdate(timeUs_t currentTimeUs)
     static uint32_t idlecounter = 0;
     if (!ARMING_FLAG(ARMED)) {
         if (idlecounter++ % 4 != 0) {
+            ignoreTaskShortExecTime();
             return;
         }
     }
@@ -1091,6 +1097,7 @@ void osdUpdate(timeUs_t currentTimeUs)
         if (doDrawScreen) {
             displayDrawScreen(osdDisplayPort);
         }
+        ignoreTaskShortExecTime();
     }
     ++counter;
 }

src/main/scheduler/scheduler.c

@@ -26,6 +26,8 @@
 
 #include "platform.h"
 
+#include "drivers/accgyro/accgyro.h"
+
 #include "build/build_config.h"
 #include "build/debug.h"
 
@@ -34,14 +36,15 @@
 #include "common/utils.h"
 
 #include "drivers/time.h"
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/system.h"
 
 #include "fc/core.h"
 #include "fc/tasks.h"
 
 #include "scheduler.h"
 
-#define TASK_AVERAGE_EXECUTE_FALLBACK_US 30 // Default task average time if USE_TASK_STATISTICS is not defined
+#include "sensors/gyro_init.h"
-#define TASK_AVERAGE_EXECUTE_PADDING_US 5   // Add a little padding to the average execution time
 
 // DEBUG_SCHEDULER, timings for:
 // 0 - gyroUpdate()
@@ -49,21 +52,48 @@
 // 2 - time spent in scheduler
 // 3 - time spent executing check function
 
+// DEBUG_SCHEDULER_DETERMINISM, requires USE_LATE_TASK_STATISTICS to be defined
+// 0 - Gyro task start cycle time in 10th of a us
+// 1 - ID of late task
+// 2 - Amount task is late in 10th of a us
+// 3 - Gyro lock skew in clock cycles
+
+extern task_t tasks[];
+
 static FAST_DATA_ZERO_INIT task_t *currentTask = NULL;
-static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskTime;
+static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecRate;
+static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecTime;
+
+// More than this many cycles overdue will be considered late
+#define DEBUG_SCHEDULER_DETERMINISM_THRESHOLD 10
+
+int32_t schedLoopStartCycles;
+static int32_t schedLoopStartMinCycles;
+static int32_t schedLoopStartMaxCycles;
+static uint32_t schedLoopStartDeltaDownCycles;
+static uint32_t schedLoopStartDeltaUpCycles;
+
+int32_t taskGuardCycles;
+static int32_t taskGuardMinCycles;
+static int32_t taskGuardMaxCycles;
+static uint32_t taskGuardDeltaDownCycles;
+static uint32_t taskGuardDeltaUpCycles;
 
 static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasks;
 static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasksSamples;
 
-static FAST_DATA_ZERO_INIT bool calculateTaskStatistics;
 FAST_DATA_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
 
 static FAST_DATA_ZERO_INIT int taskQueuePos = 0;
 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT int taskQueueSize = 0;
 
-static FAST_DATA int periodCalculationBasisOffset = offsetof(task_t, lastExecutedAtUs);
+static bool optimizeSchedRate;
+
 static FAST_DATA_ZERO_INIT bool gyroEnabled;
 
+static int32_t desiredPeriodCycles;
+static uint32_t lastTargetCycles;
+
 // No need for a linked list for the queue, since items are only inserted at startup
 
 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
@@ -145,7 +175,6 @@ void taskSystemLoad(timeUs_t currentTimeUs)
 #endif
 }
 
-#if defined(USE_TASK_STATISTICS)
 timeUs_t checkFuncMaxExecutionTimeUs;
 timeUs_t checkFuncTotalExecutionTimeUs;
 timeUs_t checkFuncMovingSumExecutionTimeUs;
@@ -158,14 +187,12 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
     checkFuncInfo->averageExecutionTimeUs = checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
     checkFuncInfo->averageDeltaTimeUs = checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
 }
-#endif
 
 void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
 {
     taskInfo->isEnabled = queueContains(getTask(taskId));
     taskInfo->desiredPeriodUs = getTask(taskId)->desiredPeriodUs;
     taskInfo->staticPriority = getTask(taskId)->staticPriority;
-#if defined(USE_TASK_STATISTICS)
     taskInfo->taskName = getTask(taskId)->taskName;
     taskInfo->subTaskName = getTask(taskId)->subTaskName;
     taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
@@ -179,7 +206,6 @@ void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
     taskInfo->runCount = getTask(taskId)->runCount;
     taskInfo->execTime = getTask(taskId)->execTime;
 #endif
-#endif
 }
 
 void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
@@ -191,6 +217,11 @@ void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
         task_t *task = getTask(taskId);
         task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
     }
+
+    // Catch the case where the gyro loop is adjusted
+    if (taskId == TASK_GYRO) {
+        desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs);
+    }
 }
 
 void setTaskEnabled(taskId_e taskId, bool enabled)
@@ -216,20 +247,22 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
     }
 }
 
-// Called by tasks executing what are known to be short states
+// Called by tasks executing what are known to be short states. Tasks should call this routine in all states
-void ignoreTaskTime()
+// except the one which takes the longest to execute.
+void ignoreTaskStateTime()
 {
-    ignoreCurrentTaskTime = true;
+    ignoreCurrentTaskExecRate = true;
+    ignoreCurrentTaskExecTime = true;
 }
 
-void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse)
+// Called by tasks without state machines executing in what is known to be a shorter time than peak
+void ignoreTaskShortExecTime()
 {
-    calculateTaskStatistics = calculateTaskStatisticsToUse;
+    ignoreCurrentTaskExecTime = true;
 }
 
 void schedulerResetTaskStatistics(taskId_e taskId)
 {
-#if defined(USE_TASK_STATISTICS)
     if (taskId == TASK_SELF) {
         currentTask->movingSumExecutionTimeUs = 0;
         currentTask->movingSumDeltaTimeUs = 0;
@@ -241,14 +274,10 @@ void schedulerResetTaskStatistics(taskId_e taskId)
         getTask(taskId)->totalExecutionTimeUs = 0;
         getTask(taskId)->maxExecutionTimeUs = 0;
     }
-#else
-    UNUSED(taskId);
-#endif
 }
 
 void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
 {
-#if defined(USE_TASK_STATISTICS)
     if (taskId == TASK_SELF) {
         currentTask->maxExecutionTimeUs = 0;
     } else if (taskId < TASK_COUNT) {
@@ -259,37 +288,46 @@ void schedulerResetTaskMaxExecutionTime(taskId_e taskId)
         task->runCount = 0;
 #endif
     }
-#else
-    UNUSED(taskId);
-#endif
 }
 
-#if defined(USE_TASK_STATISTICS)
 void schedulerResetCheckFunctionMaxExecutionTime(void)
 {
     checkFuncMaxExecutionTimeUs = 0;
 }
-#endif
 
 void schedulerInit(void)
 {
-    calculateTaskStatistics = true;
     queueClear();
     queueAdd(getTask(TASK_SYSTEM));
+
+    schedLoopStartMinCycles = clockMicrosToCycles(SCHED_START_LOOP_MIN_US);
+    schedLoopStartMaxCycles = clockMicrosToCycles(SCHED_START_LOOP_MAX_US);
+    schedLoopStartCycles = schedLoopStartMinCycles;
+    schedLoopStartDeltaDownCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_DOWN_STEP;
+    schedLoopStartDeltaUpCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_UP_STEP;
+
+    taskGuardMinCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MIN_US);
+    taskGuardMaxCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MAX_US);
+    taskGuardCycles = taskGuardMinCycles;
+    taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP;
+    taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP;
+
+    desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs);
+
+    lastTargetCycles = getCycleCounter();
 }
 
 void schedulerOptimizeRate(bool optimizeRate)
 {
-    periodCalculationBasisOffset = optimizeRate ? offsetof(task_t, lastDesiredAt) : offsetof(task_t, lastExecutedAtUs);
+    optimizeSchedRate = optimizeRate;
 }
 
 inline static timeUs_t getPeriodCalculationBasis(const task_t* task)
 {
-    if (task->staticPriority == TASK_PRIORITY_REALTIME) {
+    if ((task->staticPriority == TASK_PRIORITY_REALTIME)  && (optimizeSchedRate)) {
-        return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset);
+            return task->lastDesiredAt;
-    } else {
-        return task->lastExecutedAtUs;
     }
+    return task->lastExecutedAtUs;
 }
 
 FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
@@ -298,38 +336,33 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
 
     if (selectedTask) {
         currentTask = selectedTask;
-        ignoreCurrentTaskTime = false;
+        ignoreCurrentTaskExecRate = false;
-#if defined(USE_TASK_STATISTICS)
+        ignoreCurrentTaskExecTime = false;
         float period = currentTimeUs - selectedTask->lastExecutedAtUs;
-#endif
         selectedTask->lastExecutedAtUs = currentTimeUs;
         selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriodUs) * selectedTask->desiredPeriodUs;
         selectedTask->dynamicPriority = 0;
 
         // Execute task
-#if defined(USE_TASK_STATISTICS)
+        const timeUs_t currentTimeBeforeTaskCallUs = micros();
-        if (calculateTaskStatistics) {
+        selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
-            const timeUs_t currentTimeBeforeTaskCallUs = micros();
+        taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
-            selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
+        if (!ignoreCurrentTaskExecRate) {
-            taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
+            // Record task execution rate and max execution time
-            if (!ignoreCurrentTaskTime) {
+            selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs);
-                selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs);
+            selectedTask->lastStatsAtUs = currentTimeUs;
-                selectedTask->lastStatsAtUs = currentTimeUs;
+            selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
-                selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+        }
-                selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+        if (!ignoreCurrentTaskExecTime) {
-                selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
+            // Update estimate of expected task duration
+            selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+            selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
             }
             selectedTask->totalExecutionTimeUs += taskExecutionTimeUs;   // time consumed by scheduler + task
             selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
 #if defined(USE_LATE_TASK_STATISTICS)
-            selectedTask->runCount++;
+        selectedTask->runCount++;
 #endif
-        } else
-#endif
-        {
-            selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs);
-            selectedTask->taskFunc(currentTimeUs);
-        }
     }
 
     return taskExecutionTimeUs;
@@ -350,79 +383,147 @@ static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynami
 
 FAST_CODE void scheduler(void)
 {
-#if defined(USE_LATE_TASK_STATISTICS)
-    static task_t *lastTask;
-#endif
     // Cache currentTime
     const timeUs_t schedulerStartTimeUs = micros();
-    timeUs_t currentTimeUs = schedulerStartTimeUs;
+    timeUs_t currentTimeUs;
+    uint32_t nowCycles;
     timeUs_t taskExecutionTimeUs = 0;
     task_t *selectedTask = NULL;
     uint16_t selectedTaskDynamicPriority = 0;
     uint16_t waitingTasks = 0;
-    bool realtimeTaskRan = false;
+    uint32_t nextTargetCycles = 0;
-    timeDelta_t gyroTaskDelayUs = 0;
+    int32_t schedLoopRemainingCycles;
 
     if (gyroEnabled) {
         // Realtime gyro/filtering/PID tasks get complete priority
         task_t *gyroTask = getTask(TASK_GYRO);
-        const timeUs_t gyroExecuteTimeUs = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriodUs;
+        nowCycles = getCycleCounter();
-        gyroTaskDelayUs = cmpTimeUs(gyroExecuteTimeUs, currentTimeUs);  // time until the next expected gyro sample
+#if defined(UNIT_TEST)
-        if (gyroTaskDelayUs <= 0) {
+        lastTargetCycles = clockMicrosToCycles(gyroTask->lastExecutedAtUs);
-            taskExecutionTimeUs = schedulerExecuteTask(gyroTask, currentTimeUs);
+#endif
+        nextTargetCycles = lastTargetCycles + desiredPeriodCycles;
+        schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+
+        if (schedLoopRemainingCycles < -desiredPeriodCycles) {
+            /* A task has so grossly overrun that at entire gyro cycle has been skipped
+             * This is most likely to occur when connected to the configurator via USB as the serial
+             * task is non-deterministic
+             * Recover as best we can, advancing scheduling by a whole number of cycles
+             */
+            nextTargetCycles += desiredPeriodCycles * (1 + (schedLoopRemainingCycles / -desiredPeriodCycles));
+            schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+        }
+
+        // Tune out the time lost between completing the last task execution and re-entering the scheduler
+        if ((schedLoopRemainingCycles < schedLoopStartMinCycles) &&
+            (schedLoopStartCycles < schedLoopStartMaxCycles)) {
+            schedLoopStartCycles += schedLoopStartDeltaUpCycles;
+        }
+
+        // Once close to the timing boundary, poll for it's arrival
+        if (schedLoopRemainingCycles < schedLoopStartCycles) {
+            // Record the interval between scheduling the gyro task
+            static int32_t lastRealtimeStartCycles = 0;
+
+            if (schedLoopStartCycles > schedLoopStartMinCycles) {
+                schedLoopStartCycles -= schedLoopStartDeltaDownCycles;
+            }
+#if !defined(UNIT_TEST)
+            while (schedLoopRemainingCycles > 0) {
+                nowCycles = getCycleCounter();
+                schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+            }
+#endif
+            DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastRealtimeStartCycles)));
+            lastRealtimeStartCycles = nowCycles;
+
+            currentTimeUs = clockCyclesToMicros(nowCycles);
+            taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs);
+
             if (gyroFilterReady()) {
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
             }
             if (pidLoopReady()) {
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
             }
-            currentTimeUs = micros();
-            realtimeTaskRan = true;
 
-#if defined(USE_LATE_TASK_STATISTICS)
+            lastTargetCycles = nextTargetCycles;
-           // Late, so make a note of the offending task
+
-            if (gyroTaskDelayUs < -1) {
+#ifdef USE_GYRO_EXTI
-                if (lastTask) {
+            gyroDev_t *gyro = gyroActiveDev();
-                    lastTask->lateCount++;
+
+            // Bring the scheduler into lock with the gyro
+            if (gyro->gyroModeSPI != GYRO_EXTI_NO_INT) {
+                // Track the actual gyro rate over given number of cycle times and set the expected timebase
+                static uint32_t terminalGyroRateCount = 0;
+                static int32_t sampleRateStartCycles;
+
+                if ((terminalGyroRateCount == 0)) {
+                    terminalGyroRateCount = gyro->detectedEXTI + GYRO_RATE_COUNT;
+                    sampleRateStartCycles = nowCycles;
+                }
+
+                if (gyro->detectedEXTI >= terminalGyroRateCount) {
+                    // Calculate the number of clock cycles on average between gyro interrupts
+                    uint32_t sampleCycles = nowCycles - sampleRateStartCycles;
+                    desiredPeriodCycles = sampleCycles / GYRO_RATE_COUNT;
+                    sampleRateStartCycles = nowCycles;
+                    terminalGyroRateCount += GYRO_RATE_COUNT;
+                }
+
+                // Track the actual gyro rate over given number of cycle times and remove skew
+                static uint32_t terminalGyroLockCount = 0;
+                static int32_t accGyroSkew = 0;
+
+                int32_t gyroSkew = cmpTimeCycles(nextTargetCycles, gyro->gyroSyncEXTI) % desiredPeriodCycles;
+                if (gyroSkew > (desiredPeriodCycles / 2)) {
+                    gyroSkew -= desiredPeriodCycles;
+                }
+
+                accGyroSkew += gyroSkew;
+
+                if ((terminalGyroLockCount == 0)) {
+                    terminalGyroLockCount = gyro->detectedEXTI + GYRO_LOCK_COUNT;
+                }
+
+                if (gyro->detectedEXTI >= terminalGyroLockCount) {
+                    terminalGyroLockCount += GYRO_LOCK_COUNT;
+
+                    // Move the desired start time of the gyroTask
+                    lastTargetCycles -= (accGyroSkew/GYRO_LOCK_COUNT);
+                    DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 3, clockCyclesTo10thMicros(accGyroSkew/GYRO_LOCK_COUNT));
+                    accGyroSkew = 0;
                 }
             }
-            lastTask = NULL;
 #endif
-        }
+       }
     }
 
-    if (!gyroEnabled || realtimeTaskRan || (gyroTaskDelayUs > GYRO_TASK_GUARD_INTERVAL_US)) {
+    nowCycles = getCycleCounter();
-        // The task to be invoked
+    schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+
+
+    if (!gyroEnabled || (schedLoopRemainingCycles > (int32_t)clockMicrosToCycles(CHECK_GUARD_MARGIN_US))) {
+        currentTimeUs = micros();
 
         // Update task dynamic priorities
         for (task_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 currentTimeBeforeCheckFuncCallUs = micros();
-#else
-                    const timeUs_t currentTimeBeforeCheckFuncCallUs = currentTimeUs;
-#endif
                     // Increase priority for event driven tasks
                     if (task->dynamicPriority > 0) {
-                        task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAtUs) / task->desiredPeriodUs);
+                        task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->desiredPeriodUs);
                         task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
                         waitingTasks++;
-                    } else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, cmpTimeUs(currentTimeBeforeCheckFuncCallUs, task->lastExecutedAtUs))) {
+                    } else if (task->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
-#if defined(SCHEDULER_DEBUG)
+                        const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
-                        DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCallUs);
+                        DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs);
-#endif
+                        checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
-#if defined(USE_TASK_STATISTICS)
+                        checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
-                        if (calculateTaskStatistics) {
+                        checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs;   // time consumed by scheduler + task
-                            const uint32_t checkFuncExecutionTimeUs = micros() - currentTimeBeforeCheckFuncCallUs;
+                        checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
-                            checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+                        task->lastSignaledAtUs = currentTimeUs;
-                            checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
-                            checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs;   // time consumed by scheduler + task
-                            checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs);
-                        }
-#endif
-                        task->lastSignaledAtUs = currentTimeBeforeCheckFuncCallUs;
                         task->taskAgeCycles = 1;
                         task->dynamicPriority = 1 + task->staticPriority;
                         waitingTasks++;
@@ -432,7 +533,7 @@ FAST_CODE void scheduler(void)
                 } 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->desiredPeriodUs);
+                    task->taskAgeCycles = (cmpTimeUs(currentTimeUs, getPeriodCalculationBasis(task)) / task->desiredPeriodUs);
                     if (task->taskAgeCycles > 0) {
                         task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
                         waitingTasks++;
@@ -450,35 +551,46 @@ FAST_CODE void scheduler(void)
         totalWaitingTasks += waitingTasks;
 
         if (selectedTask) {
-            timeDelta_t taskRequiredTimeUs = TASK_AVERAGE_EXECUTE_FALLBACK_US;  // default average time if task statistics are not available
+            timeDelta_t taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
-#if defined(USE_TASK_STATISTICS)
-            if (calculateTaskStatistics) {
-                taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US;
 #if defined(USE_LATE_TASK_STATISTICS)
-                selectedTask->execTime = taskRequiredTimeUs;
+            selectedTask->execTime = taskRequiredTimeUs;
 #endif
-            }
+            int32_t taskRequiredTimeCycles = (int32_t)clockMicrosToCycles((uint32_t)taskRequiredTimeUs);
-#endif
-            // Add in the time spent so far in check functions and the scheduler logic
-            taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs);
-            if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) {
-                taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
-            } else {
-                selectedTask = NULL;
-            }
 
+            nowCycles = getCycleCounter();
+            schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+
+            // Allow a little extra time
+            taskRequiredTimeCycles += taskGuardCycles;
+
+            if (!gyroEnabled || (taskRequiredTimeCycles < schedLoopRemainingCycles)) {
+                taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
+                nowCycles = getCycleCounter();
+                schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+
+                if (schedLoopRemainingCycles < taskGuardMinCycles) {
+                    if (taskGuardCycles < taskGuardMaxCycles) {
+                        taskGuardCycles += taskGuardDeltaUpCycles;
+                    }
+                } else  if (taskGuardCycles > taskGuardMinCycles) {
+                    taskGuardCycles -= taskGuardDeltaDownCycles;
+                }
 #if defined(USE_LATE_TASK_STATISTICS)
-            lastTask = selectedTask;
+                if (schedLoopRemainingCycles < DEBUG_SCHEDULER_DETERMINISM_THRESHOLD) {
-#endif
+                    DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 1, selectedTask - tasks);
+                    DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 2, clockCyclesTo10thMicros(schedLoopRemainingCycles));
+                    selectedTask->lateCount++ ;
+                }
+#endif  // USE_LATE_TASK_STATISTICS
+            } else if (selectedTask->taskAgeCycles > TASK_AGE_EXPEDITE_COUNT) {
+                // If a task has been unable to run, then reduce it's recorded estimated run time to ensure
+                // it's ultimate scheduling
+                selectedTask->movingSumExecutionTimeUs *= TASK_AGE_EXPEDITE_SCALE;
+            }
         }
     }
 
-
-#if defined(SCHEDULER_DEBUG)
     DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTimeUs); // time spent in scheduler
-#else
-    UNUSED(taskExecutionTimeUs);
-#endif
 
 #if defined(UNIT_TEST)
     readSchedulerLocals(selectedTask, selectedTaskDynamicPriority, waitingTasks);
@@ -494,3 +606,9 @@ uint16_t getAverageSystemLoadPercent(void)
 {
     return averageSystemLoadPercent;
 }
+
+float schedulerGetCycleTimeMultipler(void)
+{
+    return (float)clockMicrosToCycles(getTask(TASK_GYRO)->desiredPeriodUs) / desiredPeriodCycles;
+
+}

src/main/scheduler/scheduler.h

@@ -27,14 +27,29 @@
 #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
 
 #define LOAD_PERCENTAGE_ONE 100
 
+#define SCHED_START_LOOP_MIN_US         4   // Wait at start of scheduler loop if gyroTask is nearly due
+#define SCHED_START_LOOP_MAX_US         12
+#define SCHED_START_LOOP_DOWN_STEP      50  // Fraction of a us to reduce start loop wait
+#define SCHED_START_LOOP_UP_STEP        1   // Fraction of a us to increase start loop wait
+
+#define TASK_GUARD_MARGIN_MIN_US        2   // Add an amount to the estimate of a task duration
+#define TASK_GUARD_MARGIN_MAX_US        5
+#define TASK_GUARD_MARGIN_DOWN_STEP     50  // Fraction of a us to reduce task guard margin
+#define TASK_GUARD_MARGIN_UP_STEP       1   // Fraction of a us to increase task guard margin
+
+#define CHECK_GUARD_MARGIN_US           2   // Add a margin to the amount of time allowed for a check function to run
+
+#define TASK_AGE_EXPEDITE_COUNT         1   // Make aged tasks more schedulable
+#define TASK_AGE_EXPEDITE_SCALE         0.9 // By scaling their expected execution time
+
+// Gyro interrupt counts over which to measure loop time and skew
+#define GYRO_RATE_COUNT 32000
+#define GYRO_LOCK_COUNT 400
+
 typedef enum {
     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
@@ -164,10 +179,8 @@ typedef enum {
 
 typedef struct {
     // Configuration
-#if defined(USE_TASK_STATISTICS)
     const char * taskName;
     const char * subTaskName;
-#endif
     bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
     void (*taskFunc)(timeUs_t currentTimeUs);
     timeDelta_t desiredPeriodUs;        // target period of execution
@@ -181,7 +194,6 @@ typedef struct {
     timeUs_t lastSignaledAtUs;          // time of invocation event for event-driven tasks
     timeUs_t lastDesiredAt;             // time of last desired execution
 
-#if defined(USE_TASK_STATISTICS)
     // Statistics
     float    movingAverageCycleTimeUs;
     timeUs_t movingSumExecutionTimeUs;  // moving sum over 32 samples
@@ -194,7 +206,6 @@ typedef struct {
     uint32_t lateCount;
     timeUs_t execTime;
 #endif
-#endif
 } task_t;
 
 void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo);
@@ -202,8 +213,8 @@ void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo);
 void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
 void setTaskEnabled(taskId_e taskId, bool newEnabledState);
 timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
-void ignoreTaskTime();
+void ignoreTaskStateTime();
-void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics);
+void ignoreTaskShortExecTime();
 void schedulerResetTaskStatistics(taskId_e taskId);
 void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
 void schedulerResetCheckFunctionMaxExecutionTime(void);
@@ -215,3 +226,4 @@ void taskSystemLoad(timeUs_t currentTimeUs);
 void schedulerOptimizeRate(bool optimizeRate);
 void schedulerEnableGyro(void);
 uint16_t getAverageSystemLoadPercent(void);
+float schedulerGetCycleTimeMultipler(void);

src/main/sensors/barometer.c

@@ -389,7 +389,13 @@ uint32_t baroUpdate(void)
     // Tell the scheduler to ignore how long this task takes unless the pressure is being read
     // as that takes the longest
     if (state != BAROMETER_NEEDS_PRESSURE_READ) {
-        ignoreTaskTime();
+        ignoreTaskStateTime();
+    }
+
+    if (busBusy(&baro.dev.dev, NULL)) {
+        // If the bus is busy, simply return to have another go later
+        ignoreTaskStateTime();
+        return sleepTime;
     }
 
     switch (state) {
@@ -422,7 +428,7 @@ uint32_t baroUpdate(void)
             if (baro.dev.read_up(&baro.dev)) {
                 state = BAROMETER_NEEDS_PRESSURE_SAMPLE;
             } else {
-                ignoreTaskTime();
+                ignoreTaskStateTime();
             }
         break;
 

src/main/sensors/compass.c

@@ -81,26 +81,26 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
 // 3. Slave I2C device on SPI gyro
 
 #if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963))
-    compassConfig->mag_bustype = BUS_TYPE_SPI;
+    compassConfig->mag_busType = BUS_TYPE_SPI;
     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE));
     compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN);
     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
     compassConfig->mag_i2c_address = 0;
 #elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
-    compassConfig->mag_bustype = BUS_TYPE_I2C;
+    compassConfig->mag_busType = BUS_TYPE_I2C;
     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE);
     compassConfig->mag_i2c_address = 0;
     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
     compassConfig->mag_spi_csn = IO_TAG_NONE;
 #elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
-    compassConfig->mag_bustype = BUS_TYPE_MPU_SLAVE;
+    compassConfig->mag_busType = BUS_TYPE_MPU_SLAVE;
     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
     compassConfig->mag_i2c_address = 0;
     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
     compassConfig->mag_spi_csn = IO_TAG_NONE;
 #else
     compassConfig->mag_hardware = MAG_NONE;
-    compassConfig->mag_bustype = BUS_TYPE_NONE;
+    compassConfig->mag_busType = BUS_TYPE_NONE;
     compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
     compassConfig->mag_i2c_address = 0;
     compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
@@ -115,7 +115,7 @@ static uint8_t magInit = 0;
 void compassPreInit(void)
 {
 #ifdef USE_SPI
-    if (compassConfig()->mag_bustype == BUS_TYPE_SPI) {
+    if (compassConfig()->mag_busType == BUS_TYPE_SPI) {
         spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1);
     }
 #endif
@@ -136,7 +136,7 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
     magDev->magIntExtiTag = compassConfig()->interruptTag;
 #endif
 
-    switch (compassConfig()->mag_bustype) {
+    switch (compassConfig()->mag_busType) {
 #ifdef USE_I2C
     case BUS_TYPE_I2C:
         i2cBusSetInstance(dev, compassConfig()->mag_i2c_device);

src/main/sensors/compass.h

@@ -49,7 +49,7 @@ extern mag_t mag;
 typedef struct compassConfig_s {
     uint8_t mag_alignment;                  // mag alignment
     uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
-    uint8_t mag_bustype;
+    uint8_t mag_busType;
     uint8_t mag_i2c_device;
     uint8_t mag_i2c_address;
     uint8_t mag_spi_device;

src/main/sensors/gyro_init.c

@@ -78,6 +78,11 @@
 #define ACTIVE_GYRO (&gyro.gyroSensor1)
 #endif
 
+// The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer
+// This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other
+// gyro types are supported with SPI DMA.
+#define GYRO_BUF_SIZE 32
+
 static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
 
 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
@@ -635,6 +640,11 @@ bool gyroInit(void)
     }
 
     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+        static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
+        // SPI DMA buffer required per device
+        gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
+        gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2];
+
         gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1));
         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection;
         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware;
@@ -646,6 +656,10 @@ bool gyroInit(void)
     }
 
     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+        static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
+        // SPI DMA buffer required per device
+        gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
+        gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2];
         gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;
         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware;

src/main/target/ALIENFLIGHTF1/target.h

@@ -32,7 +32,6 @@
 #define USE_EXTI
 #define MAG_INT_EXTI            PC14
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #define USE_GYRO
 #define USE_GYRO_MPU6050

src/main/target/ALIENFLIGHTF3/target.h

@@ -41,7 +41,6 @@
 #define BEEPER_PIN              PA5
 
 #define USE_EXTI
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 
 // Using MPU6050 for the moment.

src/main/target/ALIENFLIGHTF4/target.h

@@ -40,7 +40,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC14
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/ALIENFLIGHTNGF7/target.h

@@ -38,7 +38,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC14
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/BEEROTORF4/target.h

@@ -36,7 +36,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PA8
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/BLUEJAYF4/target.h

@@ -47,7 +47,6 @@
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC5
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #define GYRO_1_CS_PIN           PC4
 #define GYRO_1_SPI_INSTANCE     SPI1

src/main/target/CC3D/target.h

@@ -33,8 +33,6 @@
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PA3
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
-
 #define USE_SPI
 #define USE_SPI_DEVICE_1
 #define USE_SPI_DEVICE_2

src/main/target/CRAZYBEEF3FR/target.h

@@ -134,7 +134,7 @@
 #define USE_RX_FRSKY_SPI_X
 #define USE_RX_SFHSS_SPI
 #define USE_RX_REDPINE_SPI
-#define DEFAULT_RX_FEATURE      FEATURE_RX_SPI	
+#define DEFAULT_RX_FEATURE      FEATURE_RX_SPI
 #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X
 #define USE_RX_FRSKY_SPI_TELEMETRY
 

src/main/target/DOGE/target.h

@@ -115,7 +115,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC13
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/ELLE0/target.h

@@ -33,7 +33,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PB5
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/FF_FORTINIF4/target.h

@@ -54,7 +54,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC4
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
@@ -110,7 +109,7 @@
 #define UART1_RX_PIN            PB7
 #else 
 #define UART1_RX_PIN            PA10
-#endif	
+#endif  
 #define UART1_TX_PIN            PA9
 
 #define USE_UART4

src/main/target/FF_PIKOF4/target.h

@@ -48,7 +48,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC4
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/FURYF4/target.c

@@ -32,10 +32,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     DEF_TIM(TIM8, CH4,  PC9, TIM_USE_PPM,   0, 0 ), // PPM IN
     DEF_TIM(TIM2, CH4,  PA3, TIM_USE_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6_CH3
     DEF_TIM(TIM3, CH3,  PB0, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST7_CH5
-    DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA2_ST6_CH0
+    DEF_TIM(TIM3, CH4,  PB1, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST2_CH5
     DEF_TIM(TIM2, CH3,  PA2, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1_CH3
 
     DEF_TIM(TIM5, CH1,  PA0, TIM_USE_LED,   0, 0 ), // LED_STRIP - DMA1_ST2_CH6
-    DEF_TIM(TIM11, CH1, PB9,  TIM_USE_CAMERA_CONTROL,  0, 0), // CAM_CTL
+    DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAM_CTL
 
 };

src/main/target/FURYF4/target.h

@@ -20,6 +20,8 @@
 
 #pragma once
 
+#define USE_LATE_TASK_STATISTICS
+
 #ifdef FURYF4OSD
 #define TARGET_BOARD_IDENTIFIER "FY4O"
 #define USBD_PRODUCT_STRING     "FuryF4OSD"
@@ -158,4 +160,4 @@
 #define TARGET_IO_PORTD         (BIT(2))
 
 #define USABLE_TIMER_CHANNEL_COUNT 7
-#define USED_TIMERS             ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(11) )
+#define USED_TIMERS             ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(11) )

src/main/target/KAKUTEF4/target.h

@@ -56,7 +56,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC5
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
@@ -73,7 +72,7 @@
 #if defined(FLYWOOF405)
 //------MPU6000
 #define USE_GYRO_SPI_MPU6000
-#define USE_ACC_SPI_MPU6000								  
+#define USE_ACC_SPI_MPU6000                               
 #endif
 
 #if defined(KAKUTEF4V2) || defined(FLYWOOF405)       // There is invertor on RXD3(PB11), so PB10/PB11 can't be used as I2C2.
@@ -154,8 +153,8 @@
 #define USE_PINIOBOX
 #else
 #define ESCSERIAL_TIMER_TX_PIN  PC7  // (HARDARE=0,PPM)
-#endif	
+#endif  
-									   
+                                       
 #define USE_SPI
 #define USE_SPI_DEVICE_1 //ICM20689
 #define SPI1_NSS_PIN            PC4

src/main/target/LUX_RACE/target.h

@@ -48,7 +48,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PA5
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/MAMBAF722/target.h

@@ -115,7 +115,7 @@
 
 #define USE_ADC
 #define ADC_INSTANCE            ADC3
-#define ADC3_DMA_OPT            0
+#define ADC3_DMA_OPT            1
 
 #define VBAT_ADC_PIN            PC1
 #define RSSI_ADC_PIN            PC2

src/main/target/MICROSCISKY/target.h

@@ -35,9 +35,7 @@
 
 #define USE_EXTI
 #define MAG_INT_EXTI            PC14
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MAG_DATA_READY_INTERRUPT
 #define USE_MAG_DATA_READY_SIGNAL
 
 // SPI2

src/main/target/NERO/target.h

@@ -40,7 +40,6 @@
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PB2
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #define GYRO_1_CS_PIN           PC4
 #define GYRO_1_SPI_INSTANCE     SPI1

src/main/target/REVO/target.h

@@ -20,6 +20,8 @@
 
 #pragma once
 
+#define USE_LATE_TASK_STATISTICS
+
 #if defined(AIRBOTF4)
 #define TARGET_BOARD_IDENTIFIER "AIR4"
 #define USBD_PRODUCT_STRING     "AirbotF4"

src/main/target/REVONANO/target.h

@@ -19,6 +19,9 @@
  */
 
 #pragma once
+
+#define USE_LATE_TASK_STATISTICS
+
 #define TARGET_BOARD_IDENTIFIER "REVN"
 
 #define USBD_PRODUCT_STRING "Revo Nano"

src/main/target/SITL/target.c

@@ -334,6 +334,25 @@ uint32_t millis(void) {
     return millis64() & 0xFFFFFFFF;
 }
 
+uint32_t clockCyclesToMicros(uint32_t clockCycles)
+{
+    return clockCycles;
+}
+
+int32_t clockCyclesTo10thMicros(int32_t clockCycles)
+{
+    return clockCycles;
+}
+
+uint32_t clockMicrosToCycles(uint32_t micros)
+{
+    return micros;
+}
+uint32_t getCycleCounter(void)
+{
+    return (uint32_t) (micros64() & 0xFFFFFFFF);
+}
+
 void microsleep(uint32_t usec) {
     struct timespec ts;
     ts.tv_sec = 0;

src/main/target/SPARKY2/target.h

@@ -40,7 +40,6 @@
 #define USE_EXTI
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC5
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 

src/main/target/STM32F4DISCOVERY/target.h

@@ -43,7 +43,6 @@
 #define USE_GYRO_EXTI
 #define GYRO_1_EXTI_PIN         PC4
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #define GYRO_1_CS_PIN           PC4
 #define GYRO_1_SPI_INSTANCE     SPI1

src/main/target/WORMFC/target.h

@@ -69,7 +69,6 @@
 #define GYRO_1_EXTI_PIN         PC4
 #endif
 #define USE_MPU_DATA_READY_SIGNAL
-//#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #if defined(PIRXF4)
 #define GYRO_1_CS_PIN          PC4

src/main/target/common_pre.h

@@ -28,8 +28,6 @@
 // -Wpadded can be turned on to check padding of structs
 //#pragma GCC diagnostic warning "-Wpadded"
 
-//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons
-
 #ifdef STM32F1
 #define MINIMAL_CLI
 // Using RX DMA disables the use of receive callbacks
@@ -92,6 +90,7 @@
 #define USE_TIMER_MGMT
 #define USE_PERSISTENT_OBJECTS
 #define USE_CUSTOM_DEFAULTS_ADDRESS
+#define USE_LATE_TASK_STATISTICS
 #endif // STM32F7
 
 #ifdef STM32H7
@@ -133,6 +132,7 @@
 #define USE_MCO
 #define USE_DMA_SPEC
 #define USE_TIMER_MGMT
+#define USE_LATE_TASK_STATISTICS
 #endif
 
 #if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
@@ -231,7 +231,6 @@ extern uint8_t _dmaram_end__;
 
 #define USE_CLI
 #define USE_SERIAL_PASSTHROUGH
-#define USE_TASK_STATISTICS
 #define USE_GYRO_REGISTER_DUMP  // Adds gyroregisters command to cli to dump configured register values
 #define USE_IMU_CALC
 #define USE_PPM

src/test/unit/cli_unittest.cc

@@ -202,6 +202,10 @@ uint16_t batteryWarningVoltage;
 uint8_t useHottAlarmSoundPeriod (void) { return 0; }
 const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 400000}; // see baudRate_e
 
+uint8_t debugMode;
+int32_t schedLoopStartCycles;
+int32_t taskGuardCycles;
+
 uint32_t micros(void) {return 0;}
 
 int32_t getAmperage(void) {
@@ -324,7 +328,6 @@ uint8_t serialRead(serialPort_t *){return 0;}
 void bufWriterAppend(bufWriter_t *, uint8_t ch){ printf("%c", ch); }
 void serialWriteBufShim(void *, const uint8_t *, int) {}
 bufWriter_t *bufWriterInit(uint8_t *, int, bufWrite_t, void *) {return NULL;}
-void schedulerSetCalulateTaskStatistics(bool) {}
 void setArmingDisabled(armingDisableFlags_e) {}
 
 void waitForSerialPortToFinishTransmitting(serialPort_t *) {}

src/test/unit/flight_imu_unittest.cc

@@ -250,4 +250,6 @@ void mixerSetThrottleAngleCorrection(int) {};
 bool gpsRescueIsRunning(void) { return false; }
 bool isFixedWing(void) { return false; }
 void pinioBoxTaskControl(void) {}
+void ignoreTaskShortExecTime(void) {}
+void ignoreTaskStateTime(void) {}
 }

src/test/unit/ledstrip_unittest.cc

@@ -395,7 +395,7 @@ bool isFlipOverAfterCrashActive(void) { return false; }
 
 void ws2811LedStripEnable(void) { }
 
-void setUsedLedCount(unsigned) { };
+void setUsedLedCount(unsigned) { }
 void pinioBoxTaskControl(void) {}
-void ignoreTaskTime(void) {}
+void ignoreTaskShortExecTime(void) {}
 }

src/test/unit/link_quality_unittest.cc

@@ -559,4 +559,7 @@ extern "C" {
     float getMotorOutputLow(void) { return 1000.0; }
 
     float getMotorOutputHigh(void) { return 2047.0; }
+
+    void ignoreTaskShortExecTime(void) {}
+    void ignoreTaskStateTime(void) {}
 }

src/test/unit/osd_unittest.cc

@@ -1319,4 +1319,6 @@ extern "C" {
     bool isUpright(void) { return true; }
     float getMotorOutputLow(void) { return 1000.0; }
     float getMotorOutputHigh(void) { return 2047.0; }
+    void ignoreTaskShortExecTime(void) {}
+    void ignoreTaskStateTime(void) {}
 }

src/test/unit/scheduler_unittest.cc

@@ -51,10 +51,18 @@ extern "C" {
     bool taskPidRan = false;
     bool taskFilterReady = false;
     bool taskPidReady = false;
+    uint8_t activePidLoopDenom = 1;
+
+    int16_t debug[1];
+    uint8_t debugMode = 0;
 
     // set up micros() to simulate time
     uint32_t simulatedTime = 0;
     uint32_t micros(void) { return simulatedTime; }
+    uint32_t clockCyclesToMicros(uint32_t x) { return x/10;}
+    int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
+    uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
+    uint32_t getCycleCounter(void) {return simulatedTime * 10;}
 
     // set up tasks to take a simulated representative time to execute
     bool gyroFilterReady(void) { return taskFilterReady; }
@@ -463,6 +471,7 @@ TEST(SchedulerUnittest, TestGyroTask)
 
     // 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);
@@ -507,102 +516,3 @@ TEST(SchedulerUnittest, TestGyroTask)
     EXPECT_EQ(static_cast<task_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<taskId_e>(taskId), false);
-    }
-    setTaskEnabled(TASK_GYRO, true);
-    setTaskEnabled(TASK_ACCEL, true);
-
-#if defined(USE_TASK_STATISTICS)
-    // set the average run time for TASK_ACCEL
-    tasks[TASK_ACCEL].movingSumExecutionTimeUs = 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;
-    tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime;
-    tasks[TASK_ACCEL].lastExecutedAtUs = 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(&tasks[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;
-    tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2;
-    tasks[TASK_ACCEL].lastExecutedAtUs = 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<task_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;
-    tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2;
-    tasks[TASK_ACCEL].lastExecutedAtUs = 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<task_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;
-    tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ);
-    tasks[TASK_ACCEL].lastExecutedAtUs = 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(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
-}

src/test/unit/target.h

@@ -20,6 +20,7 @@
 #define SCHEDULER_DELAY_LIMIT 1
 #define TASK_GYROPID_DESIRED_PERIOD 100
 
+#define DMA_DATA
 #define DMA_DATA_ZERO_INIT
 
 #define USE_ACC
@@ -63,7 +64,6 @@
 #define USE_UART5
 #define USE_SOFTSERIAL1
 #define USE_SOFTSERIAL2
-#define USE_TASK_STATISTICS
 
 #define SERIAL_PORT_COUNT 8
 

src/test/unit/ws2811_unittest.cc

@@ -32,6 +32,8 @@ extern "C" {
 
 extern "C" {
     void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex);
+    void ignoreTaskShortExecTime(void) {}
+    void ignoreTaskStateTime(void) {}
 }
 
 TEST(WS2812, updateDMABuffer) {
@@ -89,3 +91,4 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) {
 
 void ws2811LedStripDMAEnable(void) {}
 }
+