diff --git a/src/main/common/time.h b/src/main/common/time.h
new file mode 100644
index 0000000000..1a1323bc1d
--- /dev/null
+++ b/src/main/common/time.h
@@ -0,0 +1,33 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include
+
+#include "platform.h"
+
+// millisecond time
+typedef uint32_t timeMs_t ;
+// microsecond time
+#ifdef USE_64BIT_TIME
+typedef uint64_t timeUs_t;
+#define TIMEUS_MAX UINT64_MAX
+#else
+typedef uint32_t timeUs_t;
+#define TIMEUS_MAX UINT32_MAX
+#endif
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index a69ed6e530..623b8aed14 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -28,6 +28,7 @@
#include "scheduler/scheduler.h"
#include "common/maths.h"
+#include "common/time.h"
#include "common/utils.h"
#include "drivers/system.h"
@@ -114,9 +115,9 @@ cfTask_t *queueNext(void)
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
}
-void taskSystem(uint32_t currentTime)
+void taskSystem(timeUs_t currentTimeUs)
{
- UNUSED(currentTime);
+ UNUSED(currentTimeUs);
// Calculate system load
if (totalWaitingTasksSamples > 0) {
@@ -128,9 +129,9 @@ void taskSystem(uint32_t currentTime)
#ifndef SKIP_TASK_STATISTICS
#define MOVING_SUM_COUNT 32
-uint32_t checkFuncMaxExecutionTime;
-uint32_t checkFuncTotalExecutionTime;
-uint32_t checkFuncMovingSumExecutionTime;
+timeUs_t checkFuncMaxExecutionTime;
+timeUs_t checkFuncTotalExecutionTime;
+timeUs_t checkFuncMovingSumExecutionTime;
void getCheckFuncInfo(cfCheckFuncInfo_t *checkFuncInfo)
{
@@ -192,16 +193,16 @@ void schedulerInit(void)
void scheduler(void)
{
// Cache currentTime
- const uint32_t currentTime = micros();
+ const timeUs_t currentTimeUs = micros();
// Check for realtime tasks
- uint32_t timeToNextRealtimeTask = UINT32_MAX;
+ timeUs_t timeToNextRealtimeTask = TIMEUS_MAX;
for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
- const uint32_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
- if ((int32_t)(currentTime - nextExecuteAt) >= 0) {
+ const timeUs_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
+ if ((int32_t)(currentTimeUs - nextExecuteAt) >= 0) {
timeToNextRealtimeTask = 0;
} else {
- const uint32_t newTimeInterval = nextExecuteAt - currentTime;
+ const timeUs_t newTimeInterval = nextExecuteAt - currentTimeUs;
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
}
}
@@ -216,10 +217,10 @@ 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();
+ const timeUs_t currentTimeBeforeCheckFuncCall = micros();
// Increase priority for event driven tasks
if (task->dynamicPriority > 0) {
- task->taskAgeCycles = 1 + ((currentTime - task->lastSignaledAt) / task->desiredPeriod);
+ task->taskAgeCycles = 1 + ((currentTimeUs - task->lastSignaledAt) / task->desiredPeriod);
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCall, currentTimeBeforeCheckFuncCall - task->lastExecutedAt)) {
@@ -244,7 +245,7 @@ void scheduler(void)
} else {
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
// Task age is calculated from last execution
- task->taskAgeCycles = ((currentTime - task->lastExecutedAt) / task->desiredPeriod);
+ task->taskAgeCycles = ((currentTimeUs - task->lastExecutedAt) / task->desiredPeriod);
if (task->taskAgeCycles > 0) {
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
@@ -270,24 +271,24 @@ void scheduler(void)
if (selectedTask != NULL) {
// Found a task that should be run
- selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt;
- selectedTask->lastExecutedAt = currentTime;
+ selectedTask->taskLatestDeltaTime = currentTimeUs - selectedTask->lastExecutedAt;
+ selectedTask->lastExecutedAt = currentTimeUs;
selectedTask->dynamicPriority = 0;
// Execute task
- const uint32_t currentTimeBeforeTaskCall = micros();
+ const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
#ifndef SKIP_TASK_STATISTICS
- const uint32_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
+ const timeUs_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_SET(DEBUG_SCHEDULER, 2, micros() - currentTime - taskExecutionTime); // time spent in scheduler
+ DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
} else {
- DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTime);
+ DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
#endif
}
}
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 3531154df9..2e0d0efbb7 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -17,6 +17,8 @@
#pragma once
+#include "common/time.h"
+
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,
@@ -27,21 +29,21 @@ typedef enum {
} cfTaskPriority_e;
typedef struct {
- uint32_t maxExecutionTime;
- uint32_t totalExecutionTime;
- uint32_t averageExecutionTime;
+ timeUs_t maxExecutionTime;
+ timeUs_t totalExecutionTime;
+ timeUs_t averageExecutionTime;
} cfCheckFuncInfo_t;
typedef struct {
const char * taskName;
const char * subTaskName;
bool isEnabled;
- uint32_t desiredPeriod;
+ timeUs_t desiredPeriod;
uint8_t staticPriority;
- uint32_t maxExecutionTime;
- uint32_t totalExecutionTime;
- uint32_t averageExecutionTime;
- uint32_t latestDeltaTime;
+ timeUs_t maxExecutionTime;
+ timeUs_t totalExecutionTime;
+ timeUs_t averageExecutionTime;
+ timeUs_t latestDeltaTime;
} cfTaskInfo_t;
typedef enum {
@@ -108,23 +110,23 @@ typedef struct {
/* Configuration */
const char * taskName;
const char * subTaskName;
- bool (*checkFunc)(uint32_t currentTime, uint32_t currentDeltaTime);
- void (*taskFunc)(uint32_t currentTime);
+ bool (*checkFunc)(timeUs_t currentTimeUs, uint32_t currentDeltaTimeUs);
+ void (*taskFunc)(timeUs_t currentTimeUs);
uint32_t desiredPeriod; // target period of execution
const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero
/* Scheduling */
uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation
uint16_t taskAgeCycles;
- uint32_t lastExecutedAt; // last time of invocation
- uint32_t lastSignaledAt; // time of invocation event for event-driven tasks
+ timeUs_t lastExecutedAt; // last time of invocation
+ timeUs_t lastSignaledAt; // time of invocation event for event-driven tasks
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
+ timeUs_t movingSumExecutionTime; // moving sum over 32 samples
+ timeUs_t maxExecutionTime;
+ timeUs_t totalExecutionTime; // total time consumed by task since boot
#endif
} cfTask_t;
@@ -139,7 +141,7 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId);
void schedulerInit(void);
void scheduler(void);
-void taskSystem(uint32_t currentTime);
+void taskSystem(timeUs_t currentTime);
#define LOAD_PERCENTAGE_ONE 100