diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 979871c53f..e9352bc807 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -48,7 +48,6 @@
 #include "fc/fc_core.h"
 #include "fc/fc_rc.h"
 #include "fc/fc_dispatch.h"
-#include "fc/fc_tasks.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
@@ -96,9 +95,22 @@
 #include "telemetry/telemetry.h"
 
 #ifdef USE_BST
-static void taskBstMasterProcess(timeUs_t currentTimeUs);
+#include "COLIBRI_RACE/i2c_bst.h"
 #endif
 
+#ifdef USE_USB_CDC_HID
+//TODO: Make it platform independent in the future
+#ifdef STM32F4
+#include "vcpf4/usbd_cdc_vcp.h"
+#include "usbd_hid_core.h"
+#elif defined(STM32F7)
+#include "usbd_cdc_interface.h"
+#include "usbd_hid.h"
+#endif
+#endif
+
+#include "fc_tasks.h"
+
 static void taskMain(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc
index 424793ccbd..a4a51e85e6 100644
--- a/src/test/unit/scheduler_unittest.cc
+++ b/src/test/unit/scheduler_unittest.cc
@@ -259,7 +259,7 @@ TEST(SchedulerUnittest, TestQueueArray)
     EXPECT_EQ(lastTaskPrev, taskQueueArray[enqueuedTasks - 2]);
     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]); // NULL at end of queue
     EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks]);
-    EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks - 1]);
+    EXPECT_EQ(NULL, taskQueueArray[enqueuedTasks + 1]);
     EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT_UNITTEST + 1]);
 
     taskQueueArray[enqueuedTasks - 1] = 0;