diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b08469ab0..1dc859056 100644 --- a/src/main/flight/pid.c +++ b/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) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 915567b34..19322dcd5 100644 --- a/src/main/io/serial_cli.c +++ b/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); } } } diff --git a/src/main/main.c b/src/main/main.c index 15160b39b..fd4225273 100644 --- a/src/main/main.c +++ b/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 diff --git a/src/main/mw.c b/src/main/mw.c index bd31b150c..8308f9a9d 100644 --- a/src/main/mw.c +++ b/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,18 +750,31 @@ 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) { - pidUpdateCountdown = masterConfig.pid_process_denom; - taskMainPidLoop(); - } else { - pidUpdateCountdown--; + + switch (pidUpdateCountdown) { + case(1): + pidCycleTime = micros() - previousPidUpdateTime; + previousPidUpdateTime = micros(); + + pidUpdateCountdown = masterConfig.pid_process_denom; + taskMainPidLoop(); + if (masterConfig.pid_process_denom > 1) realTimeCycle = false; + break; + case(2): + realTimeCycle = true; + pidUpdateCountdown--; + break; + default: + pidUpdateCountdown--; } + break; } - break; } } diff --git a/src/main/scheduler.c b/src/main/scheduler.c index 3c7155b39..a34d4b7c4 100755 --- a/src/main/scheduler.c +++ b/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 = GUARD_INTERVAL; + realtimeGuardInterval = constrain(maxNonRealtimeTaskTime, MIN_GUARD_INTERVAL, MAX_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 - cfTasks[selectedTaskId].maxExecutionTime = MAX(cfTasks[selectedTaskId].maxExecutionTime, taskExecutionTime); + 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; diff --git a/src/main/scheduler.h b/src/main/scheduler.h index 18d09a7c6..9b8ce3801 100755 --- a/src/main/scheduler.h +++ b/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);