Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Log the standard deviation of gyro cycle time in SCHEDULER_DETERMINISM and TIMING_ACCURACY (#13377)

Browse source 
* 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 <Rm2k-Freak@web.de>

* Update src/main/scheduler/scheduler.c

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

---------

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
Co-authored-by: Jan Post <Rm2k-Freak@web.de>
This commit is contained in:
Steve Evans 2024-02-26 22:29:09 +01:00 • committed by GitHub
parent 8a506d219f
commit 4f1b36fb30
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
1 changed files with 43 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

56
src/main/scheduler/scheduler.c
View file

@ -24,6 +24,7 @@
#include <stdint.h>
#include <string.h>
#include <limits.h>
#include <math.h>
#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;
}
}
}