philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Final adjustments

Browse Source 
Added the and adjusted the final part to the scheduler. Added ability to
add task functionality that will run. Updated main file to its intended
state. Further added led functinality along with led warnings.
This commit is contained in:
Jonas Holmberg 2016-09-26 08:53:50 +02:00
parent c2ff566c46
commit fc5ca4330c
12 changed files with 788 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
UAV-ControlSystem/inc/Scheduler/scheduler.h
View File

@ -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 *
* *
* 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 <stdbool.h>
#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_ */

22
UAV-ControlSystem/inc/Scheduler/tasks.h
View File

@ -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: *
* *
* 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 <stdint.h>
#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_ */

111
UAV-ControlSystem/inc/drivers/led.h
View File

@ -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_ */

16
UAV-ControlSystem/inc/stm32f4xx_revo.h
View File

@ -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 */

1
UAV-ControlSystem/inc/system_variables.h
View File

@ -1,4 +1,3 @@
/*
/**********************************************************************
* NAME: adc.h *
* AUTHOR: Philip Johansson *

21
UAV-ControlSystem/inc/utilities/math_helpers.h Normal file
View File

@ -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_ */

261
UAV-ControlSystem/src/Scheduler/scheduler.c
View File

@ -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 <string.h>
#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 taskReadyQueuePos = 0;
static int taskReadyQueueSize = 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: *
* 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

56
UAV-ControlSystem/src/Scheduler/tasks.c
View File

@ -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
};

187
UAV-ControlSystem/src/drivers/led.c
View File

@ -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();
}

123
UAV-ControlSystem/src/main.c
View File

@ -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 <string.h>
/* 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(;;);

71
UAV-ControlSystem/src/tasks_main.c
View File

@ -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: *
* *
* 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 <stdint.h>
#include <stdbool.h>
#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 */

10
UAV-ControlSystem/src/utilities/math_helpers.c Normal file
View File

@ -0,0 +1,10 @@
/*
* math_helpers.c
*
* Created on: 21 sep. 2016
* Author: holmis
*/
#include "utilities/math_helpers.h"