diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index 6e14dfefbd336014d386b89a585950aad993731c..ba7c009b288c8e9fbcba420de672942548849993 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -28,18 +28,11 @@
using namespace std;
using namespace miosix;
+using namespace Boardcore::Constants;
namespace Boardcore
{
-namespace Constants
-{
-static constexpr unsigned int TICKS_PER_MS =
- miosix::TICK_FREQ / 1000; // Number of ticks in a millisecond
-static constexpr unsigned int MS_PER_TICK =
- 1000 / miosix::TICK_FREQ; // Number of milliseconds in a tick
-} // namespace Constants
-
TaskScheduler::EventQueue TaskScheduler::makeAgenda()
{
std::vector<Event> agendaStorage{};
@@ -60,7 +53,7 @@ TaskScheduler::TaskScheduler(miosix::Priority priority)
}
size_t TaskScheduler::addTask(function_t function, uint32_t period,
- Policy policy, int64_t startTick)
+ Policy policy, int64_t startTime)
{
// In the case of early returns, using RAII mutex wrappers to unlock the
// mutex would cause it to be locked and unlocked one more time before
@@ -77,20 +70,22 @@ size_t TaskScheduler::addTask(function_t function, uint32_t period,
return 0;
}
+ auto periodNs = period * NS_IN_MS;
+
if (policy == Policy::ONE_SHOT)
{
- startTick += period;
+ startTime += periodNs;
}
// Insert a new task with the given parameters
- tasks.emplace_back(function, period, policy, startTick);
+ tasks.emplace_back(function, periodNs, policy, startTime);
size_t id = tasks.size() - 1;
// Only add the task to the agenda if the scheduler is running
// Otherwise, the agenda will be populated when the scheduler is started
if (isRunning())
{
- agenda.emplace(id, startTick);
+ agenda.emplace(id, startTime);
}
condvar.broadcast(); // Signals the run thread
@@ -122,7 +117,7 @@ void TaskScheduler::enableTask(size_t id)
}
task.enabled = true;
- agenda.emplace(id, getTick() + task.period * Constants::TICKS_PER_MS);
+ agenda.emplace(id, getTime() + task.period);
mutex.unlock();
}
@@ -186,21 +181,23 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats()
void TaskScheduler::populateAgenda()
{
- int64_t currentTick = getTick();
+ int64_t currentTime = getTime();
for (size_t id = 1; id < tasks.size(); id++)
{
Task& task = tasks[id];
- int64_t nextTick = task.startTick;
- // Normalize the tasks start time if they precede the current tick
- if (nextTick < currentTick)
+ int64_t nextTime = task.startTime;
+ // Normalize the tasks start time if they precede the current time
+ if (nextTime < currentTime)
{
- nextTick +=
- ((currentTick - nextTick) / task.period + 1) * task.period;
+ int64_t timeSinceStart = currentTime - nextTime;
+ int64_t periodsMissed = timeSinceStart / task.period;
+ int64_t periodsToSkip = periodsMissed + 1;
+ nextTime += periodsToSkip * task.period;
}
- agenda.emplace(id, nextTick);
+ agenda.emplace(id, nextTime);
}
}
@@ -221,18 +218,18 @@ void TaskScheduler::run()
return;
}
- int64_t startTick = getTick();
+ int64_t startTime = getTime();
Event nextEvent = agenda.top();
Task& nextTask = tasks[nextEvent.taskId];
// If the task has the SKIP policy and its execution was missed, we need
// to move it forward to match the period
- if (nextEvent.nextTick < startTick && nextTask.policy == Policy::SKIP)
+ if (nextEvent.nextTime < startTime && nextTask.policy == Policy::SKIP)
{
agenda.pop();
- enqueue(nextEvent, startTick);
+ enqueue(nextEvent, startTime);
}
- else if (nextEvent.nextTick <= startTick)
+ else if (nextEvent.nextTime <= startTime)
{
agenda.pop();
@@ -254,42 +251,42 @@ void TaskScheduler::run()
}
// Enqueue only on a valid task
- updateStats(nextEvent, startTick, getTick());
- enqueue(nextEvent, startTick);
+ updateStats(nextEvent, startTime, getTime());
+ enqueue(nextEvent, startTime);
}
}
else
{
Unlock<FastMutex> unlock(lock);
- Thread::sleepUntil(nextEvent.nextTick);
+ Thread::nanoSleepUntil(nextEvent.nextTime);
}
}
}
-void TaskScheduler::updateStats(const Event& event, int64_t startTick,
- int64_t endTick)
+void TaskScheduler::updateStats(const Event& event, int64_t startTime,
+ int64_t endTime)
{
Task& task = tasks[event.taskId];
// Activation stats
- float activationError = startTick - event.nextTick;
- task.activationStats.add(activationError * Constants::MS_PER_TICK);
+ float activationError = startTime - event.nextTime;
+ task.activationStats.add(activationError / NS_IN_MS);
// Period stats
int64_t lastCall = task.lastCall;
if (lastCall >= 0)
- task.periodStats.add((startTick - lastCall) * Constants::MS_PER_TICK);
+ task.periodStats.add((startTime - lastCall) / NS_IN_MS);
- // Update the last call tick to the current start tick for the next
+ // Update the last call time to the current start time for the next
// iteration
- task.lastCall = startTick;
+ task.lastCall = startTime;
// Workload stats
- task.workloadStats.add(endTick - startTick);
+ task.workloadStats.add((endTime - startTime) / NS_IN_MS);
}
-void TaskScheduler::enqueue(Event event, int64_t startTick)
+void TaskScheduler::enqueue(Event event, int64_t startTime)
{
Task& task = tasks[event.taskId];
switch (task.policy)
@@ -300,21 +297,26 @@ void TaskScheduler::enqueue(Event event, int64_t startTick)
task.enabled = false;
return;
case Policy::SKIP:
+ {
+ // Compute the number of missed periods since the last execution
+ int64_t timeSinceLastExec = startTime - event.nextTime;
+ int64_t periodsMissed = timeSinceLastExec / task.period;
+
+ // Schedule the task executon to the next aligned period, by
+ // skipping over the missed ones
+ // E.g. 3 periods have passed since last execution, the next viable
+ // schedule time is after 4 periods
+ int64_t periodsToSkip = periodsMissed + 1;
+ // Update the task to run at the next viable timeslot, while still
+ // being aligned to the original one
+ event.nextTime += periodsToSkip * task.period;
+
// Updated the missed events count
- task.missedEvents += (startTick - event.nextTick) / task.period;
-
- // Compute the number of periods between the tick the event should
- // have been run and the tick it actually run. Than adds 1 and
- // multiply the period to get the next execution tick still aligned
- // to the original one.
- // E.g. If a task has to run once every 2 ticks and start at tick 0
- // but for whatever reason the first execution is at tick 3, then
- // the next execution will be at tick 4.
- event.nextTick +=
- ((startTick - event.nextTick) / task.period + 1) * task.period;
+ task.missedEvents += static_cast<uint32_t>(periodsMissed);
break;
+ }
case Policy::RECOVER:
- event.nextTick += task.period * Constants::TICKS_PER_MS;
+ event.nextTime += task.period;
break;
}
@@ -324,15 +326,15 @@ void TaskScheduler::enqueue(Event event, int64_t startTick)
}
TaskScheduler::Task::Task()
- : function(nullptr), period(0), startTick(0), enabled(false),
+ : function(nullptr), period(0), startTime(0), enabled(false),
policy(Policy::SKIP), lastCall(-1), activationStats(), periodStats(),
workloadStats(), missedEvents(0), failedEvents(0)
{
}
-TaskScheduler::Task::Task(function_t function, uint32_t period, Policy policy,
- int64_t startTick)
- : function(function), period(period), startTick(startTick), enabled(true),
+TaskScheduler::Task::Task(function_t function, int64_t period, Policy policy,
+ int64_t startTime)
+ : function(function), period(period), startTime(startTime), enabled(true),
policy(policy), lastCall(-1), activationStats(), periodStats(),
workloadStats(), missedEvents(0), failedEvents(0)
{
diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h
index 6496eb9bf57c065ac76dcd00505da13495ae5b48..80fb22c63608cceec014f0b16239a249406e10ee 100644
--- a/src/shared/scheduler/TaskScheduler.h
+++ b/src/shared/scheduler/TaskScheduler.h
@@ -78,7 +78,7 @@ public:
* it is supposed to (e.g. another thread occupies the CPU), the scheduler
* doesn't recover the missed executions but instead skips those and
* continues normally. This ensures that all the events are aligned with
- * the original start tick. In other words, the period and the start tick of
+ * the original start time. In other words, the period and the start time of
* a task specifies the time slots the task has to be executed. If one of
* this time slots can't be used, that specific execution won't be
* recovered.
@@ -91,7 +91,7 @@ public:
{
ONE_SHOT, ///< Run the task one single timer.
SKIP, // Skips lost executions and stays aligned with the original
- // start tick.
+ // start time.
RECOVER ///< Prioritize the number of executions over the period.
};
@@ -109,12 +109,13 @@ public:
* @param function Function to be called periodically.
* @param period Inter call period [ms].
* @param policy Task policy, default is SKIP.
- * @param startTick First activation time, useful for synchronizing tasks.
+ * @param startTime Absolute system time of the first activation time,
+ * useful for synchronizing tasks [ns]
* @return The ID of the task if it was added successfully, 0 otherwise.
*/
size_t addTask(function_t function, uint32_t period,
Policy policy = Policy::RECOVER,
- int64_t startTick = miosix::getTick());
+ int64_t startTime = miosix::getTime());
/**
* @brief Enables the task with the given id.
@@ -136,13 +137,13 @@ private:
struct Task
{
function_t function;
- uint32_t period; // [ms]
- int64_t startTick; ///< First activation time, useful for synchronizing
+ int64_t period; ///< [ns]
+ int64_t startTime; ///< First activation time, useful for synchronizing
///< tasks.
bool enabled; ///< Whether the task should be executed.
Policy policy;
- int64_t lastCall; ///< Last activation tick for statistics computation.
- Stats activationStats; ///< Stats about activation tick error.
+ int64_t lastCall; ///< Last activation time for statistics computation.
+ Stats activationStats; ///< Stats about activation time error.
Stats periodStats; ///< Stats about period error.
Stats workloadStats; ///< Stats about time the task takes to compute.
uint32_t missedEvents; ///< Number of events that could not be run.
@@ -157,12 +158,12 @@ private:
* @brief Creates a task with the given parameters
*
* @param function The std::function to be called
- * @param period The Period in [ms]
+ * @param period The Period in [ns]
* @param policy The task policy in case of a miss
- * @param startTick The first activation time
+ * @param startTime The first activation time
*/
- explicit Task(function_t function, uint32_t period, Policy policy,
- int64_t startTick);
+ explicit Task(function_t function, int64_t period, Policy policy,
+ int64_t startTime);
// Delete copy constructor and copy assignment operator to avoid copying
// and force moving
@@ -183,27 +184,27 @@ private:
struct Event
{
size_t taskId; ///< The task to execute.
- int64_t nextTick; ///< Tick of next activation.
+ int64_t nextTime; ///< Absolute time of next activation.
- Event(size_t taskId, int64_t nextTick)
- : taskId(taskId), nextTick(nextTick)
+ Event(size_t taskId, int64_t nextTime)
+ : taskId(taskId), nextTime(nextTime)
{
}
/**
- * @brief Compare two events based on the next tick.
- * @note This is used to have the event with the lowest tick first in
+ * @brief Compare two events based on the next time.
+ * @note This is used to have the event with the lowest time first in
* the agenda. Newly pushed events are moved up in the queue (see
- * heap bubble-up) until the other tick is lower.
+ * heap bubble-up) until the other time is lower.
*/
bool operator>(const Event& other) const
{
- return this->nextTick > other.nextTick;
+ return this->nextTime > other.nextTime;
}
};
// Use `std::greater` as the comparator to have elements with the lowest
- // tick first. Requires operator `>` to be defined for Event.
+ // time first. Requires operator `>` to be defined for Event.
using EventQueue =
std::priority_queue<Event, std::vector<Event>, std::greater<Event>>;
@@ -227,13 +228,13 @@ private:
/**
* @brief Update task statistics (Intended for when the task is executed).
*
- * This function changes the task last call tick to the startTick.
+ * This function changes the task last call time to the startTime.
*
* \param event Current event.
- * \param startTick Start of execution tick.
- * \param endTick End of execution tick.
+ * \param startTime Start of execution time.
+ * \param endTime End of execution time.
*/
- void updateStats(const Event& event, int64_t startTick, int64_t endTick);
+ void updateStats(const Event& event, int64_t startTime, int64_t endTime);
/**
* @brief (Re)Enqueue an event into the agenda based on the scheduling
@@ -242,22 +243,23 @@ private:
* Requires the mutex to be locked!
*
* \param event Event to be scheduled. Note: this parameter is modified, the
- * nextTick field is updated in order to respect the task interval.
- * \param startTick Activation tick, needed to update the nextTick value of
+ * nextTime field is updated in order to respect the task interval.
+ * \param startTime Activation time, needed to update the nextTime value of
* the event.
*/
- void enqueue(Event event, int64_t startTick);
+ void enqueue(Event event, int64_t startTime);
static TaskStatsResult fromTaskIdPairToStatsResult(const Task& task,
size_t id)
{
- return TaskStatsResult{id,
- task.period,
- task.activationStats.getStats(),
- task.periodStats.getStats(),
- task.workloadStats.getStats(),
- task.missedEvents,
- task.failedEvents};
+ return TaskStatsResult{
+ id,
+ static_cast<uint32_t>(task.period / Constants::NS_IN_MS),
+ task.activationStats.getStats(),
+ task.periodStats.getStats(),
+ task.workloadStats.getStats(),
+ task.missedEvents,
+ task.failedEvents};
}
miosix::FastMutex mutex; ///< Mutex to protect tasks and agenda.
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index 87481cb6a6df7fa7a73cbbdf89a22167fa6d01e3..c6b492a0b4d8664c0bcc9cf2ebd6fcfa002dd79a 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -51,6 +51,9 @@ static constexpr float MSL_TEMPERATURE = 288.15f; // [Kelvin]
static constexpr float B21_LATITUDE = 45.501141;
static constexpr float B21_LONGITUDE = 9.156281;
+
+static constexpr long long NS_IN_MS = 1000000ll; // Nanoseconds in 1 ms
+static constexpr long long NS_IN_S = 1000000000ll; // Nanoseconds in 1 s
} // namespace Constants
} // namespace Boardcore