Skip to content

Commit

Permalink
Log the standard deviation of gyro cycle time in SCHEDULER_DETERMINIS…
Browse files Browse the repository at this point in the history 
…M and TIMING_ACCURACY (betaflight#13377)

* Log the standard deviation of gyro cycle time in SCHEDULER_DETERMINISM and TIMING_ACCURACY

* Update src/main/scheduler/scheduler.c

Co-authored-by: Jan Post <[email protected]>

* Update src/main/scheduler/scheduler.c

Co-authored-by: Jan Post <[email protected]>

---------

Co-authored-by: Mark Haslinghuis <[email protected]>
Co-authored-by: Jan Post <[email protected]>
  • Loading branch information
@SteveCEvans @haslinghuis @KarateBrot
3 people authored Feb 26, 2024
1 parent 8a506d2 commit 4f1b36f
Showing 1 changed file with 43 additions and 13 deletions.
56 changes: 43 additions & 13 deletions src/main/scheduler/scheduler.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <stdint.h>
#include <string.h>
#include <limits.h>
#include <math.h>

#include "platform.h"

Expand Down Expand Up @@ -63,13 +64,15 @@
// 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
// 1 - Tasks late in last second
// 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[];

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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());

Expand All @@ -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;
Expand Down Expand Up @@ -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)) {
Expand All @@ -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;

gyroCyclesNow = cmpTimeCycles(gyro->gyroSyncEXTI, lastGyroSyncEXTI);

if (gyroCycles) {
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;
Expand All @@ -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;
}
}
}
Expand Down

0 comments on commit 4f1b36f

Please sign in to comment.