diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index a058bba..5c17864 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -1,11 +1,22 @@ /************************************************************************** -* NAME: scheduler.h * -* 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 * +* NAME: scheduler.h * +* * +* 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. * +* * ***************************************************************************/ #ifndef SCHEDULER_H_ @@ -15,6 +26,7 @@ #include #include "stm32f4xx_revo.h" +#define taskAgeCycleCounterSize 16 /* Enum containing all the possible task priorities */ typedef enum @@ -28,6 +40,20 @@ typedef enum } taskPriority_t; +/* struct used to obtain task information */ +typedef struct { + const char * taskName; + const char * subTaskName; + bool isEnabled; + uint32_t desiredPeriod; + uint8_t staticPriority; + uint32_t maxExecutionTime; + uint32_t totalExecutionTime; + uint32_t averageExecutionTime; + uint32_t latestDeltaTime; +} taskInformation_t; + + /* Struct used to contain the information for each task */ typedef struct { @@ -102,54 +128,100 @@ typedef enum TASK_SELF }taskId_t; -/* Variables --------------------------------------------------------------------------------- */ +/* Variables -------------------------------------------------------------*/ extern task_t SystemTasks[TASK_COUNT]; /* All the tasks that exist in the system */ -extern uint16_t CpuLoad; /* The current load on the cpu Todo */ extern uint16_t averageSystemLoadPercent; /* The average load on the system Todo */ -/* Functions that operate can operate on the tasks ------------------------------------------- */ +/* Functions that operate can operate on the tasks -----------------------*/ /************************************************************************** -* BRIEF: Enables or disables a specific task in the system * -* INFORMATION: Takes the id of the task(task_t) along with a true or * -* false value. If true the task will be added to the task queue, if false * -* it will be removed. * +* 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); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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); +/************************************************************************** +* 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); + + /* Functions that handle the scheduler ------------------------------------------------------- */ /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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); +/************************************************************************** +* 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]); +#endif + + #endif /* SCHEDULER_H_ */ diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index 69503e4..9b5ee73 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -1,8 +1,17 @@ /************************************************************************** -* NAME: tasks.h * -* 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: * +* NAME: tasks.h * +* * +* 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: Adds the initial task values for each task that can be in * +* the system, in the c file. In the h file functions that * +* when called will* perform the action of its corresponding * +* task. * +* * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * @@ -13,6 +22,11 @@ #include +#define GetUpdateRateHz(x) (1000000 / x) //x is the desired hz value given in micro seconds +#define GetUpdateRateMs(x) (x*1000) //X is the desired ms value given as micro seconds +#define GetUpdateRateUs(x) (x) //X is the desired us value given as micro seconds (Its the same, used for clarification) +#define GetUpdateRateSeconds(x) (x * 1000000) //X is the desired second value given as micro seconds + //All the task functions in the system, one for each task void systemTaskSystem(void); void systemTaskGyroPid(void); @@ -30,10 +44,8 @@ void systemTaskAltitude(void); void systemTaskBeeper(void); //Tasks used only for testing purposes -#ifdef USE_DEBUG_TASKS void systemTaskDebug_1(void); void systemTaskDebug_2(void); void systemTaskDebug_3(void); -#endif #endif /* SCHEDULER_TASKS_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/led.h b/UAV-ControlSystem/inc/drivers/led.h index e344720..72795ed 100644 --- a/UAV-ControlSystem/inc/drivers/led.h +++ b/UAV-ControlSystem/inc/drivers/led.h @@ -1,13 +1,112 @@ -/* - * led.h - * - * Created on: 21 sep. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: led.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Contains some led functionality. * +* * +* INFORMATION: Contains functions to configure pins to leds. It also * +* contains some led error codes that can be used in the * +* system, although at the time hardcoded to work only with * +* the revo on board leds. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #ifndef DRIVERS_LED_H_ #define DRIVERS_LED_H_ +//Update rate for the led +#define LED_UPDATE_TIMER 100000 + +//Different led warning approaches +typedef enum +{ + LEDWARNING_LED0_BLINK = 0, + LEDWARNING_LED1_BLINK, + LEDWARNING_LED0_ON, + LEDWARNING_LED1_ON, + LEDWARNING_LED0_BLINK_ONCE, + LEDWARNING_LED1_BLINK_ONCE, + LEDWARNING_OFF +}ledWarnings_t; + + +/************************************************************************** +* BRIEF: Enables pins so that they can be use for a Led +* +* INFORMATION: Given a pin and port enables that configuration to use led +**************************************************************************/ +void ledEnable(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Enables the two leds on board leds on the revolution board +* +* INFORMATION: +**************************************************************************/ +void ledReavoEnable(void); + + +/************************************************************************** +* BRIEF: Toggles a led +* +* INFORMATION: Given a pin and port, it attempts to toggle a led that +* should be linked with the given combination. +**************************************************************************/ +void ledToggle(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns on a led. +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOn(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns off a led. +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOff(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns on a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOnInverted(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns off a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOffInverted(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Updates the warning leds in the system +* +* INFORMATION: Checks if any led warning should be active and if its the +* case perform some led activities on a specific led +**************************************************************************/ +void ledIndicatorsUpdate(void); + + +/************************************************************************** +* BRIEF: Change the warning type of a led +* +* INFORMATION: Change the warning type of led given a warningId that is +* obtained from ledWarnings_t. +**************************************************************************/ +void ledSetWarningType(uint16_t warningId); #endif /* DRIVERS_LED_H_ */ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index 6b11c39..9fb748c 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -18,10 +18,10 @@ //Code here //Leds? (taken from betaflight REVO h file) -#define Led0_PIN GPIO_PIN_5 //Correct? -#define Led0_GPIO_PORT GPIOB //Correct? -#define Led1 GPIO_PIN_4 //Correct? -#define Led1_GPIO_PORT GPIOB //Correct? +#define Led0_PIN GPIO_PIN_5 //blue +#define Led0_GPIO_PORT GPIOB +#define Led1 GPIO_PIN_4 //yellow +#define Led1_GPIO_PORT GPIOB //Servo out, To be used for motors? @@ -79,7 +79,6 @@ #define USART6_RX_PIN GPIO_PIN_7 #define USART6_RX_PORT GPIOC #define USART6_TX_PIN GPIO_PIN_6 - #define USART6_TX_PORT GPIOC @@ -91,8 +90,15 @@ #define MPU6000_SPI_INSTANCE SPI1 //Dont know if necessary for us to specify + /* Led Warnings */ +#define USE_LEDS +#define USE_LED_WARNINGS +#define USE_LED_WARNINGS_MISSED_PERIOD +//#define USE_LED_WARNINGS_SYSTEM_LOAD + /* Scheduler */ #define USE_DEBUG_TASKS //Only to be used when testing scheduler, not when intending to run the whole system +#define USE_TASK_AGE_CYCLE_STATISTICS /* Baro */ diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 48e4968..2ae0606 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -1,4 +1,3 @@ -/* /********************************************************************** * NAME: adc.h * * AUTHOR: Philip Johansson * diff --git a/UAV-ControlSystem/inc/utilities/math_helpers.h b/UAV-ControlSystem/inc/utilities/math_helpers.h new file mode 100644 index 0000000..6a2c114 --- /dev/null +++ b/UAV-ControlSystem/inc/utilities/math_helpers.h @@ -0,0 +1,21 @@ +/* + * math_helpers.h + * + * Created on: 21 sep. 2016 + * Author: holmis + */ + +#ifndef UTILITIES_MATH_HELPERS_H_ +#define UTILITIES_MATH_HELPERS_H_ + + +#define M_PIf 3.14159265358979323846f + +#define RAD (M_PIf / 180.0f) + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define ABS(x) ((x) > 0 ? (x) : -(x)) + + +#endif /* UTILITIES_MATH_HELPERS_H_ */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 581281b..ff3b743 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -1,53 +1,75 @@ /************************************************************************** -* NAME: Scheduler.c * -* 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: * +* 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 "stm32f4xx_revo.h" #include "Scheduler/scheduler.h" #include "Scheduler/tasks.h" #include "drivers/system_clock.h" +#include "utilities/math_helpers.h" +#include +#include "drivers/led.h" -//Todo: change for several instances where 0 is used as null to the define value NULL +/* Variables ------------------------------------------- */ -static task_t *currentTask = 0; +static task_t *currentTask = NULL; //The current task that the is run -static uint32_t totalSchedulerPasses; /* Number of times the scheduler has been invoked */ -static uint32_t totalSchedulerReadyTasks; /* The amount of tasks that totally has been ready in the scheduler */ +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; +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 int taskReadyQueuePos = 0; -static int taskReadyQueueSize = 0; +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*/ -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: * -* INFORMATION: * +* 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)); - taskReadyQueuePos = 0; taskReadyQueueSize = 0; } /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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) { @@ -65,8 +87,13 @@ bool taskReadyQueueContains(task_t *task) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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) { @@ -106,30 +133,26 @@ bool taskReadyQueueAdd(task_t *task) ++taskReadyQueueSize; return true; } - } + //If this is true it needs to be placed at the very back of the queue if (taskReadyQueue[i] == NULL ) { - //if taskReadyQueueSize > 0 we need to increment the counter i - if (taskReadyQueueSize > 0) - { - i++; - } - 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: * -* INFORMATION: * +* 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) { @@ -145,10 +168,62 @@ bool taskReadyQueueRemove(task_t *task) /************************************************************************** -* BRIEF: Enables or disables a specific task in the system * -* INFORMATION: Takes the id of the task(task_t) along with a true or * -* false value. If true the task will be added to the task queue, if false * -* it will be removed. * +* 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) { @@ -164,8 +239,10 @@ void enableTask(taskId_t taskId, bool enabled) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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) { @@ -179,8 +256,10 @@ uint32_t getTaskDeltaTime(taskId_t taskId) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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) { @@ -193,8 +272,53 @@ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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) { @@ -240,34 +364,46 @@ void initSchedulerTasks(void) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* 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) { - /*Todo: what needs to be init for the scheduler, should all the tasks - * be added to the readyTasksqueue here or should it be done at in - * another place in the code. */ //Clear the queue taskReadyQueueClear(); //Init all the ToDo: add back when it is known that the scheduler works - //initSchedulerTasks(); +#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: * -* INFORMATION: * +* 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) { - /*ToDo: implement the function micros that should give the current - * time in microseconds. Not entirely sure how it should be obtained, - * needs to be looked into and solved otherwise no time can be gotten */ - currentTime = micros(); + //Get the current time in Micro seconds + currentTime = clock_get_us(); task_t * taskToRun; - task_t * selectedTask; + task_t * selectedTask = NULL; uint16_t currentDynamicPriority = 0; uint32_t currentReadyTasks = 0; float currentAgeCyckleExact = 0; @@ -312,6 +448,15 @@ void scheduler(void) //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; @@ -321,12 +466,11 @@ void scheduler(void) //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) + 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 @@ -359,11 +503,10 @@ void scheduler(void) currentDynamicPriority = taskToRun->dynamicPriority; //update the current highest dynamic priority } } - } /* for-loop end */ - //Increase the + //Used for meassuring load totalSchedulerReadyTasks += currentReadyTasks; totalSchedulerPasses ++; @@ -378,11 +521,13 @@ void scheduler(void) selectedTask->dynamicPriority = 0; //reset the dynamic priority to 0 //handle the execution of the tasks function - const uint32_t timeBeforeExecution = micros(); + const uint32_t timeBeforeExecution = clock_get_us(); selectedTask->taskFunction(); //Run the function associated with the current task - const uint32_t TimeAfterExecution = micros(); + 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 diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index d690ce9..a08cda0 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -1,8 +1,17 @@ /************************************************************************** -* NAME: tasks.c * -* 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: * +* NAME: tasks.h * +* * +* 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: Adds the initial task values for each task that can be in * +* the system, in the c file. In the h file functions that * +* when called will* perform the action of its corresponding * +* task. * +* * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * @@ -11,10 +20,16 @@ #include "Scheduler/tasks.h" #include "Scheduler/scheduler.h" -#define GetUpdateRateHz(x) (1000000 / x) //x is the desired hz value given in micro seconds -//initiate the values for all the tasks -//This information needs to be changed when functionality and setting for the tasks needs to be changed -// - Note: desiredPeriod is given in micro seconds, writing for example (1000000 / 10) means 10 hz rate + +/************************************************************************** +* Information - Initiate the values for all the tasks. This information +* needs to be changed when functionality and setting for the tasks needs to +* be changed. Each new task added to the system needs to be added here or +* it will not work. +* +* Note - desiredPeriod is given in micro seconds, writing for example +* (1000000 / 10) means 10 hz rate +***************************************************************************/ task_t SystemTasks[TASK_COUNT] = { @@ -72,7 +87,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BATTERY", .taskFunction = systemTaskBattery, - .desiredPeriod = GetUpdateRateHz(50), + .desiredPeriod = GetUpdateRateHz(50), //50 hz update rate (20 ms) .staticPriority = PRIORITY_MEDIUM, }, @@ -81,7 +96,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BAROMETER", .taskFunction = systemTaskBaro, - .desiredPeriod = GetUpdateRateHz(20), + .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -91,7 +106,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "COMPASS", .taskFunction = systemTaskCompass, - .desiredPeriod = GetUpdateRateHz(10), + .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -101,7 +116,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "GPS", .taskFunction = systemTaskGps, - .desiredPeriod = GetUpdateRateHz(10), + .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms) .staticPriority = PRIORITY_MEDIUM, }, #endif @@ -111,17 +126,17 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "SONAR", .taskFunction = systemTaskSonar, - .desiredPeriod = GetUpdateRateHz(20), + .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms) .staticPriority = PRIORITY_LOW, }, #endif -#ifdef defined(BARO) || defined(SONAR) +#if defined(BARO) || defined(SONAR) [TASK_ALTITUDE] = { .taskName = "ALTITUDE", .taskFunction = systemTaskAltitude, - .desiredPeriod = GetUpdateRateHz(40), + .desiredPeriod = GetUpdateRateHz(40), //40 hz update rate (25 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -131,7 +146,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BEEPER", .taskFunction = systemTaskBeeper, - .desiredPeriod = GetUpdateRateHz(100), + .desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -145,25 +160,24 @@ task_t SystemTasks[TASK_COUNT] = .taskName = "DEBUG_1", .taskFunction = systemTaskDebug_1, .desiredPeriod = GetUpdateRateHz(100), - .staticPriority = PRIORITY_LOW, + .staticPriority = PRIORITY_REALTIME, }, [TASK_DEBUG_2] = { .taskName = "DEBUG_2", .taskFunction = systemTaskDebug_2, - .desiredPeriod = GetUpdateRateHz(100), - .staticPriority = PRIORITY_LOW, + .desiredPeriod = GetUpdateRateHz(40), + .staticPriority = PRIORITY_MEDIUM, }, [TASK_DEBUG_3] = { .taskName = "DEBUG_3", .taskFunction = systemTaskDebug_3, - .desiredPeriod = GetUpdateRateHz(100), + .desiredPeriod = GetUpdateRateHz(20), .staticPriority = PRIORITY_LOW, }, - #endif }; diff --git a/UAV-ControlSystem/src/drivers/led.c b/UAV-ControlSystem/src/drivers/led.c index fca0fb8..cfda577 100644 --- a/UAV-ControlSystem/src/drivers/led.c +++ b/UAV-ControlSystem/src/drivers/led.c @@ -1,8 +1,183 @@ -/* - * led.c - * - * Created on: 21 sep. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: led.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Contains some led functionality. * +* * +* INFORMATION: Contains functions to configure pins to LEDs. It also * +* contains some led error codes that can be used in the * +* system, although at the time hardcoded to work only with * +* the revo on board leds. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ + +#include "stm32f4xx.h" +#include "stm32f4xx_revo.h" +#include "drivers/system_clock.h" +#include "drivers/led.h" + +static int32_t ledTimer = 0; //Timer that keeps track of when it is ok to refresh the led warnings +static uint16_t currentLedWarning = LEDWARNING_OFF; //The current led warning that is activated in the system +/************************************************************************** +* BRIEF: Toggles a led +* +* INFORMATION: Given a pin and port, it attempts to toggle a led that +* should be linked with the given combination. +**************************************************************************/ +void ledToggle(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_TogglePin(led_port, led_pin); +} + + +/************************************************************************** +* BRIEF: Turns on a led. +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOn(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_SET); +} + + +/************************************************************************** +* BRIEF: Turns off a led. +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOff(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_RESET); +} + + +/************************************************************************** +* BRIEF: Turns on a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOnInverted(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_RESET); +} + + +/************************************************************************** +* BRIEF: Turns off a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOffInverted(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_SET); +} + + +/************************************************************************** +* BRIEF: Enables pins so that they can be use for a Led +* +* INFORMATION: Given a pin and port enables that configuration to use led +**************************************************************************/ +void ledEnable(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + GPIO_InitTypeDef gpinit; + gpinit.Pin = led_pin; + gpinit.Mode = GPIO_MODE_OUTPUT_PP; + gpinit.Pull = GPIO_PULLUP; + gpinit.Speed = GPIO_SPEED_HIGH; + HAL_GPIO_Init(led_port, &gpinit); +} + + +/************************************************************************** +* BRIEF: Enables the two leds on board leds on the revolution board +* +* INFORMATION: +**************************************************************************/ +void ledReavoEnable(void) +{ + ledEnable(Led0_PIN, GPIOB); + ledEnable(Led1, GPIOB); +} + + +/************************************************************************** +* BRIEF: Change the warning type of a led +* +* INFORMATION: Change the warning type of led given a warningId that is +* obtained from ledWarnings_t. +**************************************************************************/ +void ledSetWarningType(uint16_t warningId) +{ + currentLedWarning = warningId; +} + + +/************************************************************************** +* BRIEF: Refresh the led warnings +* +* INFORMATION: Given the current led warning perform associated action. +**************************************************************************/ +void ledIndicatorsRefresh() +{ + /* Use the two leds on the revo board for warnings, revo on board leds are inverted (off = on, vice versa) */ + switch(currentLedWarning) + { + case LEDWARNING_LED0_ON: + ledOnInverted(Led0_PIN, Led0_GPIO_PORT); + break; + case LEDWARNING_LED1_ON: + ledOnInverted(Led1, Led1_GPIO_PORT); + break; + case LEDWARNING_LED0_BLINK: + ledToggle(Led0_PIN, Led0_GPIO_PORT); + break; + case LEDWARNING_LED1_BLINK: + ledToggle(Led1, Led1_GPIO_PORT); + break; + case LEDWARNING_LED0_BLINK_ONCE: + ledOnInverted(Led0_PIN, Led0_GPIO_PORT); + ledSetWarningType(LEDWARNING_OFF); + break; + case LEDWARNING_LED1_BLINK_ONCE: + ledOnInverted(Led1, Led1_GPIO_PORT); + ledSetWarningType(LEDWARNING_OFF); + break; + case LEDWARNING_OFF: + default: + ledOffInverted(Led1, Led1_GPIO_PORT); + ledOffInverted(Led0_PIN, Led0_GPIO_PORT); + break; + } + + //Update the timer so that the led will not update unnecessarily to often + int32_t timeNow = clock_get_us(); + ledTimer = timeNow +LED_UPDATE_TIMER; +} + + +/************************************************************************** +* BRIEF: Updates the warning leds in the system +* +* INFORMATION: Checks if any led warning should be active and if its the +* case perform some led activities on a specific led +**************************************************************************/ +void ledIndicatorsUpdate(void) +{ + //get the current time + int32_t timeNow = clock_get_us(); + + //Can only refresh the leds if a certain time has passed + if (timeNow - ledTimer < 0) + return; + + //Refresh the led warnings + ledIndicatorsRefresh(); +} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 275f49c..d169208 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -12,90 +12,89 @@ #include "drivers/adc.h" #include "drivers/system_clock.h" +#include "drivers/led.h" +#include "Scheduler/scheduler.h" #include "stm32f4xx.h" #include "stm32f4xx_revo.h" #include "system_variables.h" #include "utilities.h" #include -/* Private function prototypes -----------------------------------------------*/ -static void SystemClock_Config(void); -static void Error_Handler(void); -ADC_HandleTypeDef adc_testinput_handle; // For example battery voltage -ADC_HandleTypeDef adc_test2input_handle; -uint32_t g_ADCValue; -uint32_t g_ADC2; -int g_MeasurementNumber; - - -int main(void) +/************************************************************************** +* BRIEF: Should contain all the initializations of the system, needs to +* run before the scheduler. +* +* INFORMATION: Everything that will run somewhere in the system at some +* possible point and needs to be initialized to run properly, have to do it +* inside this function. +**************************************************************************/ +void init_system() { - // Initialize the Hardware Abstraction Layer + //Init the Hardware abstraction layer (HAL) HAL_Init(); - - /*##-2- Configure PA05 IO in output push-pull mode to drive external LED ###*/ - //GPIO_InitStruct.Pin = GPIO_PIN_5 | GPIO_PIN_6; - //GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - //GPIO_InitStruct.Pull = GPIO_PULLUP; - //GPIO_InitStruct.Speed = GPIO_SPEED_FAST; - //HAL_GPIO_Init(GPIOA, &gpinit); - // Configure the system clock to 100 MHz + //Configure the clock system_clock_config(); - int i = 1; +#ifdef USE_LEDS + //Initialize the on board leds + ledReavoEnable(); + ledOffInverted(Led0_PIN, Led0_GPIO_PORT); + ledOffInverted(Led1, Led1_GPIO_PORT); + //ledEnable(GPIO_PIN_0, GPIOA); +#endif - //Add ADC Channels - adc_pin_add(ADC_CHANNEL_0); - adc_pin_add(ADC_CHANNEL_1); - adc_pin_add(ADC_CHANNEL_12); +#ifdef BARO - //Configure the ADCs - adc_configure(); +#endif - /* This is done in system_clock_config for all GPIO clocks */ - //__GPIOB_CLK_ENABLE(); +#ifdef COMPASS - GPIO_InitTypeDef gpinit; - gpinit.Pin = GPIO_PIN_5; - gpinit.Mode = GPIO_MODE_OUTPUT_PP; - gpinit.Pull = GPIO_PULLUP; - gpinit.Speed = GPIO_SPEED_HIGH; - HAL_GPIO_Init(GPIOB, &gpinit); +#endif - adc_start(); +#ifdef GPS - int num = 2000; - int j = 0; - volatile uint32_t time_us[num]; +#endif + +#ifdef SONAR + +#endif + +#if defined(BARO) || defined(SONAR) + +#endif + +#if BEEPER + +#endif + +} + +/************************************************************************** +* BRIEF: Main function of the system, every thing originates from this +* point. +* +* INFORMATION: The function is responsible for calling the Initialize +* function that will make the system ready to run. It is also responsible +* for starting and running the scheduler in a loop that never stops. +**************************************************************************/ +int main(void) +{ + //Init the system + init_system(); + + //Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler + initScheduler(); while (1) { - i++; + //Run the scheduler, responsible for distributing all the work of the running system + scheduler(); - //g_ADCValue = accumulate(g_ADCBuffer,ADC_BUFFER_LENGTH) / ADC_BUFFER_LENGTH; - //HAL_Delay(100); - int g_ADCValue = adc_read(ADC_CHANNEL_0); - int g_ADCValue1 = adc_read(ADC_CHANNEL_1); - int g_ADCValue12 = adc_read(ADC_CHANNEL_12); - - int offTime = g_ADCValue; - int onTime = 4096 - offTime; - HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_SET); - for (int i = 0; i < onTime; i++) - { - asm("nop"); - } - HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_RESET); - for (int i = 0; i < offTime; i++) - { - asm("nop"); - } - - //Get time in microseconds - if(j < num) - time_us[j++] = clock_get_us(); +#ifdef USE_LED_WARNINGS + ledIndicatorsUpdate(); +#endif } for(;;); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 26b9ae0..78ba9fb 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -1,36 +1,49 @@ /************************************************************************** -* NAME: tasks_main.c * -* PURPOSE: Holds the function implementations for the individual tasks -* that are invoked when a task is executed in the scheduler. Each task -* needs to have an associated function that has to be invoked when it is -* chosen as the task to run. Additionally optional event driven task -* functions must be implemented here as well. This file will include -* different drivers meaning that the task functions could jump around -* into other files before finishing its execution. -* INFORMATION: * +* NAME: tasks_main.c * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Implement all the functions that will be called when * +* executing a task in the scheduler. * +* * +* INFORMATION: Holds the function implementations for the individual tasks* +* that are invoked when a task is executed in the scheduler. * +* Each task needs to have an associated function that has to * +* be invoked when it is chosen as the task to run. * +* Additionally optional event driven task functions must be * +* implemented here as well. This file will include different * +* drivers meaning that the task functions could jump around * +* into other files before finishing its execution. * +* * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * ***************************************************************************/ +#include +#include + #include "Scheduler/scheduler.h" #include "Scheduler/tasks.h" #include "stm32f4xx_revo.h" +/* Drivers */ +#include "drivers/led.h" +#include "drivers/adc.h" +#include "drivers/motors.h" +#include "drivers/pwm.h" +#include "drivers/system_clock.h" -void systemTaskSystem(void) -{ -} void systemTaskGyroPid(void) { - + //Read gyro and update PID and finally update the motors. The most important task in the system } void systemTaskAccelerometer(void) { - + //update the accelerometer data } void systemTaskAttitude(void) @@ -40,12 +53,14 @@ void systemTaskAttitude(void) void systemTaskRx(void) { - + //Interpret commands to the vehicle } bool systemTaskRxCheck(uint32_t currentDeltaTime) { - + //This task is what is controlling the event activation of the systemTaskRx + //check if there is anything that has be received. + return false; } void systemTaskSerial(void) @@ -55,32 +70,32 @@ void systemTaskSerial(void) void systemTaskBattery(void) { - + //Keep track of the battery level of the system } void systemTaskBaro(void) { - + //Obtain the barometer data } void systemTaskCompass(void) { - + //Obtain compass data } void systemTaskGps(void) { - + //Obtain gps data } void systemTaskSonar(void) { - + //obtain sonar data } void systemTaskAltitude(void) { - + //Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar } void systemTaskBeeper(void) @@ -93,17 +108,25 @@ void systemTaskBeeper(void) void systemTaskDebug_1(void) { - + //ledToggle(Led0_PIN, Led0_GPIO_PORT); + clock_delay_ms(8); + //ledToggle(Led0_PIN, Led0_GPIO_PORT); } void systemTaskDebug_2(void) { - + //ledToggle(Led1, Led1_GPIO_PORT); + //clock_delay_ms(15); + clock_delay_ms(8); + //ledToggle(Led1, Led1_GPIO_PORT); } void systemTaskDebug_3(void) { - + //ledToggle(GPIO_PIN_0, GPIOA); + //clock_delay_ms(20); + clock_delay_ms(8); + //ledToggle(GPIO_PIN_0, GPIOA); } #endif /* End USE_DEBUG_TASKS */ diff --git a/UAV-ControlSystem/src/utilities/math_helpers.c b/UAV-ControlSystem/src/utilities/math_helpers.c new file mode 100644 index 0000000..c382f74 --- /dev/null +++ b/UAV-ControlSystem/src/utilities/math_helpers.c @@ -0,0 +1,10 @@ +/* + * math_helpers.c + * + * Created on: 21 sep. 2016 + * Author: holmis + */ + +#include "utilities/math_helpers.h" + +