From f2c39aecec9f2dac289b9c40a38291fef0f4f02f Mon Sep 17 00:00:00 2001
From: Jonas Holmberg <holmis.1993@gmail.com>
Date: Fri, 16 Sep 2016 10:59:12 +0200
Subject: [PATCH] Just little

just to change branch.
---
 UAV-ControlSystem/src/Scheduler/scheduler.c | 46 +++++++++++++++++----
 UAV-ControlSystem/src/main.c                |  1 +
 2 files changed, 38 insertions(+), 9 deletions(-)

diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c
index adb09a0..d9b9aad 100644
--- a/UAV-ControlSystem/src/Scheduler/scheduler.c
+++ b/UAV-ControlSystem/src/Scheduler/scheduler.c
@@ -202,10 +202,12 @@ void scheduler(void)
 	 * needs to be looked into and solved otherwise no time can be gotten */
 	currentTime = micros();
 	task_t * taskToRun;
-	uint32_t currentDynamicPriority = 0;
+	task_t * selectedTask;
+	uint16_t currentDynamicPriority = 0;
 	uint32_t currentReadyTasks = 0;
 	float currentAgeCyckleExact = 0;
 	float ageCycleExact = 0;
+	bool isRealTime = false;
 
 	/* Check if a task has entered a new period and assign its new dynamic priority */
 	/* Go through all the tasks to check if they should be able to run */
@@ -240,24 +242,50 @@ void scheduler(void)
 		}
 
 		//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)
 		{
-
-		}
-		else if(taskToRun->dynamicPriority == currentDynamicPriority)	//The comparing tasks have the same priority
-		{
-			//if the tasks have the same priority check who is closest to its deadline and run that one
-			if (ageCycleExact > currentAgeCyckleExact)
+			bool changeTask = false;	//flag used to check if the task should be updated
+			/* If a realtime task is found the bool isRealTime will be set to true
+			 * meaning no other tasks than realtime will be able to enter the function */
+			if (taskToRun->staticPriority == REALTIME)		//if it is a realtime task
 			{
-				//If the ageCycleExact is larger than the one of the current task we will change the priority of the one to run
+				isRealTime = true;		//toggle this flag so that no non realtime task can be selected this cycle
 
+				changeTask = true;
+			}
+			else if (!isRealTime)	//If task is not realtime
+			{
+				if (taskToRun->dynamicPriority == currentDynamicPriority)	//if the tasks have the same priority
+				{
+					if (ageCycleExact > currentAgeCyckleExact)	//if the current tasks deadline is closer than selected task
+					{
+						changeTask = true;
+						currentAgeCyckleExact = ageCycleExact;	//update the value
+					}
+				}
+				else
+				{
+					changeTask = true;
+				}
 			}
 
+			if (changeTask == true)		//if the task should change
+			{
+				selectedTask = taskToRun;	//update the selected task
+				currentDynamicPriority = taskToRun->dynamicPriority;	//update the current highest dynamic priority
+			}
 		}
 
 	}	/* for-loop end */
 
 
+	//Increase the
+	totalSchedulerReadyTasks += currentReadyTasks;
+	totalSchedulerPasses ++;
+
+
+	currentTask = selectedTask;
+
 
 }
 
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 560e48e..f6a143d 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -38,6 +38,7 @@ int main(void)
 	  gpinit.Speed = GPIO_SPEED_FAST;
 
 
+
 	  /*##-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;