diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 26effdebf8..5d5c834412 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -24,6 +24,7 @@ #include #include #include +#include #include "platform.h" @@ -63,6 +64,7 @@ // 4 - Minimum Gyro period in 100th of a us // 5 - Maximum Gyro period in 100th of a us // 6 - Span of Gyro period in 100th of a us +// 7 - Standard deviation of gyro cycle time in 100th of a us // DEBUG_TIMING_ACCURACY, requires USE_LATE_TASK_STATISTICS to be defined // 0 - % CPU busy @@ -70,6 +72,7 @@ // 2 - Total lateness in last second in 10ths us // 3 - Total tasks run in last second // 4 - 10ths % of tasks late in last second +// 7 - Standard deviation of gyro cycle time in 100th of a us extern task_t tasks[]; @@ -110,6 +113,7 @@ static uint32_t lateTaskTotal = 0; static int16_t taskCount = 0; static uint32_t lateTaskPercentage = 0; static uint32_t nextTimingCycles; +static int32_t gyroCyclesNow; #endif static timeMs_t lastFailsafeCheckMs = 0; @@ -453,6 +457,12 @@ FAST_CODE void scheduler(void) { static uint32_t checkCycles = 0; static uint32_t scheduleCount = 0; +#if defined(USE_LATE_TASK_STATISTICS) + static uint32_t gyroCyclesMean = 0; + static uint32_t gyroCyclesCount = 0; + static uint64_t gyroCyclesTotal = 0; + static float devSquared = 0.0f; +#endif #if !defined(UNIT_TEST) const timeUs_t schedulerStartTimeUs = micros(); #endif @@ -507,7 +517,6 @@ FAST_CODE void scheduler(void) nowCycles = getCycleCounter(); schedLoopRemainingCycles = cmpTimeCycles(nextTargetCycles, nowCycles); } - DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastTargetCycles))); #endif currentTimeUs = micros(); taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs); @@ -533,6 +542,13 @@ FAST_CODE void scheduler(void) } #if defined(USE_LATE_TASK_STATISTICS) + gyroCyclesNow = cmpTimeCycles(nowCycles, lastTargetCycles); + gyroCyclesTotal += gyroCyclesNow; + gyroCyclesCount++; + DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(gyroCyclesNow)); + int32_t deviationCycles = gyroCyclesNow - gyroCyclesMean; + devSquared += deviationCycles * deviationCycles; + // % CPU busy DEBUG_SET(DEBUG_TIMING_ACCURACY, 0, getAverageSystemLoadPercent()); @@ -550,6 +566,16 @@ FAST_CODE void scheduler(void) // 10ths % of tasks late in last second DEBUG_SET(DEBUG_TIMING_ACCURACY, 4, lateTaskPercentage); + float gyroCyclesStdDev = sqrt(devSquared/gyroCyclesCount); + int32_t gyroCyclesStdDev100thus = clockCyclesTo100thMicros((int32_t)gyroCyclesStdDev); + DEBUG_SET(DEBUG_TIMING_ACCURACY, 7, gyroCyclesStdDev100thus); + DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 7, gyroCyclesStdDev100thus); + + gyroCyclesMean = gyroCyclesTotal/gyroCyclesCount; + + devSquared = 0.0f; + gyroCyclesTotal = 0; + gyroCyclesCount = 0; lateTaskCount = 0; lateTaskTotal = 0; taskCount = 0; @@ -581,10 +607,6 @@ FAST_CODE void scheduler(void) // Track the actual gyro rate over given number of cycle times and remove skew static uint32_t terminalGyroLockCount = 0; static int32_t accGyroSkew = 0; - static int32_t minGyroPeriod = (int32_t)INT_MAX; - static int32_t maxGyroPeriod = (int32_t)INT_MIN; - static uint32_t lastGyroSyncEXTI; - int32_t gyroCycles; int32_t gyroSkew = cmpTimeCycles(nextTargetCycles, gyro->gyroSyncEXTI) % desiredPeriodCycles; if (gyroSkew > (desiredPeriodCycles / 2)) { @@ -593,22 +615,28 @@ FAST_CODE void scheduler(void) accGyroSkew += gyroSkew; - gyroCycles = cmpTimeCycles(gyro->gyroSyncEXTI, lastGyroSyncEXTI); +#if defined(USE_LATE_TASK_STATISTICS) + static int32_t minGyroPeriod = (int32_t)INT_MAX; + static int32_t maxGyroPeriod = (int32_t)INT_MIN; + static uint32_t lastGyroSyncEXTI; - if (gyroCycles) { + gyroCyclesNow = cmpTimeCycles(gyro->gyroSyncEXTI, lastGyroSyncEXTI); + + if (gyroCyclesNow) { lastGyroSyncEXTI = gyro->gyroSyncEXTI; // If we're syncing to a short cycle, divide by eight if (gyro->gyroShortPeriod != 0) { - gyroCycles /= 8; + gyroCyclesNow /= 8; } - if (gyroCycles < minGyroPeriod) { - minGyroPeriod = gyroCycles; + if (gyroCyclesNow < minGyroPeriod) { + minGyroPeriod = gyroCyclesNow; } // Crude detection of missed cycles caused by configurator traffic - if ((gyroCycles > maxGyroPeriod) && (gyroCycles < (1.5 * minGyroPeriod))) { - maxGyroPeriod = gyroCycles; + if ((gyroCyclesNow > maxGyroPeriod) && (gyroCyclesNow < (1.5 * minGyroPeriod))) { + maxGyroPeriod = gyroCyclesNow; } } +#endif if (terminalGyroLockCount == 0) { terminalGyroLockCount = gyro->detectedEXTI + GYRO_LOCK_COUNT; @@ -620,13 +648,15 @@ FAST_CODE void scheduler(void) // Move the desired start time of the gyroTask lastTargetCycles -= (accGyroSkew/GYRO_LOCK_COUNT); +#if defined(USE_LATE_TASK_STATISTICS) DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 3, clockCyclesTo10thMicros(accGyroSkew/GYRO_LOCK_COUNT)); DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 4, clockCyclesTo100thMicros(minGyroPeriod)); DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 5, clockCyclesTo100thMicros(maxGyroPeriod)); DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 6, clockCyclesTo100thMicros(maxGyroPeriod - minGyroPeriod)); - accGyroSkew = 0; minGyroPeriod = INT_MAX; maxGyroPeriod = INT_MIN; +#endif + accGyroSkew = 0; } } }