From 8ba8bba7a34afb513d6ee64175363b6ae98fdd5c Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 19 Jan 2016 09:00:07 +0000
Subject: [PATCH] Removed isEnabled flag, replaced by task being in queue or
 not.

---
 src/main/scheduler.c       | 68 +++++++++++++++++++++++++++-----------
 src/main/scheduler.h       |  1 -
 src/main/scheduler_tasks.c |  1 -
 3 files changed, 49 insertions(+), 21 deletions(-)

diff --git a/src/main/scheduler.c b/src/main/scheduler.c
index ca30c48f25..a1795d5c56 100755
--- a/src/main/scheduler.c
+++ b/src/main/scheduler.c
@@ -20,11 +20,13 @@
 #include <stdbool.h>
 #include <stdlib.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <platform.h>
 
 #include "scheduler.h"
 #include "debug.h"
+#include "build_config.h"
 
 #include "common/maths.h"
 
@@ -46,34 +48,57 @@ uint16_t averageSystemLoadPercent = 0;
 
 static int queuePos = 0;
 static int queueSize = 0;
+// No need for a linked list for the queue, since items are only inserted at startup
 static cfTask_t* queueArray[TASK_COUNT];
 
-static void queueInit(void)
+STATIC_UNIT_TESTED void queueClear(void)
 {
+    memset(queueArray, 0, sizeof(queueArray));
     queuePos = 0;
     queueSize = 0;
-    // put the enabled tasks in the queue in priority order
-    const cfTaskPriority_e priorities[] =
-        {TASK_PRIORITY_MAX, TASK_PRIORITY_REALTIME, TASK_PRIORITY_HIGH, TASK_PRIORITY_MEDIUM, TASK_PRIORITY_LOW, TASK_PRIORITY_IDLE};
-    for (unsigned int ii = 0; ii < sizeof(priorities); ++ii) {
-        const cfTaskPriority_e priority = priorities[ii];
-        for (int taskId = 0; taskId < TASK_COUNT; ++taskId) {
-            cfTask_t *task = &cfTasks[taskId];
-            if (task->staticPriority == priority && task->isEnabled == true) {
-                queueArray[queuePos] = task;
-                ++queuePos;
-                ++queueSize;
-            }
+}
+
+STATIC_UNIT_TESTED void queueAdd(cfTask_t *task)
+{
+    for (int ii = 0; ii < TASK_COUNT; ++ii) {
+        if (queueArray[ii] == task) {
+            return;
+        }
+        if (queueArray[ii] == NULL || queueArray[ii]->staticPriority < task->staticPriority) {
+            memmove(&queueArray[ii+1], &queueArray[ii], sizeof(task) * (TASK_COUNT - ii - 1));
+            queueArray[ii] = task;
+            return;
         }
     }
 }
-static cfTask_t *queueFirst(void)
+
+STATIC_UNIT_TESTED void queueRemove(cfTask_t *task)
+{
+    for (int ii = 0; ii < TASK_COUNT; ++ii) {
+        if (queueArray[ii] == task) {
+            memmove(&queueArray[ii], &queueArray[ii+1], sizeof(task) * (TASK_COUNT - ii - 1));
+            return;
+        }
+    }
+}
+
+STATIC_UNIT_TESTED bool queueContains(cfTask_t *task)
+{
+    for (int ii = 0; ii < TASK_COUNT; ++ii) {
+        if (queueArray[ii] == task) {
+            return true;
+        }
+    }
+    return false;
+}
+
+STATIC_UNIT_TESTED cfTask_t *queueFirst(void)
 {
     queuePos = 0;
     return queueSize > 0 ? queueArray[0] : NULL;
 }
 
-static cfTask_t *queueNext(void) {
+STATIC_UNIT_TESTED cfTask_t *queueNext(void) {
     ++queuePos;
     return queuePos < queueSize ? queueArray[queuePos] : NULL;
 }
@@ -105,7 +130,7 @@ void taskSystem(void)
 void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
 {
     taskInfo->taskName = cfTasks[taskId].taskName;
-    taskInfo->isEnabled= cfTasks[taskId].isEnabled;
+    taskInfo->isEnabled = queueContains(&cfTasks[taskId]);
     taskInfo->desiredPeriod = cfTasks[taskId].desiredPeriod;
     taskInfo->staticPriority = cfTasks[taskId].staticPriority;
     taskInfo->maxExecutionTime = cfTasks[taskId].maxExecutionTime;
@@ -122,11 +147,15 @@ void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros)
     }
 }
 
-void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState)
+void setTaskEnabled(cfTaskId_e taskId, bool enabled)
 {
     if (taskId == TASK_SELF || taskId < TASK_COUNT) {
         cfTask_t *task = taskId == TASK_SELF ? currentTask : &cfTasks[taskId];
-        task->isEnabled = newEnabledState;
+        if (enabled && task->taskFunc) {
+            queueAdd(task);
+        } else {
+            queueRemove(task);
+        }
     }
 }
 
@@ -142,7 +171,8 @@ uint32_t getTaskDeltaTime(cfTaskId_e taskId)
 
 void schedulerInit(void)
 {
-    queueInit();
+    queueClear();
+    queueAdd(&cfTasks[TASK_SYSTEM]);
 }
 
 void scheduler(void)
diff --git a/src/main/scheduler.h b/src/main/scheduler.h
index 8dc04c5038..b2cc3938e5 100755
--- a/src/main/scheduler.h
+++ b/src/main/scheduler.h
@@ -90,7 +90,6 @@ typedef struct {
     const char * taskName;
     bool (*checkFunc)(uint32_t currentDeltaTime);
     void (*taskFunc)(void);
-    bool isEnabled;
     uint32_t desiredPeriod;     // target period of execution
     uint8_t staticPriority;     // dynamicPriority grows in steps of this size, shouldn't be zero
 
diff --git a/src/main/scheduler_tasks.c b/src/main/scheduler_tasks.c
index 94a4ba2b2b..cc2d76b795 100644
--- a/src/main/scheduler_tasks.c
+++ b/src/main/scheduler_tasks.c
@@ -42,7 +42,6 @@ void taskSystem(void);
 
 cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_SYSTEM] = {
-        .isEnabled = true,
         .taskName = "SYSTEM",
         .taskFunc = taskSystem,
         .desiredPeriod = 1000000 / 10,              // run every 100 ms