Final adjustments

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.

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.     *
+* AUTHOR: 	   Jonas Holmberg											  *
-* INFORMATION: Many elements and ideas obtained from beta/cleanflight     *
+* 																		  *
+* 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                *
+* BRIEF: 	   Enables or disables a task to be able to run 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 *
+* INFORMATION: Given an id for a task it can be added or removed from the
-* it will be removed.                                                     *
+* active tasks in the system. Meaning tasks that can be selected by the
+* scheduler to run.
 **************************************************************************/
 void enableTask(taskId_t taskId, bool enabled);
 
+
 /**************************************************************************
-* BRIEF:                                                                  *
+* BRIEF: 	   Returns the delta time value of a task.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   Gives a task a new period.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   The main scheduler function.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   Initiates the scheduler and makes it ready to run.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   Enables the tasks that should run.
-* INFORMATION:                                                            *
+*
+* 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_ */

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.     *
+* AUTHOR:  	   Jonas Holmberg											  *
-* INFORMATION:                                                            *
+*                                                            			  *
+* 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_ */

UAV-ControlSystem/inc/drivers/led.h

@@ -1,13 +1,112 @@
-/*
+/**************************************************************************
- * led.h
+* NAME: 	   led.h                                                      *
- *
+* 	                                                                      *
- *  Created on: 21 sep. 2016
+* AUTHOR: 	   Jonas Holmberg 											  *
- *      Author: holmis
+*																	      *
- */
+* 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_ */

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_PIN					GPIO_PIN_5		//blue
-#define Led0_GPIO_PORT				GPIOB			//Correct?
+#define Led0_GPIO_PORT				GPIOB
-#define Led1						GPIO_PIN_4		//Correct?
+#define Led1						GPIO_PIN_4		//yellow
-#define Led1_GPIO_PORT				GPIOB			//Correct?
+#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 */

UAV-ControlSystem/inc/system_variables.h

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

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

UAV-ControlSystem/src/Scheduler/scheduler.c

@@ -1,53 +1,75 @@
 /**************************************************************************
-* NAME: Scheduler.c                                                       *
+* 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.     *
+* AUTHOR: 	   Jonas Holmberg											  *
-* INFORMATION:                                                            *
+* 																		  *
+* 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 totalSchedulerPasses = 0;				/* 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 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;						//The number of tasks in the ready queue
-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*/
 
+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:                                                                  *
+* BRIEF:   	   Clears the ready queue.                                                               *
-* INFORMATION:                                                            *
+* 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:                                                                  *
+* BRIEF: 	   Checks if a task already exists in the ready queue.
-* INFORMATION:                                                            *
+*                                                                         *
+* 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:                                                                  *
+* BRIEF:       Adds a new task to the ready queue.
-* INFORMATION:                                                            *
+*                                                                         *
+* 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:                                                                  *
+* BRIEF:   	   Removes a task from the ready queue.
-* INFORMATION:                                                            *
+*                                                                         *
+* 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                *
+* BRIEF:	   The function that will run when the scheduler chooses to run
-* INFORMATION: Takes the id of the task(task_t) along with a true or      *
+* 			   the SYSTEM task.
-* false value. If true the task will be added to the task queue, if false *
+* 			                                                                *
-* it will be removed.                                                     *
+* 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:                                                                  *
+* BRIEF: 	   Returns the delta time value of a task.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   Gives a task a new period.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   Returns an array of ageCyckle values observed when
-* INFORMATION:                                                            *
+* 			   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:                                                                  *
+* BRIEF: 	   Initiates the scheduler and makes it ready to run.
-* INFORMATION:                                                            *
+*
+* 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:                                                                  *
+* BRIEF: 	   The main scheduler function.
-* INFORMATION:                                                            *
+*
+* 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
+	//Get the current time in Micro seconds
-	 * time in microseconds. Not entirely sure how it should be obtained,
+	currentTime = clock_get_us();
-	 * needs to be looked into and solved otherwise no time can be gotten */
-	currentTime = micros();
 	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

UAV-ControlSystem/src/Scheduler/tasks.c

@@ -1,8 +1,17 @@
 /**************************************************************************
-* NAME: tasks.c                                                           *
+* 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.     *
+* AUTHOR:  	   Jonas Holmberg											  *
-* INFORMATION:                                                            *
+*                                                            			  *
+* 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
+* Information - Initiate the values for all the tasks. This information
-// - Note: desiredPeriod is given in micro seconds, writing for example (1000000 / 10) means 10 hz rate
+* 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),
+					.desiredPeriod = GetUpdateRateHz(40),
-					.staticPriority = PRIORITY_LOW,
+					.staticPriority = PRIORITY_MEDIUM,
 				},
 
 		[TASK_DEBUG_3] =
 				{
 					.taskName = "DEBUG_3",
 					.taskFunction = systemTaskDebug_3,
-					.desiredPeriod = GetUpdateRateHz(100),
+					.desiredPeriod = GetUpdateRateHz(20),
 					.staticPriority = PRIORITY_LOW,
 				},
-
 #endif
 
 };

UAV-ControlSystem/src/drivers/led.c

@@ -1,8 +1,183 @@
-/*
+/**************************************************************************
- * led.c
+* NAME: 	   led.h                                                      *
- *
+* 	                                                                      *
- *  Created on: 21 sep. 2016
+* AUTHOR: 	   Jonas Holmberg 											  *
- *      Author: holmis
+*																	      *
- */
+* 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();
+}

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 <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;
+* BRIEF: Should contain all the initializations of the system, needs to
-uint32_t g_ADCValue;
+* run before the scheduler.
-uint32_t g_ADC2;
+*
-int g_MeasurementNumber;
+* 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.
-int main(void)
+**************************************************************************/
+void init_system()
 {
-	// Initialize the Hardware Abstraction Layer
+	//Init the Hardware abstraction layer (HAL)
 	HAL_Init();
 
-
+	//Configure the clock
-	  /*##-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
 	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
+#ifdef BARO
-	adc_pin_add(ADC_CHANNEL_0);
-	adc_pin_add(ADC_CHANNEL_1);
-	adc_pin_add(ADC_CHANNEL_12);
 
-	//Configure the ADCs
+#endif
-	adc_configure();
 
-	/* This is done in system_clock_config for all GPIO clocks */
+#ifdef COMPASS
-	//__GPIOB_CLK_ENABLE();
 
-	GPIO_InitTypeDef gpinit;
+#endif
-	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);
 
-	adc_start();
+#ifdef GPS
 
-	int num = 2000;
+#endif
-	int j = 0;
+
-	volatile uint32_t time_us[num];
+#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;
+#ifdef USE_LED_WARNINGS
-		//HAL_Delay(100);
+		ledIndicatorsUpdate();
-		int g_ADCValue = adc_read(ADC_CHANNEL_0);
+#endif
-		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();
 	}
 
 	for(;;);

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
+* AUTHOR: 	   Jonas Holmberg											  *
-* 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
+* PURPOSE: 	   Implement all the functions that will be called when		  *
-* functions must be implemented here as well. This file will include
+* 			   executing a task in the scheduler.						  *
-* different drivers meaning that the task functions could jump around
+*																		  *
-* into other files before finishing its execution.
+* INFORMATION: Holds the function implementations for the individual tasks*
-* INFORMATION:                                                            *
+* 			   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 */

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"
+
+