diff --git a/src/main/build/debug.c b/src/main/build/debug.c index da5195bcf..83408e477 100644 --- a/src/main/build/debug.c +++ b/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" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 97f84d701..cc8ddb5fe 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index c374491d3..f81d7e1a6 100644 --- a/src/main/cli/cli.c +++ b/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 averageLoad = taskInfo.averageExecutionTimeUs == 0 ? 0 : (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 1000; + const int maxLoad = taskInfo.maxExecutionTimeUs == 0 ? 0 : (taskInfo.maxExecutionTimeUs * taskFrequency) / 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; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b5c83a78a..c9e560364 100644 --- a/src/main/cli/settings.c +++ b/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 diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 534cae0b3..17d35ab10 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/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 diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index bfc6f1b8e..f02726472 100644 --- a/src/main/drivers/bus_spi.c +++ b/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) { - 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); - } + spiIrqHandler(dev); } -// 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; } diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 70a8f348d..4e0268129 100644 --- a/src/main/drivers/bus_spi.h +++ b/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); diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index 3dbf7cb13..f8c2d2733 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/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->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY; + initRx->Mode = LL_DMA_MODE_NORMAL; + 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->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - initRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE; + initRx->Priority = LL_DMA_PRIORITY_LOW; + initRx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + 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 uint8_t dummyRxByte; + STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; + 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) { - /* 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)) { +#if !defined(STM32G4) && !defined(STM32H7) + if (dev->bus->dmaRx) { #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); - } + uint8_t *rxData = segment->rxData; + LL_DMA_InitTypeDef *initRx = bus->initRx; + + 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 - dmaRx->userParam = (uint32_t)dev; +#if !defined(STM32G4) && !defined(STM32H7) + if (dmaRx) { +#endif + // Use the correct callback argument + dmaRx->userParam = (uint32_t)dev; - // Clear transfer flags - 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); + // Clear transfer flags + 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); #ifdef STM32G4 - // Disable channels to enable update - LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream); - LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream); + // Disable channels to enable update + LL_DMA_DisableChannel(dmaTx->dma, dmaTx->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 - * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress - */ - LL_DMA_EnableIT_TC(dmaRx->dma, dmaRx->stream); + /* 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 + */ + LL_DMA_EnableIT_TC(dmaRx->dma, dmaRx->stream); - // Update channels - LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); - LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); + // Update channels + LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); + 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 - LL_DMA_EnableChannel(dmaTx->dma, dmaTx->stream); - LL_DMA_EnableChannel(dmaRx->dma, dmaRx->stream); + // Enable channels + LL_DMA_EnableChannel(dmaTx->dma, dmaTx->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 *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; + DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref; + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; - // Disable streams to enable update - LL_DMA_WriteReg(streamRegsTx, CR, 0U); - LL_DMA_WriteReg(streamRegsRx, CR, 0U); + // Disable streams to enable update + LL_DMA_WriteReg(streamRegsTx, 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 - * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress - */ - LL_EX_DMA_EnableIT_TC(streamRegsRx); + /* 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 + */ + LL_EX_DMA_EnableIT_TC(streamRegsRx); - // Update streams - LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); - LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); + // Update streams + LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx); + LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx); - /* 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). - */ + /* 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 & 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_DMA_EnableStream(dmaTx->dma, dmaTx->stream); - LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream); - 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_StartMasterTransfer(dev->bus->busType_u.spi.instance); + LL_SPI_SetTransferSize(dev->bus->busType_u.spi.instance, dev->bus->curSegment->len); + LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream); + LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream); + 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_StartMasterTransfer(dev->bus->busType_u.spi.instance); #else - // Enable streams - LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream); - LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream); + // Enable streams + LL_DMA_EnableStream(dmaTx->dma, dmaTx->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(dmaRx->dma, dmaRx->stream); + LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream); + LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream); #else - LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream); - LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream); + LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream); + LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream); #endif - // Clear transfer flags - DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); + // Clear transfer flags + DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF); - LL_SPI_DisableDMAReq_TX(instance); - LL_SPI_DisableDMAReq_RX(instance); + LL_SPI_DisableDMAReq_TX(instance); + LL_SPI_DisableDMAReq_RX(instance); #if defined(STM32H7) - LL_SPI_ClearFlag_TXTF(dev->bus->busType_u.spi.instance); - LL_SPI_Disable(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); +#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; } } } diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c index ffd616d91..61d70b9f7 100644 --- a/src/main/drivers/bus_spi_stdperiph.c +++ b/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); - initRx->DMA_Channel = bus->dmaRx->channel; - initRx->DMA_DIR = DMA_DIR_PeripheralToMemory; - initRx->DMA_Mode = DMA_Mode_Normal; - 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; + if (bus->dmaRx) { + DMA_InitTypeDef *initRx = bus->initRx; + + DMA_StructInit(initRx); + initRx->DMA_Channel = bus->dmaRx->channel; + initRx->DMA_DIR = DMA_DIR_PeripheralToMemory; + initRx->DMA_Mode = DMA_Mode_Normal; + 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 uint8_t dummyRxByte; + STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; + 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) { - 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 (dev->bus->dmaRx) { + uint8_t *rxData = segment->rxData; + DMA_InitTypeDef *initRx = bus->initRx; + + if (rxData) { + 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 - dmaRx->userParam = (uint32_t)dev; + // Use the correct callback argument + dmaRx->userParam = (uint32_t)dev; - // Clear transfer flags - 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); + // Clear transfer flags + 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); - // Disable streams to enable update - streamRegsTx->CR = 0U; - streamRegsRx->CR = 0U; + // Disable streams to enable update + streamRegsTx->CR = 0U; + streamRegsRx->CR = 0U; - /* 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 - */ - DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE); + /* 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 + */ + DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE); - // Update streams - DMA_Init(streamRegsTx, dev->bus->initTx); - DMA_Init(streamRegsRx, dev->bus->initRx); + // Update streams + DMA_Init(streamRegsTx, dev->bus->initTx); + DMA_Init(streamRegsRx, dev->bus->initRx); - /* 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). - */ + /* 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 streams - DMA_Cmd(streamRegsTx, ENABLE); - DMA_Cmd(streamRegsRx, ENABLE); + // Enable streams + DMA_Cmd(streamRegsTx, ENABLE); + DMA_Cmd(streamRegsRx, ENABLE); - /* 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); + /* 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); + } 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 - streamRegsTx->CR = 0U; - streamRegsRx->CR = 0U; + if (dmaRx) { + DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref; - 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; } } } diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index d2b624f35..2dc6672bd 100644 --- a/src/main/drivers/display.c +++ b/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) diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index 985fc0ac7..3a287bd19 100644 --- a/src/main/drivers/display.h +++ b/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); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 6cff247d9..b4635144d 100644 --- a/src/main/drivers/max7456.c +++ b/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 - delayMicroseconds(100); + // Wait for 200us before polling for completion of reset + 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++) { - spiWriteReg(dev, i, reg); + STATIC_DMA_DATA_AUTO uint8_t buf[32]; + 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; - for (int k = 0; k < MAX_CHARS2UPDATE; k++) { + maxSpiBufStartIndex = spiUseMOSI_DMA(dev) ? MAX_BYTES2SEND : MAX_BYTES2SEND_POLLED; + + // 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; - spiBuff[buff_len++] = pos >> 8; - spiBuff[buff_len++] = MAX7456ADD_DMAL; - spiBuff[buff_len++] = pos & 0xff; - spiBuff[buff_len++] = MAX7456ADD_DMDI; - spiBuff[buff_len++] = buffer[pos]; + if (buffer[pos] == 0xff) { + buffer[pos] = ' '; + } + + if (setAddress || !autoInc) { + 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) { - segments[0].txData = spiBuff; - segments[0].len = buff_len; + if (autoInc) { + if (!setAddress) { + spiBuf[spiBufIndex++] = MAX7456ADD_DMDI; + spiBuf[spiBufIndex++] = END_STRING; + } - // Ensure any prior DMA has completed - spiWaitClaim(dev); + spiBuf[spiBufIndex++] = MAX7456ADD_DMM; + 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) diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index 4ded7ac34..2d76d3d66 100644 --- a/src/main/drivers/max7456.h +++ b/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); diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 784818880..55275c39e 100644 --- a/src/main/fc/init.c +++ b/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; diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index f9313385d..8e43f3277 100644 --- a/src/main/fc/tasks.c +++ b/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 { - CHECK, PROCESS, MODES, UPDATE -} rxState = CHECK; +typedef enum { + RX_STATE_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 - if (rxState != MODES) { - ignoreTaskStateTime(); + static timeUs_t rxStateDurationUs[RX_STATE_COUNT]; + timeUs_t executeTimeUs; + 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: - rxState = PROCESS; + case RX_STATE_CHECK: + 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 diff --git a/src/main/flight/position.c b/src/main/flight/position.c index fc6062135..db78036c1 100644 --- a/src/main/flight/position.c +++ b/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); diff --git a/src/main/io/displayport_crsf.c b/src/main/io/displayport_crsf.c index 9d7435968..f6cb031a9 100644 --- a/src/main/io/displayport_crsf.c +++ b/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; diff --git a/src/main/io/displayport_frsky_osd.c b/src/main/io/displayport_frsky_osd.c index 0db8fa719..05bd4ea84 100644 --- a/src/main/io/displayport_frsky_osd.c +++ b/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(); diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c index 521b32549..e854e2cf7 100644 --- a/src/main/io/displayport_hott.c +++ b/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; diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index c520610cd..52c7659c6 100644 --- a/src/main/io/displayport_max7456.c +++ b/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 0; + return max7456DrawScreen(); } 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; diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index bd2e5bc42..5040bea2e 100644 --- a/src/main/io/displayport_msp.c +++ b/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) diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c index 2422e06bf..45d76c49e 100644 --- a/src/main/io/displayport_oled.c +++ b/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; diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c index fe04b2b1c..3da52ffb2 100644 --- a/src/main/io/displayport_srxl.c +++ b/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; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 33ab67642..de5273def 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -230,15 +230,19 @@ typedef struct { #endif // USE_GPS_UBLOX typedef enum { - GPS_UNKNOWN, - GPS_INITIALIZING, - GPS_INITIALIZED, - GPS_CHANGE_BAUD, - GPS_CONFIGURE, - GPS_RECEIVING_DATA, - GPS_LOST_COMMUNICATION + GPS_STATE_UNKNOWN, + GPS_STATE_INITIALIZING, + GPS_STATE_INITIALIZED, + GPS_STATE_CHANGE_BAUD, + GPS_STATE_CONFIGURE, + GPS_STATE_RECEIVING_DATA, + 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; - timeUs_t endTimeUs; + static gpsState_e gpsStateDurationUs[GPS_STATE_COUNT]; + 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_INITIALIZED: + case GPS_STATE_UNKNOWN: + case GPS_STATE_INITIALIZED: break; - case GPS_INITIALIZING: - case GPS_CHANGE_BAUD: - case GPS_CONFIGURE: + case GPS_STATE_INITIALIZING: + case GPS_STATE_CHANGE_BAUD: + 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 diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 06730f646..59cf89cbd 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1066,8 +1066,8 @@ static void applyStatusProfile(timeUs_t now) { } if (!timActive) { - // Call ignoreTaskShortExecTime() unless data is being processed - ignoreTaskShortExecTime(); + // Call ignoreTaskExecTime() unless data is being processed + 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 - ignoreTaskShortExecTime(); + // Call ignoreTaskExecTime() unless data is being processed + ignoreTaskExecTime(); return; } @@ -1282,8 +1282,8 @@ void ledStripUpdate(timeUs_t currentTimeUs) break; } } else { - // Call ignoreTaskShortExecTime() unless data is being processed - ignoreTaskShortExecTime(); + // Call ignoreTaskExecTime() unless data is being processed + ignoreTaskExecTime(); } } diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 1c1b81d66..744259893 100644 --- a/src/main/msp/msp.c +++ b/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(); diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index df0fa3dcc..41ab98627 100644 --- a/src/main/osd/osd.c +++ b/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); } -/* - * Called periodically by the scheduler - */ +typedef enum { + 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()) { - showVisualBeeper = true; - } + break; - // don't touch buffers if DMA transaction is in progress - if (displayIsTransferInProgress(osdDisplayPort)) { - ignoreTaskShortExecTime(); - return; - } + case OSD_STATE_CHECK: + if (isBeeperOn()) { + showVisualBeeper = true; + } -#ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED - static uint32_t idlecounter = 0; - if (!ARMING_FLAG(ARMED)) { - if (idlecounter++ % 4 != 0) { - ignoreTaskShortExecTime(); + // don't touch buffers if DMA transaction is in progress + if (displayIsTransferInProgress(osdDisplayPort)) { + break; + } + + 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 - if (counter % OSD_DRAW_FREQ_DENOM == 0) { - osdRefresh(currentTimeUs); + osdState = OSD_STATE_UPDATE_STATS1; + break; + + case OSD_STATE_UPDATE_STATS1: + osdDrawStats1(currentTimeUs); showVisualBeeper = false; - } else { - bool doDrawScreen = true; -#if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) - // For the MSP displayPort device only do the drawScreen once per - // logical OSD cycle as there is no output buffering needing to be flushed. - if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_MSP) { - doDrawScreen = (counter % OSD_DRAW_FREQ_DENOM == 1); - } + + osdState = OSD_STATE_UPDATE_STATS2; + break; + + case OSD_STATE_UPDATE_STATS2: + osdDrawStats2(currentTimeUs); + + 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) { - displayDrawScreen(osdDisplayPort); + { + osdState = OSD_STATE_UPDATE_ALARMS; + 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) diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index d6c1b2c94..e807c5bdc 100644 --- a/src/main/osd/osd.h +++ b/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_TASK_FREQUENCY_MAX 300 -#define OSD_TASK_FREQUENCY_DEFAULT 60 +#define OSD_FRAMERATE_MIN_HZ 1 +#define OSD_FRAMERATE_MAX_HZ 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); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 952eb3bb6..cd46a812c 100644 --- a/src/main/osd/osd_elements.c +++ b/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 - 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 +uint8_t osdGetActiveElementCount() +{ + return activeOsdElementCount; +} - // synchronize the blinking with the OSD task loop - if (++blinkLoopCounter >= lrintf(osdConfig()->task_frequency / OSD_DRAW_FREQ_DENOM / (OSD_BLINK_FREQUENCY_HZ * 2))) { - blinkState = !blinkState; - blinkLoopCounter = 0; +// Return true if there are more elements to draw +bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + bool retval = true; + + if (activeElement >= activeOsdElementCount) { + return false; } - for (unsigned i = 0; i < activeOsdElementCount; i++) { - if (!backgroundLayerSupported) { - // If the background layer isn't supported then we - // have to draw the element's static layer as well. - osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[i]); - } - osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[i]); + if (!backgroundLayerSupported) { + // If the background layer isn't supported then we + // have to draw the element's static layer as well. + osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[activeElement]); } + + 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)); diff --git a/src/main/osd/osd_elements.h b/src/main/osd/osd_elements.h index 31ee7cc0a..ec09dec52 100644 --- a/src/main/osd/osd_elements.h +++ b/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); diff --git a/src/main/pg/sdcard.c b/src/main/pg/sdcard.c index b9226cfc0..c19b71ee8 100644 --- a/src/main/pg/sdcard.c +++ b/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 diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 8a457af13..e6ab4c37d 100644 --- a/src/main/scheduler/scheduler.c +++ b/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) { - averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples; - totalWaitingTasksSamples = 0; - totalWaitingTasks = 0; + if (deltaTime) { + averageSystemLoadPercent = 100 * taskTotalExecutionTime / deltaTime; + taskTotalExecutionTime = 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->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging + task = currentTask; } else if (taskId < TASK_COUNT) { - task_t *task = getTask(taskId); - task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging + task = getTask(taskId); + } 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 -// except the one which takes the longest to execute. +// Called by tasks executing what are known to be short states 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->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; - selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; - } - selectedTask->totalExecutionTimeUs += taskExecutionTimeUs; // time consumed by scheduler + task - selectedTask->movingAverageCycleTimeUs += 0.05f * (period - selectedTask->movingAverageCycleTimeUs); + + 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) { - DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 1, selectedTask - tasks); - DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 2, clockCyclesTo10thMicros(schedLoopRemainingCycles)); - selectedTask->lateCount++ ; - } + taskCount++; #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; +} diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 391cfbc46..3c9805655 100644 --- a/src/main/scheduler/scheduler.h +++ b/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); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 6df8c8f97..df51e8d6a 100644 --- a/src/main/sensors/barometer.c +++ b/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, - BAROMETER_NEEDS_TEMPERATURE_SAMPLE, - BAROMETER_NEEDS_PRESSURE_START, - BAROMETER_NEEDS_PRESSURE_READ, - BAROMETER_NEEDS_PRESSURE_SAMPLE, - BAROMETER_NEEDS_TEMPERATURE_START + BARO_STATE_TEMPERATURE_READ = 0, + BARO_STATE_TEMPERATURE_SAMPLE, + BARO_STATE_PRESSURE_START, + BARO_STATE_PRESSURE_READ, + BARO_STATE_PRESSURE_SAMPLE, + 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 - // as that takes the longest - if (state != BAROMETER_NEEDS_PRESSURE_READ) { - ignoreTaskStateTime(); + // Where we are using a state machine call ignoreTaskExecRate() for all states bar one + if (state != BARO_STATE_TEMPERATURE_START) { + ignoreTaskExecRate(); } 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; } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index e4b828178..2d2c6d514 100644 --- a/src/main/sensors/barometer.h +++ b/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); diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 68e5de4d6..75bec34ef 100644 --- a/src/main/target/common_pre.h +++ b/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 diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index 4994018f5..2e6c40f40 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/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); - return USBD_DeviceDesc_Composite; + *length = sizeof(USBD_DeviceDesc_Composite); + return USBD_DeviceDesc_Composite; } #endif *length = sizeof(USBD_DeviceDesc); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 5c539bf04..50ef8b897 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/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) {} } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index a5e822612..65f13f581 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/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) {} } diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 712a3b33e..cd21cb0e0 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/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); - osdRefresh(simulationTime); + while (osdUpdateCheck(simulationTime, 0)) { + 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); - osdRefresh(simulationTime); + simulationTime += 1000000; + + 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); - osdRefresh(simulationTime); + simulationTime += 100000; + 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); - osdRefresh(simulationTime); + simulationTime += 1000000; + 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); - osdRefresh(simulationTime); - displayPortTestBufferSubstring(8, 1, "%c%1d:%2d", SYM_LINK_QUALITY, m, x); + simulationTime += 100000; + while (osdUpdateCheck(simulationTime, 0)) { + 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; - osdRefresh(simulationTime); + simulationTime = startTime + i*0.1e6; + 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; - osdRefresh(simulationTime); + while (osdUpdateCheck(simulationTime, 0)) { + 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 - simulationTime += 0.2e6; - osdRefresh(simulationTime); + // Blinking should happen at 2Hz + simulationTime = startTime + i*0.25e6; + 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; - osdRefresh(simulationTime); + simulationTime += 1000000; + 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) {} } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 952af76d8..ecf472b4a 100644 --- a/src/test/unit/osd_unittest.cc +++ b/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 - simulationTime += 0.2e6; - osdRefresh(simulationTime); + // Blinking should happen at 2Hz + simulationTime = startTime + i*0.25e6; + 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); - osdRefresh(simulationTime); + // Delay as the warnings are flashing + 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 - simulationTime += 0.2e6; - osdRefresh(simulationTime); + // Blinking should happen at 2Hz + simulationTime = startTime + i*0.25e6; + 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 - simulationTime += 0.2e6; - osdRefresh(simulationTime); + // Blinking should happen at 2Hz + simulationTime = startTime + i*0.25e6; + 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) {} } diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 3f1ea4d7b..1192c89bb 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/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(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(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 diff --git a/src/test/unit/unittest_displayport.h b/src/test/unit/unittest_displayport.h index bd0137238..88168b42f 100644 --- a/src/test/unit/unittest_displayport.h +++ b/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; diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc index b2f65edf0..796940001 100644 --- a/src/test/unit/ws2811_unittest.cc +++ b/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) {} }