Track state execution time for OSD, baro, rx and GPS tasks and inform scheduler of next state execution time
This commit is contained in:
parent
a63172cc1f
commit
ab1baccc66
|
@ -95,5 +95,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"D_LPF",
|
||||
"VTX_TRAMP",
|
||||
"GHST",
|
||||
"SCHEDULER_DETERMINISM"
|
||||
"SCHEDULER_DETERMINISM",
|
||||
"TIMING_ACCURACY"
|
||||
};
|
||||
|
|
|
@ -94,6 +94,7 @@ typedef enum {
|
|||
DEBUG_VTX_TRAMP,
|
||||
DEBUG_GHST,
|
||||
DEBUG_SCHEDULER_DETERMINISM,
|
||||
DEBUG_TIMING_ACCURACY,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -31,7 +31,7 @@
|
|||
|
||||
displayPort_t hottDisplayPort;
|
||||
|
||||
static int hottDrawScreen(displayPort_t *displayPort)
|
||||
static bool hottDrawScreen(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
return 0;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -38,7 +38,7 @@
|
|||
|
||||
displayPort_t srxlDisplayPort;
|
||||
|
||||
static int srxlDrawScreen(displayPort_t *displayPort)
|
||||
static bool srxlDrawScreen(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
return 0;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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) {}
|
||||
}
|
||||
|
|
|
@ -397,5 +397,5 @@ void ws2811LedStripEnable(void) { }
|
|||
|
||||
void setUsedLedCount(unsigned) { }
|
||||
void pinioBoxTaskControl(void) {}
|
||||
void ignoreTaskShortExecTime(void) {}
|
||||
void ignoreTaskExecTime(void) {}
|
||||
}
|
||||
|
|
|
@ -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) {}
|
||||
}
|
||||
|
|
|
@ -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) {}
|
||||
}
|
||||
|
|
|
@ -44,7 +44,6 @@ const int TEST_DISPATCH_TIME = 1;
|
|||
extern "C" {
|
||||
task_t * unittest_scheduler_selectedTask;
|
||||
uint8_t unittest_scheduler_selectedTaskDynPrio;
|
||||
uint16_t unittest_scheduler_waitingTasks;
|
||||
timeDelta_t unittest_scheduler_taskRequiredTimeUs;
|
||||
bool taskGyroRan = false;
|
||||
bool taskFilterRan = false;
|
||||
|
@ -402,14 +401,12 @@ TEST(SchedulerUnittest, TestTwoTasks)
|
|||
// no tasks should run, since neither task's desired time has elapsed
|
||||
scheduler();
|
||||
EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
|
||||
|
||||
// 500 microseconds later, TASK_ACCEL desiredPeriodUs has elapsed
|
||||
simulatedTime += 500;
|
||||
// TASK_ACCEL should now run
|
||||
scheduler();
|
||||
EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(1, unittest_scheduler_waitingTasks);
|
||||
EXPECT_EQ(5000 + TEST_UPDATE_ACCEL_TIME, simulatedTime);
|
||||
|
||||
simulatedTime += 1000 - TEST_UPDATE_ACCEL_TIME;
|
||||
|
@ -420,7 +417,6 @@ TEST(SchedulerUnittest, TestTwoTasks)
|
|||
scheduler();
|
||||
// No task should have run
|
||||
EXPECT_EQ(static_cast<task_t*>(0), unittest_scheduler_selectedTask);
|
||||
EXPECT_EQ(0, unittest_scheduler_waitingTasks);
|
||||
|
||||
simulatedTime = startTime + 10500; // TASK_ACCEL and TASK_ATTITUDE desiredPeriodUss have elapsed
|
||||
// of the two TASK_ACCEL should run first
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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) {}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue