diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp index eb63590d3ac89ade8d832fc9b10fdf179f6d6f25..b7e9645016c002163c410a0d3cbabe2cdcd82459 100644 --- a/src/shared/scheduler/TaskScheduler.cpp +++ b/src/shared/scheduler/TaskScheduler.cpp @@ -23,10 +23,13 @@ #include "TaskScheduler.h" #include <diagnostic/SkywardStack.h> +#include <utils/TimeUtils.h> #include <algorithm> +#include <mutex> using namespace std; +using namespace std::chrono; using namespace miosix; namespace Boardcore @@ -51,52 +54,47 @@ TaskScheduler::TaskScheduler(miosix::Priority priority) tasks.emplace_back(); } -size_t TaskScheduler::addTask(function_t function, uint32_t period, - Policy policy, int64_t startTick) +size_t TaskScheduler::addTask(function_t function, nanoseconds period, + Policy policy, time_point<steady_clock> 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 - // returning, because of the destructor being called on the Unlock object - // first and then on the Lock object. To avoid this, we don't use RAII - // wrappers and manually lock and unlock the mutex instead. - mutex.lock(); + std::unique_lock<miosix::FastMutex> lock{mutex}; if (tasks.size() >= MAX_TASKS) { // Unlock the mutex to release the scheduler resources before logging - mutex.unlock(); + lock.unlock(); LOG_ERR(logger, "Full task scheduler"); return 0; } if (policy == Policy::ONE_SHOT) { - startTick += period; + startTime += period; } // Insert a new task with the given parameters - tasks.emplace_back(function, period, policy, startTick); + tasks.emplace_back(function, period.count(), policy, + startTime.time_since_epoch().count()); 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.time_since_epoch().count()); } condvar.broadcast(); // Signals the run thread - mutex.unlock(); return id; } void TaskScheduler::enableTask(size_t id) { - mutex.lock(); + std::unique_lock<miosix::FastMutex> lock{mutex}; if (id > tasks.size() - 1) { - mutex.unlock(); + lock.unlock(); LOG_ERR(logger, "Tried to enable an out-of-range task, id = {}", id); return; } @@ -108,29 +106,30 @@ void TaskScheduler::enableTask(size_t id) // exception if (task.empty()) { - mutex.unlock(); + lock.unlock(); LOG_WARN(logger, "Tried to enable an empty task, id = {}", id); return; } task.enabled = true; - agenda.emplace(id, Kernel::getOldTick() + task.period); - mutex.unlock(); + agenda.emplace(id, miosix::getTime() + task.period); } void TaskScheduler::disableTask(size_t id) { - mutex.lock(); + std::unique_lock<miosix::FastMutex> lock{mutex}; if (id > tasks.size() - 1) { - mutex.unlock(); + lock.unlock(); LOG_ERR(logger, "Tried to disable an out-of-range task, id = {}", id); return; } - tasks[id].enabled = false; - mutex.unlock(); + Task& task = tasks[id]; + task.enabled = false; + // Reset the last call time to avoid incorrect period statistics + task.lastCall = -1; } bool TaskScheduler::start() @@ -178,21 +177,24 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats() void TaskScheduler::populateAgenda() { - int64_t currentTick = Kernel::getOldTick(); + int64_t currentTime = miosix::getTime(); for (size_t id = 1; id < tasks.size(); id++) { - Task& task = tasks[id]; + Task& task = tasks[id]; + int64_t startTime = task.startTime; - int64_t nextTick = task.startTick; - // Normalize the tasks start time if they precede the current tick - if (nextTick < currentTick) + // Shift the task's start time if it precedes the current time + // to avoid clumping all tasks at the beginning (see issue #91) + if (startTime < currentTime) { - nextTick += - ((currentTick - nextTick) / task.period + 1) * task.period; + int64_t timeSinceStart = currentTime - startTime; + int64_t periodsMissed = timeSinceStart / task.period; + int64_t periodsToSkip = periodsMissed + 1; + startTime += periodsToSkip * task.period; } - agenda.emplace(id, nextTick); + agenda.emplace(id, startTime); } } @@ -213,19 +215,12 @@ void TaskScheduler::run() return; } - int64_t startTick = Kernel::getOldTick(); + int64_t startTime = miosix::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) - { - agenda.pop(); - enqueue(nextEvent, startTick); - } - else if (nextEvent.nextTick <= startTick) + if (nextEvent.nextTime <= startTime) { + Task& nextTask = tasks[nextEvent.taskId]; agenda.pop(); // Execute the task function @@ -246,42 +241,42 @@ void TaskScheduler::run() } // Enqueue only on a valid task - updateStats(nextEvent, startTick, Kernel::getOldTick()); - enqueue(nextEvent, startTick); + updateStats(nextEvent, startTime, miosix::getTime()); + enqueue(nextEvent, startTime); } } else { Unlock<FastMutex> unlock(lock); - Kernel::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); + float activationTime = startTime - event.nextTime; + task.activationStats.add(activationTime / Constants::NS_IN_MS); - // Period stats int64_t lastCall = task.lastCall; if (lastCall >= 0) - task.periodStats.add((startTick - lastCall)); - - // Update the last call tick to the current start tick for the next + { + float periodTime = startTime - lastCall; + task.periodStats.add(periodTime / Constants::NS_IN_MS); + } + // 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); + float workloadTime = endTime - startTime; + task.workloadStats.add(workloadTime / Constants::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) @@ -292,21 +287,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; + event.nextTime += task.period; break; } @@ -316,15 +316,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 84f5c97d7777846582618ab19645f95e55e5ce5c..3b506c22fcff49e4dc110fe1441d683086bac856 100644 --- a/src/shared/scheduler/TaskScheduler.h +++ b/src/shared/scheduler/TaskScheduler.h @@ -26,9 +26,11 @@ #include <Singleton.h> #include <debug/debug.h> #include <diagnostic/PrintLogger.h> +#include <units/Frequency.h> #include <utils/KernelTime.h> #include <utils/Stats/Stats.h> +#include <chrono> #include <cstdint> #include <functional> #include <list> @@ -70,29 +72,32 @@ public: /** * @brief Task behavior policy. + * Determines the behavior of the scheduler for a specific task. * - * This policies allows to change the behavior of the scheduler for the - * specific task: - * - ONE_SHOT: Allows to run the task once. When it is executed, it is - * removed from the tasks list. - * - SKIP: If for whatever reason a task can't be executed when - * 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 - * 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. - * - RECOVER: On the other hand, the RECOVER policy ensures that the missed - * executions are run. However, this will cause the period to not be - * respected and the task will run consecutively for some time (See issue - * #91). + * - ONE_SHOT: Runs the task once and subsequently removes it from the task + * list. This is useful for one-off tasks. + * + * - SKIP: Skips missed executions. This is useful for tasks that need to + * execute periodically but can skip some executions. + * If the task misses one or more executions, the scheduler will skip the + * missed executions, run the task once and re-schedule the task for + * future execution. The scheduler will try to align the task execution time + * with the original start time, but actual execution time is not guaranteed + * to be aligned with the period. + * + * - RECOVER: Recovers missed executions. This is useful for + * tasks that need to reach an overall number of iterations, but don't care + * too much about timing. + * Missed executions are recovered immediately, so this may cause one or + * more tasks to clump at the beginning of the task queue until all missed + * executions are recovered, causing the period to not be respected (see + * issue #91). */ enum class Policy : uint8_t { 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. }; @@ -100,7 +105,8 @@ public: 1); /** - * @brief Add a task function to the scheduler with an auto generated ID. + * @brief Add a millisecond-period task function to the scheduler with an + * auto generated ID. * * Note that each task has it's own unique ID, even one shot tasks! * @@ -108,14 +114,70 @@ public: * executed immediately, otherwise after the given period. * * @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 periodMs Inter call period [ms]. + * @param policy Task policy, default is RECOVER. + * @param startTick Absolute system tick of the first activation, useful + * for synchronizing tasks [ms] * @return The ID of the task if it was added successfully, 0 otherwise. */ - size_t addTask(function_t function, uint32_t period, + size_t addTask(function_t function, uint32_t periodMs, Policy policy = Policy::RECOVER, - int64_t startTick = Kernel::getOldTick()); + int64_t startTick = Kernel::getOldTick()) + { + auto period = std::chrono::milliseconds{periodMs}; + auto startTime = std::chrono::time_point<std::chrono::steady_clock>{ + std::chrono::milliseconds{startTick}}; + + return addTask(function, period, policy, startTime); + } + + /** + * @brief Add a task function with the given frequency to the scheduler with + * an auto generated ID. + * + * Note that each task has it's own unique ID, even one shot tasks! + * + * For one shot tasks, the period is used as a delay. If 0 the task will be + * executed immediately, otherwise after the given period. + * + * @param function Function to be called periodically. + * @param frequency Task frequency [Hz]. + * @param policy Task policy, default is RECOVER. + * @param startTime Absolute system time of the first activation, useful for + * synchronizing tasks [ns] + * @return The ID of the task if it was added successfully, 0 otherwise. + */ + size_t addTask(function_t function, Units::Frequency::Hertz frequency, + Policy policy = Policy::RECOVER, + std::chrono::time_point<std::chrono::steady_clock> + startTime = std::chrono::steady_clock::now()) + { + auto period = std::chrono::nanoseconds{ + static_cast<int64_t>(sToNs(1) / frequency.value())}; + + return addTask(function, period, policy, startTime); + } + + /** + * @brief Add a task function with the given period to the scheduler with an + * auto generated ID. + * + * Note that each task has it's own unique ID, even one shot tasks! + * + * For one shot tasks, the period is used as a delay. If 0 the task will be + * executed immediately, otherwise after the given period. + * + * @param function Function to be called periodically. + * @param period Inter call period [ns]. + * @param policy Task policy, default is RECOVER. + * @param startTime Absolute system time of the first activation, useful for + * synchronizing tasks [ns] + * @return The ID of the task if it was added successfully, 0 otherwise. + */ + size_t addTask(function_t function, std::chrono::nanoseconds period, + Policy policy = Policy::RECOVER, + std::chrono::time_point<std::chrono::steady_clock> + startTime = std::chrono::steady_clock::now()); /** * @brief Enables the task with the given id. @@ -137,13 +199,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. @@ -158,12 +220,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 @@ -184,27 +246,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>>; @@ -228,13 +290,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 @@ -243,17 +305,17 @@ 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, + std::chrono::nanoseconds{task.period}, task.activationStats.getStats(), task.periodStats.getStats(), task.workloadStats.getStats(), diff --git a/src/shared/scheduler/TaskSchedulerData.h b/src/shared/scheduler/TaskSchedulerData.h index 8b612714a0f1bda9de9a29914a9f7d71560248c3..916669c65700af218c8f3ae64b62028d0edb780f 100644 --- a/src/shared/scheduler/TaskSchedulerData.h +++ b/src/shared/scheduler/TaskSchedulerData.h @@ -24,6 +24,7 @@ #include <utils/Stats/Stats.h> +#include <chrono> #include <cstdint> #include <ostream> @@ -44,7 +45,7 @@ namespace Boardcore struct TaskStatsResult { size_t id; - uint32_t period; + std::chrono::nanoseconds period; StatsResult activationStats; StatsResult periodStats; StatsResult workloadStats; @@ -61,15 +62,16 @@ struct TaskStatsResult void print(std::ostream& os) const { - os << (int)id << "," << period << "," << activationStats.minValue << "," - << activationStats.maxValue << "," << activationStats.mean << "," - << activationStats.stdDev << "," << activationStats.nSamples << "," - << periodStats.minValue << "," << periodStats.maxValue << "," - << periodStats.mean << "," << periodStats.stdDev << "," - << periodStats.nSamples << "," << workloadStats.minValue << "," - << workloadStats.maxValue << "," << workloadStats.mean << "," - << workloadStats.stdDev << "," << workloadStats.nSamples << "," - << missedEvents << "," << failedEvents << "\n"; + os << (int)id << "," << period.count() << "," + << activationStats.minValue << "," << activationStats.maxValue << "," + << activationStats.mean << "," << activationStats.stdDev << "," + << activationStats.nSamples << "," << periodStats.minValue << "," + << periodStats.maxValue << "," << periodStats.mean << "," + << periodStats.stdDev << "," << periodStats.nSamples << "," + << workloadStats.minValue << "," << workloadStats.maxValue << "," + << workloadStats.mean << "," << workloadStats.stdDev << "," + << workloadStats.nSamples << "," << missedEvents << "," + << failedEvents << "\n"; } }; diff --git a/src/shared/units/Frequency.h b/src/shared/units/Frequency.h new file mode 100644 index 0000000000000000000000000000000000000000..5a92d3728e7f2de29b408fc83df345abfbd8eaad --- /dev/null +++ b/src/shared/units/Frequency.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Niccolò Betto + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <cstdint> + +#include "Units.h" + +namespace Boardcore +{ + +namespace Units +{ + +namespace Frequency +{ + +template <class Ratio = std::ratio<1>> +using Frequency = Unit<UnitKind::Frequency, Ratio>; + +using Hertz = Frequency<>; +using Kilohertz = Frequency<std::kilo>; + +// Integers +constexpr auto operator""_hz(unsigned long long n) +{ + return Hertz(static_cast<float>(n)); +}; + +constexpr auto operator""_khz(unsigned long long n) +{ + return Kilohertz(static_cast<float>(n)); +}; + +// Floats +constexpr auto operator""_hz(long double n) +{ + return Hertz(static_cast<float>(n)); +}; + +constexpr auto operator""_khz(long double n) +{ + return Kilohertz(static_cast<float>(n)); +}; + +} // namespace Frequency + +} // namespace Units + +} // namespace Boardcore diff --git a/src/shared/units/Units.h b/src/shared/units/Units.h index 6e59cbcd052c0cf7835dafe233871f686542b8b7..fd26b2b59c1fe9ee6caa75fb74b3de7f4b5b3c65 100644 --- a/src/shared/units/Units.h +++ b/src/shared/units/Units.h @@ -40,6 +40,7 @@ enum class UnitKind Time, Speed, Acceleration, + Frequency, }; // Base class to implement custom measurement units logic. @@ -68,7 +69,7 @@ public: } template <UnitKind TargetKind, class TargetRatio = Ratio> - constexpr explicit operator Unit<TargetKind, TargetRatio>() const + constexpr operator Unit<TargetKind, TargetRatio>() const { return Unit<TargetKind, TargetRatio>(value<TargetRatio>()); } diff --git a/src/tests/scheduler/test-taskscheduler.cpp b/src/tests/scheduler/test-taskscheduler.cpp index bf842aa1f0db2459bde6148dd5dc667e1950f45e..07ef20a1c19b7d706d8c9bcb124b6f4d412b6c90 100644 --- a/src/tests/scheduler/test-taskscheduler.cpp +++ b/src/tests/scheduler/test-taskscheduler.cpp @@ -35,10 +35,20 @@ GpioPin pin5 = GpioPin(GPIOD_BASE, 9); bool taskLogEnabled; ///< A flag to enable/disable task logging +// Proxy sleep function to print when the main thread sleeps +namespace Thread +{ +void sleep(unsigned int ms) +{ + printf("Main thread sleeping for %u ms\n", ms); + miosix::Thread::sleep(ms); +} +} // namespace Thread + void task2Hz() { pin1.high(); - delayUs(1); + delayUs(100); pin1.low(); if (taskLogEnabled) @@ -50,7 +60,7 @@ void task2Hz() void task5Hz() { pin2.high(); - delayUs(1); + delayUs(100); pin2.low(); if (taskLogEnabled) @@ -62,21 +72,21 @@ void task5Hz() void task500Hz() { pin3.high(); - delayUs(1); + delayUs(100); pin3.low(); } void task1KHz() { pin4.high(); - delayUs(1); + delayUs(100); pin4.low(); } void signalPin5() { pin5.high(); - delayUs(1); + delayUs(100); pin5.low(); } @@ -110,18 +120,25 @@ void setup() void printTaskStats(TaskScheduler& scheduler) { - printf("Tasks stats:\n"); + printf("* Tasks stats\n"); for (auto stat : scheduler.getTaskStats()) { - printf("- %d:\n", stat.id); - printf("\tActivation: %.2f, %.2f\n", stat.activationStats.mean, - stat.activationStats.stdDev); - printf("\tPeriod: %.2f, %.2f\n", stat.periodStats.mean, - stat.periodStats.stdDev); - printf("\tWorkload: %.2f, %.2f\n", stat.workloadStats.mean, - stat.workloadStats.stdDev); - printf("\tMissed events: %ld\n", stat.missedEvents); - printf("\tFailed events: %ld\n", stat.failedEvents); + float frequency = 1.0f / stat.period.count() * std::nano::den; + fmt::print( + "| Task ID {} | Frequency {} Hz:\n" + "|\t Average[ms] StdDev[ms]\n" + "|\tActivation: {:12.3g} {:12.3g}\n" + "|\tPeriod: {:12.3g} {:12.3g}\n" + "|\tWorkload: {:12.3g} {:12.3g}\n" + "|\t------------------------------------------\n" + "|\tExecutions: {:12}\n" + "|\tMissed events: {:12}\n" + "|\tFailed events: {:12}\n|\n", + stat.id, frequency, stat.activationStats.mean, + stat.activationStats.stdDev, stat.periodStats.mean, + stat.periodStats.stdDev, stat.workloadStats.mean, + stat.workloadStats.stdDev, stat.activationStats.nSamples, + stat.missedEvents, stat.failedEvents); } } @@ -130,13 +147,18 @@ void printTaskStats(TaskScheduler& scheduler) */ void test_general_purpose() { + using namespace Boardcore::Units::Frequency; + using namespace std::chrono_literals; + TaskScheduler scheduler{}; - int task1 = scheduler.addTask(f2Hz, 500); - scheduler.addTask(f5Hz, 200); - int task3 = scheduler.addTask(f500Hz, 2, TaskScheduler::Policy::RECOVER); - scheduler.addTask(f1KHz, 1, TaskScheduler::Policy::RECOVER); - scheduler.addTask(f1KHz, 1, TaskScheduler::Policy::RECOVER); + int task1 = scheduler.addTask([] { delayUs(150); }, 2_hz); + scheduler.addTask([] { delayUs(150); }, 5_hz); + int task3 = scheduler.addTask([] { delayUs(100); }, 500_hz, + TaskScheduler::Policy::RECOVER); + scheduler.addTask([] { delayUs(100); }, 1_khz, + TaskScheduler::Policy::RECOVER); + scheduler.addTask([] { delayUs(100); }, 1ms, TaskScheduler::Policy::SKIP); printf("4 tasks added (2Hz 5Hz 500Hz 1KHz)\n"); printf("The scheduler will be started in 2 seconds\n"); @@ -187,7 +209,7 @@ void test_fill_scheduler() TaskScheduler scheduler{}; printf("Adding tasks until the scheduler is full\n"); - size_t taskCount = 0; + int taskCount = 0; // Fill up the scheduler with tasks do { @@ -203,12 +225,12 @@ void test_fill_scheduler() // Subtract one because the 0-th task is reserved if (taskCount != TaskScheduler::MAX_TASKS - 1) { - printf("Error: couldn't fill the scheduler: taskCount = %zu \n", + printf("Error: couldn't fill the scheduler: taskCount = %d \n", taskCount); return; } - printf("Done adding tasks: taskCount = %zu\n", taskCount); + printf("Done adding tasks: taskCount = %d\n", taskCount); printf("Trying to add another task\n"); // Try to add another task @@ -218,7 +240,7 @@ void test_fill_scheduler() return; } - printf("Added tasks successfully\n"); + printf("Adding a tasks failed as expected, all good\n"); printf("Starting the scheduler\n"); scheduler.start(); @@ -314,7 +336,7 @@ void test_edge_cases() printf("Starting the scheduler\n"); scheduler.start(); - printf("Starting the scheduler again"); + printf("Starting the scheduler again\n"); if (scheduler.start()) { printf("Error: started the scheduler twice\n"); @@ -322,13 +344,13 @@ void test_edge_cases() Thread::sleep(1000); - printf("Disabling out-of-range tasks with IDs 0 and 256"); + printf("Disabling out-of-range tasks with IDs 0 and 256\n"); scheduler.disableTask(0); scheduler.disableTask(256); Thread::sleep(1000); - printf("Enabling out-of-range tasks with IDs 0 and 256"); + printf("Enabling out-of-range tasks with IDs 0 and 256\n"); scheduler.enableTask(0); scheduler.enableTask(256); @@ -365,12 +387,40 @@ void test_long_range() scheduler.stop(); } +/** + * @brief Tests the scheduler with tasks running at a high frequency + */ +void test_high_frequency() +{ + using namespace Units::Frequency; + + TaskScheduler scheduler{}; + scheduler.addTask([&] { delayUs(10); }, 1_khz); + scheduler.addTask([&] { delayUs(10); }, 1_khz); + scheduler.addTask([&] { delayUs(10); }, 2_khz); + scheduler.addTask([&] { delayUs(10); }, 2_khz); + + printf("4 tasks added (1KHz 1KHz 2KHz 2KHz)\n"); + + printf("Starting the scheduler\n"); + scheduler.start(); + + Thread::sleep(5 * 1000); + + printf("Stopping the scheduler\n"); + scheduler.stop(); + + printTaskStats(scheduler); +} + } // namespace int main() { setup(); + printf("\n"); + // Avoid clutter from tasks since this test will add a lot of tasks taskLogEnabled = false; printf("=> Running the fill scheduler test\n"); @@ -399,6 +449,11 @@ int main() printf("\n"); + printf("=> Running the high frequency task test\n"); + test_high_frequency(); + + printf("\n"); + // Avoid clutter from tasks since this test will run for a while taskLogEnabled = false; printf("=> Running the long range test, this may take a while\n");