diff --git a/.vscode/settings.json b/.vscode/settings.json
index b90bff8b4f7f467c18dcf057d5f34cc3f21c17a5..1641f81b44c6020c1fa98d5444302f7a55bc6e5c 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -273,6 +273,7 @@
"stof",
"SWSTART",
"syaw",
+ "taskscheduler",
"TCIE",
"TCIF",
"TDHR",
diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index 0ce00b344ab27d8e9306b3f255db367f14f44676..d22747bd980a16afb360e03a80db97a19cc04e45 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -35,6 +35,8 @@ namespace Boardcore
TaskScheduler::TaskScheduler()
: ActiveObject(STACK_MIN_FOR_SKYWARD, miosix::PRIORITY_MAX - 1)
{
+ stopFlag = false;
+ running = false;
}
uint8_t TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id,
@@ -42,26 +44,27 @@ uint8_t TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id,
{
Lock<FastMutex> lock(mutex);
+ // Check if in the corresponding id there's already a task
+ if (tasks[id] != nullptr)
+ {
+ LOG_ERR(logger, "Full task scheduler, id = 255");
+ return 0;
+ }
+
// Register the task into the map
- Task task = {function, period, id, policy, -1, {}, {}, {}, 0, 0};
- auto result = tasks.insert({id, task});
+ Task* task =
+ new Task{function, period, id, true, policy, -1, {}, {}, {}, 0, 0};
+ tasks[id] = task;
if (policy == Policy::ONE_SHOT)
startTick += period;
- if (result.second)
- {
- // Add the task first event in the agenda
- Event event = {&(result.first->second), startTick};
- agenda.push(event);
- condvar.broadcast(); // Signals the run thread
- }
- else
- {
- LOG_ERR(logger, "Trying to add a task which id is already present");
- }
+ // Add the task first event in the agenda
+ Event event = {task, startTick};
+ agenda.push(event);
+ condvar.broadcast(); // Signals the run thread
- return result.second ? id : 0;
+ return id;
}
uint8_t TaskScheduler::addTask(function_t function, uint32_t period,
@@ -70,37 +73,30 @@ uint8_t TaskScheduler::addTask(function_t function, uint32_t period,
uint8_t id = 1;
// Find a suitable id for the new task
- auto it = tasks.cbegin(), end = tasks.cend();
- for (; it != end && id == it->first; ++it, ++id)
+ for (; tasks[id] != nullptr && id < 255; ++id)
;
- return addTask(function, period, id, policy, startTick) != 0 ? id : 0;
+ return addTask(function, period, id, policy, startTick);
}
bool TaskScheduler::removeTask(uint8_t id)
{
Lock<FastMutex> lock(mutex);
- if (tasks.find(id) == tasks.end())
+ // Check if the task is actually present
+ if (tasks[id] == nullptr)
{
LOG_ERR(logger, "Attempting to remove a task not registered");
return false;
}
- // Find the task in the agenda and remove it
- std::priority_queue<Event> newAgenda;
- while (agenda.size() > 0)
- {
- Event event = agenda.top();
- agenda.pop();
-
- if (event.task->id != id)
- newAgenda.push(event);
- }
- agenda = newAgenda;
+ // Set the validity of the task to false
+ tasks[id]->valid = false;
- // Remove the task from the tasks map
- tasks.erase(id);
+ // Remove the task from the tasks array
+ // We do not deallocate the task here because of future deallocation when
+ // popped from queue
+ tasks[id] = nullptr;
return true;
}
@@ -112,7 +108,7 @@ bool TaskScheduler::start()
if (running)
return false;
- // Normalize the tasks start time if they preceed the current tick
+ // Normalize the tasks start time if they precede the current tick
normalizeTasks();
return ActiveObject::start();
@@ -120,8 +116,8 @@ bool TaskScheduler::start()
void TaskScheduler::stop()
{
- stopFlag = true; // Signal the run function to stop
- condvar.signal(); // Wake the run function even if there are no tasks
+ stopFlag = true; // Signal the run function to stop
+ condvar.broadcast(); // Wake the run function even if there are no tasks
ActiveObject::stop();
}
@@ -132,8 +128,11 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats()
vector<TaskStatsResult> result;
- std::transform(tasks.begin(), tasks.end(), std::back_inserter(result),
- fromTaskIdPairToStatsResult);
+ for (auto& task : tasks)
+ {
+ if (task != nullptr)
+ result.push_back(fromTaskIdPairToStatsResult(task));
+ }
return result;
}
@@ -177,7 +176,7 @@ void TaskScheduler::run()
// 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 &&
- agenda.top().task->policy == Policy::SKIP)
+ nextEvent.task->policy == Policy::SKIP)
{
agenda.pop();
enqueue(nextEvent, startTick);
@@ -187,6 +186,7 @@ void TaskScheduler::run()
agenda.pop();
// Execute the task function
+ if (nextEvent.task->valid)
{
Unlock<FastMutex> unlock(lock);
@@ -206,7 +206,14 @@ void TaskScheduler::run()
}
else
{
+ if (!nextEvent.task->valid)
+ {
+ delete nextEvent.task;
+ agenda.pop();
+ }
+
Unlock<FastMutex> unlock(lock);
+
Thread::sleepUntil(nextEvent.nextTick);
}
}
@@ -215,7 +222,7 @@ void TaskScheduler::run()
void TaskScheduler::updateStats(const Event& event, int64_t startTick,
int64_t endTick)
{
- const float tickToMs = 1000.f / TICK_FREQ;
+ constexpr float tickToMs = 1000.f / TICK_FREQ;
// Activation stats
float activationError = startTick - event.nextTick;
@@ -236,14 +243,16 @@ void TaskScheduler::updateStats(const Event& event, int64_t startTick,
void TaskScheduler::enqueue(Event& event, int64_t startTick)
{
- const float msToTick = TICK_FREQ / 1000.f;
+ constexpr float msToTick = TICK_FREQ / 1000.f;
switch (event.task->policy)
{
case Policy::ONE_SHOT:
// If the task is one shot we won't push it to the agenda and we'll
// remove it from the tasks map.
- tasks.erase(event.task->id);
+ tasks[event.task->id] = nullptr;
+ delete event.task;
+
return;
case Policy::SKIP:
// Updated the missed events count
diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h
index 16dc8bc3dc56af6b15b90a33fbe6b7893d00b44d..119605437a6bd5c2672ebce6a599a8e5a1780a60 100644
--- a/src/shared/scheduler/TaskScheduler.h
+++ b/src/shared/scheduler/TaskScheduler.h
@@ -24,6 +24,7 @@
#include <ActiveObject.h>
#include <Singleton.h>
+#include <debug/debug.h>
#include <diagnostic/PrintLogger.h>
#include <utils/Stats/Stats.h>
@@ -69,7 +70,7 @@ public:
* 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 insted skips those and
+ * 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
@@ -90,27 +91,6 @@ public:
TaskScheduler();
- /**
- * @brief Add a task function to the scheduler.
- *
- * Note that each task has it's own unique ID, even one shot tasks!
- * Therefore, if a task already exists with the same id, the function will
- * fail and return false.
- *
- * 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.
- * @param id Task identification number.
- * @param policy Task policy, default is SKIP.
- * @param startTick First activation time, useful for synchronizing tasks.
- * @return true if the task was added successfully.
- */
- uint8_t addTask(function_t function, uint32_t period, uint8_t id,
- Policy policy = Policy::SKIP,
- int64_t startTick = miosix::getTick());
-
/**
* @brief Add a task function to the scheduler with an auto generated id.
*
@@ -157,6 +137,7 @@ private:
function_t function;
uint32_t period;
uint8_t id;
+ bool valid;
Policy policy;
int64_t lastCall; ///< Last activation tick for statistics computation.
Stats activationStats; ///< Stats about activation tick error.
@@ -181,6 +162,27 @@ private:
void run() override;
+ /**
+ * @brief Add a task function to the scheduler.
+ *
+ * Note that each task has it's own unique ID, even one shot tasks!
+ * Therefore, if a task already exists with the same id, the function will
+ * fail and return false.
+ *
+ * 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.
+ * @param id Task identification number.
+ * @param policy Task policy, default is SKIP.
+ * @param startTick First activation time, useful for synchronizing tasks.
+ * @return true if the task was added successfully.
+ */
+ uint8_t addTask(function_t function, uint32_t period, uint8_t id,
+ Policy policy = Policy::SKIP,
+ int64_t startTick = miosix::getTick());
+
/**
* @brief Update task statistics (Intended for when the task is executed).
*
@@ -205,20 +207,20 @@ private:
*/
void enqueue(Event& event, int64_t startTick);
- static TaskStatsResult fromTaskIdPairToStatsResult(
- const std::pair<const uint8_t, Boardcore::TaskScheduler::Task>& task)
+ static TaskStatsResult fromTaskIdPairToStatsResult(Task* task)
{
- return TaskStatsResult{task.second.id,
- task.second.period,
- task.second.activationStats.getStats(),
- task.second.periodStats.getStats(),
- task.second.workloadStats.getStats(),
- task.second.missedEvents,
- task.second.failedEvents};
+
+ return TaskStatsResult{task->id,
+ task->period,
+ task->activationStats.getStats(),
+ task->periodStats.getStats(),
+ task->workloadStats.getStats(),
+ task->missedEvents,
+ task->failedEvents};
}
miosix::FastMutex mutex; ///< Mutex to protect tasks and agenda.
- std::map<uint8_t, Task> tasks; ///< Holds all tasks to be scheduled.
+ std::array<Task*, 256> tasks{}; ///< Holds all tasks to be scheduled.
miosix::ConditionVariable condvar; ///< Used when agenda is empty.
std::priority_queue<Event> agenda; ///< Ordered list of functions.
diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp
index dcd390d358183c3c2bac0cf0a532eacadfcceb36..ee35f9ab6fe83ac7cc3345b8a64848b059ae1b08 100644
--- a/src/shared/sensors/SensorManager.cpp
+++ b/src/shared/sensors/SensorManager.cpp
@@ -210,7 +210,7 @@ void SensorManager::initScheduler()
{ sampler->sampleAndCallback(); });
scheduler->addTask(samplerUpdateFunction, sampler->getSamplingPeriod(),
- sampler->getID(), TaskScheduler::Policy::RECOVER);
+ TaskScheduler::Policy::RECOVER);
}
}
diff --git a/src/tests/actuators/test-servo.cpp b/src/tests/actuators/test-servo.cpp
index 3606640a5468e02a6e8b2dae060b440640a70bbe..893321c20ba25647a9ba402542831d014053874c 100644
--- a/src/tests/actuators/test-servo.cpp
+++ b/src/tests/actuators/test-servo.cpp
@@ -80,7 +80,7 @@ int main()
// Start a periodic task to move the first three servos
TaskScheduler scheduler;
- scheduler.addTask(&moveServo, 2 * 1000, 1);
+ uint8_t id = scheduler.addTask(&moveServo, 2 * 1000);
scheduler.start();
// Control the fourth servo manually
diff --git a/src/tests/scheduler/test-taskscheduler.cpp b/src/tests/scheduler/test-taskscheduler.cpp
index 05017ac1323bec3bb0fafe171ce4c34e09976969..097809b6b7670287f8cdf39f24d17c5aad165718 100644
--- a/src/tests/scheduler/test-taskscheduler.cpp
+++ b/src/tests/scheduler/test-taskscheduler.cpp
@@ -91,10 +91,10 @@ int main()
TaskScheduler::function_t f1KHz{&task1KHz};
TaskScheduler::function_t blockingF{&blockingTask};
- scheduler.addTask(f2Hz, 500, 1);
- scheduler.addTask(f5Hz, 200, 2);
- scheduler.addTask(f500Hz, 2, 3, TaskScheduler::Policy::RECOVER);
- scheduler.addTask(f1KHz, 1, 4);
+ scheduler.addTask(f2Hz, 500);
+ scheduler.addTask(f5Hz, 200);
+ scheduler.addTask(f500Hz, 2, TaskScheduler::Policy::RECOVER);
+ scheduler.addTask(f1KHz, 1, TaskScheduler::Policy::RECOVER);
printf("4 tasks added (2Hz 5Hz 500Hz 1KHz)\n");
printf("The scheduler will be started in 2 seconds\n");
@@ -115,14 +115,14 @@ int main()
printf("Now readding task 1 and 3\n");
signalPin5();
- scheduler.addTask(f2Hz, 500, 1);
- scheduler.addTask(f500Hz, 2, 3, TaskScheduler::Policy::RECOVER);
+ scheduler.addTask(f2Hz, 500);
+ scheduler.addTask(f500Hz, 2, TaskScheduler::Policy::RECOVER);
Thread::sleep(4 * 1000);
printf("Now adding a one shot task which will take 100ms to complete\n");
signalPin5();
- scheduler.addTask(blockingF, 0, 5, TaskScheduler::Policy::ONE_SHOT);
+ scheduler.addTask(blockingF, 0, TaskScheduler::Policy::ONE_SHOT);
Thread::sleep(4 * 1000);