diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 8e5d52b5a2..03dbd13c9b 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -175,6 +175,12 @@ int32_t clockCyclesTo10thMicros(int32_t clockCycles) return 10 * clockCycles / (int32_t)usTicks; } +// Note that this conversion is signed as this is used for periods rather than absolute timestamps +int32_t clockCyclesTo100thMicros(int32_t clockCycles) +{ + return 100 * clockCycles / (int32_t)usTicks; +} + uint32_t clockMicrosToCycles(uint32_t micros) { return micros * usTicks; diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 8e97eed28c..34e05a3d58 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -68,6 +68,7 @@ void cycleCounterInit(void); int32_t clockCyclesToMicros(int32_t clockCycles); float clockCyclesToMicrosf(int32_t clockCycles); int32_t clockCyclesTo10thMicros(int32_t clockCycles); +int32_t clockCyclesTo100thMicros(int32_t clockCycles); uint32_t clockMicrosToCycles(uint32_t micros); uint32_t getCycleCounter(void); #if defined(STM32H7) || defined(STM32G4) diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 95a598ad8c..a072d2205b 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -23,6 +23,7 @@ #include #include #include +#include #include "platform.h" @@ -59,6 +60,9 @@ // 1 - ID of late task // 2 - Amount task is late in 10th of a us // 3 - Gyro lock skew in 10th of a us +// 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 // DEBUG_TIMING_ACCURACY, requires USE_LATE_TASK_STATISTICS to be defined // 0 - % CPU busy @@ -562,6 +566,10 @@ 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)) { @@ -570,6 +578,23 @@ FAST_CODE void scheduler(void) accGyroSkew += gyroSkew; + gyroCycles = cmpTimeCycles(gyro->gyroSyncEXTI, lastGyroSyncEXTI); + + if (gyroCycles) { + lastGyroSyncEXTI = gyro->gyroSyncEXTI; + // If we're syncing to a short cycle, divide by eight + if (gyro->gyroShortPeriod != 0) { + gyroCycles /= 8; + } + if (gyroCycles < minGyroPeriod) { + minGyroPeriod = gyroCycles; + } + // Crude detection of missed cycles caused by configurator traffic + if ((gyroCycles > maxGyroPeriod) && (gyroCycles < (1.5 * minGyroPeriod))) { + maxGyroPeriod = gyroCycles; + } + } + if (terminalGyroLockCount == 0) { terminalGyroLockCount = gyro->detectedEXTI + GYRO_LOCK_COUNT; } @@ -579,8 +604,14 @@ FAST_CODE void scheduler(void) // Move the desired start time of the gyroTask lastTargetCycles -= (accGyroSkew/GYRO_LOCK_COUNT); + 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; } } } diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 37dfac14be..a677529a63 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -437,6 +437,11 @@ int32_t clockCyclesTo10thMicros(int32_t clockCycles) return clockCycles; } +int32_t clockCyclesTo100thMicros(int32_t clockCycles) +{ + return clockCycles; +} + uint32_t clockMicrosToCycles(uint32_t micros) { return micros; diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index 9d2f4bc2a7..3ea8f68df7 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -76,6 +76,7 @@ extern "C" { uint32_t millis(void) { return simulatedTime/1000; } // Note simplistic mapping suitable only for short unit tests int32_t clockCyclesToMicros(int32_t x) { return x/10;} int32_t clockCyclesTo10thMicros(int32_t x) { return x;} + int32_t clockCyclesTo100thMicros(int32_t x) { return x;} uint32_t clockMicrosToCycles(uint32_t x) { return x*10;} uint32_t getCycleCounter(void) {return simulatedTime * 10;}