Track state execution time for OSD, baro, rx and GPS tasks and inform scheduler of next state execution time

src/main/build/debug.c

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

src/main/build/debug.h

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

src/main/cli/cli.c

@@ -4907,7 +4907,6 @@ static void cliTasks(const char *cmdName, char *cmdline)
 {
     UNUSED(cmdName);
     UNUSED(cmdline);
-    int maxLoadSum = 0;
     int averageLoadSum = 0;
 
 #ifndef MINIMAL_CLI
@@ -4927,10 +4926,9 @@ static void cliTasks(const char *cmdName, char *cmdline)
         if (taskInfo.isEnabled) {
             int taskFrequency = taskInfo.averageDeltaTimeUs == 0 ? 0 : lrintf(1e6f / taskInfo.averageDeltaTimeUs);
             cliPrintf("%02d - (%15s) ", taskId, taskInfo.taskName);
-            const int maxLoad = taskInfo.maxExecutionTimeUs == 0 ? 0 :(taskInfo.maxExecutionTimeUs * taskFrequency + 5000) / 1000;
+            const int maxLoad = taskInfo.maxExecutionTimeUs == 0 ? 0 : (taskInfo.maxExecutionTimeUs * taskFrequency) / 1000;
-            const int averageLoad = taskInfo.averageExecutionTimeUs == 0 ? 0 : (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 1000;
+            const int averageLoad = taskInfo.averageExecutionTimeUs == 0 ? 0 : (taskInfo.averageExecutionTimeUs * taskFrequency) / 1000;
             if (taskId != TASK_SERIAL) {
-                maxLoadSum += maxLoad;
                 averageLoadSum += averageLoad;
             }
             if (systemConfig()->task_statistics) {
@@ -4957,7 +4955,7 @@ static void cliTasks(const char *cmdName, char *cmdline)
         cfCheckFuncInfo_t checkFuncInfo;
         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);
+        cliPrintLinef("Total (excluding SERIAL) %33d.%1d%%", averageLoadSum/10, averageLoadSum%10);
         if (debugMode == DEBUG_SCHEDULER_DETERMINISM) {
             extern int32_t schedLoopStartCycles, taskGuardCycles;
 

src/main/cli/settings.c

@@ -1462,7 +1462,7 @@ const clivalue_t valueTable[] = {
     { "osd_camera_frame_width",     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { OSD_CAMERA_FRAME_MIN_WIDTH, OSD_CAMERA_FRAME_MAX_WIDTH }, PG_OSD_CONFIG, offsetof(osdConfig_t, camera_frame_width) },
     { "osd_camera_frame_height",    VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { OSD_CAMERA_FRAME_MIN_HEIGHT, OSD_CAMERA_FRAME_MAX_HEIGHT }, PG_OSD_CONFIG, offsetof(osdConfig_t, camera_frame_height) },
     { "osd_stat_avg_cell_value",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, stat_show_cell_value) },
-    { "osd_task_frequency",         VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { OSD_TASK_FREQUENCY_MIN, OSD_TASK_FREQUENCY_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, task_frequency) },
+    { "osd_framerate_hz",           VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { OSD_FRAMERATE_MIN_HZ, OSD_FRAMERATE_MAX_HZ }, PG_OSD_CONFIG, offsetof(osdConfig_t, framerate_hz) },
     { "osd_menu_background",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CMS_BACKGROUND }, PG_OSD_CONFIG, offsetof(osdConfig_t, cms_background_type) },
 #endif // end of #ifdef USE_OSD
 

src/main/drivers/accgyro/accgyro_mpu.c

@@ -272,7 +272,6 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
 
         // We need some offset from the gyro interrupts to ensure sampling after the interrupt
         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

src/main/drivers/bus_spi.c

@@ -398,6 +398,64 @@ uint16_t spiCalculateDivider(uint32_t freq)
     return divisor;
 }
 
+// Interrupt handler for SPI receive DMA completion
+static void spiIrqHandler(const extDevice_t *dev)
+{
+    busDevice_t *bus = dev->bus;
+    busSegment_t *nextSegment;
+
+    if (bus->curSegment->callback) {
+        switch(bus->curSegment->callback(dev->callbackArg)) {
+        case BUS_BUSY:
+            // Repeat the last DMA segment
+            bus->curSegment--;
+            // Reinitialise the cached init values as segment is not progressing
+            spiInternalInitStream(dev, true);
+            break;
+
+        case BUS_ABORT:
+            bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
+            return;
+
+        case BUS_READY:
+        default:
+            // Advance to the next DMA segment
+            break;
+        }
+    }
+
+    // Advance through the segment list
+    nextSegment = bus->curSegment + 1;
+
+    if (nextSegment->len == 0) {
+        // If a following transaction has been linked, start it
+        if (nextSegment->txData) {
+            const extDevice_t *nextDev = (const extDevice_t *)nextSegment->txData;
+            busSegment_t *nextSegments = (busSegment_t *)nextSegment->rxData;
+            nextSegment->txData = NULL;
+            // The end of the segment list has been reached
+            spiSequenceStart(nextDev, nextSegments);
+        } else {
+            // The end of the segment list has been reached, so mark transactions as complete
+            bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
+        }
+    } else {
+        bus->curSegment = nextSegment;
+
+        // After the completion of the first segment setup the init structure for the subsequent segment
+        if (bus->initSegment) {
+            spiInternalInitStream(dev, false);
+            bus->initSegment = false;
+        }
+
+        // Launch the next transfer
+        spiInternalStartDMA(dev);
+
+        // Prepare the init structures ready for the next segment to reduce inter-segment time
+        spiInternalInitStream(dev, true);
+    }
+}
+
 // Interrupt handler for SPI receive DMA completion
 static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
 {
@@ -408,7 +466,6 @@ static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
     }
 
     busDevice_t *bus = dev->bus;
-    busSegment_t *nextSegment;
 
     if (bus->curSegment->negateCS) {
         // Negate Chip Select
@@ -432,59 +489,33 @@ static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
     }
 #endif // __DCACHE_PRESENT
 
-    if (bus->curSegment->callback) {
+    spiIrqHandler(dev);
-        switch(bus->curSegment->callback(dev->callbackArg)) {
-        case BUS_BUSY:
-            // Repeat the last DMA segment
-            bus->curSegment--;
-            // Reinitialise the cached init values as segment is not progressing
-            spiInternalInitStream(dev, true);
-            break;
-
-        case BUS_ABORT:
-            bus->curSegment = (busSegment_t *)NULL;
-            return;
-
-        case BUS_READY:
-        default:
-            // Advance to the next DMA segment
-            break;
-        }
-    }
-
-    // Advance through the segment list
-    nextSegment = bus->curSegment + 1;
-
-    if (nextSegment->len == 0) {
-        // If a following transaction has been linked, start it
-        if (nextSegment->txData) {
-            const extDevice_t *nextDev = (const extDevice_t *)nextSegment->txData;
-            busSegment_t *nextSegments = (busSegment_t *)nextSegment->rxData;
-            nextSegment->txData = NULL;
-            // The end of the segment list has been reached
-            spiSequenceStart(nextDev, nextSegments);
-        } else {
-            // The end of the segment list has been reached, so mark transactions as complete
-            bus->curSegment = (busSegment_t *)NULL;
-        }
-    } else {
-        bus->curSegment = nextSegment;
-
-        // After the completion of the first segment setup the init structure for the subsequent segment
-        if (bus->initSegment) {
-            spiInternalInitStream(dev, false);
-            bus->initSegment = false;
-        }
-
-        // Launch the next transfer
-        spiInternalStartDMA(dev);
-
-        // Prepare the init structures ready for the next segment to reduce inter-segment time
-        spiInternalInitStream(dev, true);
-    }
 }
 
-// Mark this bus as being SPI
+#if !defined(STM32G4) && !defined(STM32H7)
+// Interrupt handler for SPI transmit DMA completion
+static void spiTxIrqHandler(dmaChannelDescriptor_t* descriptor)
+{
+    const extDevice_t *dev = (const extDevice_t *)descriptor->userParam;
+
+    if (!dev) {
+        return;
+    }
+
+    busDevice_t *bus = dev->bus;
+
+    spiInternalStopDMA(dev);
+
+    if (bus->curSegment->negateCS) {
+        // Negate Chip Select
+        IOHi(dev->busType_u.spi.csnPin);
+    }
+
+    spiIrqHandler(dev);
+}
+#endif
+
+// Mark this bus as being SPI and record the first owner to use it
 bool spiSetBusInstance(extDevice_t *dev, uint32_t device)
 {
     if ((device == 0) || (device > SPIDEV_COUNT)) {
@@ -492,6 +523,8 @@ bool spiSetBusInstance(extDevice_t *dev, uint32_t device)
     }
 
     dev->bus = &spiBusDevice[SPI_CFG_TO_DEV(device)];
+
+    // By default each device should use SPI DMA if the bus supports it
     dev->useDMA = true;
 
     if (dev->bus->busType == BUS_TYPE_SPI) {
@@ -622,6 +655,21 @@ void spiInitBusDMA()
             dmaSetHandler(dmaRxIdentifier, spiRxIrqHandler, NVIC_PRIO_SPI_DMA, 0);
 
             bus->useDMA = true;
+#if !defined(STM32G4) && !defined(STM32H7)
+        } else if (dmaTxIdentifier) {
+            // Transmit on DMA is adequate for OSD so worth having
+            bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier);
+            bus->dmaRx = (dmaChannelDescriptor_t *)NULL;
+
+            // Ensure streams are disabled
+            spiInternalResetStream(bus->dmaTx);
+
+            spiInternalResetDescriptors(bus);
+
+            dmaSetHandler(dmaTxIdentifier, spiTxIrqHandler, NVIC_PRIO_SPI_DMA, 0);
+
+            bus->useDMA = true;
+#endif
         } else {
             // Disassociate channels from bus
             bus->dmaRx = (dmaChannelDescriptor_t *)NULL;
@@ -648,6 +696,12 @@ void spiDmaEnable(const extDevice_t *dev, bool enable)
 }
 
 bool spiUseDMA(const extDevice_t *dev)
+{
+    // Full DMA only requires both transmit and receive}
+    return dev->bus->useDMA && dev->bus->dmaRx && dev->useDMA;
+}
+
+bool spiUseMOSI_DMA(const extDevice_t *dev)
 {
     return dev->bus->useDMA && dev->useDMA;
 }

src/main/drivers/bus_spi.h

@@ -170,6 +170,7 @@ bool spiReadWriteBufRB(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData,
 struct spiPinConfig_s;
 void spiPinConfigure(const struct spiPinConfig_s *pConfig);
 bool spiUseDMA(const extDevice_t *dev);
+bool spiUseMOSI_DMA(const extDevice_t *dev);
 void spiBusDeviceRegister(const extDevice_t *dev);
 uint8_t spiGetRegisteredDeviceCount(void);
 uint8_t spiGetExtDeviceCount(const extDevice_t *dev);

src/main/drivers/bus_spi_ll.c

@@ -165,7 +165,6 @@ void spiInitDevice(SPIDevice device)
 void spiInternalResetDescriptors(busDevice_t *bus)
 {
     LL_DMA_InitTypeDef *initTx = bus->initTx;
-    LL_DMA_InitTypeDef *initRx = bus->initRx;
 
     LL_DMA_StructInit(initTx);
 #if defined(STM32G4) || defined(STM32H7)
@@ -185,22 +184,26 @@ void spiInternalResetDescriptors(busDevice_t *bus)
     initTx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
     initTx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
 
-    LL_DMA_StructInit(initRx);
+    if (bus->dmaRx) {
+        LL_DMA_InitTypeDef *initRx = bus->initRx;
+
+        LL_DMA_StructInit(initRx);
 #if defined(STM32G4) || defined(STM32H7)
-    initRx->PeriphRequest = bus->dmaRx->channel;
+        initRx->PeriphRequest = bus->dmaRx->channel;
 #else
-    initRx->Channel = bus->dmaRx->channel;
+        initRx->Channel = bus->dmaRx->channel;
 #endif
-    initRx->Mode = LL_DMA_MODE_NORMAL;
+        initRx->Mode = LL_DMA_MODE_NORMAL;
-    initRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
+        initRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
 #if defined(STM32H7)
-    initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->RXDR;
+        initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->RXDR;
 #else
-    initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR;
+        initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR;
 #endif
-    initRx->Priority = LL_DMA_PRIORITY_LOW;
+        initRx->Priority = LL_DMA_PRIORITY_LOW;
-    initRx->PeriphOrM2MSrcIncMode  = LL_DMA_PERIPH_NOINCREMENT;
+        initRx->PeriphOrM2MSrcIncMode  = LL_DMA_PERIPH_NOINCREMENT;
-    initRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
+        initRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
+    }
 }
 
 void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
@@ -289,8 +292,8 @@ static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *
 
 void spiInternalInitStream(const extDevice_t *dev, bool preInit)
 {
-    static uint8_t dummyTxByte = 0xff;
+    STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff;
-    static uint8_t dummyRxByte;
+    STATIC_DMA_DATA_AUTO uint8_t dummyRxByte;
     busDevice_t *bus = dev->bus;
 
     busSegment_t *segment = bus->curSegment;
@@ -304,12 +307,10 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
         }
     }
 
-    uint8_t *txData = segment->txData;
-    uint8_t *rxData = segment->rxData;
     int len = segment->len;
 
+    uint8_t *txData = segment->txData;
     LL_DMA_InitTypeDef *initTx = bus->initTx;
-    LL_DMA_InitTypeDef *initRx = bus->initRx;
 
     if (txData) {
 #ifdef __DCACHE_PRESENT
@@ -328,29 +329,34 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
         initTx->MemoryOrM2MDstAddress = (uint32_t)txData;
         initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
     } else {
-        dummyTxByte = 0xff;
         initTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte;
         initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT;
     }
     initTx->NbData = len;
 
-    if (rxData) {
+#if !defined(STM32G4) && !defined(STM32H7) 
-        /* Flush the D cache for the start and end of the receive buffer as
+    if (dev->bus->dmaRx) {
-         * the cache will be invalidated after the transfer and any valid data
-         * just before/after must be in memory at that point
-         */
-#ifdef __DCACHE_PRESENT
-        // No need to flush/invalidate DTCM memory
-#ifdef STM32H7
-        if ((rxData < &_dmaram_start__) || (rxData >= &_dmaram_end__)) {
-#else
-        // No need to flush DTCM memory
-        if (!IS_DTCM(rxData)) {
 #endif
-            SCB_CleanInvalidateDCache_by_Addr(
+        uint8_t *rxData = segment->rxData;
-                    (uint32_t *)((uint32_t)rxData & ~CACHE_LINE_MASK),
+        LL_DMA_InitTypeDef *initRx = bus->initRx;
-                    (((uint32_t)rxData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK);
+
-        }
+        if (rxData) {
+            /* Flush the D cache for the start and end of the receive buffer as
+             * the cache will be invalidated after the transfer and any valid data
+             * just before/after must be in memory at that point
+             */
+#ifdef __DCACHE_PRESENT
+            // No need to flush/invalidate DTCM memory
+#ifdef STM32H7
+            if ((rxData < &_dmaram_start__) || (rxData >= &_dmaram_end__)) {
+#else
+            // No need to flush DTCM memory
+            if (!IS_DTCM(rxData)) {
+#endif
+                SCB_CleanInvalidateDCache_by_Addr(
+                        (uint32_t *)((uint32_t)rxData & ~CACHE_LINE_MASK),
+                        (((uint32_t)rxData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK);
+            }
 #endif // __DCACHE_PRESENT
         initRx->MemoryOrM2MDstAddress = (uint32_t)rxData;
         initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
@@ -359,6 +365,9 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
         initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT;
     }
     initRx->NbData = len;
+#if !defined(STM32G4) && !defined(STM32H7) 
+    }
+#endif
 }
 
 void spiInternalStartDMA(const extDevice_t *dev)
@@ -371,72 +380,107 @@ void spiInternalStartDMA(const extDevice_t *dev)
     dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
     dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
 
-    // Use the correct callback argument
+#if !defined(STM32G4) && !defined(STM32H7) 
-    dmaRx->userParam = (uint32_t)dev;
+    if (dmaRx) {
+#endif
+        // Use the correct callback argument
+        dmaRx->userParam = (uint32_t)dev;
 
-    // Clear transfer flags
+        // Clear transfer flags
-    DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+        DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
-    DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+        DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
 
 #ifdef STM32G4
-    // Disable channels to enable update
+        // Disable channels to enable update
-    LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream);
+        LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream);
-    LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream);
+        LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream);
 
-    /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
+        /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
-     * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
+         * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
-     */
+         */
-    LL_DMA_EnableIT_TC(dmaRx->dma, dmaRx->stream);
+        LL_DMA_EnableIT_TC(dmaRx->dma, dmaRx->stream);
 
-    // Update channels
+        // Update channels
-    LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
+        LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
-    LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
+        LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
 
-    LL_SPI_EnableDMAReq_RX(dev->bus->busType_u.spi.instance);
+        LL_SPI_EnableDMAReq_RX(dev->bus->busType_u.spi.instance);
 
-    // Enable channels
+        // Enable channels
-    LL_DMA_EnableChannel(dmaTx->dma, dmaTx->stream);
+        LL_DMA_EnableChannel(dmaTx->dma, dmaTx->stream);
-    LL_DMA_EnableChannel(dmaRx->dma, dmaRx->stream);
+        LL_DMA_EnableChannel(dmaRx->dma, dmaRx->stream);
 
-    LL_SPI_EnableDMAReq_TX(dev->bus->busType_u.spi.instance);
+        LL_SPI_EnableDMAReq_TX(dev->bus->busType_u.spi.instance);
 #else
-    DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
+        DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
-    DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
+        DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
 
-    // Disable streams to enable update
+        // Disable streams to enable update
-    LL_DMA_WriteReg(streamRegsTx, CR, 0U);
+        LL_DMA_WriteReg(streamRegsTx, CR, 0U);
-    LL_DMA_WriteReg(streamRegsRx, CR, 0U);
+        LL_DMA_WriteReg(streamRegsRx, CR, 0U);
 
-    /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
+        /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
-     * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
+         * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
-     */
+         */
-    LL_EX_DMA_EnableIT_TC(streamRegsRx);
+        LL_EX_DMA_EnableIT_TC(streamRegsRx);
 
-    // Update streams
+        // Update streams
-    LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
+        LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
-    LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
+        LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
 
-    /* Note from AN4031
+        /* Note from AN4031
-     *
+         *
-     * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
+         * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
-     * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
+         * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
-     * the first required data to the peripheral (in case of memory-to-peripheral transfer).
+         * the first required data to the peripheral (in case of memory-to-peripheral transfer).
-     */
+         */
 
-    // Enable the SPI DMA Tx & Rx requests
+        // Enable the SPI DMA Tx & Rx requests
 #if defined(STM32H7)
-    LL_SPI_SetTransferSize(dev->bus->busType_u.spi.instance, dev->bus->curSegment->len);
+        LL_SPI_SetTransferSize(dev->bus->busType_u.spi.instance, dev->bus->curSegment->len);
-    LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
+        LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
-    LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
+        LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
-    SET_BIT(dev->bus->busType_u.spi.instance->CFG1, SPI_CFG1_RXDMAEN | SPI_CFG1_TXDMAEN);
+        SET_BIT(dev->bus->busType_u.spi.instance->CFG1, SPI_CFG1_RXDMAEN | SPI_CFG1_TXDMAEN);
-    LL_SPI_Enable(dev->bus->busType_u.spi.instance);
+        LL_SPI_Enable(dev->bus->busType_u.spi.instance);
-    LL_SPI_StartMasterTransfer(dev->bus->busType_u.spi.instance);
+        LL_SPI_StartMasterTransfer(dev->bus->busType_u.spi.instance);
 #else
-    // Enable streams
+        // Enable streams
-    LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
+        LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
-    LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
+        LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
 
-    SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN);
+        SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN);
+#endif
+#if !defined(STM32G4) && !defined(STM32H7) 
+    } else {
+        DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
+
+        // Use the correct callback argument
+        dmaTx->userParam = (uint32_t)dev;
+
+        // Clear transfer flags
+        DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+        // Disable streams to enable update
+        LL_DMA_WriteReg(streamRegsTx, CR, 0U);
+
+        LL_EX_DMA_EnableIT_TC(streamRegsTx);
+
+        // Update streams
+        LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
+
+        /* Note from AN4031
+         *
+         * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
+         * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
+         * the first required data to the peripheral (in case of memory-to-peripheral transfer).
+         */
+
+        // Enable the SPI DMA Tx request
+        // Enable streams
+        LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
+
+        SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN);
+    }
 #endif
 #endif
 }
@@ -449,23 +493,48 @@ void spiInternalStopDMA (const extDevice_t *dev)
     dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
     SPI_TypeDef *instance = bus->busType_u.spi.instance;
 
-    // Disable the DMA engine and SPI interface
+#if !defined(STM32G4) && !defined(STM32H7)
+    if (dmaRx) {
+#endif
+        // Disable the DMA engine and SPI interface
 #ifdef STM32G4
-    LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream);
+        LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream);
-    LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream);
+        LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream);
 #else
-    LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream);
+        LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream);
-    LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
+        LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
 #endif
 
-    // Clear transfer flags
+        // Clear transfer flags
-    DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+        DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
 
-    LL_SPI_DisableDMAReq_TX(instance);
+        LL_SPI_DisableDMAReq_TX(instance);
-    LL_SPI_DisableDMAReq_RX(instance);
+        LL_SPI_DisableDMAReq_RX(instance);
 #if defined(STM32H7)
-    LL_SPI_ClearFlag_TXTF(dev->bus->busType_u.spi.instance);
+        LL_SPI_ClearFlag_TXTF(dev->bus->busType_u.spi.instance);
-    LL_SPI_Disable(dev->bus->busType_u.spi.instance);
+        LL_SPI_Disable(dev->bus->busType_u.spi.instance);
+#endif
+#if !defined(STM32G4) && !defined(STM32H7) 
+    } else {
+        SPI_TypeDef *instance = bus->busType_u.spi.instance;
+
+        // Ensure the current transmission is complete
+        while (LL_SPI_IsActiveFlag_BSY(instance));
+
+        // Drain the RX buffer
+        while (LL_SPI_IsActiveFlag_RXNE(instance)) {
+            instance->DR;
+        }
+
+        // Disable the DMA engine and SPI interface
+        LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
+
+        DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+        LL_SPI_DisableDMAReq_TX(instance);
+#endif
+#if !defined(STM32G4) && !defined(STM32H7)
+    }
 #endif
 }
 
@@ -523,6 +592,11 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
 
     // Check that any reads are cache aligned and of multiple cache lines in length
     for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) {
+        // Check there is no receive data as only transmit DMA is available
+        if ((checkSegment->rxData) && (bus->dmaRx == (dmaChannelDescriptor_t *)NULL)) {
+            dmaSafe = false;
+            break;
+        }
 #ifdef STM32H7
         // Check if RX data can be DMAed
         if ((checkSegment->rxData) &&
@@ -599,7 +673,7 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
                     break;
 
                 case BUS_ABORT:
-                    bus->curSegment = (busSegment_t *)NULL;
+                    bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
                     return;
 
                 case BUS_READY:
@@ -619,7 +693,7 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
             spiSequenceStart(nextDev, nextSegments);
         } else {
             // The end of the segment list has been reached, so mark transactions as complete
-            bus->curSegment = (busSegment_t *)NULL;
+            bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
         }
     }
 }

src/main/drivers/bus_spi_stdperiph.c

@@ -105,7 +105,6 @@ void spiInitDevice(SPIDevice device)
 void spiInternalResetDescriptors(busDevice_t *bus)
 {
     DMA_InitTypeDef *initTx = bus->initTx;
-    DMA_InitTypeDef *initRx = bus->initRx;
 
     DMA_StructInit(initTx);
     initTx->DMA_Channel = bus->dmaTx->channel;
@@ -117,14 +116,18 @@ void spiInternalResetDescriptors(busDevice_t *bus)
     initTx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
     initTx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
 
-    DMA_StructInit(initRx);
+    if (bus->dmaRx) {
-    initRx->DMA_Channel = bus->dmaRx->channel;
+        DMA_InitTypeDef *initRx = bus->initRx;
-    initRx->DMA_DIR = DMA_DIR_PeripheralToMemory;
+
-    initRx->DMA_Mode = DMA_Mode_Normal;
+        DMA_StructInit(initRx);
-    initRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR;
+        initRx->DMA_Channel = bus->dmaRx->channel;
-    initRx->DMA_Priority = DMA_Priority_Low;
+        initRx->DMA_DIR = DMA_DIR_PeripheralToMemory;
-    initRx->DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+        initRx->DMA_Mode = DMA_Mode_Normal;
-    initRx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+        initRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR;
+        initRx->DMA_Priority = DMA_Priority_Low;
+        initRx->DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+        initRx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+    }
 }
 
 void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
@@ -159,8 +162,8 @@ static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *
 
 void spiInternalInitStream(const extDevice_t *dev, bool preInit)
 {
-    static uint8_t dummyTxByte = 0xff;
+    STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff;
-    static uint8_t dummyRxByte;
+    STATIC_DMA_DATA_AUTO uint8_t dummyRxByte;
     busDevice_t *bus = dev->bus;
 
     volatile busSegment_t *segment = bus->curSegment;
@@ -174,12 +177,10 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
         }
     }
 
-    uint8_t *txData = segment->txData;
-    uint8_t *rxData = segment->rxData;
     int len = segment->len;
 
+    uint8_t *txData = segment->txData;
     DMA_InitTypeDef *initTx = bus->initTx;
-    DMA_InitTypeDef *initRx = bus->initRx;
 
     if (txData) {
         initTx->DMA_Memory0BaseAddr = (uint32_t)txData;
@@ -191,21 +192,25 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
     }
     initTx->DMA_BufferSize = len;
 
-    if (rxData) {
+    if (dev->bus->dmaRx) {
-        initRx->DMA_Memory0BaseAddr = (uint32_t)rxData;
+        uint8_t *rxData = segment->rxData;
-        initRx->DMA_MemoryInc = DMA_MemoryInc_Enable;
+        DMA_InitTypeDef *initRx = bus->initRx;
-    } else {
+
-        initRx->DMA_Memory0BaseAddr = (uint32_t)&dummyRxByte;
+        if (rxData) {
-        initRx->DMA_MemoryInc = DMA_MemoryInc_Disable;
+            initRx->DMA_Memory0BaseAddr = (uint32_t)rxData;
+            initRx->DMA_MemoryInc = DMA_MemoryInc_Enable;
+        } else {
+            initRx->DMA_Memory0BaseAddr = (uint32_t)&dummyRxByte;
+            initRx->DMA_MemoryInc = DMA_MemoryInc_Disable;
+        }
+        // If possible use 16 bit memory writes to prevent atomic access issues on gyro data
+        if ((initRx->DMA_Memory0BaseAddr & 0x1) || (len & 0x1)) {
+            initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+        } else {
+            initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+        }
+        initRx->DMA_BufferSize = len;
     }
-    // If possible use 16 bit memory writes to prevent atomic access issues on gyro data
-    if ((initRx->DMA_Memory0BaseAddr & 0x1) || (len & 0x1))
-    {
-        initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
-    } else {
-        initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
-    }
-    initRx->DMA_BufferSize = len;
 }
 
 void spiInternalStartDMA(const extDevice_t *dev)
@@ -216,41 +221,70 @@ void spiInternalStartDMA(const extDevice_t *dev)
     dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
     dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
     DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
-    DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
+    if (dmaRx) {
+        DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
 
-    // Use the correct callback argument
+        // Use the correct callback argument
-    dmaRx->userParam = (uint32_t)dev;
+        dmaRx->userParam = (uint32_t)dev;
 
-    // Clear transfer flags
+        // Clear transfer flags
-    DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+        DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
-    DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+        DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
 
-    // Disable streams to enable update
+        // Disable streams to enable update
-    streamRegsTx->CR = 0U;
+        streamRegsTx->CR = 0U;
-    streamRegsRx->CR = 0U;
+        streamRegsRx->CR = 0U;
 
-    /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
+        /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
-     * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
+         * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
-     */
+         */
-    DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE);
+        DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE);
 
-    // Update streams
+        // Update streams
-    DMA_Init(streamRegsTx, dev->bus->initTx);
+        DMA_Init(streamRegsTx, dev->bus->initTx);
-    DMA_Init(streamRegsRx, dev->bus->initRx);
+        DMA_Init(streamRegsRx, dev->bus->initRx);
 
-    /* Note from AN4031
+        /* Note from AN4031
-     *
+         *
-     * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
+         * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
-     * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
+         * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
-     * the first required data to the peripheral (in case of memory-to-peripheral transfer).
+         * the first required data to the peripheral (in case of memory-to-peripheral transfer).
-     */
+         */
 
-    // Enable streams
+        // Enable streams
-    DMA_Cmd(streamRegsTx, ENABLE);
+        DMA_Cmd(streamRegsTx, ENABLE);
-    DMA_Cmd(streamRegsRx, ENABLE);
+        DMA_Cmd(streamRegsRx, ENABLE);
 
-    /* Enable the SPI DMA Tx & Rx requests */
+        /* Enable the SPI DMA Tx & Rx requests */
-    SPI_I2S_DMACmd(dev->bus->busType_u.spi.instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE);
+        SPI_I2S_DMACmd(dev->bus->busType_u.spi.instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE);
+    } else {
+        // Use the correct callback argument
+        dmaTx->userParam = (uint32_t)dev;
+
+        // Clear transfer flags
+        DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+
+        // Disable stream to enable update
+        streamRegsTx->CR = 0U;
+
+        DMA_ITConfig(streamRegsTx, DMA_IT_TC, ENABLE);
+
+        // Update stream
+        DMA_Init(streamRegsTx, dev->bus->initTx);
+
+        /* Note from AN4031
+         *
+         * If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
+         * (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
+         * the first required data to the peripheral (in case of memory-to-peripheral transfer).
+         */
+
+        // Enable stream
+        DMA_Cmd(streamRegsTx, ENABLE);
+
+        /* Enable the SPI DMA Tx request */
+        SPI_I2S_DMACmd(dev->bus->busType_u.spi.instance, SPI_I2S_DMAReq_Tx, ENABLE);
+    }
 }
 
 
@@ -258,15 +292,31 @@ void spiInternalStopDMA (const extDevice_t *dev)
 {
     dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
     dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
-    DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
-    DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
     SPI_TypeDef *instance = dev->bus->busType_u.spi.instance;
+    DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
 
-    // Disable streams
+    if (dmaRx) {
-    streamRegsTx->CR = 0U;
+        DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
-    streamRegsRx->CR = 0U;
 
-    SPI_I2S_DMACmd(instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE);
+        // Disable streams
+        streamRegsTx->CR = 0U;
+        streamRegsRx->CR = 0U;
+
+        SPI_I2S_DMACmd(instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE);
+    } else {
+        // Ensure the current transmission is complete
+        while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY));
+
+        // Drain the RX buffer
+        while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE)) {
+            instance->DR;
+        }
+
+        // Disable stream
+        streamRegsTx->CR = 0U;
+
+        SPI_I2S_DMACmd(instance, SPI_I2S_DMAReq_Tx, DISABLE);
+    }
 }
 
 // DMA transfer setup and start
@@ -307,7 +357,8 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
 
     // Check that any there are no attempts to DMA to/from CCD SRAM
     for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) {
-        if (((checkSegment->rxData) && IS_CCM(checkSegment->rxData)) ||
+        // Check there is no receive data as only transmit DMA is available
+        if (((checkSegment->rxData) && (IS_CCM(checkSegment->rxData) || (bus->dmaRx == (dmaChannelDescriptor_t *)NULL))) ||
             ((checkSegment->txData) && IS_CCM(checkSegment->txData))) {
             dmaSafe = false;
             break;
@@ -348,7 +399,7 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
                     break;
 
                 case BUS_ABORT:
-                    bus->curSegment = (busSegment_t *)NULL;
+                    bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
                     return;
 
                 case BUS_READY:
@@ -368,7 +419,7 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
             spiSequenceStart(nextDev, nextSegments);
         } else {
             // The end of the segment list has been reached, so mark transactions as complete
-            bus->curSegment = (busSegment_t *)NULL;
+            bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
         }
     }
 }

src/main/drivers/display.c

@@ -38,9 +38,10 @@ void displayClearScreen(displayPort_t *instance)
     instance->cursorRow = -1;
 }
 
-void displayDrawScreen(displayPort_t *instance)
+// Return true if screen still being transferred
+bool displayDrawScreen(displayPort_t *instance)
 {
-    instance->vTable->drawScreen(instance);
+    return instance->vTable->drawScreen(instance);
 }
 
 int displayScreenSize(const displayPort_t *instance)
@@ -103,9 +104,9 @@ bool displayIsSynced(const displayPort_t *instance)
     return instance->vTable->isSynced(instance);
 }
 
-void displayHeartbeat(displayPort_t *instance)
+bool displayHeartbeat(displayPort_t *instance)
 {
-    instance->vTable->heartbeat(instance);
+    return instance->vTable->heartbeat(instance);
 }
 
 void displayRedraw(displayPort_t *instance)

src/main/drivers/display.h

@@ -88,7 +88,7 @@ typedef struct displayPortVTable_s {
     int (*grab)(displayPort_t *displayPort);
     int (*release)(displayPort_t *displayPort);
     int (*clearScreen)(displayPort_t *displayPort);
-    int (*drawScreen)(displayPort_t *displayPort);
+    bool (*drawScreen)(displayPort_t *displayPort);
     int (*screenSize)(const displayPort_t *displayPort);
     int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t attr, const char *text);
     int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t attr, uint8_t c);
@@ -113,13 +113,13 @@ void displayRelease(displayPort_t *instance);
 void displayReleaseAll(displayPort_t *instance);
 bool displayIsGrabbed(const displayPort_t *instance);
 void displayClearScreen(displayPort_t *instance);
-void displayDrawScreen(displayPort_t *instance);
+bool displayDrawScreen(displayPort_t *instance);
 int displayScreenSize(const displayPort_t *instance);
 void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y);
 int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *s);
 int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, uint8_t c);
 bool displayIsTransferInProgress(const displayPort_t *instance);
-void displayHeartbeat(displayPort_t *instance);
+bool displayHeartbeat(displayPort_t *instance);
 void displayRedraw(displayPort_t *instance);
 bool displayIsSynced(const displayPort_t *instance);
 uint16_t displayTxBytesFree(const displayPort_t *instance);

src/main/drivers/max7456.c

@@ -115,6 +115,9 @@
 #define VIN_IS_PAL(val)  (!STAT_IS_LOS(val) && STAT_IS_PAL(val))
 #define VIN_IS_NTSC(val)  (!STAT_IS_LOS(val) && STAT_IS_NTSC(val))
 
+// DMM register bits
+#define DMM_AUTO_INC 0x01
+
 // Kluege warning!
 // There are occasions that NTSC is not detected even with !LOS (AB7456 specific?)
 // When this happens, lower 3 bits of STAT register is read as zero.
@@ -182,7 +185,7 @@ typedef struct max7456Layer_s {
     uint8_t buffer[VIDEO_BUFFER_CHARS_PAL];
 } max7456Layer_t;
 
-static DMA_DATA_ZERO_INIT max7456Layer_t displayLayers[MAX7456_SUPPORTED_LAYER_COUNT];
+static max7456Layer_t displayLayers[MAX7456_SUPPORTED_LAYER_COUNT];
 static displayPortLayer_e activeLayer = DISPLAYPORT_LAYER_FOREGROUND;
 
 extDevice_t max7456Device;
@@ -199,11 +202,12 @@ uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
 
 static uint8_t shadowBuffer[VIDEO_BUFFER_CHARS_PAL];
 
-//Max chars to update in one idle
+//Max bytes to update in one call to max7456DrawScreen()
 
-#define MAX_CHARS2UPDATE    100
+#define MAX_BYTES2SEND          250
+#define MAX_BYTES2SEND_POLLED   25
 
-static uint8_t spiBuff[MAX_CHARS2UPDATE*6];
+static DMA_DATA uint8_t spiBuf[MAX_BYTES2SEND];
 
 static uint8_t  videoSignalCfg;
 static uint8_t  videoSignalReg  = OSD_ENABLE; // OSD_ENABLE required to trigger first ReInit
@@ -223,8 +227,6 @@ static displayPortBackground_e deviceBackgroundType = DISPLAY_BACKGROUND_TRANSPA
 static uint8_t previousBlackWhiteRegister = INVALID_PREVIOUS_REGISTER_STATE;
 static uint8_t previousInvertRegister = INVALID_PREVIOUS_REGISTER_STATE;
 
-static void max7456DrawScreenSlow(void);
-
 static uint8_t *getLayerBuffer(displayPortLayer_e layer)
 {
     return displayLayers[layer].buffer;
@@ -279,7 +281,6 @@ static void max7456ClearLayer(displayPortLayer_e layer)
 void max7456ReInit(void)
 {
     uint8_t srdata = 0;
-    static bool firstInit = true;
 
     switch (videoSignalCfg) {
     case VIDEO_SYSTEM_PAL:
@@ -322,13 +323,8 @@ void max7456ReInit(void)
 
     max7456SetRegisterVM1();
 
-    // Clear shadow to force redraw all screen in non-dma mode.
+    // Clear shadow to force redraw all screen
     max7456ClearShadowBuffer();
-
-    if (firstInit) {
-        max7456DrawScreenSlow();
-        firstInit = false;
-    }
 }
 
 void max7456PreInit(const max7456Config_t *max7456Config)
@@ -428,8 +424,8 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP
     // force soft reset on Max7456
     spiWriteReg(dev, MAX7456ADD_VM0, MAX7456_RESET);
 
-    // Wait for 100us before polling for completion of reset
+    // Wait for 200us before polling for completion of reset
-    delayMicroseconds(100);
+    delayMicroseconds(200);
 
     // Wait for reset to complete
     while ((spiReadRegMsk(dev, MAX7456ADD_VM0) & MAX7456_RESET) != 0x00);
@@ -475,9 +471,12 @@ void max7456Brightness(uint8_t black, uint8_t white)
 
     if (reg != previousBlackWhiteRegister) {
         previousBlackWhiteRegister = reg;
-        for (int i = MAX7456ADD_RB0; i <= MAX7456ADD_RB15; i++) {
+        STATIC_DMA_DATA_AUTO uint8_t buf[32];
-            spiWriteReg(dev, i, reg);
+        for (int i = MAX7456ADD_RB0, j = 0; i <= MAX7456ADD_RB15; i++) {
+            buf[j++] = i;
+            buf[j++] = reg;
         }
+        spiReadWriteBuf(dev, buf, NULL, sizeof(buf));
     }
 }
 
@@ -548,7 +547,7 @@ bool max7456BuffersSynced(void)
     return true;
 }
 
-void max7456ReInitIfRequired(bool forceStallCheck)
+bool max7456ReInitIfRequired(bool forceStallCheck)
 {
     static timeMs_t lastSigCheckMs = 0;
     static timeMs_t videoDetectTimeMs = 0;
@@ -603,10 +602,11 @@ void max7456ReInitIfRequired(bool forceStallCheck)
         lastSigCheckMs = nowMs;
     }
 
-    //------------   end of (re)init-------------------------------------
+    return stalled;
 }
 
-void max7456DrawScreen(void)
+// Return true if screen still being transferred
+bool max7456DrawScreen(void)
 {
     static uint16_t pos = 0;
     // This routine doesn't block so need to use static data
@@ -616,23 +616,67 @@ void max7456DrawScreen(void)
     };
 
     if (!fontIsLoading) {
-
-        // (Re)Initialize MAX7456 at startup or stall is detected.
-
-        max7456ReInitIfRequired(false);
-
         uint8_t *buffer = getActiveLayerBuffer();
+        int spiBufIndex = 0;
+        int maxSpiBufStartIndex;
+        bool setAddress = true;
+        bool autoInc = false;
+        int posLimit = pos + (maxScreenSize / 2);
 
-        int buff_len = 0;
+        maxSpiBufStartIndex = spiUseMOSI_DMA(dev) ? MAX_BYTES2SEND : MAX_BYTES2SEND_POLLED;
-        for (int k = 0; k < MAX_CHARS2UPDATE; k++) {
+
+        // Abort for now if the bus is still busy
+        if (spiIsBusy(dev)) {
+            // Not finished yet
+            return true;
+        }
+
+        // Ensure any prior DMA has completed before overwriting the buffer
+        spiWaitClaim(dev);
+
+        // Allow for 8 bytes followed by an ESCAPE and reset of DMM at end of buffer
+        maxSpiBufStartIndex -= 12;
+
+        // Initialise the transfer buffer
+        while ((spiBufIndex < maxSpiBufStartIndex) && (pos < posLimit)) {
             if (buffer[pos] != shadowBuffer[pos]) {
-                spiBuff[buff_len++] = MAX7456ADD_DMAH;
+                if (buffer[pos] == 0xff) {
-                spiBuff[buff_len++] = pos >> 8;
+                    buffer[pos] = ' ';
-                spiBuff[buff_len++] = MAX7456ADD_DMAL;
+                }
-                spiBuff[buff_len++] = pos & 0xff;
+
-                spiBuff[buff_len++] = MAX7456ADD_DMDI;
+                if (setAddress || !autoInc) {
-                spiBuff[buff_len++] = buffer[pos];
+                    if (buffer[pos + 1] != shadowBuffer[pos + 1]) {
+                        // It's worth auto incrementing
+                        spiBuf[spiBufIndex++] = MAX7456ADD_DMM;
+                        spiBuf[spiBufIndex++] = displayMemoryModeReg | DMM_AUTO_INC;
+                        autoInc = true;
+                    } else {
+                        // It's not worth auto incrementing
+                        spiBuf[spiBufIndex++] = MAX7456ADD_DMM;
+                        spiBuf[spiBufIndex++] = displayMemoryModeReg;
+                        autoInc = false;
+                    }
+
+                    spiBuf[spiBufIndex++] = MAX7456ADD_DMAH;
+                    spiBuf[spiBufIndex++] = pos >> 8;
+                    spiBuf[spiBufIndex++] = MAX7456ADD_DMAL;
+                    spiBuf[spiBufIndex++] = pos & 0xff;
+
+                    setAddress = false;
+                }
+
+                spiBuf[spiBufIndex++] = MAX7456ADD_DMDI;
+                spiBuf[spiBufIndex++] = buffer[pos];
+
                 shadowBuffer[pos] = buffer[pos];
+            } else {
+                if (!setAddress) {
+                    setAddress = true;
+                    if (autoInc) {
+                        spiBuf[spiBufIndex++] = MAX7456ADD_DMDI;
+                        spiBuf[spiBufIndex++] = END_STRING;
+                    }
+                }
             }
 
             if (++pos >= maxScreenSize) {
@@ -641,70 +685,34 @@ void max7456DrawScreen(void)
             }
         }
 
-        if (buff_len) {
+        if (autoInc) {
-            segments[0].txData = spiBuff;
+            if (!setAddress) {
-            segments[0].len = buff_len;
+                spiBuf[spiBufIndex++] = MAX7456ADD_DMDI;
+                spiBuf[spiBufIndex++] = END_STRING;
+            }
 
-            // Ensure any prior DMA has completed
+            spiBuf[spiBufIndex++] = MAX7456ADD_DMM;
-            spiWaitClaim(dev);
+            spiBuf[spiBufIndex++] = displayMemoryModeReg;
+        }
+
+        if (spiBufIndex) {
+            segments[0].txData = spiBuf;
+            segments[0].len = spiBufIndex;
 
             spiSequence(dev, &segments[0]);
 
-            // Non-blocking, so transfer still in progress
+            // Non-blocking, so transfer still in progress if using DMA
         }
     }
+
+    return (pos != 0);
 }
 
-static void max7456DrawScreenSlow(void)
-{
-    bool escapeCharFound = false;
-    uint8_t *buffer = getActiveLayerBuffer();
-
-    // Enable auto-increment mode and update every character in the active buffer.
-    uint8_t dma_regs[3];
-
-    dma_regs[0] = displayMemoryModeReg | 1;
-    dma_regs[1] = 0;
-    dma_regs[2] = 0;
-
-    spiWriteRegBuf(dev, MAX7456ADD_DMM, dma_regs, sizeof(dma_regs));
-
-    // The "escape" character 0xFF must be skipped as it causes the MAX7456 to exit auto-increment mode.
-    for (int xx = 0; xx < maxScreenSize; xx++) {
-        if (buffer[xx] == END_STRING) {
-            escapeCharFound = true;
-            spiWriteReg(dev, MAX7456ADD_DMDI, ' ');  // replace the 0xFF character with a blank in the first pass to avoid terminating auto-increment
-        } else {
-            spiWriteReg(dev, MAX7456ADD_DMDI, buffer[xx]);
-        }
-        shadowBuffer[xx] = buffer[xx];
-    }
-
-    spiWriteReg(dev, MAX7456ADD_DMDI, END_STRING);
-
-    // Turn off auto increment
-    spiWriteReg(dev, MAX7456ADD_DMM, displayMemoryModeReg);
-
-    // If we found any of the "escape" character 0xFF, then make a second pass
-    // to update them with direct addressing
-    if (escapeCharFound) {
-        dma_regs[2] = END_STRING;
-        for (int xx = 0; xx < maxScreenSize; xx++) {
-            if (buffer[xx] == END_STRING) {
-                dma_regs[0] = xx >> 8;
-                dma_regs[1] = xx & 0xFF;
-                spiWriteRegBuf(dev, MAX7456ADD_DMAH, dma_regs, sizeof(dma_regs));
-            }
-        }
-    }
-}
-
-
 // should not be used when armed
 void max7456RefreshAll(void)
 {
     max7456ReInitIfRequired(true);
-    max7456DrawScreenSlow();
+    while (max7456DrawScreen());
 }
 
 bool max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)

src/main/drivers/max7456.h

@@ -49,7 +49,8 @@ void    max7456PreInit(const struct max7456Config_s *max7456Config);
 max7456InitStatus_e max7456Init(const struct max7456Config_s *max7456Config, const struct vcdProfile_s *vcdProfile, bool cpuOverclock);
 void    max7456Invert(bool invert);
 void    max7456Brightness(uint8_t black, uint8_t white);
-void    max7456DrawScreen(void);
+bool    max7456ReInitIfRequired(bool forceStallCheck);
+bool     max7456DrawScreen(void);
 bool    max7456WriteNvm(uint8_t char_address, const uint8_t *font_data);
 uint8_t max7456GetRowsCount(void);
 void    max7456Write(uint8_t x, uint8_t y, const char *buff);

src/main/fc/init.c

@@ -915,7 +915,7 @@ void init(void)
 
 #if defined(USE_MAX7456)
         case OSD_DISPLAYPORT_DEVICE_MAX7456:
-            // If there is a max7456 chip for the OSD configured and detectd then use it.
+            // If there is a max7456 chip for the OSD configured and detected then use it.
             if (max7456DisplayPortInit(vcdProfile(), &osdDisplayPort) || device == OSD_DISPLAYPORT_DEVICE_MAX7456) {
                 osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_MAX7456;
                 break;

src/main/fc/tasks.c

@@ -113,6 +113,9 @@
 
 #include "tasks.h"
 
+// Add a margin to the task duration estimation
+#define RX_TASK_MARGIN 5
+
 static void taskMain(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
@@ -159,42 +162,53 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
 }
 #endif
 
-static enum {
+typedef enum {
-    CHECK, PROCESS, MODES, UPDATE
+    RX_STATE_CHECK,
-} rxState = CHECK;
+    RX_STATE_PROCESS,
+    RX_STATE_MODES,
+    RX_STATE_UPDATE,
+    RX_STATE_COUNT
+} rxState_e;
+
+static rxState_e rxState = RX_STATE_CHECK;
 
 bool taskUpdateRxMainInProgress()
 {
-    return (rxState != CHECK);
+    return (rxState != RX_STATE_CHECK);
 }
 
 static void taskUpdateRxMain(timeUs_t currentTimeUs)
 {
-    // Where we are using a state machine call ignoreTaskStateTime() for all states bar one
+    static timeUs_t rxStateDurationUs[RX_STATE_COUNT];
-    if (rxState != MODES) {
+    timeUs_t executeTimeUs;
-        ignoreTaskStateTime();
+    timeUs_t existingDurationUs;
+    rxState_e oldRxState = rxState;
+
+    // Where we are using a state machine call schedulerIgnoreTaskExecRate() for all states bar one
+    if (rxState != RX_STATE_UPDATE) {
+        ignoreTaskExecRate();
     }
 
     switch (rxState) {
     default:
-    case CHECK:
+    case RX_STATE_CHECK:
-        rxState = PROCESS;
+        rxState = RX_STATE_PROCESS;
         break;
 
-    case PROCESS:
+    case RX_STATE_PROCESS:
         if (!processRx(currentTimeUs)) {
-            rxState = CHECK;
+            rxState = RX_STATE_CHECK;
             break;
         }
-        rxState = MODES;
+        rxState = RX_STATE_MODES;
         break;
 
-    case MODES:
+    case RX_STATE_MODES:
         processRxModes(currentTimeUs);
-        rxState = UPDATE;
+        rxState = RX_STATE_UPDATE;
         break;
 
-    case UPDATE:
+    case RX_STATE_UPDATE:
         // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
         updateRcCommands();
         updateArmingStatus();
@@ -204,9 +218,26 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
             sendRcDataToHid();
         }
 #endif
-        rxState = CHECK;
+        rxState = RX_STATE_CHECK;
         break;
     }
+
+    if (getIgnoreTaskExecTime()) {
+        return;
+    }
+
+    executeTimeUs = micros() - currentTimeUs;
+
+    existingDurationUs = rxStateDurationUs[oldRxState] / TASK_STATS_MOVING_SUM_COUNT;
+
+    // If the execution time is higher than expected, double the weight in the moving average
+    if (executeTimeUs > existingDurationUs) {
+        rxStateDurationUs[oldRxState] += executeTimeUs - existingDurationUs;
+    }
+
+    rxStateDurationUs[oldRxState] += executeTimeUs - existingDurationUs;
+
+    schedulerSetNextStateTime((rxStateDurationUs[rxState] / TASK_STATS_MOVING_SUM_COUNT) + RX_TASK_MARGIN);
 }
 
 
@@ -216,7 +247,7 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
     UNUSED(currentTimeUs);
 
     if (sensors(SENSOR_BARO)) {
-        const uint32_t newDeadline = baroUpdate();
+        const uint32_t newDeadline = baroUpdate(currentTimeUs);
         if (newDeadline != 0) {
             rescheduleTask(TASK_SELF, newDeadline);
         }
@@ -370,7 +401,7 @@ void tasksInit(void)
 #endif
 
 #ifdef USE_OSD
-    rescheduleTask(TASK_OSD, TASK_PERIOD_HZ(osdConfig()->task_frequency));
+    rescheduleTask(TASK_OSD, TASK_PERIOD_HZ(osdConfig()->framerate_hz));
     setTaskEnabled(TASK_OSD, featureIsEnabled(FEATURE_OSD) && osdGetDisplayPort(NULL));
 #endif
 
@@ -478,7 +509,7 @@ task_t tasks[TASK_COUNT] = {
 #endif
 
 #ifdef USE_OSD
-    [TASK_OSD] = DEFINE_TASK("OSD", NULL, NULL, osdUpdate, TASK_PERIOD_HZ(OSD_TASK_FREQUENCY_DEFAULT), TASK_PRIORITY_LOW),
+    [TASK_OSD] = DEFINE_TASK("OSD", NULL, osdUpdateCheck, osdUpdate, TASK_PERIOD_HZ(OSD_FRAMERATE_DEFAULT_HZ), TASK_PRIORITY_LOW),
 #endif
 
 #ifdef USE_TELEMETRY

src/main/flight/position.c

@@ -38,6 +38,8 @@
 
 #include "io/gps.h"
 
+#include "scheduler/scheduler.h"
+
 #include "sensors/sensors.h"
 #include "sensors/barometer.h"
 
@@ -94,6 +96,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 
     const uint32_t dTime = currentTimeUs - previousTimeUs;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
+        ignoreTaskExecTime();
         return;
     }
     previousTimeUs = currentTimeUs;
@@ -167,7 +170,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 #endif
     }
 
-	
+
 
     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);

src/main/io/displayport_crsf.c

@@ -65,7 +65,7 @@ static int crsfRelease(displayPort_t *displayPort)
     return crsfClearScreen(displayPort);
 }
 
-static int crsfDrawScreen(displayPort_t *displayPort)
+static bool crsfDrawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     return 0;

src/main/io/displayport_frsky_osd.c

@@ -54,7 +54,7 @@ static int clearScreen(displayPort_t *displayPort)
     return 0;
 }
 
-static int drawScreen(displayPort_t *displayPort)
+static bool drawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     frskyOsdUpdate();

src/main/io/displayport_hott.c

@@ -31,7 +31,7 @@
 
 displayPort_t hottDisplayPort;
 
-static int hottDrawScreen(displayPort_t *displayPort)
+static bool hottDrawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     return 0;

src/main/io/displayport_max7456.c

@@ -72,12 +72,11 @@ static int clearScreen(displayPort_t *displayPort)
     return 0;
 }
 
-static int drawScreen(displayPort_t *displayPort)
+// Return true if screen still being transferred
+static bool drawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    max7456DrawScreen();
+    return max7456DrawScreen();
-
-    return 0;
 }
 
 static int screenSize(const displayPort_t *displayPort)
@@ -130,7 +129,9 @@ static void redraw(displayPort_t *displayPort)
 static int heartbeat(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    return 0;
+
+    // (Re)Initialize MAX7456 at startup or stall is detected.
+    return max7456ReInitIfRequired(false);
 }
 
 static uint32_t txBytesFree(const displayPort_t *displayPort)
@@ -240,7 +241,6 @@ bool max7456DisplayPortInit(const vcdProfile_t *vcdProfile, displayPort_t **disp
     case MAX7456_INIT_OK:
         // MAX7456 configured and detected
         displayInit(&max7456DisplayPort, &max7456VTable, DISPLAYPORT_DEVICE_TYPE_MAX7456);
-        redraw(&max7456DisplayPort);
         *displayPort = &max7456DisplayPort;
 
         break;

src/main/io/displayport_msp.c

@@ -64,7 +64,9 @@ static int heartbeat(displayPort_t *displayPort)
     // heartbeat is used to:
     // a) ensure display is not released by MW OSD software
     // b) prevent OSD Slave boards from displaying a 'disconnected' status.
-    return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+    output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+
+    return 0;
 }
 
 static int grab(displayPort_t *displayPort)
@@ -86,10 +88,12 @@ static int clearScreen(displayPort_t *displayPort)
     return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
 }
 
-static int drawScreen(displayPort_t *displayPort)
+static bool drawScreen(displayPort_t *displayPort)
 {
     uint8_t subcmd[] = { 4 };
-    return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+    output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
+
+    return 0;
 }
 
 static int screenSize(const displayPort_t *displayPort)

src/main/io/displayport_oled.c

@@ -48,7 +48,7 @@ static int oledClearScreen(displayPort_t *displayPort)
     return 0;
 }
 
-static int oledDrawScreen(displayPort_t *displayPort)
+static bool oledDrawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     return 0;

src/main/io/displayport_srxl.c

@@ -38,7 +38,7 @@
 
 displayPort_t srxlDisplayPort;
 
-static int srxlDrawScreen(displayPort_t *displayPort)
+static bool srxlDrawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     return 0;

src/main/io/gps.c

@@ -230,15 +230,19 @@ typedef struct {
 #endif // USE_GPS_UBLOX
 
 typedef enum {
-    GPS_UNKNOWN,
+    GPS_STATE_UNKNOWN,
-    GPS_INITIALIZING,
+    GPS_STATE_INITIALIZING,
-    GPS_INITIALIZED,
+    GPS_STATE_INITIALIZED,
-    GPS_CHANGE_BAUD,
+    GPS_STATE_CHANGE_BAUD,
-    GPS_CONFIGURE,
+    GPS_STATE_CONFIGURE,
-    GPS_RECEIVING_DATA,
+    GPS_STATE_RECEIVING_DATA,
-    GPS_LOST_COMMUNICATION
+    GPS_STATE_LOST_COMMUNICATION,
+    GPS_STATE_COUNT
 } gpsState_e;
 
+// Max time to wait for received data
+#define GPS_MAX_WAIT_DATA_RX 30
+
 gpsData_t gpsData;
 
 
@@ -290,12 +294,12 @@ void gpsInit(void)
     memset(gpsPacketLog, 0x00, sizeof(gpsPacketLog));
 
     // init gpsData structure. if we're not actually enabled, don't bother doing anything else
-    gpsSetState(GPS_UNKNOWN);
+    gpsSetState(GPS_STATE_UNKNOWN);
 
     gpsData.lastMessage = millis();
 
     if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
-        gpsSetState(GPS_INITIALIZED);
+        gpsSetState(GPS_STATE_INITIALIZED);
         return;
     }
 
@@ -326,7 +330,7 @@ void gpsInit(void)
     }
 
     // signal GPS "thread" to initialize when it gets to it
-    gpsSetState(GPS_INITIALIZING);
+    gpsSetState(GPS_STATE_INITIALIZING);
 }
 
 #ifdef USE_GPS_NMEA
@@ -336,7 +340,7 @@ void gpsInitNmea(void)
     uint32_t now;
 #endif
     switch (gpsData.state) {
-        case GPS_INITIALIZING:
+        case GPS_STATE_INITIALIZING:
 #if !defined(GPS_NMEA_TX_ONLY)
            now = millis();
            if (now - gpsData.state_ts < 1000) {
@@ -352,11 +356,11 @@ void gpsInitNmea(void)
                gpsData.state_position++;
            } else {
                // we're now (hopefully) at the correct rate, next state will switch to it
-               gpsSetState(GPS_CHANGE_BAUD);
+               gpsSetState(GPS_STATE_CHANGE_BAUD);
            }
            break;
 #endif
-        case GPS_CHANGE_BAUD:
+        case GPS_STATE_CHANGE_BAUD:
 #if !defined(GPS_NMEA_TX_ONLY)
            now = millis();
            if (now - gpsData.state_ts < 1000) {
@@ -375,7 +379,7 @@ void gpsInitNmea(void)
                serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
            }
 #endif
-               gpsSetState(GPS_RECEIVING_DATA);
+               gpsSetState(GPS_STATE_RECEIVING_DATA);
             break;
     }
 }
@@ -423,7 +427,7 @@ void gpsInitUblox(void)
 
 
     switch (gpsData.state) {
-        case GPS_INITIALIZING:
+        case GPS_STATE_INITIALIZING:
             now = millis();
             if (now - gpsData.state_ts < GPS_BAUDRATE_CHANGE_DELAY)
                 return;
@@ -446,18 +450,18 @@ void gpsInitUblox(void)
                 gpsData.state_position++;
             } else {
                 // we're now (hopefully) at the correct rate, next state will switch to it
-                gpsSetState(GPS_CHANGE_BAUD);
+                gpsSetState(GPS_STATE_CHANGE_BAUD);
             }
             break;
-        case GPS_CHANGE_BAUD:
+        case GPS_STATE_CHANGE_BAUD:
             serialSetBaudRate(gpsPort, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex]);
-            gpsSetState(GPS_CONFIGURE);
+            gpsSetState(GPS_STATE_CONFIGURE);
             break;
-        case GPS_CONFIGURE:
+        case GPS_STATE_CONFIGURE:
 
             // Either use specific config file for GPS or let dynamically upload config
             if ( gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF ) {
-                gpsSetState(GPS_RECEIVING_DATA);
+                gpsSetState(GPS_STATE_RECEIVING_DATA);
                 break;
             }
 
@@ -638,7 +642,7 @@ void gpsInitUblox(void)
 
             if (gpsData.messageState >= GPS_MESSAGE_STATE_INITIALIZED) {
                 // ublox should be initialised, try receiving
-                gpsSetState(GPS_RECEIVING_DATA);
+                gpsSetState(GPS_STATE_RECEIVING_DATA);
             }
             break;
     }
@@ -675,16 +679,18 @@ static void updateGpsIndicator(timeUs_t currentTimeUs)
 
 void gpsUpdate(timeUs_t currentTimeUs)
 {
-    static timeUs_t maxTimeUs = 0;
+    static gpsState_e gpsStateDurationUs[GPS_STATE_COUNT];
-    timeUs_t endTimeUs;
+    timeUs_t executeTimeUs;
+    gpsState_e gpsCurState = gpsData.state;
+
 
     // read out available GPS bytes
     if (gpsPort) {
-        while (serialRxBytesWaiting(gpsPort)) {
+        while (serialRxBytesWaiting(gpsPort) && (cmpTimeUs(micros(), currentTimeUs) < GPS_MAX_WAIT_DATA_RX)) {
             gpsNewData(serialRead(gpsPort));
         }
     } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
-        gpsSetState(GPS_RECEIVING_DATA);
+        gpsSetState(GPS_STATE_RECEIVING_DATA);
         gpsData.lastMessage = millis();
         sensorsSet(SENSOR_GPS);
         onGpsNewData();
@@ -692,17 +698,17 @@ void gpsUpdate(timeUs_t currentTimeUs)
     }
 
     switch (gpsData.state) {
-        case GPS_UNKNOWN:
+        case GPS_STATE_UNKNOWN:
-        case GPS_INITIALIZED:
+        case GPS_STATE_INITIALIZED:
             break;
 
-        case GPS_INITIALIZING:
+        case GPS_STATE_INITIALIZING:
-        case GPS_CHANGE_BAUD:
+        case GPS_STATE_CHANGE_BAUD:
-        case GPS_CONFIGURE:
+        case GPS_STATE_CONFIGURE:
             gpsInitHardware();
             break;
 
-        case GPS_LOST_COMMUNICATION:
+        case GPS_STATE_LOST_COMMUNICATION:
             gpsData.timeouts++;
             if (gpsConfig()->autoBaud) {
                 // try another rate
@@ -712,15 +718,15 @@ void gpsUpdate(timeUs_t currentTimeUs)
             gpsData.lastMessage = millis();
             gpsSol.numSat = 0;
             DISABLE_STATE(GPS_FIX);
-            gpsSetState(GPS_INITIALIZING);
+            gpsSetState(GPS_STATE_INITIALIZING);
             break;
 
-        case GPS_RECEIVING_DATA:
+        case GPS_STATE_RECEIVING_DATA:
             // check for no data/gps timeout/cable disconnection etc
             if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
                 // remove GPS from capability
                 sensorsClear(SENSOR_GPS);
-                gpsSetState(GPS_LOST_COMMUNICATION);
+                gpsSetState(GPS_STATE_LOST_COMMUNICATION);
 #ifdef USE_GPS_UBLOX
             } else {
                 if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON) { // Only if autoconfig is enabled
@@ -744,6 +750,13 @@ void gpsUpdate(timeUs_t currentTimeUs)
             break;
     }
 
+    executeTimeUs = micros() - currentTimeUs;
+
+    if (executeTimeUs > gpsStateDurationUs[gpsCurState]) {
+        gpsStateDurationUs[gpsCurState] = executeTimeUs;
+    }
+    schedulerSetNextStateTime(gpsStateDurationUs[gpsData.state]);
+
     if (sensors(SENSOR_GPS)) {
         updateGpsIndicator(currentTimeUs);
     }
@@ -755,17 +768,6 @@ void gpsUpdate(timeUs_t currentTimeUs)
         updateGPSRescueState();
     }
 #endif
-    // 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 {
-        ignoreTaskShortExecTime();
-        // Decay max time
-        maxTimeUs--;
-    }
 }
 
 static void gpsNewData(uint16_t c)
@@ -810,7 +812,7 @@ bool gpsNewFrame(uint8_t c)
 // Check for healthy communications
 bool gpsIsHealthy()
 {
-    return (gpsData.state == GPS_RECEIVING_DATA);
+    return (gpsData.state == GPS_STATE_RECEIVING_DATA);
 }
 
 /* This is a light implementation of a GPS frame decoding

src/main/io/ledstrip.c

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

src/main/msp/msp.c

@@ -2887,6 +2887,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             return MSP_RESULT_ERROR;
         }
 
+        // This is going to take some time and won't be done where real-time performance is needed so
+        // ignore how long it takes to avoid confusing the scheduler
+        ignoreTaskStateTime();
+
         writeEEPROM();
         readEEPROM();
 

src/main/osd/osd.c

@@ -188,6 +188,14 @@ const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
     OSD_STAT_TOTAL_DIST,
 };
 
+// Group elements in a number of groups to reduce task scheduling overhead
+#define OSD_GROUP_COUNT 20
+// Aim to render a group of elements within a target time
+#define OSD_ELEMENT_RENDER_TARGET 40
+// Allow a margin by which a group render can exceed that of the sum of the elements before declaring insane
+// This will most likely be violated by a USB interrupt whilst using the CLI
+#define OSD_ELEMENT_RENDER_GROUP_MARGIN 5
+
 // Format a float to the specified number of decimal places with optional rounding.
 // OSD symbols can optionally be placed before and after the formatted number (use SYM_NONE for no symbol).
 // The formatString can be used for customized formatting of the integer part. Follow the printf style.
@@ -291,27 +299,6 @@ void osdAnalyzeActiveElements(void)
     osdDrawActiveElementsBackground(osdDisplayPort);
 }
 
-static void osdDrawElements(void)
-{
-    // Hide OSD when OSDSW mode is active
-    if (IS_RC_MODE_ACTIVE(BOXOSD)) {
-        displayClearScreen(osdDisplayPort);
-        return;
-    }
-
-    if (backgroundLayerSupported) {
-        // Background layer is supported, overlay it onto the foreground
-        // so that we only need to draw the active parts of the elements.
-        displayLayerCopy(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND, DISPLAYPORT_LAYER_BACKGROUND);
-    } else {
-        // Background layer not supported, just clear the foreground in preparation
-        // for drawing the elements including their backgrounds.
-        displayClearScreen(osdDisplayPort);
-    }
-
-    osdDrawActiveElements(osdDisplayPort);
-}
-
 const uint16_t osdTimerDefault[OSD_TIMER_COUNT] = {
         OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10),
         OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10)
@@ -382,7 +369,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->camera_frame_height = 11;
 
     osdConfig->stat_show_cell_value = false;
-    osdConfig->task_frequency = OSD_TASK_FREQUENCY_DEFAULT;
+    osdConfig->framerate_hz = OSD_FRAMERATE_DEFAULT_HZ;
     osdConfig->cms_background_type = DISPLAY_BACKGROUND_TRANSPARENT;
 }
 
@@ -457,7 +444,6 @@ static void osdCompleteInitialization(void)
 
     osdElementsInit(backgroundLayerSupported);
     osdAnalyzeActiveElements();
-    displayCommitTransaction(osdDisplayPort);
 
     osdIsReady = true;
 }
@@ -474,10 +460,6 @@ void osdInit(displayPort_t *osdDisplayPortToUse, osdDisplayPortDevice_e displayP
 #ifdef USE_CMS
     cmsDisplayPortRegister(osdDisplayPort);
 #endif
-
-    if (displayCheckReady(osdDisplayPort, true)) {
-        osdCompleteInitialization();
-    }
 }
 
 static void osdResetStats(void)
@@ -936,11 +918,12 @@ static timeDelta_t osdShowArmed(void)
     return ret;
 }
 
-STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
+static bool osdStatsVisible = false;
+static bool osdStatsEnabled = false;
+
+STATIC_UNIT_TESTED void osdDrawStats1(timeUs_t currentTimeUs)
 {
     static timeUs_t lastTimeUs = 0;
-    static bool osdStatsEnabled = false;
-    static bool osdStatsVisible = false;
     static timeUs_t osdStatsRefreshTimeUs;
 
     // detect arm/disarm
@@ -963,7 +946,6 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
         armState = ARMING_FLAG(ARMED);
     }
 
-
     if (ARMING_FLAG(ARMED)) {
         osdUpdateStats();
         timeUs_t deltaT = currentTimeUs - lastTimeUs;
@@ -991,7 +973,10 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
         }
     }
     lastTimeUs = currentTimeUs;
+}
 
+void osdDrawStats2(timeUs_t currentTimeUs)
+{
     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
 
     if (resumeRefreshAt) {
@@ -1000,7 +985,6 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
             if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
                 resumeRefreshAt = currentTimeUs;
             }
-            displayHeartbeat(osdDisplayPort);
             return;
         } else {
             displayClearScreen(osdDisplayPort);
@@ -1009,13 +993,15 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
             stats.armed_time = 0;
         }
     }
-
 #ifdef USE_ESC_SENSOR
     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
         osdEscDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
     }
 #endif
+}
 
+void osdDrawStats3()
+{
 #if defined(USE_ACC)
     if (sensors(SENSOR_ACC)
        && (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
@@ -1027,79 +1013,311 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
         osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
     }
 #endif
-
-#ifdef USE_CMS
-    if (!displayIsGrabbed(osdDisplayPort))
-#endif
-    {
-        osdUpdateAlarms();
-        osdDrawElements();
-        displayHeartbeat(osdDisplayPort);
-    }
-    displayCommitTransaction(osdDisplayPort);
 }
 
-/*
+typedef enum {
- * Called periodically by the scheduler
+    OSD_STATE_INIT,
- */
+    OSD_STATE_IDLE,
+    OSD_STATE_CHECK,
+    OSD_STATE_UPDATE_STATS1,
+    OSD_STATE_UPDATE_STATS2,
+    OSD_STATE_UPDATE_STATS3,
+    OSD_STATE_UPDATE_ALARMS,
+    OSD_STATE_UPDATE_CANVAS,
+    OSD_STATE_UPDATE_ELEMENTS,
+    OSD_STATE_UPDATE_HEARTBEAT,
+    OSD_STATE_COMMIT,
+    OSD_STATE_TRANSFER,
+    OSD_STATE_COUNT
+} osdState_e;
+
+osdState_e osdState = OSD_STATE_INIT;
+
+#define OSD_UPDATE_INTERVAL_US (1000000 / osdConfig()->framerate_hz)
+
+// Called periodically by the scheduler
+bool osdUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
+{
+    UNUSED(currentDeltaTimeUs);
+    static timeUs_t osdUpdateDueUs = 0;
+
+    if (osdState == OSD_STATE_IDLE) {
+        // If the OSD is due a refresh, mark that as being the case
+        if (cmpTimeUs(currentTimeUs, osdUpdateDueUs) > 0) {
+            osdState = OSD_STATE_CHECK;
+
+            // Determine time of next update
+            if (osdUpdateDueUs) {
+                osdUpdateDueUs += OSD_UPDATE_INTERVAL_US;
+            } else {
+                osdUpdateDueUs = currentTimeUs + OSD_UPDATE_INTERVAL_US;
+            }
+        }
+    }
+
+    return (osdState != OSD_STATE_IDLE);
+}
+
+// Called when there is OSD update work to be done
 void osdUpdate(timeUs_t currentTimeUs)
 {
-    static uint32_t counter = 0;
+    static timeUs_t osdStateDurationUs[OSD_STATE_COUNT] = { 0 };
+    static timeUs_t osdElementDurationUs[OSD_ITEM_COUNT] = { 0 };
+    static timeUs_t osdElementGroupMembership[OSD_ITEM_COUNT];
+    static timeUs_t osdElementGroupTargetUs[OSD_GROUP_COUNT] = { 0 };
+    static timeUs_t osdElementGroupDurationUs[OSD_GROUP_COUNT] = { 0 };
+    static uint8_t osdElementGroup;
+    static bool firstPass = true;
+    uint8_t osdCurElementGroup = 0;
+    timeUs_t executeTimeUs;
+    osdState_e osdCurState = osdState;
 
-    if (!osdIsReady) {
+    if (osdState != OSD_STATE_UPDATE_CANVAS) {
+        ignoreTaskExecRate();
+    }
+
+    switch (osdState) {
+    case OSD_STATE_INIT:
         if (!displayCheckReady(osdDisplayPort, false)) {
             // Frsky osd need a display redraw after search for MAX7456 devices
             if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
                 displayRedraw(osdDisplayPort);
             } else {
-                ignoreTaskShortExecTime();
+                ignoreTaskExecTime();
             }
             return;
         }
 
         osdCompleteInitialization();
-    }
+        displayRedraw(osdDisplayPort);
+        osdState = OSD_STATE_COMMIT;
 
-    if (isBeeperOn()) {
+        break;
-        showVisualBeeper = true;
-    }
 
-    // don't touch buffers if DMA transaction is in progress
+    case OSD_STATE_CHECK:
-    if (displayIsTransferInProgress(osdDisplayPort)) {
+        if (isBeeperOn()) {
-        ignoreTaskShortExecTime();
+            showVisualBeeper = true;
-        return;
+        }
-    }
 
-#ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
+        // don't touch buffers if DMA transaction is in progress
-    static uint32_t idlecounter = 0;
+        if (displayIsTransferInProgress(osdDisplayPort)) {
-    if (!ARMING_FLAG(ARMED)) {
+            break;
-        if (idlecounter++ % 4 != 0) {
+        }
-            ignoreTaskShortExecTime();
+
+        osdState = OSD_STATE_UPDATE_HEARTBEAT;
+        break;
+
+    case OSD_STATE_UPDATE_HEARTBEAT:
+        if (displayHeartbeat(osdDisplayPort)) {
+            // Extraordinary action was taken, so return without allowing osdStateDurationUs table to be updated
             return;
         }
-    }
-#endif
 
-    // redraw values in buffer
+        osdState = OSD_STATE_UPDATE_STATS1;
-    if (counter % OSD_DRAW_FREQ_DENOM == 0) {
+        break;
-        osdRefresh(currentTimeUs);
+
+    case OSD_STATE_UPDATE_STATS1:
+        osdDrawStats1(currentTimeUs);
         showVisualBeeper = false;
-    } else {
+
-        bool doDrawScreen = true;
+        osdState = OSD_STATE_UPDATE_STATS2;
-#if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT)
+        break;
-        // For the MSP displayPort device only do the drawScreen once per
+
-        // logical OSD cycle as there is no output buffering needing to be flushed.
+    case OSD_STATE_UPDATE_STATS2:
-        if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_MSP) {
+        osdDrawStats2(currentTimeUs);
-            doDrawScreen = (counter % OSD_DRAW_FREQ_DENOM == 1);
+
-        }
+        osdState = OSD_STATE_UPDATE_STATS3;
+        break;
+
+    case OSD_STATE_UPDATE_STATS3:
+        osdDrawStats3();
+
+#ifdef USE_CMS
+        if (!displayIsGrabbed(osdDisplayPort))
 #endif
-        // Redraw a portion of the chars per idle to spread out the load and SPI bus utilization
+        {
-        if (doDrawScreen) {
+            osdState = OSD_STATE_UPDATE_ALARMS;
-            displayDrawScreen(osdDisplayPort);
+            break;
         }
-        ignoreTaskShortExecTime();
+
+        osdState = OSD_STATE_COMMIT;
+        break;
+
+    case OSD_STATE_UPDATE_ALARMS:
+        osdUpdateAlarms();
+
+        if (resumeRefreshAt) {
+            osdState = OSD_STATE_IDLE;
+        } else {
+            osdState = OSD_STATE_UPDATE_CANVAS;
+        }
+        break;
+
+    case OSD_STATE_UPDATE_CANVAS:
+        // Hide OSD when OSDSW mode is active
+        if (IS_RC_MODE_ACTIVE(BOXOSD)) {
+            displayClearScreen(osdDisplayPort);
+            osdState = OSD_STATE_COMMIT;
+            break;
+        }
+
+        if (backgroundLayerSupported) {
+            // Background layer is supported, overlay it onto the foreground
+            // so that we only need to draw the active parts of the elements.
+            displayLayerCopy(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND, DISPLAYPORT_LAYER_BACKGROUND);
+        } else {
+            // Background layer not supported, just clear the foreground in preparation
+            // for drawing the elements including their backgrounds.
+            displayClearScreen(osdDisplayPort);
+        }
+
+#ifdef USE_GPS
+        static bool lastGpsSensorState;
+        // Handle the case that the GPS_SENSOR may be delayed in activation
+        // or deactivate if communication is lost with the module.
+        const bool currentGpsSensorState = sensors(SENSOR_GPS);
+        if (lastGpsSensorState != currentGpsSensorState) {
+            lastGpsSensorState = currentGpsSensorState;
+            osdAnalyzeActiveElements();
+        }
+#endif // USE_GPS
+
+        osdSyncBlink();
+
+        uint8_t elementGroup;
+        uint8_t activeElements = osdGetActiveElementCount();
+
+        // Reset groupings
+        for (elementGroup = 0; elementGroup < OSD_GROUP_COUNT; elementGroup++) {
+            if (osdElementGroupDurationUs[elementGroup] > (osdElementGroupTargetUs[elementGroup] + OSD_ELEMENT_RENDER_GROUP_MARGIN)) {
+                osdElementGroupDurationUs[elementGroup] = 0;
+            }
+            osdElementGroupTargetUs[elementGroup] = 0;
+        }
+
+        elementGroup = 0;
+
+        // Based on the current element rendering, group to execute in approx 40us
+        for (uint8_t curElement = 0; curElement < activeElements; curElement++) {
+            if ((osdElementGroupTargetUs[elementGroup] == 0) ||
+                ((osdElementGroupTargetUs[elementGroup] + osdElementDurationUs[curElement]) <= OSD_ELEMENT_RENDER_TARGET) ||
+                (elementGroup == (OSD_GROUP_COUNT - 1))) {
+                osdElementGroupTargetUs[elementGroup] += osdElementDurationUs[curElement];
+                // If group membership changes, reset the stats for the group
+                if (osdElementGroupMembership[curElement] != elementGroup) {
+                    osdElementGroupDurationUs[elementGroup] = 0;
+                }
+                osdElementGroupMembership[curElement] = elementGroup;
+            } else {
+                elementGroup++;
+                // Try again for this element
+                curElement--;
+            }
+        }
+
+        // Start with group 0
+        osdElementGroup = 0;
+
+        if (activeElements > 0) {
+            osdState = OSD_STATE_UPDATE_ELEMENTS;
+        } else {
+            osdState = OSD_STATE_COMMIT;
+        }
+        break;
+
+    case OSD_STATE_UPDATE_ELEMENTS:
+        {
+            osdCurElementGroup = osdElementGroup;
+            bool moreElements = true;
+
+            do {
+                timeUs_t startElementTime = micros();
+                uint8_t osdCurElement = osdGetActiveElement();
+
+                // This element should be rendered in the next group
+                if (osdElementGroupMembership[osdCurElement] != osdElementGroup) {
+                    osdElementGroup++;
+                    break;
+                }
+
+                moreElements = osdDrawNextActiveElement(osdDisplayPort, currentTimeUs);
+
+                executeTimeUs = micros() - startElementTime;
+
+                if (executeTimeUs > osdElementDurationUs[osdCurElement]) {
+                    osdElementDurationUs[osdCurElement] = executeTimeUs;
+                }
+            } while (moreElements);
+
+            if (moreElements) {
+                // There are more elements to draw
+                break;
+            }
+
+            osdElementGroup = 0;
+
+            osdState = OSD_STATE_COMMIT;
+        }
+        break;
+
+    case OSD_STATE_COMMIT:
+        displayCommitTransaction(osdDisplayPort);
+
+        if (resumeRefreshAt) {
+            osdState = OSD_STATE_IDLE;
+        } else {
+            osdState = OSD_STATE_TRANSFER;
+        }
+        break;
+
+    case OSD_STATE_TRANSFER:
+        // Wait for any current transfer to complete
+        if (displayIsTransferInProgress(osdDisplayPort)) {
+            break;
+        }
+
+        // Transfer may be broken into many parts
+        if (displayDrawScreen(osdDisplayPort)) {
+            break;
+        }
+
+        firstPass = false;
+        osdState = OSD_STATE_IDLE;
+        break;
+
+    case OSD_STATE_IDLE:
+    default:
+        osdState = OSD_STATE_IDLE;
+        break;
+    }
+
+    executeTimeUs = micros() - currentTimeUs;
+
+
+    // On the first pass no element groups will have been formed, so all elements will have been
+    // rendered which is unrepresentative, so ignore
+    if (!firstPass) {
+        if (osdCurState == OSD_STATE_UPDATE_ELEMENTS) {
+            if (executeTimeUs > osdElementGroupDurationUs[osdCurElementGroup]) {
+                osdElementGroupDurationUs[osdCurElementGroup] = executeTimeUs;
+            }
+        }
+
+        if (executeTimeUs > osdStateDurationUs[osdCurState]) {
+            osdStateDurationUs[osdCurState] = executeTimeUs;
+        }
+    }
+
+    if (osdState == OSD_STATE_UPDATE_ELEMENTS) {
+        schedulerSetNextStateTime(osdElementGroupDurationUs[osdElementGroup]);
+    } else {
+        if (osdState == OSD_STATE_IDLE) {
+            schedulerSetNextStateTime(osdStateDurationUs[OSD_STATE_CHECK]);
+        } else {
+            schedulerSetNextStateTime(osdStateDurationUs[osdState]);
+        }
+        ignoreTaskExecTime();
     }
-    ++counter;
 }
 
 void osdSuppressStats(bool flag)

src/main/osd/osd.h

@@ -49,9 +49,9 @@ extern const char * const osdTimerSourceNames[OSD_NUM_TIMER_TYPES];
 #define OSD_CAMERA_FRAME_MIN_HEIGHT 2
 #define OSD_CAMERA_FRAME_MAX_HEIGHT 16    // Rows supported by MAX7456 (PAL)
 
-#define OSD_TASK_FREQUENCY_MIN 30
+#define OSD_FRAMERATE_MIN_HZ 1
-#define OSD_TASK_FREQUENCY_MAX 300
+#define OSD_FRAMERATE_MAX_HZ 60
-#define OSD_TASK_FREQUENCY_DEFAULT 60
+#define OSD_FRAMERATE_DEFAULT_HZ 12
 
 #define OSD_PROFILE_BITS_POS 11
 #define OSD_PROFILE_MASK    (((1 << OSD_PROFILE_COUNT) - 1) << OSD_PROFILE_BITS_POS)
@@ -300,7 +300,7 @@ typedef struct osdConfig_s {
     uint8_t logo_on_arming_duration;          // display duration in 0.1s units
     uint8_t camera_frame_width;               // The width of the box for the camera frame element
     uint8_t camera_frame_height;              // The height of the box for the camera frame element
-    uint16_t task_frequency;
+    uint16_t framerate_hz;
     uint8_t cms_background_type;              // For supporting devices, determines whether the CMS background is transparent or opaque
     uint8_t stat_show_cell_value;
 } osdConfig_t;
@@ -339,6 +339,7 @@ extern escSensorData_t *osdEscDataCombined;
 #endif
 
 void osdInit(displayPort_t *osdDisplayPort, osdDisplayPortDevice_e displayPortDevice);
+bool osdUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
 void osdUpdate(timeUs_t currentTimeUs);
 
 void osdStatSetState(uint8_t statIndex, bool enabled);

src/main/osd/osd_elements.c

@@ -219,6 +219,7 @@ static uint8_t activeOsdElementArray[OSD_ITEM_COUNT];
 static bool backgroundLayerSupported = false;
 
 // Blink control
+#define OSD_BLINK_FREQUENCY_HZ 2
 static bool blinkState = true;
 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32];
 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
@@ -1803,37 +1804,42 @@ static void osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_
     }
 }
 
-#define OSD_BLINK_FREQUENCY_HZ 2.5f
+static uint8_t activeElement = 0;
 
-void osdDrawActiveElements(displayPort_t *osdDisplayPort)
+uint8_t osdGetActiveElement()
 {
-    static unsigned blinkLoopCounter = 0;
+    return activeElement;
+}
 
-#ifdef USE_GPS
+uint8_t osdGetActiveElementCount()
-    static bool lastGpsSensorState;
+{
-    // Handle the case that the GPS_SENSOR may be delayed in activation
+    return activeOsdElementCount;
-    // or deactivate if communication is lost with the module.
+}
-    const bool currentGpsSensorState = sensors(SENSOR_GPS);
-    if (lastGpsSensorState != currentGpsSensorState) {
-        lastGpsSensorState = currentGpsSensorState;
-        osdAnalyzeActiveElements();
-    }
-#endif // USE_GPS
 
-    // synchronize the blinking with the OSD task loop
+// Return true if there are more elements to draw
-    if (++blinkLoopCounter >= lrintf(osdConfig()->task_frequency / OSD_DRAW_FREQ_DENOM / (OSD_BLINK_FREQUENCY_HZ * 2))) {
+bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs)
-        blinkState = !blinkState;
+{
-        blinkLoopCounter = 0;
+    UNUSED(currentTimeUs);
+    bool retval = true;
+
+    if (activeElement >= activeOsdElementCount) {
+        return false;
     }
 
-    for (unsigned i = 0; i < activeOsdElementCount; i++) {
+    if (!backgroundLayerSupported) {
-        if (!backgroundLayerSupported) {
+        // If the background layer isn't supported then we
-            // If the background layer isn't supported then we
+        // have to draw the element's static layer as well.
-            // have to draw the element's static layer as well.
+        osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[activeElement]);
-            osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[i]);
-        }
-        osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[i]);
     }
+
+    osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[activeElement]);
+
+    if (++activeElement >= activeOsdElementCount) {
+        activeElement = 0;
+        retval = false;
+    }
+
+    return retval;
 }
 
 void osdDrawActiveElementsBackground(displayPort_t *osdDisplayPort)
@@ -1854,6 +1860,17 @@ void osdElementsInit(bool backgroundLayerFlag)
     activeOsdElementCount = 0;
 }
 
+void osdSyncBlink() {
+    static int blinkCount = 0;
+
+    // If the OSD blink is due a transition, do so
+    // Task runs at osdConfig()->framerate_hz Hz, so this will cycle at 2Hz
+    if (++blinkCount == ((osdConfig()->framerate_hz / OSD_BLINK_FREQUENCY_HZ) / 2)) {
+        blinkCount = 0;
+        blinkState = !blinkState;
+    }
+}
+
 void osdResetAlarms(void)
 {
     memset(blinkBits, 0, sizeof(blinkBits));

src/main/osd/osd_elements.h

@@ -55,9 +55,12 @@ int32_t osdGetSpeedToSelectedUnit(int32_t value);
 char osdGetSpeedToSelectedUnitSymbol(void);
 char osdGetTemperatureSymbolForSelectedUnit(void);
 void osdAddActiveElements(void);
-void osdDrawActiveElements(displayPort_t *osdDisplayPort);
+uint8_t osdGetActiveElement();
+uint8_t osdGetActiveElementCount();
+bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs);
 void osdDrawActiveElementsBackground(displayPort_t *osdDisplayPort);
 void osdElementsInit(bool backgroundLayerFlag);
+void osdSyncBlink();
 void osdResetAlarms(void);
 void osdUpdateAlarms(void);
 bool osdElementsNeedAccelerometer(void);

src/main/pg/sdcard.c

@@ -56,6 +56,10 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
     config->mode = SDCARD_MODE_NONE;
 #endif
 
+#if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too
+    config->mode = SDCARD_MODE_SDIO;
+#endif
+
 #ifdef USE_SDCARD_SPI
     // These settings do not work for Unified Targets
     // They are only left in place to support legacy targets

src/main/scheduler/scheduler.c

@@ -58,15 +58,18 @@
 // 2 - Amount task is late in 10th of a us
 // 3 - Gyro lock skew in clock cycles
 
+// DEBUG_TIMING_ACCURACY, requires USE_LATE_TASK_STATISTICS to be defined
+// 0 - % CPU busy
+// 1 - Tasks late in last second
+// 2 - Total lateness in last second in 10ths us
+// 3 - Total tasks run in last second
+
 extern task_t tasks[];
 
 static FAST_DATA_ZERO_INIT task_t *currentTask = NULL;
 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;
@@ -79,9 +82,6 @@ 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;
-
 FAST_DATA_ZERO_INIT uint16_t averageSystemLoadPercent = 0;
 
 static FAST_DATA_ZERO_INIT int taskQueuePos = 0;
@@ -94,6 +94,13 @@ static FAST_DATA_ZERO_INIT bool gyroEnabled;
 static int32_t desiredPeriodCycles;
 static uint32_t lastTargetCycles;
 
+#if defined(USE_LATE_TASK_STATISTICS)
+static int16_t lateTaskCount = 0;
+static uint32_t lateTaskTotal = 0;
+static int16_t taskCount = 0;
+static uint32_t nextTimingCycles;
+#endif
+
 // 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
@@ -160,16 +167,20 @@ FAST_CODE task_t *queueNext(void)
     return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
 }
 
+static timeUs_t taskTotalExecutionTime = 0;
+
 void taskSystemLoad(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
+    static timeUs_t lastExecutedAtUs;
+    timeDelta_t deltaTime = cmpTimeUs(currentTimeUs, lastExecutedAtUs);
 
     // Calculate system load
-    if (totalWaitingTasksSamples > 0) {
+    if (deltaTime) {
-        averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples;
+        averageSystemLoadPercent = 100 * taskTotalExecutionTime / deltaTime;
-        totalWaitingTasksSamples = 0;
+        taskTotalExecutionTime = 0;
-        totalWaitingTasks = 0;
+        lastExecutedAtUs = currentTimeUs;
     }
+
 #if defined(SIMULATOR_BUILD)
     averageSystemLoadPercent = 0;
 #endif
@@ -197,7 +208,7 @@ void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
     taskInfo->subTaskName = getTask(taskId)->subTaskName;
     taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs;
     taskInfo->totalExecutionTimeUs = getTask(taskId)->totalExecutionTimeUs;
-    taskInfo->averageExecutionTimeUs = getTask(taskId)->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+    taskInfo->averageExecutionTimeUs = getTask(taskId)->anticipatedExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
     taskInfo->averageDeltaTimeUs = getTask(taskId)->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
     taskInfo->latestDeltaTimeUs = getTask(taskId)->taskLatestDeltaTimeUs;
     taskInfo->movingAverageCycleTimeUs = getTask(taskId)->movingAverageCycleTimeUs;
@@ -210,13 +221,16 @@ void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo)
 
 void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
 {
+    task_t *task;
+
     if (taskId == TASK_SELF) {
-        task_t *task = currentTask;
+        task = currentTask;
-        task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
     } else if (taskId < TASK_COUNT) {
-        task_t *task = getTask(taskId);
+        task = getTask(taskId);
-        task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
+    } else {
+        return;
     }
+    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) {
@@ -247,29 +261,39 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId)
     }
 }
 
-// Called by tasks executing what are known to be short states. Tasks should call this routine in all states
+// Called by tasks executing what are known to be short states
-// except the one which takes the longest to execute.
 void ignoreTaskStateTime()
 {
     ignoreCurrentTaskExecRate = true;
     ignoreCurrentTaskExecTime = true;
 }
 
+// Called by tasks with state machines to only count one state as determining rate
+void ignoreTaskExecRate()
+{
+    ignoreCurrentTaskExecRate = true;
+}
+
 // Called by tasks without state machines executing in what is known to be a shorter time than peak
-void ignoreTaskShortExecTime()
+void ignoreTaskExecTime()
 {
     ignoreCurrentTaskExecTime = true;
 }
 
+bool getIgnoreTaskExecTime()
+{
+    return ignoreCurrentTaskExecTime;
+}
+
 void schedulerResetTaskStatistics(taskId_e taskId)
 {
     if (taskId == TASK_SELF) {
-        currentTask->movingSumExecutionTimeUs = 0;
+        currentTask->anticipatedExecutionTimeUs = 0;
         currentTask->movingSumDeltaTimeUs = 0;
         currentTask->totalExecutionTimeUs = 0;
         currentTask->maxExecutionTimeUs = 0;
     } else if (taskId < TASK_COUNT) {
-        getTask(taskId)->movingSumExecutionTimeUs = 0;
+        getTask(taskId)->anticipatedExecutionTimeUs = 0;
         getTask(taskId)->movingSumDeltaTimeUs = 0;
         getTask(taskId)->totalExecutionTimeUs = 0;
         getTask(taskId)->maxExecutionTimeUs = 0;
@@ -315,6 +339,10 @@ void schedulerInit(void)
     desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs);
 
     lastTargetCycles = getCycleCounter();
+
+#if defined(USE_LATE_TASK_STATISTICS)
+    nextTimingCycles = lastTargetCycles;
+#endif
 }
 
 void schedulerOptimizeRate(bool optimizeRate)
@@ -330,6 +358,13 @@ inline static timeUs_t getPeriodCalculationBasis(const task_t* task)
     return task->lastExecutedAtUs;
 }
 
+static timeDelta_t taskNextStateTime;
+
+FAST_CODE void schedulerSetNextStateTime(timeDelta_t nextStateTime)
+{
+    taskNextStateTime = nextStateTime;
+}
+
 FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs)
 {
     timeUs_t taskExecutionTimeUs = 0;
@@ -338,28 +373,37 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
         currentTask = selectedTask;
         ignoreCurrentTaskExecRate = false;
         ignoreCurrentTaskExecTime = false;
+        taskNextStateTime = -1;
         float period = currentTimeUs - selectedTask->lastExecutedAtUs;
         selectedTask->lastExecutedAtUs = currentTimeUs;
-        selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriodUs) * selectedTask->desiredPeriodUs;
+        selectedTask->lastDesiredAt += selectedTask->desiredPeriodUs;
         selectedTask->dynamicPriority = 0;
 
         // Execute task
         const timeUs_t currentTimeBeforeTaskCallUs = micros();
         selectedTask->taskFunc(currentTimeBeforeTaskCallUs);
         taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs;
+        taskTotalExecutionTime += taskExecutionTimeUs;
         if (!ignoreCurrentTaskExecRate) {
             // Record task execution rate and max execution time
             selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs);
+            selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
             selectedTask->lastStatsAtUs = currentTimeUs;
+        }
+
+        // Update estimate of expected task duration
+        if (taskNextStateTime != -1) {
+            selectedTask->anticipatedExecutionTimeUs = taskNextStateTime * TASK_STATS_MOVING_SUM_COUNT;
+        } else if (!ignoreCurrentTaskExecTime) {
+            selectedTask->anticipatedExecutionTimeUs += taskExecutionTimeUs - selectedTask->anticipatedExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+        }
+
+        if (!ignoreCurrentTaskExecTime) {
             selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs);
         }
-        if (!ignoreCurrentTaskExecTime) {
+
-            // Update estimate of expected task duration
+        selectedTask->totalExecutionTimeUs += taskExecutionTimeUs;   // time consumed by scheduler + task
-            selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+        selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs);
-            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++;
 #endif
@@ -371,26 +415,24 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi
 #if defined(UNIT_TEST)
 task_t *unittest_scheduler_selectedTask;
 uint8_t unittest_scheduler_selectedTaskDynamicPriority;
-uint16_t unittest_scheduler_waitingTasks;
 
-static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynamicPriority, uint16_t waitingTasks)
+static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynamicPriority)
 {
     unittest_scheduler_selectedTask = selectedTask;
     unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority;
-    unittest_scheduler_waitingTasks = waitingTasks;
 }
 #endif
 
 FAST_CODE void scheduler(void)
 {
-    // Cache currentTime
+#if !defined(UNIT_TEST)
     const timeUs_t schedulerStartTimeUs = micros();
+#endif
     timeUs_t currentTimeUs;
     uint32_t nowCycles;
     timeUs_t taskExecutionTimeUs = 0;
     task_t *selectedTask = NULL;
     uint16_t selectedTaskDynamicPriority = 0;
-    uint16_t waitingTasks = 0;
     uint32_t nextTargetCycles = 0;
     int32_t schedLoopRemainingCycles;
 
@@ -422,9 +464,6 @@ FAST_CODE void scheduler(void)
 
         // 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;
             }
@@ -433,10 +472,8 @@ FAST_CODE void scheduler(void)
                 nowCycles = getCycleCounter();
                 schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
             }
+            DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastTargetCycles)));
 #endif
-            DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastRealtimeStartCycles)));
-            lastRealtimeStartCycles = nowCycles;
-
             currentTimeUs = clockCyclesToMicros(nowCycles);
             taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs);
 
@@ -447,6 +484,25 @@ FAST_CODE void scheduler(void)
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
             }
 
+#if defined(USE_LATE_TASK_STATISTICS)
+            // % CPU busy
+            DEBUG_SET(DEBUG_TIMING_ACCURACY, 0, getAverageSystemLoadPercent());
+
+            if (cmpTimeCycles(nextTimingCycles, nowCycles) < 0) {
+                nextTimingCycles += clockMicrosToCycles(1000000);
+
+                // Tasks late in last second
+                DEBUG_SET(DEBUG_TIMING_ACCURACY, 1, lateTaskCount);
+                // Total lateness in last second in us
+                DEBUG_SET(DEBUG_TIMING_ACCURACY, 2, clockCyclesTo10thMicros(lateTaskTotal));
+                // Total tasks run in last second
+                DEBUG_SET(DEBUG_TIMING_ACCURACY, 3, taskCount);
+
+                lateTaskCount = 0;
+                lateTaskTotal = 0;
+                taskCount = 0;
+            }
+#endif
             lastTargetCycles = nextTargetCycles;
 
 #ifdef USE_GYRO_EXTI
@@ -502,7 +558,6 @@ FAST_CODE void scheduler(void)
     nowCycles = getCycleCounter();
     schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
 
-
     if (!gyroEnabled || (schedLoopRemainingCycles > (int32_t)clockMicrosToCycles(CHECK_GUARD_MARGIN_US))) {
         currentTimeUs = micros();
 
@@ -515,10 +570,11 @@ FAST_CODE void scheduler(void)
                     if (task->dynamicPriority > 0) {
                         task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->desiredPeriodUs);
                         task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
-                        waitingTasks++;
                     } else if (task->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
                         const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
+#if !defined(UNIT_TEST)
                         DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs);
+#endif
                         checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
                         checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
                         checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs;   // time consumed by scheduler + task
@@ -526,7 +582,6 @@ FAST_CODE void scheduler(void)
                         task->lastSignaledAtUs = currentTimeUs;
                         task->taskAgeCycles = 1;
                         task->dynamicPriority = 1 + task->staticPriority;
-                        waitingTasks++;
                     } else {
                         task->taskAgeCycles = 0;
                     }
@@ -536,7 +591,6 @@ FAST_CODE void scheduler(void)
                     task->taskAgeCycles = (cmpTimeUs(currentTimeUs, getPeriodCalculationBasis(task)) / task->desiredPeriodUs);
                     if (task->taskAgeCycles > 0) {
                         task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
-                        waitingTasks++;
                     }
                 }
 
@@ -547,11 +601,8 @@ FAST_CODE void scheduler(void)
             }
         }
 
-        totalWaitingTasksSamples++;
-        totalWaitingTasks += waitingTasks;
-
         if (selectedTask) {
-            timeDelta_t taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
+            timeDelta_t taskRequiredTimeUs = selectedTask->anticipatedExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
 #if defined(USE_LATE_TASK_STATISTICS)
             selectedTask->execTime = taskRequiredTimeUs;
 #endif
@@ -564,11 +615,24 @@ FAST_CODE void scheduler(void)
             taskRequiredTimeCycles += taskGuardCycles;
 
             if (!gyroEnabled || (taskRequiredTimeCycles < schedLoopRemainingCycles)) {
+                uint32_t antipatedEndCycles = nowCycles + taskRequiredTimeCycles;
                 taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
                 nowCycles = getCycleCounter();
-                schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles);
+                int32_t cyclesOverdue = cmpTimeCycles(nowCycles, antipatedEndCycles);
 
-                if (schedLoopRemainingCycles < taskGuardMinCycles) {
+#if defined(USE_LATE_TASK_STATISTICS)
+                if (cyclesOverdue > 0) {
+                    if ((currentTask - tasks) != TASK_SERIAL) {
+                        DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 1, currentTask - tasks);
+                        DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 2, clockCyclesTo10thMicros(cyclesOverdue));
+                        currentTask->lateCount++;
+                        lateTaskCount++;
+                        lateTaskTotal += cyclesOverdue;
+                    }
+                }
+#endif  // USE_LATE_TASK_STATISTICS
+
+                if ((cyclesOverdue > 0) || (-cyclesOverdue < taskGuardMinCycles)) {
                     if (taskGuardCycles < taskGuardMaxCycles) {
                         taskGuardCycles += taskGuardDeltaUpCycles;
                     }
@@ -576,24 +640,22 @@ FAST_CODE void scheduler(void)
                     taskGuardCycles -= taskGuardDeltaDownCycles;
                 }
 #if defined(USE_LATE_TASK_STATISTICS)
-                if (schedLoopRemainingCycles < DEBUG_SCHEDULER_DETERMINISM_THRESHOLD) {
+                taskCount++;
-                    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;
+                selectedTask->anticipatedExecutionTimeUs *= TASK_AGE_EXPEDITE_SCALE;
             }
         }
     }
 
+#if !defined(UNIT_TEST)
     DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTimeUs); // time spent in scheduler
+#endif
 
 #if defined(UNIT_TEST)
-    readSchedulerLocals(selectedTask, selectedTaskDynamicPriority, waitingTasks);
+    readSchedulerLocals(selectedTask, selectedTaskDynamicPriority);
 #endif
 }
 
@@ -606,3 +668,8 @@ uint16_t getAverageSystemLoadPercent(void)
 {
     return averageSystemLoadPercent;
 }
+
+float schedulerGetCycleTimeMultiplier(void)
+{
+    return (float)clockMicrosToCycles(getTask(TASK_GYRO)->desiredPeriodUs) / desiredPeriodCycles;
+}

src/main/scheduler/scheduler.h

@@ -36,7 +36,7 @@
 #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_MIN_US        3   // 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
@@ -196,7 +196,7 @@ typedef struct {
 
     // Statistics
     float    movingAverageCycleTimeUs;
-    timeUs_t movingSumExecutionTimeUs;  // moving sum over 32 samples
+    timeUs_t anticipatedExecutionTimeUs;  // moving sum over 32 samples
     timeUs_t movingSumDeltaTimeUs;      // moving sum over 32 samples
     timeUs_t maxExecutionTimeUs;
     timeUs_t totalExecutionTimeUs;      // total time consumed by task since boot
@@ -214,11 +214,15 @@ void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs);
 void setTaskEnabled(taskId_e taskId, bool newEnabledState);
 timeDelta_t getTaskDeltaTimeUs(taskId_e taskId);
 void ignoreTaskStateTime();
-void ignoreTaskShortExecTime();
+void ignoreTaskExecRate();
+void ignoreTaskExecTime();
+bool getIgnoreTaskExecTime();
 void schedulerResetTaskStatistics(taskId_e taskId);
 void schedulerResetTaskMaxExecutionTime(taskId_e taskId);
 void schedulerResetCheckFunctionMaxExecutionTime(void);
 
+void schedulerSetNextStateTime(timeDelta_t nextStateTime);
+
 void schedulerInit(void);
 void scheduler(void);
 timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs);
@@ -226,3 +230,4 @@ void taskSystemLoad(timeUs_t currentTimeUs);
 void schedulerOptimizeRate(bool optimizeRate);
 void schedulerEnableGyro(void);
 uint16_t getAverageSystemLoadPercent(void);
+float schedulerGetCycleTimeMultiplier(void);

src/main/sensors/barometer.c

@@ -364,12 +364,13 @@ static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPre
 }
 
 typedef enum {
-    BAROMETER_NEEDS_TEMPERATURE_READ = 0,
+    BARO_STATE_TEMPERATURE_READ = 0,
-    BAROMETER_NEEDS_TEMPERATURE_SAMPLE,
+    BARO_STATE_TEMPERATURE_SAMPLE,
-    BAROMETER_NEEDS_PRESSURE_START,
+    BARO_STATE_PRESSURE_START,
-    BAROMETER_NEEDS_PRESSURE_READ,
+    BARO_STATE_PRESSURE_READ,
-    BAROMETER_NEEDS_PRESSURE_SAMPLE,
+    BARO_STATE_PRESSURE_SAMPLE,
-    BAROMETER_NEEDS_TEMPERATURE_START
+    BARO_STATE_TEMPERATURE_START,
+    BARO_STATE_COUNT
 } barometerState_e;
 
 
@@ -377,17 +378,19 @@ bool isBaroReady(void) {
     return baroReady;
 }
 
-uint32_t baroUpdate(void)
+uint32_t baroUpdate(timeUs_t currentTimeUs)
 {
-    static barometerState_e state = BAROMETER_NEEDS_PRESSURE_START;
+    static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
+    static barometerState_e state = BARO_STATE_PRESSURE_START;
+    barometerState_e oldState = state;
+    timeUs_t executeTimeUs;
     timeUs_t sleepTime = 1000; // Wait 1ms between states
 
     DEBUG_SET(DEBUG_BARO, 0, state);
 
-    // Tell the scheduler to ignore how long this task takes unless the pressure is being read
+    // Where we are using a state machine call ignoreTaskExecRate() for all states bar one
-    // as that takes the longest
+    if (state != BARO_STATE_TEMPERATURE_START) {
-    if (state != BAROMETER_NEEDS_PRESSURE_READ) {
+        ignoreTaskExecRate();
-        ignoreTaskStateTime();
     }
 
     if (busBusy(&baro.dev.dev, NULL)) {
@@ -398,39 +401,39 @@ uint32_t baroUpdate(void)
 
     switch (state) {
         default:
-        case BAROMETER_NEEDS_TEMPERATURE_START:
+        case BARO_STATE_TEMPERATURE_START:
             baro.dev.start_ut(&baro.dev);
-            state = BAROMETER_NEEDS_TEMPERATURE_READ;
+            state = BARO_STATE_TEMPERATURE_READ;
             sleepTime = baro.dev.ut_delay;
             break;
 
-        case BAROMETER_NEEDS_TEMPERATURE_READ:
+        case BARO_STATE_TEMPERATURE_READ:
             if (baro.dev.read_ut(&baro.dev)) {
-                state = BAROMETER_NEEDS_TEMPERATURE_SAMPLE;
+                state = BARO_STATE_TEMPERATURE_SAMPLE;
             }
-        break;
+            break;
 
-        case BAROMETER_NEEDS_TEMPERATURE_SAMPLE:
+        case BARO_STATE_TEMPERATURE_SAMPLE:
             if (baro.dev.get_ut(&baro.dev)) {
-                state = BAROMETER_NEEDS_PRESSURE_START;
+                state = BARO_STATE_PRESSURE_START;
             }
-        break;
+            break;
 
-        case BAROMETER_NEEDS_PRESSURE_START:
+        case BARO_STATE_PRESSURE_START:
             baro.dev.start_up(&baro.dev);
-            state = BAROMETER_NEEDS_PRESSURE_READ;
+            state = BARO_STATE_PRESSURE_READ;
             sleepTime = baro.dev.up_delay;
-        break;
+            break;
 
-        case BAROMETER_NEEDS_PRESSURE_READ:
+        case BARO_STATE_PRESSURE_READ:
             if (baro.dev.read_up(&baro.dev)) {
-                state = BAROMETER_NEEDS_PRESSURE_SAMPLE;
+                state = BARO_STATE_PRESSURE_SAMPLE;
             } else {
                 ignoreTaskStateTime();
             }
-        break;
+            break;
 
-        case BAROMETER_NEEDS_PRESSURE_SAMPLE:
+        case BARO_STATE_PRESSURE_SAMPLE:
             if (!baro.dev.get_up(&baro.dev)) {
                 break;
             }
@@ -440,9 +443,9 @@ uint32_t baroUpdate(void)
             baro.baroTemperature = baroTemperature;
             baroPressureSum = recalculateBarometerTotal(baroPressureSum, baroPressure);
             if (baro.dev.combined_read) {
-                state = BAROMETER_NEEDS_PRESSURE_START;
+                state = BARO_STATE_PRESSURE_START;
             } else {
-                state = BAROMETER_NEEDS_TEMPERATURE_START;
+                state = BARO_STATE_TEMPERATURE_START;
             }
 
             DEBUG_SET(DEBUG_BARO, 1, baroTemperature);
@@ -453,6 +456,14 @@ uint32_t baroUpdate(void)
             break;
     }
 
+    executeTimeUs = micros() - currentTimeUs;
+
+    if (executeTimeUs > baroStateDurationUs[oldState]) {
+        baroStateDurationUs[oldState] = executeTimeUs;
+    }
+
+    schedulerSetNextStateTime(baroStateDurationUs[state]);
+
     return sleepTime;
 }
 

src/main/sensors/barometer.h

@@ -67,7 +67,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse);
 bool baroIsCalibrationComplete(void);
 void baroStartCalibration(void);
 void baroSetGroundLevel(void);
-uint32_t baroUpdate(void);
+uint32_t baroUpdate(timeUs_t currentTimeUs);
 bool isBaroReady(void);
 int32_t baroCalculateAltitude(void);
 void performBaroCalibrationCycle(void);

src/main/target/common_pre.h

@@ -45,6 +45,7 @@
 #ifdef STM32F4
 #if defined(STM32F40_41xxx)
 #define USE_FAST_DATA
+#define USE_LATE_TASK_STATISTICS
 #endif
 #define USE_DSHOT
 #define USE_DSHOT_BITBANG
@@ -66,7 +67,6 @@
 #if defined(STM32F40_41xxx) || defined(STM32F411xE)
 #define USE_OVERCLOCK
 #endif
-
 #endif // STM32F4
 
 #ifdef STM32F7
@@ -164,9 +164,16 @@
 #else
 #define FAST_CODE                   __attribute__((section(".tcm_code")))
 #endif
+// Handle case where we'd prefer code to be in ITCM, but it won't fit on the F745
+#ifdef STM32F745xx
+#define FAST_CODE_PREF
+#else
+#define FAST_CODE_PREF                  __attribute__((section(".tcm_code")))
+#endif
 #define FAST_CODE_NOINLINE          NOINLINE
 #else
 #define FAST_CODE
+#define FAST_CODE_PREF
 #define FAST_CODE_NOINLINE
 #endif // USE_ITCM_RAM
 

src/main/vcpf4/usbd_desc.c

@@ -224,8 +224,8 @@ uint8_t *  USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length)
     (void)speed;
 #ifdef USE_USB_CDC_HID
     if (usbDevConfig()->type == COMPOSITE) {
-	    *length = sizeof(USBD_DeviceDesc_Composite);
+        *length = sizeof(USBD_DeviceDesc_Composite);
-	    return USBD_DeviceDesc_Composite;
+        return USBD_DeviceDesc_Composite;
     }
 #endif
     *length = sizeof(USBD_DeviceDesc);

src/test/unit/flight_imu_unittest.cc

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

src/test/unit/ledstrip_unittest.cc

@@ -397,5 +397,5 @@ void ws2811LedStripEnable(void) { }
 
 void setUsedLedCount(unsigned) { }
 void pinioBoxTaskControl(void) {}
-void ignoreTaskShortExecTime(void) {}
+void ignoreTaskExecTime(void) {}
 }

src/test/unit/link_quality_unittest.cc

@@ -89,7 +89,7 @@ extern "C" {
 
     timeUs_t simulationTime = 0;
 
-    void osdRefresh(timeUs_t currentTimeUs);
+    void osdUpdate(timeUs_t currentTimeUs);
     uint16_t updateLinkQualitySamples(uint16_t value);
 #define LINK_QUALITY_SAMPLE_COUNT 16
 }
@@ -113,9 +113,8 @@ extern "C" {
 }
 void setDefaultSimulationState()
 {
-
     setLinkQualityDirect(LINK_QUALITY_MAX_VALUE);
-
+    osdConfigMutable()->framerate_hz = 12;
 }
 /*
  * Performs a test of the OSD actions on arming.
@@ -127,9 +126,13 @@ void doTestArm(bool testEmpty = true)
     // craft has been armed
     ENABLE_ARMING_FLAG(ARMED);
 
+    simulationTime += 0.1e6;
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     // arming alert displayed
@@ -141,7 +144,10 @@ void doTestArm(bool testEmpty = true)
 
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     // arming alert disappears
@@ -172,7 +178,10 @@ void doTestDisarm()
 
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     // post flight statistics displayed
@@ -203,6 +212,11 @@ TEST(LQTest, TestInit)
     // OSD is initialised
     osdInit(&testDisplayPort, OSD_DISPLAYPORT_DEVICE_AUTO);
 
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
+
     // then
     // display buffer should contain splash screen
     displayPortTestBufferSubstring(7, 8, "MENU:THR MID");
@@ -212,7 +226,10 @@ TEST(LQTest, TestInit)
     // when
     // splash screen timeout has elapsed
     simulationTime += 4e6;
-    osdUpdate(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     // display buffer should be empty
@@ -227,8 +244,6 @@ TEST(LQTest, TestInit)
 TEST(LQTest, TestElement_LQ_SOURCE_NONE_SAMPLES)
 {
     // given
-
-
     linkQualitySource = LQ_SOURCE_NONE;
 
     osdElementConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
@@ -241,9 +256,12 @@ TEST(LQTest, TestElement_LQ_SOURCE_NONE_SAMPLES)
         setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
     }
 
+    simulationTime += 1000000;
 
-    displayClearScreen(&testDisplayPort);
+    while (osdUpdateCheck(simulationTime, 0)) {
-    osdRefresh(simulationTime);
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     displayPortTestBufferSubstring(8, 1, "%c9", SYM_LINK_QUALITY);
@@ -254,12 +272,15 @@ TEST(LQTest, TestElement_LQ_SOURCE_NONE_SAMPLES)
         setLinkQualityDirect(updateLinkQualitySamples(0));
     }
 
-    displayClearScreen(&testDisplayPort);
+    simulationTime += 1000000;
-    osdRefresh(simulationTime);
+
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     displayPortTestBufferSubstring(8, 1, "%c4", SYM_LINK_QUALITY);
-
 }
 /*
  * Tests the Tests the OSD_LINK_QUALITY element values  default LQ_SOURCE_NONE
@@ -268,7 +289,6 @@ TEST(LQTest, TestElement_LQ_SOURCE_NONE_VALUES)
 {
     // given
 
-
     linkQualitySource = LQ_SOURCE_NONE;
 
     osdElementConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
@@ -280,8 +300,11 @@ TEST(LQTest, TestElement_LQ_SOURCE_NONE_VALUES)
     for (int testdigit = 10; testdigit > 0; testdigit--) {
         testscale = testdigit * 102.3;
         setLinkQualityDirect(testscale);
-        displayClearScreen(&testDisplayPort);
+        simulationTime += 100000;
-        osdRefresh(simulationTime);
+        while (osdUpdateCheck(simulationTime, 0)) {
+            osdUpdate(simulationTime);
+            simulationTime += 10;
+        }
 #ifdef DEBUG_OSD
         printf("%d %d\n",testscale, testdigit);
         displayPortTestPrint();
@@ -294,6 +317,7 @@ TEST(LQTest, TestElement_LQ_SOURCE_NONE_VALUES)
         }
     }
 }
+
 /*
  * Tests the OSD_LINK_QUALITY element LQ RX_PROTOCOL_CRSF.
  */
@@ -307,8 +331,11 @@ TEST(LQTest, TestElementLQ_PROTOCOL_CRSF_VALUES)
 
     osdAnalyzeActiveElements();
 
-    displayClearScreen(&testDisplayPort);
+    simulationTime += 1000000;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // crsf setLinkQualityDirect 0-300;
 
@@ -319,18 +346,22 @@ TEST(LQTest, TestElementLQ_PROTOCOL_CRSF_VALUES)
             rxSetRfMode(m);
             // then rxGetLinkQuality Osd should be x
             // and RfMode should be m
-            displayClearScreen(&testDisplayPort);
+            simulationTime += 100000;
-            osdRefresh(simulationTime);
+            while (osdUpdateCheck(simulationTime, 0)) {
-                displayPortTestBufferSubstring(8, 1, "%c%1d:%2d", SYM_LINK_QUALITY, m, x);
+                osdUpdate(simulationTime);
+                simulationTime += 10;
             }
+            displayPortTestBufferSubstring(8, 1, "%c%1d:%2d", SYM_LINK_QUALITY, m, x);
         }
     }
+}
 /*
  * Tests the LQ Alarms
  *
 */
 TEST(LQTest, TestLQAlarm)
 {
+    timeUs_t startTime = simulationTime;
     // given
     // default state is set
     setDefaultSimulationState();
@@ -364,10 +395,17 @@ TEST(LQTest, TestLQAlarm)
 
     // then
     // no elements should flash as all values are out of alarm range
+    // Ensure a consistent start time for testing
+    simulationTime += 5000000;
+    simulationTime -= simulationTime % 1000000;
+    startTime = simulationTime;
     for (int i = 0; i < 30; i++) {
         // Check for visibility every 100ms, elements should always be visible
-        simulationTime += 0.1e6;
+        simulationTime = startTime + i*0.1e6;
-        osdRefresh(simulationTime);
+        while (osdUpdateCheck(simulationTime, 0)) {
+            osdUpdate(simulationTime);
+            simulationTime += 10;
+        }
 
 #ifdef DEBUG_OSD
         printf("%d\n", i);
@@ -377,18 +415,25 @@ TEST(LQTest, TestLQAlarm)
     }
 
     setLinkQualityDirect(512);
-    simulationTime += 60e6;
+    while (osdUpdateCheck(simulationTime, 0)) {
-    osdRefresh(simulationTime);
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     // then
     // elements showing values in alarm range should flash
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    startTime = simulationTime;
     for (int i = 0; i < 15; i++) {
-        // Blinking should happen at 5Hz
+        // Blinking should happen at 2Hz
-        simulationTime += 0.2e6;
+        simulationTime = startTime + i*0.25e6;
-        osdRefresh(simulationTime);
+        while (osdUpdateCheck(simulationTime, 0)) {
+            osdUpdate(simulationTime);
+            simulationTime += 10;
+        }
 
 #ifdef DEBUG_OSD
-        printf("%d\n", i);
         displayPortTestPrint();
 #endif
         if (i % 2 == 0) {
@@ -399,8 +444,12 @@ TEST(LQTest, TestLQAlarm)
     }
 
     doTestDisarm();
-    simulationTime += 60e6;
+    simulationTime += 1000000;
-    osdRefresh(simulationTime);
+    simulationTime -= simulationTime % 1000000;
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 }
 
 // STUBS
@@ -461,6 +510,10 @@ extern "C" {
     void failsafeOnValidDataFailed(void) { }
     void pinioBoxTaskControl(void) { }
     bool taskUpdateRxMainInProgress() { return true; }
+    void ignoreTaskStateTime(void) { }
+    void ignoreTaskExecRate(void) { }
+    void ignoreTaskExecTime(void) { }
+    void schedulerSetNextStateTime(timeDelta_t) {}
 
     void rxPwmInit(rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
     {
@@ -559,7 +612,4 @@ 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

@@ -63,7 +63,7 @@ extern "C" {
 
     #include "rx/rx.h"
 
-    void osdRefresh(timeUs_t currentTimeUs);
+    void osdUpdate(timeUs_t currentTimeUs);
     void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time);
     int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius);
 
@@ -120,6 +120,7 @@ void setDefaultSimulationState()
     memset(osdElementConfigMutable(), 0, sizeof(osdElementConfig_t));
 
     osdConfigMutable()->enabled_stats = 0;
+    osdConfigMutable()->framerate_hz = 12;
 
     rssi = 1024;
 
@@ -135,10 +136,19 @@ void setDefaultSimulationState()
 
     rcData[PITCH] = 1500;
 
-    simulationTime = 0;
     osdFlyTime = 0;
+
+    DISABLE_ARMING_FLAG(ARMED);
 }
 
+void osdRefresh()
+{
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
+    simulationTime += 0.1e6;
+}
 /*
  * Performs a test of the OSD actions on arming.
  * (reused throughout the test suite)
@@ -149,9 +159,10 @@ void doTestArm(bool testEmpty = true)
     // craft has been armed
     ENABLE_ARMING_FLAG(ARMED);
 
+    simulationTime += 5e6;
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // arming alert displayed
@@ -163,7 +174,7 @@ void doTestArm(bool testEmpty = true)
 
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // arming alert disappears
@@ -194,7 +205,7 @@ void doTestDisarm()
 
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // post flight statistics displayed
@@ -240,7 +251,10 @@ void simulateFlight(void)
     simulationBatteryVoltage = 1580;
     simulationAltitude = 100;
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     rssi = 512;
     gpsSol.groundSpeed = 800;
@@ -249,7 +263,10 @@ void simulateFlight(void)
     simulationBatteryVoltage = 1470;
     simulationAltitude = 150;
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     rssi = 256;
     gpsSol.groundSpeed = 200;
@@ -258,7 +275,10 @@ void simulateFlight(void)
     simulationBatteryVoltage = 1520;
     simulationAltitude = 200;
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     rssi = 256;
     gpsSol.groundSpeed = 800;
@@ -267,11 +287,17 @@ void simulateFlight(void)
     simulationBatteryVoltage = 1470;
     simulationAltitude = 200; // converts to 6.56168 feet which rounds to 6.6 in imperial units stats test
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     simulationBatteryVoltage = 1520;
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     rssi = 256;
     gpsSol.groundSpeed = 800;
@@ -280,11 +306,13 @@ void simulateFlight(void)
     simulationBatteryVoltage = 1470;
     simulationAltitude = 200;
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    while (osdUpdateCheck(simulationTime, 0)) {
+        osdUpdate(simulationTime);
+        simulationTime += 10;
+    }
 
     simulationBatteryVoltage = 1520;
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
 }
 
 class OsdTest : public ::testing::Test
@@ -312,6 +340,14 @@ protected:
 TEST_F(OsdTest, TestInit)
 {
     // given
+    // display port is initialised
+    displayPortTestInit();
+
+    // and
+    // default state values are set
+    setDefaultSimulationState();
+
+    // and
     // this battery configuration (used for battery voltage elements)
     batteryConfigMutable()->vbatmincellvoltage = 330;
     batteryConfigMutable()->vbatmaxcellvoltage = 430;
@@ -320,6 +356,8 @@ TEST_F(OsdTest, TestInit)
     // OSD is initialised
     osdInit(&testDisplayPort, OSD_DISPLAYPORT_DEVICE_AUTO);
 
+    osdRefresh();
+
     // then
     // display buffer should contain splash screen
     displayPortTestBufferSubstring(7, 8, "MENU:THR MID");
@@ -329,7 +367,7 @@ TEST_F(OsdTest, TestInit)
     // when
     // splash screen timeout has elapsed
     simulationTime += 4e6;
-    osdUpdate(simulationTime);
+    osdRefresh();
 
     // then
     // display buffer should be empty
@@ -362,7 +400,7 @@ TEST_F(OsdTest, TestDisarm)
 
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // post flight stats screen disappears
@@ -399,7 +437,7 @@ TEST_F(OsdTest, TestDisarmWithDismissStats)
 
     // when
     // sufficient OSD updates have been called
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // post flight stats screen disappears
@@ -446,7 +484,7 @@ TEST_F(OsdTest, TestStatsTiming)
     // and
     // these conditions occur during flight
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // and
     // the craft is disarmed
@@ -459,7 +497,7 @@ TEST_F(OsdTest, TestStatsTiming)
     // and
     // these conditions occur during flight
     simulationTime += 1e6;
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // and
     // the craft is disarmed
@@ -469,7 +507,7 @@ TEST_F(OsdTest, TestStatsTiming)
     // statistics screen should display the following
     int row = 7;
     displayPortTestBufferSubstring(2, row++, "2017-11-19 10:12:");
-    displayPortTestBufferSubstring(2, row++, "TOTAL ARM         : 00:02.50");
+    displayPortTestBufferSubstring(2, row++, "TOTAL ARM         : 00:13.61");
     displayPortTestBufferSubstring(2, row++, "LAST ARM          : 00:01");
 }
 
@@ -598,7 +636,7 @@ TEST_F(OsdTest, TestAlarms)
     osdElementConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE]       = OSD_POS(12, 1) | OSD_PROFILE_1_FLAG;
     osdElementConfigMutable()->item_pos[OSD_ITEM_TIMER_1]            = OSD_POS(20, 1) | OSD_PROFILE_1_FLAG;
     osdElementConfigMutable()->item_pos[OSD_ITEM_TIMER_2]            = OSD_POS(1, 1)  | OSD_PROFILE_1_FLAG;
-    osdElementConfigMutable()->item_pos[OSD_REMAINING_TIME_ESTIMATE] = OSD_POS(1, 2) | OSD_PROFILE_1_FLAG;
+    osdElementConfigMutable()->item_pos[OSD_REMAINING_TIME_ESTIMATE] = OSD_POS(1, 2)  | OSD_PROFILE_1_FLAG;
     osdElementConfigMutable()->item_pos[OSD_ALTITUDE]                = OSD_POS(23, 7) | OSD_PROFILE_1_FLAG;
 
     // and
@@ -611,10 +649,10 @@ TEST_F(OsdTest, TestAlarms)
 
     // and
     // this timer 1 configuration
-    osdConfigMutable()->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_HUNDREDTHS, 3);
+    osdConfigMutable()->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_HUNDREDTHS, 5);
     EXPECT_EQ(OSD_TIMER_SRC_ON, OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]));
     EXPECT_EQ(OSD_TIMER_PREC_HUNDREDTHS, OSD_TIMER_PRECISION(osdConfig()->timers[OSD_TIMER_1]));
-    EXPECT_EQ(3, OSD_TIMER_ALARM(osdConfig()->timers[OSD_TIMER_1]));
+    EXPECT_EQ(5, OSD_TIMER_ALARM(osdConfig()->timers[OSD_TIMER_1]));
 
     // and
     // this timer 2 configuration
@@ -630,21 +668,21 @@ TEST_F(OsdTest, TestAlarms)
     // when
     // time is passing by
     simulationTime += 60e6;
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // and
     // the craft is armed
     doTestArm(false);
 
     simulationTime += 70e6;
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // no elements should flash as all values are out of alarm range
     for (int i = 0; i < 30; i++) {
         // Check for visibility every 100ms, elements should always be visible
         simulationTime += 0.1e6;
-        osdRefresh(simulationTime);
+        osdRefresh();
 
 #ifdef DEBUG_OSD
         printf("%d\n", i);
@@ -652,7 +690,7 @@ TEST_F(OsdTest, TestAlarms)
         displayPortTestBufferSubstring(1,  1, "%c01:", SYM_FLY_M); // only test the minute part of the timer
         displayPortTestBufferSubstring(8,  1, "%c99", SYM_RSSI);
         displayPortTestBufferSubstring(12, 1, "%c16.8%c", SYM_BATT_FULL, SYM_VOLT);
-        displayPortTestBufferSubstring(20, 1, "%c02:", SYM_ON_M); // only test the minute part of the timer
+        displayPortTestBufferSubstring(20, 1, "%c04:", SYM_ON_M); // only test the minute part of the timer
         displayPortTestBufferSubstring(23, 7, "%c0.0%c", SYM_ALTITUDE, SYM_M);
     }
 
@@ -665,14 +703,17 @@ TEST_F(OsdTest, TestAlarms)
     simulationMahDrawn = 999999;
 
     simulationTime += 60e6;
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // elements showing values in alarm range should flash
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    timeUs_t startTime = simulationTime;
     for (int i = 0; i < 15; i++) {
-        // Blinking should happen at 5Hz
+        // Blinking should happen at 2Hz
-        simulationTime += 0.2e6;
+        simulationTime = startTime + i*0.25e6;
-        osdRefresh(simulationTime);
+        osdRefresh();
 
 #ifdef DEBUG_OSD
         printf("%d\n", i);
@@ -682,7 +723,7 @@ TEST_F(OsdTest, TestAlarms)
             displayPortTestBufferSubstring(8,  1, "%c12", SYM_RSSI);
             displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT);
             displayPortTestBufferSubstring(1,  1, "%c02:", SYM_FLY_M); // only test the minute part of the timer
-            displayPortTestBufferSubstring(20, 1, "%c03:", SYM_ON_M); // only test the minute part of the timer
+            displayPortTestBufferSubstring(20, 1, "%c05:", SYM_ON_M); // only test the minute part of the timer
             displayPortTestBufferSubstring(23, 7, "%c120.0%c", SYM_ALTITUDE, SYM_M);
         } else {
             displayPortTestBufferIsEmpty();
@@ -704,7 +745,7 @@ TEST_F(OsdTest, TestElementRssi)
     // when
     rssi = 1024;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
@@ -712,7 +753,7 @@ TEST_F(OsdTest, TestElementRssi)
     // when
     rssi = 0;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(8, 1, "%c 0", SYM_RSSI);
@@ -720,7 +761,7 @@ TEST_F(OsdTest, TestElementRssi)
     // when
     rssi = 512;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(8, 1, "%c50", SYM_RSSI);
@@ -739,7 +780,7 @@ TEST_F(OsdTest, TestElementAmperage)
     // when
     simulationBatteryAmperage = 0;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 12, "  0.00%c", SYM_AMP);
@@ -747,7 +788,7 @@ TEST_F(OsdTest, TestElementAmperage)
     // when
     simulationBatteryAmperage = 2156;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 12, " 21.56%c", SYM_AMP);
@@ -755,7 +796,7 @@ TEST_F(OsdTest, TestElementAmperage)
     // when
     simulationBatteryAmperage = 12345;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 12, "123.45%c", SYM_AMP);
@@ -774,7 +815,7 @@ TEST_F(OsdTest, TestElementMahDrawn)
     // when
     simulationMahDrawn = 0;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 11, "   0%c", SYM_MAH);
@@ -782,7 +823,7 @@ TEST_F(OsdTest, TestElementMahDrawn)
     // when
     simulationMahDrawn = 4;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 11, "   4%c", SYM_MAH);
@@ -790,7 +831,7 @@ TEST_F(OsdTest, TestElementMahDrawn)
     // when
     simulationMahDrawn = 15;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 11, "  15%c", SYM_MAH);
@@ -798,7 +839,7 @@ TEST_F(OsdTest, TestElementMahDrawn)
     // when
     simulationMahDrawn = 246;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 11, " 246%c", SYM_MAH);
@@ -806,7 +847,7 @@ TEST_F(OsdTest, TestElementMahDrawn)
     // when
     simulationMahDrawn = 1042;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 11, "1042%c", SYM_MAH);
@@ -830,7 +871,7 @@ TEST_F(OsdTest, TestElementPower)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 10, "   0W");
@@ -840,7 +881,7 @@ TEST_F(OsdTest, TestElementPower)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 10, "   1W");
@@ -850,7 +891,7 @@ TEST_F(OsdTest, TestElementPower)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 10, "  12W");
@@ -860,7 +901,7 @@ TEST_F(OsdTest, TestElementPower)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 10, " 123W");
@@ -870,7 +911,7 @@ TEST_F(OsdTest, TestElementPower)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 10, "1234W");
@@ -893,7 +934,7 @@ TEST_F(OsdTest, TestElementAltitude)
     // when
     simulationAltitude = 0;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(23, 7, "%c-", SYM_ALTITUDE);
@@ -901,7 +942,7 @@ TEST_F(OsdTest, TestElementAltitude)
     // when
     sensorsSet(SENSOR_GPS);
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(23, 7, "%c0.0%c", SYM_ALTITUDE, SYM_M);
@@ -909,7 +950,7 @@ TEST_F(OsdTest, TestElementAltitude)
     // when
     simulationAltitude = 247;  // rounds to 2.5m
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(23, 7, "%c2.5%c", SYM_ALTITUDE, SYM_M);
@@ -917,7 +958,7 @@ TEST_F(OsdTest, TestElementAltitude)
     // when
     simulationAltitude = 4247;  // rounds to 42.5m
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(23, 7, "%c42.5%c", SYM_ALTITUDE, SYM_M);
@@ -925,7 +966,7 @@ TEST_F(OsdTest, TestElementAltitude)
     // when
     simulationAltitude = -247;  // rounds to -2.5m
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(23, 7, "%c-2.5%c", SYM_ALTITUDE, SYM_M);
@@ -933,7 +974,7 @@ TEST_F(OsdTest, TestElementAltitude)
     // when
     simulationAltitude = -70;
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(23, 7, "%c-0.7%c", SYM_ALTITUDE, SYM_M);
@@ -958,7 +999,7 @@ TEST_F(OsdTest, TestElementCoreTemperature)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 8, "C%c  0%c", SYM_TEMPERATURE, SYM_C);
@@ -968,7 +1009,7 @@ TEST_F(OsdTest, TestElementCoreTemperature)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 8, "C%c 33%c", SYM_TEMPERATURE, SYM_C);
@@ -978,7 +1019,7 @@ TEST_F(OsdTest, TestElementCoreTemperature)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(1, 8, "C%c 91%c", SYM_TEMPERATURE, SYM_F);
@@ -1012,7 +1053,10 @@ TEST_F(OsdTest, TestElementWarningsBattery)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    // Delay as the warnings are flashing
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(9, 10, "BATT < FULL");
@@ -1024,7 +1068,7 @@ TEST_F(OsdTest, TestElementWarningsBattery)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(9, 10, "           ");
@@ -1036,7 +1080,11 @@ TEST_F(OsdTest, TestElementWarningsBattery)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    // Delay as the warnings are flashing
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    simulationTime += 0.25e6;
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(9, 10, "LOW BATTERY ");
@@ -1048,8 +1096,11 @@ TEST_F(OsdTest, TestElementWarningsBattery)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    // Delay as the warnings are flashing
-    osdRefresh(simulationTime);
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    simulationTime += 0.25e6;
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(9, 10, " LAND NOW   ");
@@ -1061,7 +1112,7 @@ TEST_F(OsdTest, TestElementWarningsBattery)
 
     // when
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     displayPortTestBufferSubstring(9, 10, "             ");
@@ -1149,16 +1200,19 @@ TEST_F(OsdTest, TestGpsElements)
     gpsSol.numSat = 0;
 
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // Sat indicator should blink and show "NC"
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    timeUs_t startTime = simulationTime;
     for (int i = 0; i < 15; i++) {
-        // Blinking should happen at 5Hz
+        // Blinking should happen at 2Hz
-        simulationTime += 0.2e6;
+        simulationTime = startTime + i*0.25e6;
-        osdRefresh(simulationTime);
+        osdRefresh();
 
-        if (i % 2 == 0) {
+        if (i % 2 == 1) {
             displayPortTestBufferSubstring(2, 4, "%c%cNC", SYM_SAT_L, SYM_SAT_R);
         } else {
             displayPortTestBufferIsEmpty();
@@ -1170,16 +1224,19 @@ TEST_F(OsdTest, TestGpsElements)
     gpsSol.numSat = 0;
 
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // Sat indicator should blink and show "0"
+    simulationTime += 1000000;
+    simulationTime -= simulationTime % 1000000;
+    startTime = simulationTime;
     for (int i = 0; i < 15; i++) {
-        // Blinking should happen at 5Hz
+        // Blinking should happen at 2Hz
-        simulationTime += 0.2e6;
+        simulationTime = startTime + i*0.25e6;
-        osdRefresh(simulationTime);
+        osdRefresh();
 
-        if (i % 2 == 0) {
+        if (i % 2 == 1) {
             displayPortTestBufferSubstring(2, 4, "%c%c 0", SYM_SAT_L, SYM_SAT_R);
         } else {
             displayPortTestBufferIsEmpty();
@@ -1191,14 +1248,14 @@ TEST_F(OsdTest, TestGpsElements)
     gpsSol.numSat = 10;
 
     displayClearScreen(&testDisplayPort);
-    osdRefresh(simulationTime);
+    osdRefresh();
 
     // then
     // Sat indicator should show "10" without flashing
     for (int i = 0; i < 15; i++) {
-        // Blinking should happen at 5Hz
+        // Blinking should happen at 2Hz
         simulationTime += 0.2e6;
-        osdRefresh(simulationTime);
+        osdRefresh();
 
         displayPortTestBufferSubstring(2, 4, "%c%c10", SYM_SAT_L, SYM_SAT_R);
     }
@@ -1319,6 +1376,8 @@ 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) { }
-    void ignoreTaskStateTime(void) {}
+    void ignoreTaskExecRate(void) { }
+    void ignoreTaskExecTime(void) { }
+    void schedulerSetNextStateTime(timeDelta_t) {}
 }

src/test/unit/scheduler_unittest.cc

@@ -44,7 +44,6 @@ const int TEST_DISPATCH_TIME = 1;
 extern "C" {
     task_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;
@@ -402,14 +401,12 @@ TEST(SchedulerUnittest, TestTwoTasks)
     // no tasks should run, since neither task's desired time has elapsed
     scheduler();
     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
-    EXPECT_EQ(0, unittest_scheduler_waitingTasks);
 
     // 500 microseconds later, TASK_ACCEL desiredPeriodUs has elapsed
     simulatedTime += 500;
     // TASK_ACCEL should now run
     scheduler();
     EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
-    EXPECT_EQ(1, unittest_scheduler_waitingTasks);
     EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
 
     simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
@@ -420,7 +417,6 @@ TEST(SchedulerUnittest, TestTwoTasks)
     scheduler();
     // No task should have run
     EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
-    EXPECT_EQ(0, unittest_scheduler_waitingTasks);
 
     simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriodUss have elapsed
     // of the two TASK_ACCEL should run first

src/test/unit/unittest_displayport.h

@@ -55,7 +55,7 @@ static int displayPortTestClearScreen(displayPort_t *displayPort)
     return 0;
 }
 
-static int displayPortTestDrawScreen(displayPort_t *displayPort)
+static bool displayPortTestDrawScreen(displayPort_t *displayPort)
 {
     UNUSED(displayPort);
     return 0;

src/test/unit/ws2811_unittest.cc

@@ -32,7 +32,7 @@ extern "C" {
 
 extern "C" {
     void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex);
-    void ignoreTaskShortExecTime(void) {}
+    void ignoreTaskExecTime(void) {}
     void ignoreTaskStateTime(void) {}
 }