Improved scheduler and task timing instrumentation

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;

src/main/fc/config.c

@@ -582,7 +582,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);
 

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,
     },

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);

src/main/fc/mw.c

@@ -177,9 +177,7 @@ void calculateSetpointRate(int axis, int16_t rc) {
         angleRate *= rcSuperfactor;
     }
 
-    if (debugMode == DEBUG_ANGLERATE) {
+    DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
-        debug[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(
         &currentProfile->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
+    // 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--;

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);

src/main/io/serial_cli.c

@@ -519,6 +519,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "DFILTER",
     "ANGLERATE",
     "ESC_TELEMETRY",
+    "SCHEDULER",
 };
 
 #ifdef OSD
@@ -3643,6 +3644,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

src/main/scheduler/scheduler.c

@@ -121,6 +121,17 @@ void taskSystem(uint32_t currentTime)
 }
 
 #ifndef SKIP_TASK_STATISTICS
+uint32_t checkFuncMaxExecutionTime;
+uint32_t checkFuncTotalExecutionTime;
+uint32_t checkFuncAverageExecutionTime;
+
+void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
+{
+    checkFuncInfo->maxExecutionTime = checkFuncMaxExecutionTime;
+    checkFuncInfo->totalExecutionTime = checkFuncTotalExecutionTime;
+    checkFuncInfo->averageExecutionTime = checkFuncAverageExecutionTime;
+}
+
 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 {
     taskInfo->taskName = cfTasks[taskId].taskName;
@@ -198,13 +209,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)) {
+            } else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
-                task->lastSignaledAt = currentTime;
+#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
+                checkFuncAverageExecutionTime = (checkFuncAverageExecutionTime * 31 + checkFuncExecutionTime) / 32;
+                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 +270,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->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32;
         selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
         selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
 #endif
-#if defined SCHEDULER_DEBUG
+#if defined(SCHEDULER_DEBUG)
-        debug[3] = (micros() - currentTime) - taskExecutionTime;
+        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
     }
 }

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 averageExecutionTime;  // Moving average 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
 

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