diff --git a/src/main/config/config_unittest.h b/src/main/config/config_unittest.h
index 8245598b4b..80216fe7c0 100644
--- a/src/main/config/config_unittest.h
+++ b/src/main/config/config_unittest.h
@@ -20,35 +20,6 @@
 #ifdef SRC_MAIN_SCHEDULER_C_
 #ifdef UNIT_TEST
 
-uint8_t unittest_scheduler_taskId;
-uint8_t unittest_scheduler_selectedTaskId;
-uint8_t unittest_scheduler_selectedTaskDynPrio;
-uint16_t unittest_scheduler_waitingTasks;
-uint32_t unittest_scheduler_timeToNextRealtimeTask;
-bool unittest_outsideRealtimeGuardInterval;
-
-#define SET_SCHEDULER_LOCALS() \
-    { \
-    }
-
-#define GET_SCHEDULER_LOCALS() \
-    { \
-    unittest_scheduler_taskId = taskId; \
-    unittest_scheduler_selectedTaskId = selectedTaskId; \
-    unittest_scheduler_selectedTaskDynPrio = selectedTaskDynPrio; \
-    unittest_scheduler_waitingTasks = waitingTasks; \
-    unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \
-    unittest_outsideRealtimeGuardInterval = outsideRealtimeGuardInterval; \
-    }
-
-#else
-
-#define GET_SCHEDULER_LOCALS() {}
-
-#endif
-#endif
-
-
 #ifdef SRC_MAIN_SCHEDULER_C_
 #ifdef UNIT_TEST
 
@@ -63,11 +34,11 @@ uint32_t unittest_scheduler_timeToNextRealtimeTask;
     unittest_scheduler_selectedTaskDynPrio = selectedTaskDynPrio; \
     unittest_scheduler_waitingTasks = waitingTasks; \
     unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \
+    unittest_outsideRealtimeGuardInterval = outsideRealtimeGuardInterval; \
     }
 
 #else
 
-#define SET_SCHEDULER_LOCALS() {}
 #define GET_SCHEDULER_LOCALS() {}
 
 #endif
diff --git a/src/main/scheduler.h b/src/main/scheduler.h
index b2cc3938e5..bbea64f930 100755
--- a/src/main/scheduler.h
+++ b/src/main/scheduler.h
@@ -91,7 +91,7 @@ typedef struct {
     bool (*checkFunc)(uint32_t currentDeltaTime);
     void (*taskFunc)(void);
     uint32_t desiredPeriod;     // target period of execution
-    uint8_t staticPriority;     // dynamicPriority grows in steps of this size, shouldn't be zero
+    const uint8_t staticPriority;// dynamicPriority grows in steps of this size, shouldn't be zero
 
     /* Scheduling */
     uint8_t dynamicPriority;    // measurement of how old task was last executed, used to avoid task starvation
diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc
index 1a0c9f3bec..1619a21cf1 100644
--- a/src/test/unit/scheduler_unittest.cc
+++ b/src/test/unit/scheduler_unittest.cc
@@ -94,6 +94,16 @@ TEST(SchedulerUnittest, TestPriorites)
         EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority);
 }
 
+TEST(SchedulerUnittest, TestPriorites)
+{
+    // if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked
+    EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority);
+    EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority);
+    EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority);
+    EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority);
+    EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority);
+}
+
 TEST(SchedulerUnittest, TestQueueInit)
 {
     queueClear();
@@ -132,11 +142,11 @@ TEST(SchedulerUnittest, TestQueue)
     EXPECT_EQ(NULL, queueNext());
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 
-    queueAdd(&cfTasks[TASK_BEEPER]); // TASK_PRIORITY_MEDIUM
+    queueAdd(&cfTasks[TASK_BATTERY]); // TASK_PRIORITY_MEDIUM
     EXPECT_EQ(4, queueSize());
     EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
-    EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
+    EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
     EXPECT_EQ(NULL, queueNext());
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
@@ -146,7 +156,7 @@ TEST(SchedulerUnittest, TestQueue)
     EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext());
     EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
-    EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
+    EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
     EXPECT_EQ(NULL, queueNext());
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
@@ -155,19 +165,22 @@ TEST(SchedulerUnittest, TestQueue)
     EXPECT_EQ(4, queueSize());
     EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst());
     EXPECT_EQ(&cfTasks[TASK_RX], queueNext());
-    EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext());
+    EXPECT_EQ(&cfTasks[TASK_BATTERY], queueNext());
     EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext());
     EXPECT_EQ(NULL, queueNext());
+    EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
 }
 
 TEST(SchedulerUnittest, TestQueueArray)
 {
     // test there are no "out by one" errors or buffer overruns when items are added and removed
     queueClear();
-    taskQueueArray[TASK_COUNT + 1] = deadBeefPtr;
+    taskQueueArray[TASK_COUNT + 1] = deadBeefPtr; // note, must set deadBeefPtr after queueClear
 
-    for (int taskId=0; taskId < TASK_COUNT - 1; ++taskId) {
+    for (int taskId = 0; taskId < TASK_COUNT - 1; ++taskId) {
         setTaskEnabled(static_cast<cfTaskId_e>(taskId), true);
+        EXPECT_EQ(taskId + 1, queueSize());
+        EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]);
     }
     EXPECT_EQ(TASK_COUNT - 1, queueSize());
     EXPECT_NE(static_cast<cfTask_t*>(0), taskQueueArray[TASK_COUNT - 2]);