diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index cb9d4107f71cabaa976c5cfcc04b89a7122b2933..1f8adf607950c437cac48ff11f9920af5912ea8c 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -32,39 +32,54 @@ using namespace miosix;
namespace Boardcore
{
-TaskScheduler::TaskScheduler()
- : ActiveObject(STACK_MIN_FOR_SKYWARD, miosix::PRIORITY_MAX - 1)
+TaskScheduler::TaskScheduler(miosix::Priority priority)
+ : ActiveObject(STACK_MIN_FOR_SKYWARD, priority)
{
- stopFlag = false;
- running = false;
+ // Create dynamically the tasks vector because of too much space
+ tasks = new std::array<Task, TASKS_SIZE>();
+
+ // Initialize the vector
+ for (size_t i = 1; i < TASKS_SIZE; i++)
+ {
+ function_t function;
+ (*tasks)[i] = makeTask(function, 0, i, false, Policy::SKIP);
+ }
}
+TaskScheduler::~TaskScheduler() { delete tasks; }
+
size_t TaskScheduler::addTask(function_t function, uint32_t period,
Policy policy, int64_t startTick)
{
-
Lock<FastMutex> lock(mutex);
size_t id = 1;
// Find a suitable id for the new task
- for (; id < TASKS_SIZE && tasks[id].valid == false; id++)
- ;
+ for (; id < TASKS_SIZE; id++)
+ {
+ if ((*tasks)[id].valid == false)
+ {
+ break;
+ }
+ }
// Check if in the corresponding id there's already a task
- if (tasks[id].valid)
+ if ((*tasks)[id].valid)
{
LOG_ERR(logger, "Full task scheduler, id = {:zu}", id);
return 0;
}
// Register the task into the map
- tasks[id] = makeTask(function, period, id, true, policy);
+ (*tasks)[id] = makeTask(function, period, id, true, policy);
if (policy == Policy::ONE_SHOT)
+ {
startTick += period;
+ }
// Add the task first event in the agenda
- Event event = {&(tasks[id]), startTick};
+ Event event = {&(*tasks)[id], startTick};
agenda.push(event);
condvar.broadcast(); // Signals the run thread
@@ -76,14 +91,14 @@ bool TaskScheduler::removeTask(uint8_t id)
Lock<FastMutex> lock(mutex);
// Check if the task is actually present
- if (tasks[id].valid == false)
+ if ((*tasks)[id].valid == false)
{
LOG_ERR(logger, "Attempting to remove a task not registered");
return false;
}
// Set the validity of the task to false
- tasks[id].valid = false;
+ (*tasks)[id].valid = false;
return true;
}
@@ -93,7 +108,9 @@ bool TaskScheduler::start()
// This check is necessary to prevent task normalization if the scheduler is
// already stopped
if (running)
+ {
return false;
+ }
// Normalize the tasks start time if they precede the current tick
normalizeTasks();
@@ -115,10 +132,12 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats()
vector<TaskStatsResult> result;
- for (auto& task : tasks)
+ for (auto& task : (*tasks))
{
if (task.valid)
+ {
result.push_back(fromTaskIdPairToStatsResult(task));
+ }
}
return result;
@@ -135,9 +154,11 @@ void TaskScheduler::normalizeTasks()
agenda.pop();
if (event.nextTick < currentTick)
+ {
event.nextTick +=
((currentTick - event.nextTick) / event.task->period + 1) *
event.task->period;
+ }
newAgenda.push(event);
}
@@ -155,7 +176,9 @@ void TaskScheduler::run()
// Exit if the ActiveObject has been stopped
if (shouldStop())
+ {
return;
+ }
int64_t startTick = getTick();
Event nextEvent = agenda.top();
@@ -236,8 +259,7 @@ void TaskScheduler::enqueue(Event& event, int64_t startTick)
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[event.task->id].valid = false;
- delete event.task;
+ (*tasks)[event.task->id].valid = false;
return;
case Policy::SKIP:
diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h
index 0efe40985e90d90fff586aadbb15f6fd85821f1b..ff01c6692fde0b99151453ed733a8856a42729e9 100644
--- a/src/shared/scheduler/TaskScheduler.h
+++ b/src/shared/scheduler/TaskScheduler.h
@@ -94,7 +94,9 @@ public:
RECOVER ///< Prioritize the number of executions over the period.
};
- TaskScheduler();
+ TaskScheduler(miosix::Priority priority = miosix::PRIORITY_MAX - 1);
+
+ ~TaskScheduler();
/**
* @brief Add a task function to the scheduler with an auto generated id.
@@ -217,9 +219,9 @@ private:
}
miosix::FastMutex mutex; ///< Mutex to protect tasks and agenda.
- std::array<Task, TASKS_SIZE> tasks{}; ///< Holds all tasks to be scheduled.
- miosix::ConditionVariable condvar; ///< Used when agenda is empty.
- std::priority_queue<Event> agenda; ///< Ordered list of functions.
+ std::array<Task, TASKS_SIZE>* tasks; ///< Holds all tasks to be scheduled.
+ miosix::ConditionVariable condvar; ///< Used when agenda is empty.
+ std::priority_queue<Event> agenda; ///< Ordered list of functions.
PrintLogger logger = Logging::getLogger("taskscheduler");
};
diff --git a/src/tests/catch/test-sensormanager-catch.cpp b/src/tests/catch/test-sensormanager-catch.cpp
index 9ae7ca2c111d41eae695fcb99f758245284eac8f..0b30271e9620ec2a4f5da764d5a9e86d5b0c5a15 100644
--- a/src/tests/catch/test-sensormanager-catch.cpp
+++ b/src/tests/catch/test-sensormanager-catch.cpp
@@ -37,7 +37,7 @@
using namespace Boardcore;
-static const uint8_t FIRST_TASK_ID = 1; // used to test IDs assignment to tasks
+static const size_t FIRST_TASK_ID = 1; // used to test IDs assignment to tasks
class FailingSensorCatch : public Sensor<TestData>
{
diff --git a/src/tests/scheduler/test-taskscheduler.cpp b/src/tests/scheduler/test-taskscheduler.cpp
index 097809b6b7670287f8cdf39f24d17c5aad165718..b7501d468bde77c20d5f86f0371a5340854e2a1d 100644
--- a/src/tests/scheduler/test-taskscheduler.cpp
+++ b/src/tests/scheduler/test-taskscheduler.cpp
@@ -95,6 +95,7 @@ int main()
scheduler.addTask(f5Hz, 200);
scheduler.addTask(f500Hz, 2, TaskScheduler::Policy::RECOVER);
scheduler.addTask(f1KHz, 1, 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");
@@ -102,7 +103,11 @@ int main()
printf("Now starting the task scheduler\n");
signalPin5();
- scheduler.start();
+ if (!scheduler.start())
+ {
+ printf("Error starting the task scheduler\n");
+ return 0;
+ }
Thread::sleep(4 * 1000);
@@ -113,7 +118,7 @@ int main()
Thread::sleep(4 * 1000);
- printf("Now readding task 1 and 3\n");
+ printf("Now re-adding task 1 and 3\n");
signalPin5();
scheduler.addTask(f2Hz, 500);
scheduler.addTask(f500Hz, 2, TaskScheduler::Policy::RECOVER);
@@ -122,7 +127,7 @@ int main()
printf("Now adding a one shot task which will take 100ms to complete\n");
signalPin5();
- scheduler.addTask(blockingF, 0, TaskScheduler::Policy::ONE_SHOT);
+ // scheduler.addTask(blockingF, 0, TaskScheduler::Policy::ONE_SHOT);
Thread::sleep(4 * 1000);