From d5f62be0134c2390f0a3bfb62d4a81e33108a42b Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Fri, 14 Aug 2020 16:42:20 +0100 Subject: [PATCH] Trigger gyro SPI DMA reads in EXTI handler if supported and lock gyroTask loop to gyro to eliminate missed updates and jitter --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/cli/cli.c | 24 +- src/main/cli/settings.c | 4 +- src/main/common/time.h | 1 + src/main/drivers/accgyro/accgyro.h | 18 +- src/main/drivers/accgyro/accgyro_mpu.c | 185 ++++++++-- src/main/drivers/accgyro/accgyro_mpu.h | 2 - src/main/drivers/accgyro/accgyro_mpu6050.c | 2 - src/main/drivers/accgyro/accgyro_spi_bmi270.c | 200 ++++++++-- .../drivers/accgyro/accgyro_spi_mpu6000.c | 6 +- src/main/drivers/dshot_dpwm.c | 4 +- src/main/drivers/light_ws2811strip.c | 3 + src/main/drivers/nvic.h | 2 +- src/main/drivers/system.c | 15 +- src/main/drivers/system.h | 2 + src/main/fc/tasks.c | 18 +- src/main/flight/imu.c | 3 + src/main/io/dashboard.c | 4 - src/main/io/dashboard.h | 2 - src/main/io/gps.c | 4 +- src/main/io/ledstrip.c | 12 +- src/main/osd/osd.c | 7 + src/main/scheduler/scheduler.c | 346 ++++++++++++------ src/main/scheduler/scheduler.h | 32 +- src/main/sensors/barometer.c | 10 +- src/main/sensors/compass.c | 12 +- src/main/sensors/compass.h | 2 +- src/main/sensors/gyro_init.c | 14 + src/main/target/ALIENFLIGHTF1/target.h | 1 - src/main/target/ALIENFLIGHTF3/target.h | 1 - src/main/target/ALIENFLIGHTF4/target.h | 1 - src/main/target/ALIENFLIGHTNGF7/target.h | 1 - src/main/target/BEEROTORF4/target.h | 1 - src/main/target/BLUEJAYF4/target.h | 1 - src/main/target/CC3D/target.h | 2 - src/main/target/CRAZYBEEF3FR/target.h | 2 +- src/main/target/DOGE/target.h | 1 - src/main/target/ELLE0/target.h | 1 - src/main/target/FF_FORTINIF4/target.h | 3 +- src/main/target/FF_PIKOF4/target.h | 1 - src/main/target/FURYF4/target.c | 4 +- src/main/target/FURYF4/target.h | 4 +- src/main/target/KAKUTEF4/target.h | 7 +- src/main/target/LUX_RACE/target.h | 1 - src/main/target/MAMBAF722/target.h | 2 +- src/main/target/MICROSCISKY/target.h | 2 - src/main/target/NERO/target.h | 1 - src/main/target/REVO/target.h | 2 + src/main/target/REVONANO/target.h | 3 + src/main/target/SITL/target.c | 19 + src/main/target/SPARKY2/target.h | 1 - src/main/target/STM32F4DISCOVERY/target.h | 1 - src/main/target/WORMFC/target.h | 1 - src/main/target/common_pre.h | 5 +- src/test/unit/cli_unittest.cc | 5 +- src/test/unit/flight_imu_unittest.cc | 2 + src/test/unit/ledstrip_unittest.cc | 4 +- src/test/unit/link_quality_unittest.cc | 3 + src/test/unit/osd_unittest.cc | 2 + src/test/unit/scheduler_unittest.cc | 108 +----- src/test/unit/target.h | 2 +- src/test/unit/ws2811_unittest.cc | 3 + 63 files changed, 755 insertions(+), 379 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index be19a2307..85f94a166 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -99,4 +99,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "D_LPF", "VTX_TRAMP", "GHST", + "SCHEDULER_DETERMINISM" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 425cbb852..684a01d59 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -115,6 +115,7 @@ typedef enum { DEBUG_D_LPF, DEBUG_VTX_TRAMP, DEBUG_GHST, + DEBUG_SCHEDULER_DETERMINISM, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index a47e8cedb..208c7772f 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4782,6 +4782,19 @@ static void cliStatus(const char *cmdName, char *cmdline) cliPrintf(" gyro %d", pos + 1); } } +#ifdef USE_SPI +#ifdef USE_GYRO_EXTI + if (gyroActiveDev()->gyroModeSPI != GYRO_EXTI_NO_INT) { + cliPrintf(" locked"); + } + if (gyroActiveDev()->gyroModeSPI == GYRO_EXTI_INT_DMA) { + cliPrintf(" dma"); + } +#endif + if (spiGetExtDeviceCount(&gyroActiveDev()->dev) > 1) { + cliPrintf(" shared"); + } +#endif cliPrintLinefeed(); #if defined(USE_SENSOR_NAMES) @@ -4867,7 +4880,6 @@ static void cliStatus(const char *cmdName, char *cmdline) cliPrintLinefeed(); } -#if defined(USE_TASK_STATISTICS) static void cliTasks(const char *cmdName, char *cmdline) { UNUSED(cmdName); @@ -4923,10 +4935,14 @@ static void cliTasks(const char *cmdName, char *cmdline) getCheckFuncInfo(&checkFuncInfo); cliPrintLinef("RX Check Function %19d %7d %25d", checkFuncInfo.maxExecutionTimeUs, checkFuncInfo.averageExecutionTimeUs, checkFuncInfo.totalExecutionTimeUs / 1000); cliPrintLinef("Total (excluding SERIAL) %25d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); + if (debugMode == DEBUG_SCHEDULER_DETERMINISM) { + extern int32_t schedLoopStartCycles, taskGuardCycles; + + cliPrintLinef("Scheduler start cycles %d guard cycles %d", schedLoopStartCycles, taskGuardCycles); + } schedulerResetCheckFunctionMaxExecutionTime(); } } -#endif static void printVersion(const char *cmdName, bool printBoardInfo) { @@ -6584,9 +6600,7 @@ const clicmd_t cmdTable[] = { "\treverse r|n", cliServoMix), #endif CLI_COMMAND_DEF("status", "show status", NULL, cliStatus), -#if defined(USE_TASK_STATISTICS) CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks), -#endif #ifdef USE_TIMER_MGMT CLI_COMMAND_DEF("timer", "show/set timers", "<> | list | [af|none|] | list | show", cliTimer), #endif @@ -6832,8 +6846,6 @@ void cliEnter(serialPort_t *serialPort) cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort); cliErrorWriter = cliWriter; - schedulerSetCalulateTaskStatistics(systemConfig()->task_statistics); - #ifndef MINIMAL_CLI cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'"); #else diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 604e3e496..4d08be871 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -694,7 +694,7 @@ const clivalue_t valueTable[] = { { "mag_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.roll) }, { "mag_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.pitch) }, { "mag_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_customAlignment.yaw) }, - { "mag_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_bustype) }, + { "mag_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_busType) }, { "mag_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_device) }, { "mag_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_address) }, { "mag_spi_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_device) }, @@ -1469,9 +1469,7 @@ const clivalue_t valueTable[] = { #if defined(STM32F4) || defined(STM32G4) { "system_hse_mhz", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, hseMhz) }, #endif -#if defined(USE_TASK_STATISTICS) { "task_statistics", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, task_statistics) }, -#endif { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DEBUG }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, debug_mode) }, { "rate_6pos_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, rateProfile6PosSwitch) }, #ifdef USE_OVERCLOCK diff --git a/src/main/common/time.h b/src/main/common/time.h index 70727ea7a..29f4262bf 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -44,6 +44,7 @@ typedef uint32_t timeUs_t; #define TIMEZONE_OFFSET_MINUTES_MAX 780 // +13 hours static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } +static inline int32_t cmpTimeCycles(uint32_t a, uint32_t b) { return (int32_t)(a - b); } #define FORMATTED_DATE_TIME_BUFSIZE 30 diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index e4c30d27d..39c169913 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -25,6 +25,7 @@ #include "common/axis.h" #include "common/maths.h" #include "common/sensor_alignment.h" +#include "common/time.h" #include "drivers/exti.h" #include "drivers/bus.h" #include "drivers/sensor.h" @@ -79,6 +80,13 @@ typedef enum { GYRO_RATE_32_kHz, } gyroRateKHz_e; +typedef enum { + GYRO_EXTI_INIT = 0, + GYRO_EXTI_INT_DMA, + GYRO_EXTI_INT, + GYRO_EXTI_NO_INT +} gyroModeSPI_e; + typedef struct gyroDev_s { #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) pthread_mutex_t lock; @@ -97,7 +105,15 @@ typedef struct gyroDev_s { mpuDetectionResult_t mpuDetectionResult; sensor_align_e gyroAlign; gyroRateKHz_e gyroRateKHz; - bool dataReady; + gyroModeSPI_e gyroModeSPI; +#ifdef USE_GYRO_EXTI + uint32_t detectedEXTI; + uint32_t gyroLastEXTI; + uint32_t gyroSyncEXTI; + int32_t gyroShortPeriod; + int32_t gyroDmaMaxDuration; +#endif + volatile bool dataReady; bool gyro_high_fsr; uint8_t hardware_lpf; uint8_t hardware_32khz_lpf; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index aa1252202..18d9f9370 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -67,6 +67,9 @@ #define MPU_INQUIRY_MASK 0x7E +// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity +#define GYRO_EXTI_DETECT_THRESHOLD 1000 + #ifdef USE_I2C_GYRO static void mpu6050FindRevision(gyroDev_t *gyro) { @@ -107,21 +110,60 @@ static void mpu6050FindRevision(gyroDev_t *gyro) * Gyro interrupt service routine */ #ifdef USE_GYRO_EXTI +#ifdef USE_SPI_GYRO +// Called in ISR context +// Gyro read has just completed +busStatus_e mpuIntcallback(uint32_t arg) +{ + volatile gyroDev_t *gyro = (gyroDev_t *)arg; + int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI); + + if (gyroDmaDuration > gyro->gyroDmaMaxDuration) { + gyro->gyroDmaMaxDuration = gyroDmaDuration; + } + + gyro->dataReady = true; + + return BUS_READY; +} + +static void mpuIntExtiHandler(extiCallbackRec_t *cb) +{ + // Non-blocking, so this needs to be static + static busSegment_t segments[] = { + {NULL, NULL, 15, true, mpuIntcallback}, + {NULL, NULL, 0, true, NULL}, + }; + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + + // Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does + // not have an associated timer + uint32_t nowCycles = getCycleCounter(); + int32_t gyroLastPeriod = cmpTimeCycles(nowCycles, gyro->gyroLastEXTI); + // This detects the short (~79us) EXTI interval of an MPU6xxx gyro + if ((gyro->gyroShortPeriod == 0) || (gyroLastPeriod < gyro->gyroShortPeriod)) { + gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration; + } + gyro->gyroLastEXTI = nowCycles; + + if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) { + segments[0].txData = gyro->dev.txBuf;; + segments[0].rxData = &gyro->dev.rxBuf[1]; + + if (!spiIsBusy(&gyro->dev)) { + spiSequence(&gyro->dev, &segments[0]); + } + } + + gyro->detectedEXTI++; +} +#else static void mpuIntExtiHandler(extiCallbackRec_t *cb) { -#ifdef DEBUG_MPU_DATA_READY_INTERRUPT - static uint32_t lastCalledAtUs = 0; - const uint32_t nowUs = micros(); - debug[0] = (uint16_t)(nowUs - lastCalledAtUs); - lastCalledAtUs = nowUs; -#endif gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); gyro->dataReady = true; -#ifdef DEBUG_MPU_DATA_READY_INTERRUPT - const uint32_t now2Us = micros(); - debug[1] = (uint16_t)(now2Us - nowUs); -#endif } +#endif static void mpuIntExtiInit(gyroDev_t *gyro) { @@ -177,37 +219,128 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } + #ifdef USE_SPI_GYRO bool mpuAccReadSPI(accDev_t *acc) { - STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_ACCEL_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - STATIC_DMA_DATA_AUTO uint8_t data[7]; + switch (acc->gyro->gyroModeSPI) { + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&acc->gyro->dev); - const bool ack = spiReadWriteBufRB(&acc->gyro->dev, dataToSend, data, 7); - if (!ack) { - return false; + acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80; + + busSegment_t segments[] = { + {NULL, NULL, 7, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + segments[0].txData = acc->gyro->dev.txBuf; + segments[0].rxData = &acc->gyro->dev.rxBuf[1]; + + spiSequence(&acc->gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&acc->gyro->dev); + + // Fall through + FALLTHROUGH; } - acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); - acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); - acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + + // This data was read from the gyro, which is the same SPI device as the acc + uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + acc->ADCRaw[X] = __builtin_bswap16(accData[1]); + acc->ADCRaw[Y] = __builtin_bswap16(accData[2]); + acc->ADCRaw[Z] = __builtin_bswap16(accData[3]); + break; + } + + case GYRO_EXTI_INIT: + default: + break; + } return true; } bool mpuGyroReadSPI(gyroDev_t *gyro) { - STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - STATIC_DMA_DATA_AUTO uint8_t data[7]; + uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + switch (gyro->gyroModeSPI) { + case GYRO_EXTI_INIT: + { + // Initialise the tx buffer to all 0xff + memset(gyro->dev.txBuf, 0xff, 16); +#ifdef USE_GYRO_EXTI + // Check that minimum number of interrupts have been detected - const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7); - if (!ack) { - return false; + // 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 + spiSetAtomicWait(&gyro->dev); + gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; + gyro->dev.callbackArg = (uint32_t)gyro; + gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80; + } else { + // Interrupts are present, but no DMA + gyro->gyroModeSPI = GYRO_EXTI_INT; + } + } else +#endif + { + gyro->gyroModeSPI = GYRO_EXTI_NO_INT; + } + break; } - gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); - gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); - gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&gyro->dev); + + gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80; + + busSegment_t segments[] = { + {NULL, NULL, 7, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + segments[0].txData = gyro->dev.txBuf; + segments[0].rxData = &gyro->dev.rxBuf[1]; + + spiSequence(&gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&gyro->dev); + + gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[1]); + gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[2]); + gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[3]); + break; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[5]); + gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[6]); + gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[7]); + break; + } + + default: + break; + } return true; } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index f296ad541..0077dcc26 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -24,8 +24,6 @@ #include "drivers/exti.h" #include "drivers/sensor.h" -//#define DEBUG_MPU_DATA_READY_INTERRUPT - #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \ || defined(USE_GYRO_SPI_ICM20689) #define GYRO_USES_SPI diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c index 78979e6bd..c2c479a18 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro/accgyro_mpu6050.c @@ -41,8 +41,6 @@ #include "accgyro_mpu.h" #include "accgyro_mpu6050.h" -//#define DEBUG_MPU_DATA_READY_INTERRUPT - // MPU6050, Standard address 0x68 // MPU_INT on PB13 on rev4 Naze32 hardware #define MPU6050_ADDRESS 0x68 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 62f3a8c11..46d781bbf 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -20,6 +20,8 @@ #include #include +#include +#include #include "platform.h" @@ -33,6 +35,7 @@ #include "drivers/io_impl.h" #include "drivers/nvic.h" #include "drivers/sensor.h" +#include "drivers/system.h" #include "drivers/time.h" // 10 MHz max SPI frequency @@ -123,6 +126,9 @@ typedef enum { BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB } bmi270ConfigValues_e; +// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity +#define GYRO_EXTI_DETECT_THRESHOLD 1000 + // BMI270 register reads are 16bits with the first byte a "dummy" value 0 // that must be ignored. The result is in the second byte. static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId) @@ -246,11 +252,52 @@ static void bmi270Config(gyroDev_t *gyro) extiCallbackRec_t bmi270IntCallbackRec; -#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) +/* + * Gyro interrupt service routine + */ +#ifdef USE_GYRO_EXTI +// Called in ISR context +// Gyro read has just completed +busStatus_e bmi270Intcallback(uint32_t arg) +{ + volatile gyroDev_t *gyro = (gyroDev_t *)arg; + int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI); + + if (gyroDmaDuration > gyro->gyroDmaMaxDuration) { + gyro->gyroDmaMaxDuration = gyroDmaDuration; + } + + gyro->dataReady = true; + + return BUS_READY; +} + void bmi270ExtiHandler(extiCallbackRec_t *cb) { + // Non-blocking, so this needs to be static + static busSegment_t segments[] = { + {NULL, NULL, 14, true, bmi270Intcallback}, + {NULL, NULL, 0, true, NULL}, + }; + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); - gyro->dataReady = true; + // Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does + // not have an associated timer + uint32_t nowCycles = getCycleCounter(); + gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration; + gyro->gyroLastEXTI = nowCycles; + + if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) { + segments[0].txData = gyro->dev.txBuf; + segments[0].rxData = gyro->dev.rxBuf; + + if (!spiIsBusy(&gyro->dev)) { + spiSequence(&gyro->dev, &segments[0]); + } + } + + gyro->detectedEXTI++; + } static void bmi270IntExtiInit(gyroDev_t *gyro) @@ -266,56 +313,135 @@ static void bmi270IntExtiInit(gyroDev_t *gyro) EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING); EXTIEnable(mpuIntIO, true); } +#else +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} #endif static bool bmi270AccRead(accDev_t *acc) { - enum { - IDX_REG = 0, - IDX_SKIP, - IDX_ACCEL_XOUT_L, - IDX_ACCEL_XOUT_H, - IDX_ACCEL_YOUT_L, - IDX_ACCEL_YOUT_H, - IDX_ACCEL_ZOUT_L, - IDX_ACCEL_ZOUT_H, - BUFFER_SIZE, - }; + switch (acc->gyro->gyroModeSPI) { + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&acc->gyro->dev); - STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; - STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; - spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + busSegment_t segments[] = { + {NULL, NULL, 8, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + segments[0].txData = acc->gyro->dev.txBuf; + segments[0].rxData = acc->gyro->dev.rxBuf; - acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); - acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); - acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); + spiSequence(&acc->gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&acc->gyro->dev); + + // Fall through + FALLTHROUGH; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + + // This data was read from the gyro, which is the same SPI device as the acc + uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + acc->ADCRaw[X] = accData[1]; + acc->ADCRaw[Y] = accData[2]; + acc->ADCRaw[Z] = accData[3]; + break; + } + + case GYRO_EXTI_INIT: + default: + break; + } return true; } static bool bmi270GyroReadRegister(gyroDev_t *gyro) { - enum { - IDX_REG = 0, - IDX_SKIP, - IDX_GYRO_XOUT_L, - IDX_GYRO_XOUT_H, - IDX_GYRO_YOUT_L, - IDX_GYRO_YOUT_H, - IDX_GYRO_ZOUT_L, - IDX_GYRO_ZOUT_H, - BUFFER_SIZE, - }; + uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + switch (gyro->gyroModeSPI) { + case GYRO_EXTI_INIT: + { + // Initialise the tx buffer to all 0x00 + memset(gyro->dev.txBuf, 0x00, 14); +#ifdef USE_GYRO_EXTI + // Check that minimum number of interrupts have been detected - STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; - STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + // 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 + spiSetAtomicWait(&gyro->dev); + gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; + gyro->dev.callbackArg = (uint32_t)gyro; + gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; + } else { + // Interrupts are present, but no DMA + gyro->gyroModeSPI = GYRO_EXTI_INT; + } + } else +#endif + { + gyro->gyroModeSPI = GYRO_EXTI_NO_INT; + } + break; + } - spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + // Ensure any prior DMA has completed before continuing + spiWaitClaim(&gyro->dev); - gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); - gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); - gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80; + + busSegment_t segments[] = { + {NULL, NULL, 8, true, NULL}, + {NULL, NULL, 0, true, NULL}, + }; + segments[0].txData = gyro->dev.txBuf; + segments[0].rxData = gyro->dev.rxBuf; + + spiSequence(&gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&gyro->dev); + + gyro->gyroADCRaw[X] = gyroData[1]; + gyro->gyroADCRaw[Y] = gyroData[2]; + gyro->gyroADCRaw[Z] = gyroData[3]; + + break; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + gyro->gyroADCRaw[X] = gyroData[4]; + gyro->gyroADCRaw[Y] = gyroData[5]; + gyro->gyroADCRaw[Z] = gyroData[6]; + break; + } + + default: + break; + } return true; } @@ -401,7 +527,7 @@ static void bmi270SpiGyroInit(gyroDev_t *gyro) { bmi270Config(gyro); -#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) +#if defined(USE_GYRO_EXTI) bmi270IntExtiInit(gyro); #endif } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index ae8d8d56e..63e7b6034 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -52,6 +52,8 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro); // 20 MHz max SPI frequency #define MPU6000_MAX_SPI_CLK_HZ 20000000 +#define MPU6000_SHORT_THRESHOLD 82 // Any interrupt interval less than this will be recognised as the short interval of ~79us + // Bits #define BIT_SLEEP 0x40 #define BIT_H_RESET 0x80 @@ -235,7 +237,9 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro) gyro->initFn = mpu6000SpiGyroInit; gyro->readFn = mpuGyroReadSPI; gyro->scale = GYRO_SCALE_2000DPS; - +#ifdef USE_GYRO_EXTI + gyro->gyroShortPeriod = clockMicrosToCycles(MPU6000_SHORT_THRESHOLD); +#endif return true; } diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c index 70b6c2283..de26d589d 100644 --- a/src/main/drivers/dshot_dpwm.c +++ b/src/main/drivers/dshot_dpwm.c @@ -53,7 +53,7 @@ FAST_DATA_ZERO_INIT bool useDshotTelemetry = false; FAST_DATA_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; -FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet) +FAST_CODE_NOINLINE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet) { int i; for (i = 0; i < 16; i++) { @@ -66,7 +66,7 @@ FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t p return DSHOT_DMA_BUFFER_SIZE; } -FAST_CODE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet) +FAST_CODE_NOINLINE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet) { int i; for (i = 0; i < 4; i++) { diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 0694be8f6..43db606fc 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -44,6 +44,8 @@ #include "light_ws2811strip.h" +#include "scheduler/scheduler.h" + #ifdef USE_LEDSTRIP_CACHE_MGMT // WS2811_DMA_BUFFER_SIZE is multiples of uint32_t // Number of bytes required for buffer @@ -179,6 +181,7 @@ void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat) { // don't wait - risk of infinite block, just get an update next time round if (!ws2811Initialised || ws2811LedDataTransferInProgress) { + ignoreTaskStateTime(); return; } diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index f0aeac6bb..20c9fd04e 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -70,7 +70,7 @@ #define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0) #define NVIC_PRIO_USB_WUP NVIC_BUILD_PRIORITY(1, 0) #define NVIC_PRIO_SONAR_ECHO NVIC_BUILD_PRIORITY(0x0f, 0x0f) -#define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f) +#define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0, 1) #define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 782ac0a83..77aa117f8 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -68,7 +68,7 @@ void cycleCounterInit(void) #if defined(DWT_LAR_UNLOCK_VALUE) #if defined(STM32F7) || defined(STM32H7) - DWT->LAR = DWT_LAR_UNLOCK_VALUE; + ITM->LAR = DWT_LAR_UNLOCK_VALUE; #elif defined(STM32F3) || defined(STM32F4) // Note: DWT_Type does not contain LAR member. #define DWT_LAR @@ -146,7 +146,7 @@ uint32_t micros(void) return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; } -inline uint32_t getCycleCounter(void) +uint32_t getCycleCounter(void) { return DWT->CYCCNT; } @@ -156,6 +156,17 @@ uint32_t clockCyclesToMicros(uint32_t clockCycles) return clockCycles / usTicks; } +// Note that this conversion is signed as this is used for periods rather than absolute timestamps +int32_t clockCyclesTo10thMicros(int32_t clockCycles) +{ + return 10 * clockCycles / (int32_t)usTicks; +} + +uint32_t clockMicrosToCycles(uint32_t micros) +{ + return micros * usTicks; +} + // Return system uptime in milliseconds (rollover in 49 days) uint32_t millis(void) { diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index aeebeb95b..52fbca030 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -66,6 +66,8 @@ void systemResetToBootloader(bootloaderRequestType_e requestType); bool isMPUSoftReset(void); void cycleCounterInit(void); uint32_t clockCyclesToMicros(uint32_t clockCycles); +int32_t clockCyclesTo10thMicros(int32_t clockCycles); +uint32_t clockMicrosToCycles(uint32_t micros); uint32_t getCycleCounter(void); #if defined(STM32H7) || defined(STM32G4) void systemCheckResetReason(void); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 7015f1d9e..f9313385d 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -81,6 +81,8 @@ #include "rx/rx.h" +#include "scheduler/scheduler.h" + #include "sensors/acceleration.h" #include "sensors/adcinternal.h" #include "sensors/barometer.h" @@ -91,8 +93,6 @@ #include "sensors/sensors.h" #include "sensors/rangefinder.h" -#include "scheduler/scheduler.h" - #include "telemetry/telemetry.h" #include "telemetry/crsf.h" @@ -170,9 +170,9 @@ bool taskUpdateRxMainInProgress() static void taskUpdateRxMain(timeUs_t currentTimeUs) { - // Where we are using a state machine call ignoreTaskTime() for all states bar one + // Where we are using a state machine call ignoreTaskStateTime() for all states bar one if (rxState != MODES) { - ignoreTaskTime(); + ignoreTaskStateTime(); } switch (rxState) { @@ -418,7 +418,6 @@ void tasksInit(void) #endif } -#if defined(USE_TASK_STATISTICS) #define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \ .taskName = taskNameParam, \ .subTaskName = subTaskNameParam, \ @@ -427,15 +426,6 @@ void tasksInit(void) .desiredPeriodUs = desiredPeriodParam, \ .staticPriority = staticPriorityParam \ } -#else -#define DEFINE_TASK(taskNameParam, subTaskNameParam, checkFuncParam, taskFuncParam, desiredPeriodParam, staticPriorityParam) { \ - .checkFunc = checkFuncParam, \ - .taskFunc = taskFuncParam, \ - .desiredPeriodUs = desiredPeriodParam, \ - .staticPriority = staticPriorityParam \ -} -#endif - task_t tasks[TASK_COUNT] = { [TASK_SYSTEM] = DEFINE_TASK("SYSTEM", "LOAD", NULL, taskSystemLoad, TASK_PERIOD_HZ(10), TASK_PRIORITY_MEDIUM_HIGH), diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 0eb2f9133..38ad16a46 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -45,6 +45,8 @@ #include "io/gps.h" +#include "scheduler/scheduler.h" + #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" @@ -591,6 +593,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) acc.accADC[X] = 0; acc.accADC[Y] = 0; acc.accADC[Z] = 0; + ignoreTaskStateTime(); } } #endif // USE_ACC diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 9765edb7b..03f144726 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -543,7 +543,6 @@ static void showSensorsPage(void) } -#if defined(USE_TASK_STATISTICS) static void showTasksPage(void) { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; @@ -568,7 +567,6 @@ static void showTasksPage(void) } } } -#endif #ifdef USE_BLACKBOX static void showBBPage(void) @@ -624,9 +622,7 @@ static const pageEntry_t pages[PAGE_COUNT] = { { PAGE_RX, "RX", showRxPage, PAGE_FLAGS_NONE }, { PAGE_BATTERY, "BATTERY", showBatteryPage, PAGE_FLAGS_NONE }, { PAGE_SENSORS, "SENSORS", showSensorsPage, PAGE_FLAGS_NONE }, -#if defined(USE_TASK_STATISTICS) { PAGE_TASKS, "TASKS", showTasksPage, PAGE_FLAGS_NONE }, -#endif #ifdef USE_BLACKBOX { PAGE_BB, "BLACK BOX", showBBPage, PAGE_FLAGS_NONE }, #endif diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h index 53597795a..efc58bf12 100644 --- a/src/main/io/dashboard.h +++ b/src/main/io/dashboard.h @@ -44,9 +44,7 @@ typedef enum { PAGE_RX, PAGE_PROFILE, PAGE_RPROF, -#if defined(USE_TASK_STATISTICS) PAGE_TASKS, -#endif #ifdef USE_GPS PAGE_GPS, #endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 76263d097..33ab67642 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -755,14 +755,14 @@ void gpsUpdate(timeUs_t currentTimeUs) updateGPSRescueState(); } #endif - // Call ignoreTaskTime() unless this took appreciable time + // Call ignoreTaskShortExecTime() unless this took appreciable time // Note that this will mess up the rate/Hz display under tasks, but the code // takes widely varying time to complete endTimeUs = micros(); if ((endTimeUs - currentTimeUs) > maxTimeUs) { maxTimeUs = endTimeUs - currentTimeUs; } else { - ignoreTaskTime(); + ignoreTaskShortExecTime(); // Decay max time maxTimeUs--; } diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index dd2e014ff..06730f646 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1066,8 +1066,8 @@ static void applyStatusProfile(timeUs_t now) { } if (!timActive) { - // Call ignoreTaskTime() unless data is being processed - ignoreTaskTime(); + // Call ignoreTaskShortExecTime() unless data is being processed + ignoreTaskShortExecTime(); return; // no change this update, keep old state } @@ -1253,8 +1253,8 @@ void ledStripUpdate(timeUs_t currentTimeUs) #endif if (!isWS2811LedStripReady()) { - // Call ignoreTaskTime() unless data is being processed - ignoreTaskTime(); + // Call ignoreTaskShortExecTime() unless data is being processed + ignoreTaskShortExecTime(); return; } @@ -1282,8 +1282,8 @@ void ledStripUpdate(timeUs_t currentTimeUs) break; } } else { - // Call ignoreTaskTime() unless data is being processed - ignoreTaskTime(); + // Call ignoreTaskShortExecTime() unless data is being processed + ignoreTaskShortExecTime(); } } diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 82500344f..c620d2103 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -87,6 +87,8 @@ #include "rx/crsf.h" #include "rx/rx.h" +#include "scheduler/scheduler.h" + #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/esc_sensor.h" @@ -1049,6 +1051,8 @@ void osdUpdate(timeUs_t currentTimeUs) // Frsky osd need a display redraw after search for MAX7456 devices if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) { displayRedraw(osdDisplayPort); + } else { + ignoreTaskShortExecTime(); } return; } @@ -1062,6 +1066,7 @@ void osdUpdate(timeUs_t currentTimeUs) // don't touch buffers if DMA transaction is in progress if (displayIsTransferInProgress(osdDisplayPort)) { + ignoreTaskShortExecTime(); return; } @@ -1069,6 +1074,7 @@ void osdUpdate(timeUs_t currentTimeUs) static uint32_t idlecounter = 0; if (!ARMING_FLAG(ARMED)) { if (idlecounter++ % 4 != 0) { + ignoreTaskShortExecTime(); return; } } @@ -1091,6 +1097,7 @@ void osdUpdate(timeUs_t currentTimeUs) if (doDrawScreen) { displayDrawScreen(osdDisplayPort); } + ignoreTaskShortExecTime(); } ++counter; } diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 520bf0b3f..4c47aea67 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -26,6 +26,8 @@ #include "platform.h" +#include "drivers/accgyro/accgyro.h" + #include "build/build_config.h" #include "build/debug.h" @@ -34,14 +36,15 @@ #include "common/utils.h" #include "drivers/time.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/system.h" #include "fc/core.h" #include "fc/tasks.h" #include "scheduler.h" -#define TASK_AVERAGE_EXECUTE_FALLBACK_US 30 // Default task average time if USE_TASK_STATISTICS is not defined -#define TASK_AVERAGE_EXECUTE_PADDING_US 5 // Add a little padding to the average execution time +#include "sensors/gyro_init.h" // DEBUG_SCHEDULER, timings for: // 0 - gyroUpdate() @@ -49,21 +52,48 @@ // 2 - time spent in scheduler // 3 - time spent executing check function +// DEBUG_SCHEDULER_DETERMINISM, requires USE_LATE_TASK_STATISTICS to be defined +// 0 - Gyro task start cycle time in 10th of a us +// 1 - ID of late task +// 2 - Amount task is late in 10th of a us +// 3 - Gyro lock skew in clock cycles + +extern task_t tasks[]; + static FAST_DATA_ZERO_INIT task_t *currentTask = NULL; -static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskTime; +static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecRate; +static FAST_DATA_ZERO_INIT bool ignoreCurrentTaskExecTime; + +// More than this many cycles overdue will be considered late +#define DEBUG_SCHEDULER_DETERMINISM_THRESHOLD 10 + +int32_t schedLoopStartCycles; +static int32_t schedLoopStartMinCycles; +static int32_t schedLoopStartMaxCycles; +static uint32_t schedLoopStartDeltaDownCycles; +static uint32_t schedLoopStartDeltaUpCycles; + +int32_t taskGuardCycles; +static int32_t taskGuardMinCycles; +static int32_t taskGuardMaxCycles; +static uint32_t taskGuardDeltaDownCycles; +static uint32_t taskGuardDeltaUpCycles; static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasks; static FAST_DATA_ZERO_INIT uint32_t totalWaitingTasksSamples; -static FAST_DATA_ZERO_INIT bool calculateTaskStatistics; FAST_DATA_ZERO_INIT uint16_t averageSystemLoadPercent = 0; static FAST_DATA_ZERO_INIT int taskQueuePos = 0; STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT int taskQueueSize = 0; -static FAST_DATA int periodCalculationBasisOffset = offsetof(task_t, lastExecutedAtUs); +static bool optimizeSchedRate; + static FAST_DATA_ZERO_INIT bool gyroEnabled; +static int32_t desiredPeriodCycles; +static uint32_t lastTargetCycles; + // No need for a linked list for the queue, since items are only inserted at startup STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue @@ -145,7 +175,6 @@ void taskSystemLoad(timeUs_t currentTimeUs) #endif } -#if defined(USE_TASK_STATISTICS) timeUs_t checkFuncMaxExecutionTimeUs; timeUs_t checkFuncTotalExecutionTimeUs; timeUs_t checkFuncMovingSumExecutionTimeUs; @@ -158,14 +187,12 @@ void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo) checkFuncInfo->averageExecutionTimeUs = checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; checkFuncInfo->averageDeltaTimeUs = checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; } -#endif void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo) { taskInfo->isEnabled = queueContains(getTask(taskId)); taskInfo->desiredPeriodUs = getTask(taskId)->desiredPeriodUs; taskInfo->staticPriority = getTask(taskId)->staticPriority; -#if defined(USE_TASK_STATISTICS) taskInfo->taskName = getTask(taskId)->taskName; taskInfo->subTaskName = getTask(taskId)->subTaskName; taskInfo->maxExecutionTimeUs = getTask(taskId)->maxExecutionTimeUs; @@ -179,7 +206,6 @@ void getTaskInfo(taskId_e taskId, taskInfo_t * taskInfo) taskInfo->runCount = getTask(taskId)->runCount; taskInfo->execTime = getTask(taskId)->execTime; #endif -#endif } void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs) @@ -191,6 +217,11 @@ void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs) task_t *task = getTask(taskId); task->desiredPeriodUs = MAX(SCHEDULER_DELAY_LIMIT, newPeriodUs); // Limit delay to 100us (10 kHz) to prevent scheduler clogging } + + // Catch the case where the gyro loop is adjusted + if (taskId == TASK_GYRO) { + desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs); + } } void setTaskEnabled(taskId_e taskId, bool enabled) @@ -216,20 +247,22 @@ timeDelta_t getTaskDeltaTimeUs(taskId_e taskId) } } -// Called by tasks executing what are known to be short states -void ignoreTaskTime() +// 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. +void ignoreTaskStateTime() { - ignoreCurrentTaskTime = true; + ignoreCurrentTaskExecRate = true; + ignoreCurrentTaskExecTime = true; } -void schedulerSetCalulateTaskStatistics(bool calculateTaskStatisticsToUse) +// Called by tasks without state machines executing in what is known to be a shorter time than peak +void ignoreTaskShortExecTime() { - calculateTaskStatistics = calculateTaskStatisticsToUse; + ignoreCurrentTaskExecTime = true; } void schedulerResetTaskStatistics(taskId_e taskId) { -#if defined(USE_TASK_STATISTICS) if (taskId == TASK_SELF) { currentTask->movingSumExecutionTimeUs = 0; currentTask->movingSumDeltaTimeUs = 0; @@ -241,14 +274,10 @@ void schedulerResetTaskStatistics(taskId_e taskId) getTask(taskId)->totalExecutionTimeUs = 0; getTask(taskId)->maxExecutionTimeUs = 0; } -#else - UNUSED(taskId); -#endif } void schedulerResetTaskMaxExecutionTime(taskId_e taskId) { -#if defined(USE_TASK_STATISTICS) if (taskId == TASK_SELF) { currentTask->maxExecutionTimeUs = 0; } else if (taskId < TASK_COUNT) { @@ -259,37 +288,46 @@ void schedulerResetTaskMaxExecutionTime(taskId_e taskId) task->runCount = 0; #endif } -#else - UNUSED(taskId); -#endif } -#if defined(USE_TASK_STATISTICS) void schedulerResetCheckFunctionMaxExecutionTime(void) { checkFuncMaxExecutionTimeUs = 0; } -#endif void schedulerInit(void) { - calculateTaskStatistics = true; queueClear(); queueAdd(getTask(TASK_SYSTEM)); + + schedLoopStartMinCycles = clockMicrosToCycles(SCHED_START_LOOP_MIN_US); + schedLoopStartMaxCycles = clockMicrosToCycles(SCHED_START_LOOP_MAX_US); + schedLoopStartCycles = schedLoopStartMinCycles; + schedLoopStartDeltaDownCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_DOWN_STEP; + schedLoopStartDeltaUpCycles = clockMicrosToCycles(1) / SCHED_START_LOOP_UP_STEP; + + taskGuardMinCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MIN_US); + taskGuardMaxCycles = clockMicrosToCycles(TASK_GUARD_MARGIN_MAX_US); + taskGuardCycles = taskGuardMinCycles; + taskGuardDeltaDownCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_DOWN_STEP; + taskGuardDeltaUpCycles = clockMicrosToCycles(1) / TASK_GUARD_MARGIN_UP_STEP; + + desiredPeriodCycles = (int32_t)clockMicrosToCycles((uint32_t)getTask(TASK_GYRO)->desiredPeriodUs); + + lastTargetCycles = getCycleCounter(); } void schedulerOptimizeRate(bool optimizeRate) { - periodCalculationBasisOffset = optimizeRate ? offsetof(task_t, lastDesiredAt) : offsetof(task_t, lastExecutedAtUs); + optimizeSchedRate = optimizeRate; } inline static timeUs_t getPeriodCalculationBasis(const task_t* task) { - if (task->staticPriority == TASK_PRIORITY_REALTIME) { - return *(timeUs_t*)((uint8_t*)task + periodCalculationBasisOffset); - } else { - return task->lastExecutedAtUs; + if ((task->staticPriority == TASK_PRIORITY_REALTIME) && (optimizeSchedRate)) { + return task->lastDesiredAt; } + return task->lastExecutedAtUs; } FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTimeUs) @@ -298,38 +336,33 @@ FAST_CODE timeUs_t schedulerExecuteTask(task_t *selectedTask, timeUs_t currentTi if (selectedTask) { currentTask = selectedTask; - ignoreCurrentTaskTime = false; -#if defined(USE_TASK_STATISTICS) + ignoreCurrentTaskExecRate = false; + ignoreCurrentTaskExecTime = false; float period = currentTimeUs - selectedTask->lastExecutedAtUs; -#endif selectedTask->lastExecutedAtUs = currentTimeUs; selectedTask->lastDesiredAt += (cmpTimeUs(currentTimeUs, selectedTask->lastDesiredAt) / selectedTask->desiredPeriodUs) * selectedTask->desiredPeriodUs; selectedTask->dynamicPriority = 0; // Execute task -#if defined(USE_TASK_STATISTICS) - if (calculateTaskStatistics) { - const timeUs_t currentTimeBeforeTaskCallUs = micros(); - selectedTask->taskFunc(currentTimeBeforeTaskCallUs); - taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs; - if (!ignoreCurrentTaskTime) { - selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs); - selectedTask->lastStatsAtUs = currentTimeUs; - selectedTask->movingSumExecutionTimeUs += taskExecutionTimeUs - selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; - selectedTask->movingSumDeltaTimeUs += selectedTask->taskLatestDeltaTimeUs - selectedTask->movingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; - selectedTask->maxExecutionTimeUs = MAX(selectedTask->maxExecutionTimeUs, taskExecutionTimeUs); + const timeUs_t currentTimeBeforeTaskCallUs = micros(); + selectedTask->taskFunc(currentTimeBeforeTaskCallUs); + taskExecutionTimeUs = micros() - currentTimeBeforeTaskCallUs; + if (!ignoreCurrentTaskExecRate) { + // Record task execution rate and max execution time + selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastStatsAtUs); + selectedTask->lastStatsAtUs = currentTimeUs; + 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); #if defined(USE_LATE_TASK_STATISTICS) - selectedTask->runCount++; + selectedTask->runCount++; #endif - } else -#endif - { - selectedTask->taskLatestDeltaTimeUs = cmpTimeUs(currentTimeUs, selectedTask->lastExecutedAtUs); - selectedTask->taskFunc(currentTimeUs); - } } return taskExecutionTimeUs; @@ -350,79 +383,147 @@ static void readSchedulerLocals(task_t *selectedTask, uint8_t selectedTaskDynami FAST_CODE void scheduler(void) { -#if defined(USE_LATE_TASK_STATISTICS) - static task_t *lastTask; -#endif // Cache currentTime const timeUs_t schedulerStartTimeUs = micros(); - timeUs_t currentTimeUs = schedulerStartTimeUs; + timeUs_t currentTimeUs; + uint32_t nowCycles; timeUs_t taskExecutionTimeUs = 0; task_t *selectedTask = NULL; uint16_t selectedTaskDynamicPriority = 0; uint16_t waitingTasks = 0; - bool realtimeTaskRan = false; - timeDelta_t gyroTaskDelayUs = 0; + uint32_t nextTargetCycles = 0; + int32_t schedLoopRemainingCycles; if (gyroEnabled) { // Realtime gyro/filtering/PID tasks get complete priority task_t *gyroTask = getTask(TASK_GYRO); - const timeUs_t gyroExecuteTimeUs = getPeriodCalculationBasis(gyroTask) + gyroTask->desiredPeriodUs; - gyroTaskDelayUs = cmpTimeUs(gyroExecuteTimeUs, currentTimeUs); // time until the next expected gyro sample - if (gyroTaskDelayUs <= 0) { - taskExecutionTimeUs = schedulerExecuteTask(gyroTask, currentTimeUs); + nowCycles = getCycleCounter(); +#if defined(UNIT_TEST) + lastTargetCycles = clockMicrosToCycles(gyroTask->lastExecutedAtUs); +#endif + nextTargetCycles = lastTargetCycles + desiredPeriodCycles; + schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); + + if (schedLoopRemainingCycles < -desiredPeriodCycles) { + /* A task has so grossly overrun that at entire gyro cycle has been skipped + * This is most likely to occur when connected to the configurator via USB as the serial + * task is non-deterministic + * Recover as best we can, advancing scheduling by a whole number of cycles + */ + nextTargetCycles += desiredPeriodCycles * (1 + (schedLoopRemainingCycles / -desiredPeriodCycles)); + schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); + } + + // Tune out the time lost between completing the last task execution and re-entering the scheduler + if ((schedLoopRemainingCycles < schedLoopStartMinCycles) && + (schedLoopStartCycles < schedLoopStartMaxCycles)) { + schedLoopStartCycles += schedLoopStartDeltaUpCycles; + } + + // Once close to the timing boundary, poll for it's arrival + if (schedLoopRemainingCycles < schedLoopStartCycles) { + // Record the interval between scheduling the gyro task + static int32_t lastRealtimeStartCycles = 0; + + if (schedLoopStartCycles > schedLoopStartMinCycles) { + schedLoopStartCycles -= schedLoopStartDeltaDownCycles; + } +#if !defined(UNIT_TEST) + while (schedLoopRemainingCycles > 0) { + nowCycles = getCycleCounter(); + schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); + } +#endif + DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastRealtimeStartCycles))); + lastRealtimeStartCycles = nowCycles; + + currentTimeUs = clockCyclesToMicros(nowCycles); + taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs); + if (gyroFilterReady()) { taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs); } if (pidLoopReady()) { taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs); } - currentTimeUs = micros(); - realtimeTaskRan = true; -#if defined(USE_LATE_TASK_STATISTICS) - // Late, so make a note of the offending task - if (gyroTaskDelayUs < -1) { - if (lastTask) { - lastTask->lateCount++; + lastTargetCycles = nextTargetCycles; + +#ifdef USE_GYRO_EXTI + gyroDev_t *gyro = gyroActiveDev(); + + // Bring the scheduler into lock with the gyro + if (gyro->gyroModeSPI != GYRO_EXTI_NO_INT) { + // Track the actual gyro rate over given number of cycle times and set the expected timebase + static uint32_t terminalGyroRateCount = 0; + static int32_t sampleRateStartCycles; + + if ((terminalGyroRateCount == 0)) { + terminalGyroRateCount = gyro->detectedEXTI + GYRO_RATE_COUNT; + sampleRateStartCycles = nowCycles; + } + + if (gyro->detectedEXTI >= terminalGyroRateCount) { + // Calculate the number of clock cycles on average between gyro interrupts + uint32_t sampleCycles = nowCycles - sampleRateStartCycles; + desiredPeriodCycles = sampleCycles / GYRO_RATE_COUNT; + sampleRateStartCycles = nowCycles; + terminalGyroRateCount += GYRO_RATE_COUNT; + } + + // Track the actual gyro rate over given number of cycle times and remove skew + static uint32_t terminalGyroLockCount = 0; + static int32_t accGyroSkew = 0; + + int32_t gyroSkew = cmpTimeCycles(nextTargetCycles, gyro->gyroSyncEXTI) % desiredPeriodCycles; + if (gyroSkew > (desiredPeriodCycles / 2)) { + gyroSkew -= desiredPeriodCycles; + } + + accGyroSkew += gyroSkew; + + if ((terminalGyroLockCount == 0)) { + terminalGyroLockCount = gyro->detectedEXTI + GYRO_LOCK_COUNT; + } + + if (gyro->detectedEXTI >= terminalGyroLockCount) { + terminalGyroLockCount += GYRO_LOCK_COUNT; + + // Move the desired start time of the gyroTask + lastTargetCycles -= (accGyroSkew/GYRO_LOCK_COUNT); + DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 3, clockCyclesTo10thMicros(accGyroSkew/GYRO_LOCK_COUNT)); + accGyroSkew = 0; } } - lastTask = NULL; #endif - } + } } - if (!gyroEnabled || realtimeTaskRan || (gyroTaskDelayUs > GYRO_TASK_GUARD_INTERVAL_US)) { - // The task to be invoked + nowCycles = getCycleCounter(); + schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); + + + if (!gyroEnabled || (schedLoopRemainingCycles > (int32_t)clockMicrosToCycles(CHECK_GUARD_MARGIN_US))) { + currentTimeUs = micros(); // Update task dynamic priorities for (task_t *task = queueFirst(); task != NULL; task = queueNext()) { if (task->staticPriority != TASK_PRIORITY_REALTIME) { // Task has checkFunc - event driven if (task->checkFunc) { -#if defined(SCHEDULER_DEBUG) - const timeUs_t currentTimeBeforeCheckFuncCallUs = micros(); -#else - const timeUs_t currentTimeBeforeCheckFuncCallUs = currentTimeUs; -#endif // Increase priority for event driven tasks if (task->dynamicPriority > 0) { - task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAtUs) / task->desiredPeriodUs); + task->taskAgeCycles = 1 + (cmpTimeUs(currentTimeUs, task->lastSignaledAtUs) / task->desiredPeriodUs); task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; waitingTasks++; - } else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, cmpTimeUs(currentTimeBeforeCheckFuncCallUs, task->lastExecutedAtUs))) { -#if defined(SCHEDULER_DEBUG) - DEBUG_SET(DEBUG_SCHEDULER, 3, micros() - currentTimeBeforeCheckFuncCallUs); -#endif -#if defined(USE_TASK_STATISTICS) - if (calculateTaskStatistics) { - const uint32_t checkFuncExecutionTimeUs = micros() - currentTimeBeforeCheckFuncCallUs; - checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; - checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; - checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task - checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs); - } -#endif - task->lastSignaledAtUs = currentTimeBeforeCheckFuncCallUs; + } else if (task->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) { + const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs); + DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs); + checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; + checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT; + checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task + checkFuncMaxExecutionTimeUs = MAX(checkFuncMaxExecutionTimeUs, checkFuncExecutionTimeUs); + task->lastSignaledAtUs = currentTimeUs; task->taskAgeCycles = 1; task->dynamicPriority = 1 + task->staticPriority; waitingTasks++; @@ -432,7 +533,7 @@ FAST_CODE void scheduler(void) } else { // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) // Task age is calculated from last execution - task->taskAgeCycles = ((currentTimeUs - getPeriodCalculationBasis(task)) / task->desiredPeriodUs); + task->taskAgeCycles = (cmpTimeUs(currentTimeUs, getPeriodCalculationBasis(task)) / task->desiredPeriodUs); if (task->taskAgeCycles > 0) { task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; waitingTasks++; @@ -450,35 +551,46 @@ FAST_CODE void scheduler(void) totalWaitingTasks += waitingTasks; if (selectedTask) { - timeDelta_t taskRequiredTimeUs = TASK_AVERAGE_EXECUTE_FALLBACK_US; // default average time if task statistics are not available -#if defined(USE_TASK_STATISTICS) - if (calculateTaskStatistics) { - taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT + TASK_AVERAGE_EXECUTE_PADDING_US; + timeDelta_t taskRequiredTimeUs = selectedTask->movingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT; #if defined(USE_LATE_TASK_STATISTICS) - selectedTask->execTime = taskRequiredTimeUs; + selectedTask->execTime = taskRequiredTimeUs; #endif - } -#endif - // Add in the time spent so far in check functions and the scheduler logic - taskRequiredTimeUs += cmpTimeUs(micros(), currentTimeUs); - if (!gyroEnabled || realtimeTaskRan || (taskRequiredTimeUs < gyroTaskDelayUs)) { - taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs); - } else { - selectedTask = NULL; - } + int32_t taskRequiredTimeCycles = (int32_t)clockMicrosToCycles((uint32_t)taskRequiredTimeUs); + nowCycles = getCycleCounter(); + schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); + + // Allow a little extra time + taskRequiredTimeCycles += taskGuardCycles; + + if (!gyroEnabled || (taskRequiredTimeCycles < schedLoopRemainingCycles)) { + taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs); + nowCycles = getCycleCounter(); + schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); + + if (schedLoopRemainingCycles < taskGuardMinCycles) { + if (taskGuardCycles < taskGuardMaxCycles) { + taskGuardCycles += taskGuardDeltaUpCycles; + } + } else if (taskGuardCycles > taskGuardMinCycles) { + taskGuardCycles -= taskGuardDeltaDownCycles; + } #if defined(USE_LATE_TASK_STATISTICS) - lastTask = selectedTask; -#endif + if (schedLoopRemainingCycles < DEBUG_SCHEDULER_DETERMINISM_THRESHOLD) { + DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 1, selectedTask - tasks); + DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 2, clockCyclesTo10thMicros(schedLoopRemainingCycles)); + selectedTask->lateCount++ ; + } +#endif // USE_LATE_TASK_STATISTICS + } else if (selectedTask->taskAgeCycles > TASK_AGE_EXPEDITE_COUNT) { + // If a task has been unable to run, then reduce it's recorded estimated run time to ensure + // it's ultimate scheduling + selectedTask->movingSumExecutionTimeUs *= TASK_AGE_EXPEDITE_SCALE; + } } } - -#if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - schedulerStartTimeUs - taskExecutionTimeUs); // time spent in scheduler -#else - UNUSED(taskExecutionTimeUs); -#endif #if defined(UNIT_TEST) readSchedulerLocals(selectedTask, selectedTaskDynamicPriority, waitingTasks); @@ -494,3 +606,9 @@ uint16_t getAverageSystemLoadPercent(void) { return averageSystemLoadPercent; } + +float schedulerGetCycleTimeMultipler(void) +{ + return (float)clockMicrosToCycles(getTask(TASK_GYRO)->desiredPeriodUs) / desiredPeriodCycles; + +} diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index a587bdb75..b4f428dd7 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -27,14 +27,29 @@ #define TASK_PERIOD_MS(ms) ((ms) * 1000) #define TASK_PERIOD_US(us) (us) -#define GYRO_TASK_GUARD_INTERVAL_US 10 // Don't run any other tasks if gyro task will be run soon - -#if defined(USE_TASK_STATISTICS) #define TASK_STATS_MOVING_SUM_COUNT 32 -#endif #define LOAD_PERCENTAGE_ONE 100 +#define SCHED_START_LOOP_MIN_US 4 // Wait at start of scheduler loop if gyroTask is nearly due +#define SCHED_START_LOOP_MAX_US 12 +#define SCHED_START_LOOP_DOWN_STEP 50 // Fraction of a us to reduce start loop wait +#define SCHED_START_LOOP_UP_STEP 1 // Fraction of a us to increase start loop wait + +#define TASK_GUARD_MARGIN_MIN_US 2 // Add an amount to the estimate of a task duration +#define TASK_GUARD_MARGIN_MAX_US 5 +#define TASK_GUARD_MARGIN_DOWN_STEP 50 // Fraction of a us to reduce task guard margin +#define TASK_GUARD_MARGIN_UP_STEP 1 // Fraction of a us to increase task guard margin + +#define CHECK_GUARD_MARGIN_US 2 // Add a margin to the amount of time allowed for a check function to run + +#define TASK_AGE_EXPEDITE_COUNT 1 // Make aged tasks more schedulable +#define TASK_AGE_EXPEDITE_SCALE 0.9 // By scaling their expected execution time + +// Gyro interrupt counts over which to measure loop time and skew +#define GYRO_RATE_COUNT 32000 +#define GYRO_LOCK_COUNT 400 + typedef enum { TASK_PRIORITY_REALTIME = -1, // Task will be run outside the scheduler logic TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle @@ -164,10 +179,8 @@ typedef enum { typedef struct { // Configuration -#if defined(USE_TASK_STATISTICS) const char * taskName; const char * subTaskName; -#endif bool (*checkFunc)(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); void (*taskFunc)(timeUs_t currentTimeUs); timeDelta_t desiredPeriodUs; // target period of execution @@ -181,7 +194,6 @@ typedef struct { timeUs_t lastSignaledAtUs; // time of invocation event for event-driven tasks timeUs_t lastDesiredAt; // time of last desired execution -#if defined(USE_TASK_STATISTICS) // Statistics float movingAverageCycleTimeUs; timeUs_t movingSumExecutionTimeUs; // moving sum over 32 samples @@ -194,7 +206,6 @@ typedef struct { uint32_t lateCount; timeUs_t execTime; #endif -#endif } task_t; void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo); @@ -202,8 +213,8 @@ void getTaskInfo(taskId_e taskId, taskInfo_t *taskInfo); void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs); void setTaskEnabled(taskId_e taskId, bool newEnabledState); timeDelta_t getTaskDeltaTimeUs(taskId_e taskId); -void ignoreTaskTime(); -void schedulerSetCalulateTaskStatistics(bool calculateTaskStatistics); +void ignoreTaskStateTime(); +void ignoreTaskShortExecTime(); void schedulerResetTaskStatistics(taskId_e taskId); void schedulerResetTaskMaxExecutionTime(taskId_e taskId); void schedulerResetCheckFunctionMaxExecutionTime(void); @@ -215,3 +226,4 @@ void taskSystemLoad(timeUs_t currentTimeUs); void schedulerOptimizeRate(bool optimizeRate); void schedulerEnableGyro(void); uint16_t getAverageSystemLoadPercent(void); +float schedulerGetCycleTimeMultipler(void); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 0376f09e1..54cc0b9cd 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -389,7 +389,13 @@ uint32_t baroUpdate(void) // Tell the scheduler to ignore how long this task takes unless the pressure is being read // as that takes the longest if (state != BAROMETER_NEEDS_PRESSURE_READ) { - ignoreTaskTime(); + ignoreTaskStateTime(); + } + + if (busBusy(&baro.dev.dev, NULL)) { + // If the bus is busy, simply return to have another go later + ignoreTaskStateTime(); + return sleepTime; } switch (state) { @@ -422,7 +428,7 @@ uint32_t baroUpdate(void) if (baro.dev.read_up(&baro.dev)) { state = BAROMETER_NEEDS_PRESSURE_SAMPLE; } else { - ignoreTaskTime(); + ignoreTaskStateTime(); } break; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index a29951c6f..5f077c9bf 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -81,26 +81,26 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig) // 3. Slave I2C device on SPI gyro #if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963)) - compassConfig->mag_bustype = BUS_TYPE_SPI; + compassConfig->mag_busType = BUS_TYPE_SPI; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE)); compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN); compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; #elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))) - compassConfig->mag_bustype = BUS_TYPE_I2C; + compassConfig->mag_busType = BUS_TYPE_I2C; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); compassConfig->mag_spi_csn = IO_TAG_NONE; #elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) - compassConfig->mag_bustype = BUS_TYPE_MPU_SLAVE; + compassConfig->mag_busType = BUS_TYPE_MPU_SLAVE; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); compassConfig->mag_spi_csn = IO_TAG_NONE; #else compassConfig->mag_hardware = MAG_NONE; - compassConfig->mag_bustype = BUS_TYPE_NONE; + compassConfig->mag_busType = BUS_TYPE_NONE; compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID); compassConfig->mag_i2c_address = 0; compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID); @@ -115,7 +115,7 @@ static uint8_t magInit = 0; void compassPreInit(void) { #ifdef USE_SPI - if (compassConfig()->mag_bustype == BUS_TYPE_SPI) { + if (compassConfig()->mag_busType == BUS_TYPE_SPI) { spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1); } #endif @@ -136,7 +136,7 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment) magDev->magIntExtiTag = compassConfig()->interruptTag; #endif - switch (compassConfig()->mag_bustype) { + switch (compassConfig()->mag_busType) { #ifdef USE_I2C case BUS_TYPE_I2C: i2cBusSetInstance(dev, compassConfig()->mag_i2c_device); diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index a74380072..31da30204 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -49,7 +49,7 @@ extern mag_t mag; typedef struct compassConfig_s { uint8_t mag_alignment; // mag alignment uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device - uint8_t mag_bustype; + uint8_t mag_busType; uint8_t mag_i2c_device; uint8_t mag_i2c_address; uint8_t mag_spi_device; diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 46d34b93a..0d434a9a8 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -78,6 +78,11 @@ #define ACTIVE_GYRO (&gyro.gyroSensor1) #endif +// The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer +// This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other +// gyro types are supported with SPI DMA. +#define GYRO_BUF_SIZE 32 + static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK; static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz) @@ -635,6 +640,11 @@ bool gyroInit(void) } if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE]; + // SPI DMA buffer required per device + gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2; + gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2]; + gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1)); gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection; detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware; @@ -646,6 +656,10 @@ bool gyroInit(void) } if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE]; + // SPI DMA buffer required per device + gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1; + gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2]; gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0)); gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection; detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware; diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 2ee506261..8cecbcb97 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -32,7 +32,6 @@ #define USE_EXTI #define MAG_INT_EXTI PC14 #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 218ef5ffd..d0e22c8f8 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -41,7 +41,6 @@ #define BEEPER_PIN PA5 #define USE_EXTI -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL // Using MPU6050 for the moment. diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index d91a31171..19cdb4364 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -40,7 +40,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC14 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index e7cac3f41..dc586bc82 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -38,7 +38,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC14 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 81ac4b5e0..74f309fb0 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -36,7 +36,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PA8 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index a182f1f7b..138f4e868 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -47,7 +47,6 @@ #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC5 #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define GYRO_1_CS_PIN PC4 #define GYRO_1_SPI_INSTANCE SPI1 diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 25c37df36..f3d007fd3 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -33,8 +33,6 @@ #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PA3 #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MPU_DATA_READY_INTERRUPT - #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 diff --git a/src/main/target/CRAZYBEEF3FR/target.h b/src/main/target/CRAZYBEEF3FR/target.h index aadf330f8..755976919 100644 --- a/src/main/target/CRAZYBEEF3FR/target.h +++ b/src/main/target/CRAZYBEEF3FR/target.h @@ -134,7 +134,7 @@ #define USE_RX_FRSKY_SPI_X #define USE_RX_SFHSS_SPI #define USE_RX_REDPINE_SPI -#define DEFAULT_RX_FEATURE FEATURE_RX_SPI +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X #define USE_RX_FRSKY_SPI_TELEMETRY diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 145555ef6..a414d2f57 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -115,7 +115,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC13 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/ELLE0/target.h b/src/main/target/ELLE0/target.h index 237796aea..63de01bfe 100644 --- a/src/main/target/ELLE0/target.h +++ b/src/main/target/ELLE0/target.h @@ -33,7 +33,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PB5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index acb417b08..f84c39e2c 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -54,7 +54,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC4 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -110,7 +109,7 @@ #define UART1_RX_PIN PB7 #else #define UART1_RX_PIN PA10 -#endif +#endif #define UART1_TX_PIN PA9 #define USE_UART4 diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 5bfc588ce..7a871e78c 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -48,7 +48,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC4 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index 8cf07040b..bd2e9fc29 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -32,10 +32,10 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6_CH3 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST7_CH5 - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA2_ST6_CH0 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST2_CH5 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1_CH3 DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA1_ST2_CH6 - DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAM_CTL + DEF_TIM(TIM11, CH1, PB9, TIM_USE_CAMERA_CONTROL, 0, 0), // CAM_CTL }; diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 47c96b9c6..017deca1b 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -20,6 +20,8 @@ #pragma once +#define USE_LATE_TASK_STATISTICS + #ifdef FURYF4OSD #define TARGET_BOARD_IDENTIFIER "FY4O" #define USBD_PRODUCT_STRING "FuryF4OSD" @@ -158,4 +160,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(11) ) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(11) ) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index b77e63795..d9cf287e9 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -56,7 +56,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -73,7 +72,7 @@ #if defined(FLYWOOF405) //------MPU6000 #define USE_GYRO_SPI_MPU6000 -#define USE_ACC_SPI_MPU6000 +#define USE_ACC_SPI_MPU6000 #endif #if defined(KAKUTEF4V2) || defined(FLYWOOF405) // There is invertor on RXD3(PB11), so PB10/PB11 can't be used as I2C2. @@ -154,8 +153,8 @@ #define USE_PINIOBOX #else #define ESCSERIAL_TIMER_TX_PIN PC7 // (HARDARE=0,PPM) -#endif - +#endif + #define USE_SPI #define USE_SPI_DEVICE_1 //ICM20689 #define SPI1_NSS_PIN PC4 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index b9f65c4c2..b6bdd95ad 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -48,7 +48,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PA5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 4d63db6b1..92af4fb1f 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -115,7 +115,7 @@ #define USE_ADC #define ADC_INSTANCE ADC3 -#define ADC3_DMA_OPT 0 +#define ADC3_DMA_OPT 1 #define VBAT_ADC_PIN PC1 #define RSSI_ADC_PIN PC2 diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 6ee71d8a1..2bcf61cd5 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -35,9 +35,7 @@ #define USE_EXTI #define MAG_INT_EXTI PC14 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MAG_DATA_READY_INTERRUPT #define USE_MAG_DATA_READY_SIGNAL // SPI2 diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index dd3b2ba20..790709951 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -40,7 +40,6 @@ #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PB2 #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define GYRO_1_CS_PIN PC4 #define GYRO_1_SPI_INSTANCE SPI1 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 89bcb5ba9..74bf09c17 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -20,6 +20,8 @@ #pragma once +#define USE_LATE_TASK_STATISTICS + #if defined(AIRBOTF4) #define TARGET_BOARD_IDENTIFIER "AIR4" #define USBD_PRODUCT_STRING "AirbotF4" diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 760747782..ec4270da4 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -19,6 +19,9 @@ */ #pragma once + +#define USE_LATE_TASK_STATISTICS + #define TARGET_BOARD_IDENTIFIER "REVN" #define USBD_PRODUCT_STRING "Revo Nano" diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 0b6b72f14..c388f84bb 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -334,6 +334,25 @@ uint32_t millis(void) { return millis64() & 0xFFFFFFFF; } +uint32_t clockCyclesToMicros(uint32_t clockCycles) +{ + return clockCycles; +} + +int32_t clockCyclesTo10thMicros(int32_t clockCycles) +{ + return clockCycles; +} + +uint32_t clockMicrosToCycles(uint32_t micros) +{ + return micros; +} +uint32_t getCycleCounter(void) +{ + return (uint32_t) (micros64() & 0xFFFFFFFF); +} + void microsleep(uint32_t usec) { struct timespec ts; ts.tv_sec = 0; diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index bf7cb926c..d2689104d 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -40,7 +40,6 @@ #define USE_EXTI #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/STM32F4DISCOVERY/target.h b/src/main/target/STM32F4DISCOVERY/target.h index 3f92b94db..1a8aca68e 100644 --- a/src/main/target/STM32F4DISCOVERY/target.h +++ b/src/main/target/STM32F4DISCOVERY/target.h @@ -43,7 +43,6 @@ #define USE_GYRO_EXTI #define GYRO_1_EXTI_PIN PC4 #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MPU_DATA_READY_INTERRUPT #define GYRO_1_CS_PIN PC4 #define GYRO_1_SPI_INSTANCE SPI1 diff --git a/src/main/target/WORMFC/target.h b/src/main/target/WORMFC/target.h index 82b84ca6f..6bc3ef1f8 100644 --- a/src/main/target/WORMFC/target.h +++ b/src/main/target/WORMFC/target.h @@ -69,7 +69,6 @@ #define GYRO_1_EXTI_PIN PC4 #endif #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MPU_DATA_READY_INTERRUPT #if defined(PIRXF4) #define GYRO_1_CS_PIN PC4 diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index c7fd713b5..938e05934 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -28,8 +28,6 @@ // -Wpadded can be turned on to check padding of structs //#pragma GCC diagnostic warning "-Wpadded" -//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons - #ifdef STM32F1 #define MINIMAL_CLI // Using RX DMA disables the use of receive callbacks @@ -92,6 +90,7 @@ #define USE_TIMER_MGMT #define USE_PERSISTENT_OBJECTS #define USE_CUSTOM_DEFAULTS_ADDRESS +#define USE_LATE_TASK_STATISTICS #endif // STM32F7 #ifdef STM32H7 @@ -133,6 +132,7 @@ #define USE_MCO #define USE_DMA_SPEC #define USE_TIMER_MGMT +#define USE_LATE_TASK_STATISTICS #endif #if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) @@ -231,7 +231,6 @@ extern uint8_t _dmaram_end__; #define USE_CLI #define USE_SERIAL_PASSTHROUGH -#define USE_TASK_STATISTICS #define USE_GYRO_REGISTER_DUMP // Adds gyroregisters command to cli to dump configured register values #define USE_IMU_CALC #define USE_PPM diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 22dee6367..28cab5513 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -202,6 +202,10 @@ uint16_t batteryWarningVoltage; uint8_t useHottAlarmSoundPeriod (void) { return 0; } const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 400000}; // see baudRate_e +uint8_t debugMode; +int32_t schedLoopStartCycles; +int32_t taskGuardCycles; + uint32_t micros(void) {return 0;} int32_t getAmperage(void) { @@ -324,7 +328,6 @@ uint8_t serialRead(serialPort_t *){return 0;} void bufWriterAppend(bufWriter_t *, uint8_t ch){ printf("%c", ch); } void serialWriteBufShim(void *, const uint8_t *, int) {} bufWriter_t *bufWriterInit(uint8_t *, int, bufWrite_t, void *) {return NULL;} -void schedulerSetCalulateTaskStatistics(bool) {} void setArmingDisabled(armingDisableFlags_e) {} void waitForSerialPortToFinishTransmitting(serialPort_t *) {} diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 55b177e04..5c539bf04 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -250,4 +250,6 @@ void mixerSetThrottleAngleCorrection(int) {}; bool gpsRescueIsRunning(void) { return false; } bool isFixedWing(void) { return false; } void pinioBoxTaskControl(void) {} +void ignoreTaskShortExecTime(void) {} +void ignoreTaskStateTime(void) {} } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 3356cdec1..a5e822612 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -395,7 +395,7 @@ bool isFlipOverAfterCrashActive(void) { return false; } void ws2811LedStripEnable(void) { } -void setUsedLedCount(unsigned) { }; +void setUsedLedCount(unsigned) { } void pinioBoxTaskControl(void) {} -void ignoreTaskTime(void) {} +void ignoreTaskShortExecTime(void) {} } diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 6bca72fe7..712a3b33e 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -559,4 +559,7 @@ extern "C" { float getMotorOutputLow(void) { return 1000.0; } float getMotorOutputHigh(void) { return 2047.0; } + + void ignoreTaskShortExecTime(void) {} + void ignoreTaskStateTime(void) {} } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index d397afdcb..952af76d8 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1319,4 +1319,6 @@ extern "C" { bool isUpright(void) { return true; } float getMotorOutputLow(void) { return 1000.0; } float getMotorOutputHigh(void) { return 2047.0; } + void ignoreTaskShortExecTime(void) {} + void ignoreTaskStateTime(void) {} } diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 52f057ee2..3f1ea4d7b 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -51,10 +51,18 @@ extern "C" { bool taskPidRan = false; bool taskFilterReady = false; bool taskPidReady = false; + uint8_t activePidLoopDenom = 1; + + int16_t debug[1]; + uint8_t debugMode = 0; // set up micros() to simulate time uint32_t simulatedTime = 0; uint32_t micros(void) { return simulatedTime; } + uint32_t clockCyclesToMicros(uint32_t x) { return x/10;} + int32_t clockCyclesTo10thMicros(int32_t x) { return x;} + uint32_t clockMicrosToCycles(uint32_t x) { return x*10;} + uint32_t getCycleCounter(void) {return simulatedTime * 10;} // set up tasks to take a simulated representative time to execute bool gyroFilterReady(void) { return taskFilterReady; } @@ -463,6 +471,7 @@ TEST(SchedulerUnittest, TestGyroTask) // run the scheduler scheduler(); + // the gyro task indicator should be true and the TASK_FILTER and TASK_PID indicators should be false EXPECT_TRUE(taskGyroRan); EXPECT_FALSE(taskFilterRan); @@ -507,102 +516,3 @@ TEST(SchedulerUnittest, TestGyroTask) EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); } -// Test the scheduling logic that prevents other tasks from running if they -// might interfere with the timing of the next gyro task. -TEST(SchedulerUnittest, TestGyroLookahead) -{ - static const uint32_t startTime = 4000; - - // enable task statistics - schedulerSetCalulateTaskStatistics(true); - - // disable scheduler optimize rate - schedulerOptimizeRate(false); - - // enable the gyro - schedulerEnableGyro(); - - // disable all tasks except TASK_GYRO, TASK_ACCEL - for (int taskId = 0; taskId < TASK_COUNT; ++taskId) { - setTaskEnabled(static_cast(taskId), false); - } - setTaskEnabled(TASK_GYRO, true); - setTaskEnabled(TASK_ACCEL, true); - -#if defined(USE_TASK_STATISTICS) - // set the average run time for TASK_ACCEL - tasks[TASK_ACCEL].movingSumExecutionTimeUs = TEST_UPDATE_ACCEL_TIME * TASK_STATS_MOVING_SUM_COUNT; -#endif - - /* Test that another task will run if there's plenty of time till the next gyro sample time */ - // set it up so TASK_GYRO just ran and TASK_ACCEL is ready to run - simulatedTime = startTime; - tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime; - tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000); - // reset the flags - resetGyroTaskTestFlags(); - - // run the scheduler - scheduler(); - // the gyro, filter and PID task indicators should be false - EXPECT_FALSE(taskGyroRan); - EXPECT_FALSE(taskFilterRan); - EXPECT_FALSE(taskPidRan); - // TASK_ACCEL should have run - EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask); - - /* Test that another task won't run if the time till the gyro task is less than the guard interval */ - // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run - simulatedTime = startTime; - tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + GYRO_TASK_GUARD_INTERVAL_US / 2; - tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000); - // reset the flags - resetGyroTaskTestFlags(); - - // run the scheduler - scheduler(); - // the gyro, filter and PID task indicators should be false - EXPECT_FALSE(taskGyroRan); - EXPECT_FALSE(taskFilterRan); - EXPECT_FALSE(taskPidRan); - // TASK_ACCEL should not have run - EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); - - /* Test that another task won't run if the time till the gyro task is less than the average task interval */ - // set it up so TASK_GYRO will run soon and TASK_ACCEL is ready to run - simulatedTime = startTime; - tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ) + TEST_UPDATE_ACCEL_TIME / 2; - tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000); - // reset the flags - resetGyroTaskTestFlags(); - - // run the scheduler - scheduler(); - // the gyro, filter and PID task indicators should be false - EXPECT_FALSE(taskGyroRan); - EXPECT_FALSE(taskFilterRan); - EXPECT_FALSE(taskPidRan); - // TASK_ACCEL should not have run - EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); - - /* Test that another task will run if the gyro task gets executed */ - // set it up so TASK_GYRO will run now and TASK_ACCEL is ready to run - simulatedTime = startTime; - tasks[TASK_GYRO].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(TEST_GYRO_SAMPLE_HZ); - tasks[TASK_ACCEL].lastExecutedAtUs = simulatedTime - TASK_PERIOD_HZ(1000); - // reset the flags - resetGyroTaskTestFlags(); - - // make the TASK_FILTER and TASK_PID ready to run - taskFilterReady = true; - taskPidReady = true; - - // run the scheduler - scheduler(); - // TASK_GYRO, TASK_FILTER, and TASK_PID should all run - EXPECT_TRUE(taskGyroRan); - EXPECT_TRUE(taskFilterRan); - EXPECT_TRUE(taskPidRan); - // TASK_ACCEL should have run - EXPECT_EQ(&tasks[TASK_ACCEL], unittest_scheduler_selectedTask); -} diff --git a/src/test/unit/target.h b/src/test/unit/target.h index b2a365a06..2fd7547c8 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -20,6 +20,7 @@ #define SCHEDULER_DELAY_LIMIT 1 #define TASK_GYROPID_DESIRED_PERIOD 100 +#define DMA_DATA #define DMA_DATA_ZERO_INIT #define USE_ACC @@ -63,7 +64,6 @@ #define USE_UART5 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define USE_TASK_STATISTICS #define SERIAL_PORT_COUNT 8 diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc index a190dbd1a..b2f65edf0 100644 --- a/src/test/unit/ws2811_unittest.cc +++ b/src/test/unit/ws2811_unittest.cc @@ -32,6 +32,8 @@ extern "C" { extern "C" { void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex); + void ignoreTaskShortExecTime(void) {} + void ignoreTaskStateTime(void) {} } TEST(WS2812, updateDMABuffer) { @@ -89,3 +91,4 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) { void ws2811LedStripDMAEnable(void) {} } +