From db9ec943302ee4105fec62ed59d89339ab5a56ed Mon Sep 17 00:00:00 2001
From: Jonas Holmberg <holmis.1993@gmail.com>
Date: Tue, 20 Sep 2016 15:42:05 +0200
Subject: [PATCH] Added additional things in the scheduler

The tasks should now be able to be added to the ready queue when the
schedual init.
---
 UAV-ControlSystem/inc/Scheduler/scheduler.h |  6 +-
 UAV-ControlSystem/src/Scheduler/scheduler.c | 83 ++++++++++++++++++++-
 2 files changed, 85 insertions(+), 4 deletions(-)

diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h
index 92cab4c..2f2f901 100644
--- a/UAV-ControlSystem/inc/Scheduler/scheduler.h
+++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h
@@ -138,7 +138,11 @@ void scheduler(void);
 **************************************************************************/
 void initScheduler(void);
 
-
+/**************************************************************************
+* BRIEF:                                                                  *
+* INFORMATION:                                                            *
+**************************************************************************/
+void initSchedulerTasks(void);
 
 
 #endif /* SCHEDULER_H_ */
diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c
index b6e19d5..278c9b0 100644
--- a/UAV-ControlSystem/src/Scheduler/scheduler.c
+++ b/UAV-ControlSystem/src/Scheduler/scheduler.c
@@ -43,6 +43,7 @@ void taskReadyQueueClear(void)
 	taskReadyQueueSize = 0;
 }
 
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
@@ -61,6 +62,7 @@ bool taskReadyQueueContains(task_t *task)
 	return false;
 }
 
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
@@ -123,6 +125,7 @@ bool taskReadyQueueAdd(task_t *task)
 	return false;
 }
 
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
@@ -158,6 +161,7 @@ void enableTask(taskId_t taskId, bool enabled)
 	    }
 }
 
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
@@ -172,15 +176,68 @@ uint32_t getTaskDeltaTime(taskId_t taskId)
 	    }
 }
 
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
 **************************************************************************/
 void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros)
 {
-	//ToDo: add implementation if necessary
+	if (taskId == TASK_SELF || taskId < TASK_COUNT)
+	{
+		task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
+		task->desiredPeriod = MAX(100, newPeriodMicros);  // Limit delay to 100us (10 kHz) to prevent scheduler clogging
+	}
 }
 
+
+/**************************************************************************
+* BRIEF:                                                                  *
+* INFORMATION:                                                            *
+**************************************************************************/
+void initSchedulerTasks(void)
+{
+	//Enable the tasks in the system
+	enableTask(TASK_SYSTEM, true);
+
+	enableTask(TASK_GYROPID, true);
+
+	enableTask(TASK_ACCELEROMETER, true);
+
+	enableTask(TASK_ATTITUDE, true);
+
+	enableTask(TASK_RX, true);
+
+	enableTask(TASK_SERIAL, true);
+
+	enableTask(TASK_BATTERY, true);
+
+#ifdef BARO
+	enableTask(TASK_BARO, true);
+#endif
+
+#ifdef COMPASS
+	enableTask(TASK_COMPASS, true);
+#endif
+
+#ifdef GPS
+	enableTask(TASK_GPS, true);
+#endif
+
+#ifdef SONAR
+	enableTask(TASK_SONAR, true);
+#endif
+
+#if defined(BARO) || defined(SONAR)
+	enableTask(TASK_ALTITUDE, true);
+#endif
+
+#if BEEPER
+	enableTask(TASK_BEEPER, true);
+#endif
+}
+
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
@@ -193,10 +250,11 @@ void initScheduler(void)
 	//Clear the queue
 	taskReadyQueueClear();
 
-	//add the basic system task
-	taskReadyQueueAdd(&SystemTasks[TASK_SYSTEM]);
+	//Init all the
+	initSchedulerTasks();
 }
 
+
 /**************************************************************************
 * BRIEF:                                                                  *
 * INFORMATION:                                                            *
@@ -226,6 +284,25 @@ void scheduler(void)
 		if (taskToRun->checkEventTriggered != NULL)
 		{
 			//ToDO: Handle event tasks, they should get to operate at a higher priority
+			//ToDo: test if it works
+			if (taskToRun->dynamicPriority > 0)
+			{
+				taskToRun->taskAgeCycle = 1 + ((currentTime - taskToRun->lastSignaledAt) / taskToRun->desiredPeriod);
+				taskToRun->dynamicPriority = 1 + taskToRun->staticPriority * taskToRun->taskAgeCycle;
+				currentReadyTasks++;
+			}
+			else if (taskToRun->checkEventTriggered(currentTime - taskToRun->lastExecutedAt))
+			{
+				taskToRun->lastSignaledAt = currentTime;
+				taskToRun->taskAgeCycle = 1;
+				taskToRun->dynamicPriority = 1 + taskToRun->staticPriority;
+				currentReadyTasks++;
+			}
+			else
+			{
+				taskToRun->taskAgeCycle = 0;
+			}
+
 
 		}
 		else	//Handle time controlled tasks