33a148b4dd
The finished CLI implementation. All actions should be in a working condition. It should be able to read messages from any serial writer/reader. To do later if time build an desktop application to communicate with
550 lines
20 KiB
C
550 lines
20 KiB
C
/**************************************************************************
|
|
* NAME: scheduler.c *
|
|
* *
|
|
* AUTHOR: Jonas Holmberg *
|
|
* *
|
|
* PURPOSE: Defining the the scheduler to be used in the system to *
|
|
* organize the runtime for the tasks in the system based on *
|
|
* priority. *
|
|
* INFORMATION: Many elements and ideas obtained from beta/cleanflight. *
|
|
* *
|
|
* GLOBAL VARIABLES: *
|
|
* Variable Type Description *
|
|
* -------- ---- ----------- *
|
|
* SystemTasks task_t[] Contains all the tasks that can exist in *
|
|
* the system right now *
|
|
* *
|
|
* averageSystem uint16_t The current load of the system in percent.*
|
|
* LoadPercent May or may not display correctly. *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
#include "Scheduler/scheduler.h"
|
|
#include "Scheduler/tasks.h"
|
|
#include "drivers/system_clock.h"
|
|
#include "utilities/math_helpers.h"
|
|
#include <string.h>
|
|
#include "drivers/led.h"
|
|
|
|
|
|
/* Variables ------------------------------------------- */
|
|
|
|
static task_t *currentTask = NULL; //The current task that the is run
|
|
|
|
static uint32_t totalSchedulerPasses = 0; /* Number of times the scheduler has been invoked */
|
|
static uint32_t totalSchedulerReadyTasks = 0; /* The amount of tasks that totally has been ready in the scheduler */
|
|
|
|
uint32_t currentTime = 0; //The current time in microseconds
|
|
uint16_t averageSystmeLoad = 0;
|
|
//
|
|
static int taskReadyQueueSize = 0; //The number of tasks in the ready queue
|
|
|
|
static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks that are ready to run in the system + 1 for a NULL value at the end*/
|
|
|
|
uint16_t averageSystemLoadPercent = 0; //The load of the system in percent
|
|
|
|
static uint32_t tasksExecutionTimeUs = 0; //Total execution time of the task for one pass of the system task
|
|
//static uint32_t lastSystemLoadTimeValUs = 0; //Not used right now, would be used to calculate load with time vals
|
|
|
|
uint32_t taskAgeCycleStatisitcs[taskAgeCycleCounterSize]; //Age cycle statistics array
|
|
|
|
/* Functions to operate on the task queue ------------------------------------------------------- */
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Clears the ready queue. *
|
|
* INFORMATION: Clears the entire queue and sets the size to the number of
|
|
* possible tasks that exist in the system. *
|
|
**************************************************************************/
|
|
void taskReadyQueueClear(void)
|
|
{
|
|
//Empties the queue by settin all the positions to 0(NULL)
|
|
memset(taskReadyQueue, 0, sizeof(taskReadyQueue));
|
|
taskReadyQueueSize = 0;
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Checks if a task already exists in the ready queue.
|
|
* *
|
|
* INFORMATION: Given a task it will be compared to the tasks in the queue.
|
|
* If it exist inside the queue the function will return true,
|
|
* otherwise it will return false.
|
|
**************************************************************************/
|
|
bool taskReadyQueueContains(task_t *task)
|
|
{
|
|
//Go through the taskReadyQueue to see if the current task is contained
|
|
for (int i = 0; i < taskReadyQueueSize; i++)
|
|
{
|
|
if (taskReadyQueue[i] == task)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Adds a new task to the ready queue.
|
|
* *
|
|
* INFORMATION: This function will add a new task to the system if it does
|
|
* not already exist in it, or if the task queue is full (it
|
|
* should not be able to be that if every task is implemented
|
|
* correctly). Otherwise the task will be added to the queue
|
|
* based on priority and then period. *
|
|
**************************************************************************/
|
|
bool taskReadyQueueAdd(task_t *task)
|
|
{
|
|
int i; /* Iterator */
|
|
int j; /* Inner iterator used to sort based on desired period */
|
|
|
|
//Check if the task is in the queue already
|
|
if (taskReadyQueueContains(task) || taskReadyQueueSize >= TASK_COUNT)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
//Try to add the task to the taskReadyQueue at a good position
|
|
//Order is: staticPriority > desired period > all else
|
|
//<= taskReadyQueueSize because if we go through the entire thing we should get a null value
|
|
//representing the position directly after the last one in the queue.
|
|
for (i = 0; i < taskReadyQueueSize; i++)
|
|
{
|
|
//check if the static prio of the current position in the queue is less than the task we want to add
|
|
if(taskReadyQueue[i]->staticPriority < task->staticPriority)
|
|
{
|
|
//order tasks based on lowest period first, within the ones with the same priority
|
|
//ToDo: check if setting j to i-1 initially will produce the same result, in that case add another iterator to get item from taskReadyQueue
|
|
for (j = i; taskReadyQueue[j]->staticPriority == task->staticPriority; j++ )
|
|
{
|
|
//If the new task has a shorter period place in front
|
|
if (task->desiredPeriod <= taskReadyQueue[j]->desiredPeriod)
|
|
{
|
|
//remove one increment because otherwise it will be wrong
|
|
j--;
|
|
}
|
|
}
|
|
//task does not have shorter period than any other with the same priority, add behind them
|
|
j++;
|
|
memmove(&taskReadyQueue[j+1], &taskReadyQueue[j], sizeof(task) * (taskReadyQueueSize - j));
|
|
taskReadyQueue[j] = task;
|
|
++taskReadyQueueSize;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
//If this is true it needs to be placed at the very back of the queue
|
|
if (taskReadyQueue[i] == NULL )
|
|
{
|
|
taskReadyQueue[i] = task; //ToDo: check if this works correctly, that a new NULL value will exist at i+1
|
|
taskReadyQueueSize ++;
|
|
return true;
|
|
}
|
|
|
|
//Could not add the task in the anywhere in the queue
|
|
return false;
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Removes a task from the ready queue.
|
|
* *
|
|
* INFORMATION: Given a task it will remove it from the ready queue if it
|
|
* exist somewhere inside the queue. *
|
|
**************************************************************************/
|
|
bool taskReadyQueueRemove(task_t *task)
|
|
{
|
|
for (int i = 0; i < taskReadyQueueSize; ++i) {
|
|
if (taskReadyQueue[i] == task) {
|
|
memmove(&taskReadyQueue[i], &taskReadyQueue[i+1], sizeof(task) * (taskReadyQueueSize - i));
|
|
--taskReadyQueueSize;
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: The function that will run when the scheduler chooses to run
|
|
* the SYSTEM task.
|
|
* *
|
|
* INFORMATION: This task function is responsible for calculating the load
|
|
* of the system. More things can be added to this task if
|
|
* needed. *
|
|
**************************************************************************/
|
|
void systemTaskSystem(void)
|
|
{
|
|
/* Calculate system load */
|
|
if (totalSchedulerPasses > 0) {
|
|
averageSystemLoadPercent = 100 * totalSchedulerReadyTasks / totalSchedulerPasses;
|
|
totalSchedulerPasses = 0;
|
|
totalSchedulerReadyTasks = 0;
|
|
/*uint32_t timeAtInstantUs = clock_get_us();
|
|
uint32_t timeBetween = timeAtInstantUs - lastSystemLoadTimeValUs;
|
|
float divVal = ((float)tasksExecutionTimeUs / (float)timeBetween);
|
|
|
|
averageSystemLoadPercent = 100* divVal;
|
|
lastSystemLoadTimeValUs = timeAtInstantUs;*/
|
|
tasksExecutionTimeUs = 0;
|
|
|
|
#ifdef USE_LED_WARNINGS_SYSTEM_LOAD
|
|
//ToDo: Test to light a led if the system is utilizing more than 100%
|
|
if (averageSystemLoadPercent >= 100)
|
|
{
|
|
ledSetWarningType(LEDWARNING_LED0_ON);
|
|
}
|
|
else if(averageSystemLoadPercent >= 80)
|
|
{
|
|
ledSetWarningType(LEDWARNING_LED0_BLINK);
|
|
}
|
|
else if(averageSystemLoadPercent >= 60)
|
|
{
|
|
ledSetWarningType(LEDWARNING_LED1_ON);
|
|
}
|
|
else if(averageSystemLoadPercent >= 40)
|
|
{
|
|
ledSetWarningType(LEDWARNING_LED1_BLINK);
|
|
}
|
|
else
|
|
{
|
|
ledSetWarningType(LEDWARNING_OFF);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Enables or disables a task to be able to run in the system.
|
|
*
|
|
* INFORMATION: Given an id for a task it can be added or removed from the
|
|
* active tasks in the system. Meaning tasks that can be selected by the
|
|
* scheduler to run.
|
|
**************************************************************************/
|
|
void enableTask(taskId_t taskId, bool enabled)
|
|
{
|
|
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
|
|
task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
|
|
if (enabled && task->taskFunction) {
|
|
taskReadyQueueAdd(task);
|
|
} else {
|
|
taskReadyQueueRemove(task);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Returns the delta time value of a task.
|
|
*
|
|
* INFORMATION: Given an id for a task it will return is delta time value.
|
|
* The time between its latest run and the one before.
|
|
**************************************************************************/
|
|
uint32_t getTaskDeltaTime(taskId_t taskId)
|
|
{
|
|
if (taskId == TASK_SELF || taskId < TASK_COUNT) {
|
|
task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
|
|
return task->taskLatestDeltaTime;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Gives a task a new period.
|
|
*
|
|
* INFORMATION: Given an id for a task its period can be changed to a new
|
|
* desired value.
|
|
**************************************************************************/
|
|
void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros)
|
|
{
|
|
if (taskId == TASK_SELF || taskId < TASK_COUNT)
|
|
{
|
|
task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
|
|
task->desiredPeriod = MAX(100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging
|
|
}
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Returns an array of ageCyckle values observed when
|
|
* scheduling the tasks.
|
|
*
|
|
* INFORMATION: If a task has an age cycle greater than 1 it means it has
|
|
* missed one or more "periods". Each slot in the 16 value
|
|
* sized array matches a cycle age value observed for a task.
|
|
* Cycle age 1 or less is not counted since it would be to
|
|
* many and we don't care about when it is correct, this is only
|
|
* used to observe how many is wrong. Every task will generate
|
|
* a age cycle value each scheduling attempt. Each time a task
|
|
* missed his period the values in the array that this function
|
|
* return will increase.
|
|
**************************************************************************/
|
|
#ifdef USE_TASK_AGE_CYCLE_STATISTICS
|
|
void getSchedulerAgeCycleData(uint32_t outValue[taskAgeCycleCounterSize])
|
|
{
|
|
outValue = taskAgeCycleStatisitcs;
|
|
}
|
|
#endif
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Get some information of a task.
|
|
*
|
|
* INFORMATION: Given an id for a task its current values can be obtain.
|
|
* The variable taskInfo will point to the obtained information
|
|
* and act as an out variable.
|
|
**************************************************************************/
|
|
void getTaskInfo(taskId_t taskId, taskInformation_t * taskInfo)
|
|
{
|
|
taskInfo->taskName = SystemTasks[taskId].taskName;
|
|
taskInfo->subTaskName = SystemTasks[taskId].subTaskName;
|
|
taskInfo->isEnabled = taskReadyQueueContains(&SystemTasks[taskId]);
|
|
taskInfo->desiredPeriod = SystemTasks[taskId].desiredPeriod;
|
|
taskInfo->staticPriority = SystemTasks[taskId].staticPriority;
|
|
taskInfo->maxExecutionTime = SystemTasks[taskId].maxExecutionTime;
|
|
taskInfo->totalExecutionTime = SystemTasks[taskId].totalExecutionTime;
|
|
taskInfo->averageExecutionTime = SystemTasks[taskId].averageExecutionTime;
|
|
taskInfo->latestDeltaTime = SystemTasks[taskId].taskLatestDeltaTime;
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Enables the tasks that should run.
|
|
*
|
|
* INFORMATION: All the tasks in the system that should be added to the run
|
|
* queue will be added when this function is invoked.
|
|
**************************************************************************/
|
|
void initSchedulerTasks(void)
|
|
{
|
|
//Enable the tasks in the system
|
|
enableTask(TASK_SYSTEM, true);
|
|
|
|
enableTask(TASK_GYROPID, true);
|
|
|
|
enableTask(TASK_ACCELEROMETER, true);
|
|
|
|
enableTask(TASK_ATTITUDE, true);
|
|
|
|
enableTask(TASK_RX, true);
|
|
|
|
enableTask(TASK_RX_CLI, true);
|
|
|
|
enableTask(TASK_SERIAL, true);
|
|
|
|
enableTask(TASK_BATTERY, true);
|
|
|
|
#ifdef BARO
|
|
enableTask(TASK_BARO, true);
|
|
#endif
|
|
|
|
#ifdef COMPASS
|
|
enableTask(TASK_COMPASS, true);
|
|
#endif
|
|
|
|
#ifdef GPS
|
|
enableTask(TASK_GPS, true);
|
|
#endif
|
|
|
|
#ifdef SONAR
|
|
enableTask(TASK_SONAR, true);
|
|
#endif
|
|
|
|
#if defined(BARO) || defined(SONAR)
|
|
enableTask(TASK_ALTITUDE, true);
|
|
#endif
|
|
|
|
#if BEEPER
|
|
enableTask(TASK_BEEPER, true);
|
|
#endif
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: Initiates the scheduler and makes it ready to run.
|
|
*
|
|
* INFORMATION: The init will reset the ready queue of the system and add
|
|
* all the tasks that should be added. The tasks that are
|
|
* supposed to run with the current configuration of the system
|
|
**************************************************************************/
|
|
void initScheduler(void)
|
|
{
|
|
//Clear the queue
|
|
taskReadyQueueClear();
|
|
|
|
//Init all the ToDo: add back when it is known that the scheduler works
|
|
#ifndef USE_DEBUG_TASKS
|
|
initSchedulerTasks();
|
|
#endif
|
|
|
|
//If we use debug tasks only init those
|
|
#ifdef USE_DEBUG_TASKS
|
|
enableTask(TASK_DEBUG_1, true);
|
|
enableTask(TASK_DEBUG_2, true);
|
|
enableTask(TASK_DEBUG_3, true);
|
|
enableTask(TASK_SYSTEM, true);
|
|
#endif
|
|
}
|
|
|
|
|
|
/**************************************************************************
|
|
* BRIEF: The main scheduler function.
|
|
*
|
|
* INFORMATION: This function is responsible for choosing the next task that
|
|
* the system should run. This is the main part of the system
|
|
* that will always run, and is the part of the code that will
|
|
* run in each iteration of the system loop.
|
|
**************************************************************************/
|
|
void scheduler(void)
|
|
{
|
|
//Get the current time in Micro seconds
|
|
currentTime = clock_get_us();
|
|
task_t * taskToRun;
|
|
task_t * selectedTask = NULL;
|
|
uint16_t currentDynamicPriority = 0;
|
|
uint32_t currentReadyTasks = 0;
|
|
float currentAgeCyckleExact = 0;
|
|
float ageCycleExact = 0;
|
|
bool isRealTime = false;
|
|
|
|
/* Check if a task has entered a new period and assign its new dynamic priority */
|
|
/* Go through all the tasks to check if they should be able to run */
|
|
for (int i = 0; i < taskReadyQueueSize; i++)
|
|
{
|
|
//Get the next task in the queue
|
|
taskToRun = taskReadyQueue[i];
|
|
|
|
/* Check if the task is an event, else its a time controlled task */
|
|
if (taskToRun->checkEventTriggered != NULL)
|
|
{
|
|
//ToDO: Handle event tasks, they should get to operate at a higher priority
|
|
//ToDo: test if it works
|
|
if (taskToRun->dynamicPriority > 0)
|
|
{
|
|
taskToRun->taskAgeCycle = 1 + ((currentTime - taskToRun->lastSignaledAt) / taskToRun->desiredPeriod);
|
|
taskToRun->dynamicPriority = 1 + taskToRun->staticPriority * taskToRun->taskAgeCycle;
|
|
currentReadyTasks++;
|
|
}
|
|
else if (taskToRun->checkEventTriggered(currentTime - taskToRun->lastExecutedAt))
|
|
{
|
|
taskToRun->lastSignaledAt = currentTime;
|
|
taskToRun->taskAgeCycle = 1;
|
|
taskToRun->dynamicPriority = 1 + taskToRun->staticPriority;
|
|
currentReadyTasks++;
|
|
}
|
|
else
|
|
{
|
|
taskToRun->taskAgeCycle = 0;
|
|
}
|
|
|
|
|
|
}
|
|
else //Handle time controlled tasks
|
|
{
|
|
//Calculate a new age cycle for the task. It represents if the task is in a new "period" in relation to its last execution
|
|
//If the value is 0 it is still in the same "period" as the last execution
|
|
taskToRun->taskAgeCycle = (currentTime - taskToRun->lastExecutedAt) / taskToRun->desiredPeriod;
|
|
|
|
#ifdef USE_TASK_AGE_CYCLE_STATISTICS
|
|
if (taskToRun->taskAgeCycle > 1)
|
|
taskAgeCycleStatisitcs[taskToRun->taskAgeCycle] ++; //increment the statistic counter for age cycles if we miss period
|
|
#endif
|
|
#ifdef USE_LED_WARNINGS_MISSED_PERIOD
|
|
if (taskToRun->taskAgeCycle > 1)
|
|
ledSetWarningType(LEDWARNING_LED0_BLINK_ONCE);
|
|
#endif
|
|
|
|
//May not be used. Could be used to check what task is closest to its next period instance
|
|
ageCycleExact = ((float)currentTime - (float)taskToRun->lastExecutedAt) / (float)taskToRun->desiredPeriod;
|
|
|
|
//Check if > 0, if it has entered a new "period"
|
|
if (taskToRun->taskAgeCycle > 0)
|
|
{
|
|
//Calculate the new dynamic priority for the task
|
|
taskToRun->dynamicPriority = 1 + taskToRun->taskAgeCycle * taskToRun->staticPriority;
|
|
currentReadyTasks ++; //Increase the number of ready tasks
|
|
}
|
|
}
|
|
|
|
//If the current task has the largest dynamic priority so far it could be a candidate for execution
|
|
if(taskToRun->dynamicPriority >= currentDynamicPriority && taskToRun->taskAgeCycle >= 1)
|
|
{
|
|
bool changeTask = false; //flag used to check if the task should be updated
|
|
/* If a realtime task is found the bool isRealTime will be set to true
|
|
* meaning no other tasks than realtime will be able to enter the function */
|
|
if (taskToRun->staticPriority == PRIORITY_REALTIME) //if it is a realtime task
|
|
{
|
|
isRealTime = true; //toggle this flag so that no non realtime task can be selected this cycle
|
|
|
|
changeTask = true;
|
|
}
|
|
else if (!isRealTime) //If task is not realtime
|
|
{
|
|
if (taskToRun->dynamicPriority == currentDynamicPriority) //if the tasks have the same priority
|
|
{
|
|
if (ageCycleExact > currentAgeCyckleExact) //if the current tasks deadline is closer than selected task
|
|
{
|
|
changeTask = true;
|
|
currentAgeCyckleExact = ageCycleExact; //update the value
|
|
}
|
|
}
|
|
else
|
|
{
|
|
changeTask = true;
|
|
}
|
|
}
|
|
|
|
if (changeTask == true) //if the task should change
|
|
{
|
|
selectedTask = taskToRun; //update the selected task
|
|
currentDynamicPriority = taskToRun->dynamicPriority; //update the current highest dynamic priority
|
|
}
|
|
}
|
|
} /* for-loop end */
|
|
|
|
|
|
//Used for meassuring load
|
|
totalSchedulerReadyTasks += currentReadyTasks;
|
|
totalSchedulerPasses ++;
|
|
|
|
//Assign the selected task to the pointer for the current task
|
|
currentTask = selectedTask;
|
|
|
|
//If we have found a task to run, execute the function that the task is responsible for
|
|
if (selectedTask != NULL)
|
|
{
|
|
selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt; //calculate the time between now and last execution
|
|
selectedTask->lastExecutedAt = currentTime; //update the last execution time to this moment
|
|
selectedTask->dynamicPriority = 0; //reset the dynamic priority to 0
|
|
|
|
//handle the execution of the tasks function
|
|
const uint32_t timeBeforeExecution = clock_get_us();
|
|
selectedTask->taskFunction(); //Run the function associated with the current task
|
|
const uint32_t TimeAfterExecution = clock_get_us();
|
|
const uint32_t taskExecutionTime = TimeAfterExecution - timeBeforeExecution; //calculate the execution time of the task
|
|
|
|
tasksExecutionTimeUs += taskExecutionTime; //Add the task execution time for each task execution, will be used to
|
|
|
|
//Save statistical values
|
|
selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32;
|
|
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
|
|
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|