diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 69644af5d..27377be76 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -60,5 +60,6 @@ typedef enum { DEBUG_DTERM_FILTER, DEBUG_ANGLERATE, DEBUG_ESC_TELEMETRY, + DEBUG_SCHEDULER, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 0fecf379e..583630728 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -585,7 +585,7 @@ void createDefaultConfig(master_t *config) config->gyroConfig.gyro_soft_notch_hz_2 = 200; config->gyroConfig.gyro_soft_notch_cutoff_2 = 100; - config->debug_mode = DEBUG_NONE; + config->debug_mode = DEBUG_MODE; resetAccelerometerTrims(&config->sensorTrims.accZero); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index c868a3213..860c0038f 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -72,6 +72,10 @@ #include "config/config_profile.h" #include "config/config_master.h" +#ifdef USE_BST +void taskBstMasterProcess(uint32_t currentTime); +#endif + #define TASK_PERIOD_HZ(hz) (1000000 / (hz)) #define TASK_PERIOD_MS(ms) ((ms) * 1000) #define TASK_PERIOD_US(us) (us) @@ -295,7 +299,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_GYROPID] = { .taskName = "PID", .subTaskName = "GYRO", - .taskFunc = taskMainPidLoopCheck, + .taskFunc = taskMainPidLoop, .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD, .staticPriority = TASK_PRIORITY_REALTIME, }, diff --git a/src/main/fc/fc_tasks.h b/src/main/fc/fc_tasks.h index 9de09bb15..16db100bb 100644 --- a/src/main/fc/fc_tasks.h +++ b/src/main/fc/fc_tasks.h @@ -19,11 +19,4 @@ #define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times -void taskSystem(uint32_t currentTime); -void taskMainPidLoopCheck(uint32_t currentTime); -#ifdef USE_BST -void taskBstReadWrite(uint32_t currentTime); -void taskBstMasterProcess(uint32_t currentTime); -#endif - void fcTasksInit(void); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 957a2dfed..f2c62b927 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -177,9 +177,7 @@ void calculateSetpointRate(int axis, int16_t rc) { angleRate *= rcSuperfactor; } - if (debugMode == DEBUG_ANGLERATE) { - debug[axis] = angleRate; - } + DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate); setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } @@ -687,7 +685,7 @@ void processRx(uint32_t currentTime) void subTaskPidController(void) { uint32_t startTime; - if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} + if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();} // PID - note this is function pointer set by setPIDController() pidController( ¤tProfile->pidProfile, @@ -695,7 +693,7 @@ void subTaskPidController(void) &masterConfig.accelerometerTrims, masterConfig.rxConfig.midrc ); - if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;} + if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;} } void subTaskMainSubprocesses(void) @@ -772,7 +770,7 @@ void subTaskMainSubprocesses(void) #ifdef TRANSPONDER transponderUpdate(startTime); #endif - if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;} + DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); } void subTaskMotorUpdate(void) @@ -798,7 +796,7 @@ void subTaskMotorUpdate(void) if (motorControlEnable) { writeMotors(); } - if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;} + DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); } uint8_t setPidUpdateCountDown(void) @@ -811,7 +809,7 @@ uint8_t setPidUpdateCountDown(void) } // Function for loop trigger -void taskMainPidLoopCheck(uint32_t currentTime) +void taskMainPidLoop(uint32_t currentTime) { UNUSED(currentTime); @@ -830,7 +828,15 @@ void taskMainPidLoopCheck(uint32_t currentTime) runTaskMainSubprocesses = false; } + // DEBUG_PIDLOOP, timings for: + // 0 - gyroUpdate() + // 1 - pidController() + // 2 - subTaskMainSubprocesses() + // 3 - subTaskMotorUpdate() + uint32_t startTime; + if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();} gyroUpdate(); + if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[0] = micros() - startTime;} if (pidUpdateCountdown) { pidUpdateCountdown--; diff --git a/src/main/fc/mw.h b/src/main/fc/mw.h index 8cb218a5d..3f6970da2 100644 --- a/src/main/fc/mw.h +++ b/src/main/fc/mw.h @@ -31,3 +31,4 @@ void processRx(uint32_t currentTime); void updateLEDs(void); void updateRcCommands(void); +void taskMainPidLoop(uint32_t currentTime); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7526eb2ea..5b88ad33e 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -519,6 +519,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "DFILTER", "ANGLERATE", "ESC_TELEMETRY", + "SCHEDULER", }; #ifdef OSD @@ -3646,6 +3647,9 @@ static void cliTasks(char *cmdline) } } } + cfCheckFuncInfo_t checkFuncInfo; + getCheckFuncInfo(&checkFuncInfo); + cliPrintf("RX Check Function %17d %7d %25d\r\n", checkFuncInfo.maxExecutionTime, checkFuncInfo.averageExecutionTime, checkFuncInfo.totalExecutionTime / 1000); cliPrintf("Total (excluding SERIAL) %23d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); } #endif diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 9edabadb6..a69ed6e53 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -32,6 +32,12 @@ #include "drivers/system.h" +// DEBUG_SCHEDULER, timings for: +// 0 - gyroUpdate() +// 1 - pidController() +// 2 - time spent in scheduler +// 3 - time spent executing check function + static cfTask_t *currentTask = NULL; static uint32_t totalWaitingTasks; @@ -121,6 +127,18 @@ void taskSystem(uint32_t currentTime) } #ifndef SKIP_TASK_STATISTICS +#define MOVING_SUM_COUNT 32 +uint32_t checkFuncMaxExecutionTime; +uint32_t checkFuncTotalExecutionTime; +uint32_t checkFuncMovingSumExecutionTime; + +void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo) +{ + checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime; + checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime; + checkFuncInfo->averageExecutionTime = checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT; +} + void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo) { taskInfo->taskName = cfTasks[taskId].taskName; @@ -130,7 +148,7 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo) taskInfo->staticPriority = cfTasks[taskId].staticPriority; taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime; taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime; - taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime; + taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / MOVING_SUM_COUNT; taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime; } #endif @@ -198,13 +216,25 @@ void scheduler(void) for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) { // Task has checkFunc - event driven if (task->checkFunc != NULL) { + const uint32_t currentTimeBeforeCheckFuncCall = micros(); // Increase priority for event driven tasks if (task->dynamicPriority > 0) { task->taskAgeCycles = 1 + ((currentTime - task->lastSignaledAt) / task->desiredPeriod); task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles; waitingTasks++; - } else if (task->checkFunc(currentTime, currentTime - task->lastExecutedAt)) { - task->lastSignaledAt = currentTime; + } else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) { +#if defined(SCHEDULER_DEBUG) || !defined(SKIP_TASK_STATISTICS) + const uint32_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCall; +#endif +#if defined(SCHEDULER_DEBUG) + DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTime); +#endif +#ifndef SKIP_TASK_STATISTICS + checkFuncMovingSumExecutionTime += checkFuncExecutionTime - checkFuncMovingSumExecutionTime / MOVING_SUM_COUNT; + checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task + checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime); +#endif + task->lastSignaledAt = currentTimeBeforeCheckFuncCall; task->taskAgeCycles = 1; task->dynamicPriority = 1 + task->staticPriority; waitingTasks++; @@ -247,17 +277,17 @@ void scheduler(void) // Execute task const uint32_t currentTimeBeforeTaskCall = micros(); selectedTask->taskFunc(currentTimeBeforeTaskCall); - const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; - selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32; #ifndef SKIP_TASK_STATISTICS + const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall; + selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / MOVING_SUM_COUNT; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); #endif -#if defined SCHEDULER_DEBUG - debug[3] = (micros() - currentTime) - taskExecutionTime; +#if defined(SCHEDULER_DEBUG) + DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTime - taskExecutionTime); // time spent in scheduler } else { - debug[3] = (micros() - currentTime); + DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTime); #endif } } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 026cb88af..3531154df 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -17,8 +17,6 @@ #pragma once -//#define SCHEDULER_DEBUG - typedef enum { TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle TASK_PRIORITY_LOW = 1, @@ -28,6 +26,12 @@ typedef enum { TASK_PRIORITY_MAX = 255 } cfTaskPriority_e; +typedef struct { + uint32_t maxExecutionTime; + uint32_t totalExecutionTime; + uint32_t averageExecutionTime; +} cfCheckFuncInfo_t; + typedef struct { const char * taskName; const char * subTaskName; @@ -114,11 +118,11 @@ typedef struct { uint16_t taskAgeCycles; uint32_t lastExecutedAt; // last time of invocation uint32_t lastSignaledAt; // time of invocation event for event-driven tasks + uint32_t taskLatestDeltaTime; - /* Statistics */ - uint32_t averageExecutionTime; // Moving average over 6 samples, used to calculate guard interval - uint32_t taskLatestDeltaTime; // #ifndef SKIP_TASK_STATISTICS + /* Statistics */ + uint32_t movingSumExecutionTime; // moving sum over 32 samples uint32_t maxExecutionTime; uint32_t totalExecutionTime; // total time consumed by task since boot #endif @@ -127,13 +131,15 @@ typedef struct { extern cfTask_t cfTasks[TASK_COUNT]; extern uint16_t averageSystemLoadPercent; -void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo); +void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo); +void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t *taskInfo); void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros); void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState); uint32_t getTaskDeltaTime(cfTaskId_e taskId); void schedulerInit(void); void scheduler(void); +void taskSystem(uint32_t currentTime); #define LOAD_PERCENTAGE_ONE 100 diff --git a/src/main/target/common.h b/src/main/target/common.h index a38b075a7..f6402d651 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -17,6 +17,9 @@ #pragma once +//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons +#define DEBUG_MODE DEBUG_NONE // change this to change initial debug mode + #define I2C1_OVERCLOCK true #define I2C2_OVERCLOCK true