Rework scheduler interaction with realtime process // Detailled task manager // Fix gyro sync (was broken)

Fix cycletime calculations

src/main/flight/pid.c

@@ -78,7 +78,7 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
 
 void setTargetPidLooptime(uint8_t pidProcessDenom) {
-	targetPidLooptime = targetLooptime * pidProcessDenom;
+	targetPidLooptime = targetLooptime / pidProcessDenom;
 }
 
 void pidResetErrorAngle(void)

src/main/io/serial_cli.c

@@ -46,6 +46,7 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/sdcard.h"
+#include "drivers/gyro_sync.h"
 
 #include "drivers/buf_writer.h"
 
@@ -2577,7 +2578,28 @@ static void cliTasks(char *cmdline)
     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
         getTaskInfo(taskId, &taskInfo);
         if (taskInfo.isEnabled) {
-            cliPrintf("%d - %s, max = %d us, avg = %d us, total = %d ms\r\n", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskInfo.totalExecutionTime / 1000);
+            uint16_t taskFrequency;
+            uint16_t subTaskFrequency;
+
+            if (taskId == TASK_GYROPID) {
+                subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f));
+                taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
+            } else {
+                taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f));
+            }
+
+            uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000;
+
+            cliPrintf("%d - (%s) max: %dus, avg: %dus, rate: %dhz, total: ", taskId, taskInfo.taskName, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency);
+
+            if (taskTotalTime >= 1000) {
+                cliPrintf("%ds", taskTotalTime / 1000);
+            } else {
+                cliPrintf("%dms", taskTotalTime);
+            }
+
+            if (taskId == TASK_GYROPID) cliPrintf(" (%s rate: %dhz)", taskInfo.subTaskName, subTaskFrequency);
+            cliPrintf("\r\n", taskTotalTime);
         }
     }
 }

src/main/main.c

@@ -580,9 +580,14 @@ void init(void)
     afatfs_init();
 #endif
 
-    if (masterConfig.gyro_lpf) masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
+    if (masterConfig.gyro_lpf) {
+        masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
+        masterConfig.gyro_sync_denom = 1;
+    }
+
     setTargetPidLooptime(masterConfig.pid_process_denom); // Initialize pid looptime
 
+
 #ifdef BLACKBOX
     initBlackbox();
 #endif

src/main/mw.c

@@ -737,9 +737,12 @@ void taskMainPidLoop(void)
 void taskMainPidLoopCheck(void) {
     static uint32_t previousTime;
     static uint8_t pidUpdateCountdown;
+    static uint32_t previousPidUpdateTime, pidCycleTime;
 
     if (!pidUpdateCountdown) pidUpdateCountdown = masterConfig.pid_process_denom;
 
+    uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
+
     cycleTime = micros() - previousTime;
     previousTime = micros();
 
@@ -747,20 +750,33 @@ void taskMainPidLoopCheck(void) {
     debug[0] = cycleTime;
     debug[1] = cycleTime - targetLooptime;
     debug[2] = averageSystemLoadPercent;
+    debug[3] = pidCycleTime;
 
     while (1) {
-        if (gyroSyncCheckUpdate() || ((cycleTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+        if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+
             imuUpdateGyro();
-            if (pidUpdateCountdown == 1) {
+
+            switch (pidUpdateCountdown) {
+                case(1):
+                    pidCycleTime = micros() - previousPidUpdateTime;
+                    previousPidUpdateTime = micros();
+
                     pidUpdateCountdown = masterConfig.pid_process_denom;
                     taskMainPidLoop();
-            } else {
+                    if (masterConfig.pid_process_denom > 1) realTimeCycle = false;
+                    break;
+                case(2):
+				    realTimeCycle = true;
+                    pidUpdateCountdown--;
+                    break;
+                default:
                     pidUpdateCountdown--;
             }
-        }
             break;
         }
     }
+}
 
 void taskUpdateAccelerometer(void)
 {

src/main/scheduler.c

@@ -35,12 +35,15 @@ static uint32_t totalWaitingTasks;
 static uint32_t totalWaitingTasksSamples;
 static uint32_t realtimeGuardInterval;
 
+bool realTimeCycle = true;
+
 uint32_t currentTime = 0;
 uint16_t averageWaitingTasks100 = 0;
 
 typedef struct {
     /* Configuration */
     const char * taskName;
+    const char * subTaskName;  // For future use, but now just for naming convention
     bool (*checkFunc)(uint32_t currentDeltaTime);
     void (*taskFunc)(void);
     bool isEnabled;
@@ -93,7 +96,8 @@ static cfTask_t cfTasks[TASK_COUNT] = {
     },
 
     [TASK_GYROPID] = {
-        .taskName = "GYRO/PID",
+        .taskName = "PID",
+	    .subTaskName = "GYRO",
         .taskFunc = taskMainPidLoopCheck,
         .desiredPeriod = 1000,
         .staticPriority = TASK_PRIORITY_REALTIME,
@@ -226,7 +230,8 @@ static cfTask_t cfTasks[TASK_COUNT] = {
 
 uint16_t averageSystemLoadPercent = 0;
 
-#define GUARD_INTERVAL 5
+#define MAX_GUARD_INTERVAL 100
+#define MIN_GUARD_INTERVAL 10
 
 void taskSystem(void)
 {
@@ -241,13 +246,13 @@ void taskSystem(void)
 
     /* Calculate guard interval */
     uint32_t maxNonRealtimeTaskTime = 0;
+
     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
         if (cfTasks[taskId].staticPriority != TASK_PRIORITY_REALTIME) {
             maxNonRealtimeTaskTime = MAX(maxNonRealtimeTaskTime, cfTasks[taskId].averageExecutionTime);
         }
     }
-
+    realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, MIN_GUARD_INTERVAL, MAX_GUARD_INTERVAL);
-    realtimeGuardInterval = GUARD_INTERVAL;
 #if defined SCHEDULER_DEBUG
     debug[2] = realtimeGuardInterval;
 #endif
@@ -257,12 +262,14 @@ void taskSystem(void)
 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 {
     taskInfo->taskName = cfTasks[taskId].taskName;
+    taskInfo->subTaskName = cfTasks[taskId].subTaskName;
     taskInfo->isEnabled= cfTasks[taskId].isEnabled;
     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
     taskInfo->totalExecutionTime = cfTasks[taskId].totalExecutionTime;
     taskInfo->averageExecutionTime = cfTasks[taskId].averageExecutionTime;
+    taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
 }
 #endif
 
@@ -299,6 +306,8 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
     }
 }
 
+#define MAXT_TIME_TICKS_TO_RESET 10000
+
 void scheduler(void)
 {
     uint8_t taskId;
@@ -306,6 +315,15 @@ void scheduler(void)
     uint8_t selectedTaskDynPrio;
     uint16_t waitingTasks = 0;
     uint32_t timeToNextRealtimeTask = UINT32_MAX;
+    uint32_t currentRealtimeGuardInterval;
+
+    static uint16_t maxTaskCalculationReset = MAXT_TIME_TICKS_TO_RESET;
+
+    if (realTimeCycle) {
+        currentRealtimeGuardInterval = realtimeGuardInterval;
+    } else {
+        currentRealtimeGuardInterval = MIN_GUARD_INTERVAL;
+    }
 
     /* Cache currentTime */
     currentTime = micros();
@@ -328,7 +346,7 @@ void scheduler(void)
         }
     }
 
-    bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > realtimeGuardInterval);
+    bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > currentRealtimeGuardInterval);
 
     /* Update task dynamic priorities */
     for (taskId = 0; taskId < TASK_COUNT; taskId++) {
@@ -399,7 +417,13 @@ void scheduler(void)
         cfTasks[selectedTaskId].averageExecutionTime = ((uint32_t)cfTasks[selectedTaskId].averageExecutionTime * 31 + taskExecutionTime) / 32;
 #ifndef SKIP_TASK_STATISTICS
         cfTasks[selectedTaskId].totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
+        if (maxTaskCalculationReset > 0) {
             cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime);
+            maxTaskCalculationReset--;
+        } else {
+            cfTasks[selectedTaskId].maxExecutionTime = taskExecutionTime;
+            maxTaskCalculationReset = MAXT_TIME_TICKS_TO_RESET;
+        }
 #endif
 #if defined SCHEDULER_DEBUG
         debug[3] = (micros() - currentTime) - taskExecutionTime;

src/main/scheduler.h

@@ -28,6 +28,7 @@ typedef enum {
 
 typedef struct {
     const char * taskName;
+    const char * subTaskName;
     bool         isEnabled;
     uint32_t     desiredPeriod;
     uint8_t      staticPriority;
@@ -35,6 +36,7 @@ typedef struct {
     uint32_t     totalExecutionTime;
     uint32_t     lastExecutionTime;
     uint32_t     averageExecutionTime;
+    uint32_t     latestDeltaTime;
 } cfTaskInfo_t;
 
 typedef enum {
@@ -85,6 +87,7 @@ typedef enum {
 
 extern uint16_t cpuLoad;
 extern uint16_t averageSystemLoadPercent;
+extern bool realTimeCycle;
 
 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo);
 void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros);