[TaskScheduler] Pre-allocate the agenda's backing vector

Avoids runtime re-allocations by reserving the
maximum capacity at construction time.

src/shared/scheduler/TaskScheduler.cpp

@@ -31,6 +31,7 @@ using namespace miosix;
 
 namespace Boardcore
 {
+
 namespace Constants
 {
 static constexpr unsigned int TICKS_PER_MS =
@@ -39,8 +40,16 @@ 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{};
+    agendaStorage.reserve(MAX_TASKS);
+    return EventQueue{std::greater<Event>{}, std::move(agendaStorage)};
+}
+
 TaskScheduler::TaskScheduler(miosix::Priority priority)
-    : ActiveObject(STACK_MIN_FOR_SKYWARD, priority), tasks()
+    : ActiveObject(STACK_MIN_FOR_SKYWARD, priority), tasks(),
+      agenda(makeAgenda())
 {
     // Preallocate the vector to avoid dynamic allocation later on
     tasks.reserve(MAX_TASKS);
@@ -65,17 +74,21 @@ size_t TaskScheduler::addTask(function_t function, uint32_t period,
         return 0;
     }
 
-    // Insert a new task with the given parameters
-    tasks.emplace_back(function, period, policy);
-    size_t id = tasks.size() - 1;
-
     if (policy == Policy::ONE_SHOT)
     {
         startTick += period;
     }
 
-    // Add the task first event in the agenda, performs in-place construction
+    // Insert a new task with the given parameters
+    tasks.emplace_back(function, period, policy, startTick);
+    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);
+    }
     condvar.broadcast();  // Signals the run thread
 
     mutex.unlock();
@@ -131,8 +144,8 @@ bool TaskScheduler::start()
         return false;
     }
 
-    // Normalize the tasks start time if they precede the current tick
-    normalizeTasks();
+    // Populate the agenda with the tasks we have so far
+    populateAgenda();
 
     return ActiveObject::start();
 }
@@ -163,27 +176,24 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats()
     return result;
 }
 
-void TaskScheduler::normalizeTasks()
+void TaskScheduler::populateAgenda()
 {
     int64_t currentTick = getTick();
 
-    EventQueue newAgenda;
-    while (!agenda.empty())
+    for (size_t id = 1; id < tasks.size(); id++)
     {
-        Event event = agenda.top();
-        agenda.pop();
+        Task& task = tasks[id];
 
-        if (event.nextTick < currentTick)
+        int64_t nextTick = task.startTick;
+        // Normalize the tasks start time if they precede the current tick
+        if (nextTick < currentTick)
         {
-            Task& task = tasks[event.taskId];
-            event.nextTick +=
-                ((currentTick - event.nextTick) / task.period + 1) *
-                task.period;
+            nextTick +=
+                ((currentTick - nextTick) / task.period + 1) * task.period;
         }
 
-        newAgenda.push(event);
+        agenda.emplace(id, nextTick);
     }
-    agenda = std::move(newAgenda);
 }
 
 void TaskScheduler::run()
@@ -304,16 +314,17 @@ void TaskScheduler::enqueue(Event event, int64_t startTick)
 }
 
 TaskScheduler::Task::Task()
-    : function(nullptr), period(0), enabled(false), policy(Policy::SKIP),
-      lastCall(-1), activationStats(), periodStats(), workloadStats(),
-      missedEvents(0), failedEvents(0)
+    : function(nullptr), period(0), startTick(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)
-    : function(function), period(period), enabled(true), policy(policy),
-      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),
+      policy(policy), lastCall(-1), activationStats(), periodStats(),
+      workloadStats(), missedEvents(0), failedEvents(0)
 {
 }
 

src/shared/scheduler/TaskScheduler.h

@@ -87,7 +87,7 @@ public:
      * respected and the task will run consecutively for some time (See issue
      * #91).
      */
-    enum class Policy
+    enum class Policy : uint8_t
     {
         ONE_SHOT,  ///< Run the task one single timer.
         SKIP,      // Skips lost executions and stays aligned with the original
@@ -133,16 +133,12 @@ public:
     std::vector<TaskStatsResult> getTaskStats();
 
 private:
-    /**
-     * @brief Check the start time of the tasks in the agenda and moves them in
-     * the future respecting the period in respect to the original start time.
-     */
-    void normalizeTasks();
-
     struct Task
     {
         function_t function;
         uint32_t period;    // [ms]
+        int64_t startTick;  ///< 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.
@@ -163,8 +159,10 @@ private:
          * @param function The std::function to be called
          * @param period The Period in [ms]
          * @param policy The task policy in case of a miss
+         * @param startTick The first activation time
          */
-        explicit Task(function_t function, uint32_t period, Policy policy);
+        explicit Task(function_t function, uint32_t period, Policy policy,
+                      int64_t startTick);
 
         // Delete copy constructor and copy assignment operator to avoid copying
         // and force moving
@@ -209,6 +207,21 @@ private:
     using EventQueue =
         std::priority_queue<Event, std::vector<Event>, std::greater<Event>>;
 
+    /**
+     * @brief Instantiates a new EventQueue backed by a vector with a
+     * capacity of `MAX_TASKS` to avoid reallocations when inserting new events.
+     */
+    static EventQueue makeAgenda();
+
+    /**
+     * @brief Populates the agenda prior to starting the scheduler. Checks the
+     * start time of the tasks in the agenda and moves them in the future
+     * respecting the period in respect to the original start time.
+     * @note This function must be called before starting the scheduler or the
+     * agenda will be empty.
+     */
+    void populateAgenda();
+
     void run() override;
 
     /**