Moved things to a FreeRTOS task so that we can garunte that we are meeting timings

src/tasks/system_task.cpp

@@ -0,0 +1,65 @@
+#include "system_task.h"
+
+
+SystemTask::SystemTask(ServiceManager& manager)
+:
+serviceManager(manager),
+taskHandle(nullptr)
+{
+
+}
+
+
+
+// xTaskCreatePinnedToCore(
+//     function,
+//     name,
+//     stack size,
+//     parameter,
+//     priority,
+//     handle,
+//     core
+// );
+void SystemTask::start()
+{
+    xTaskCreatePinnedToCore(
+        taskEntry,
+        "SystemTask",
+        4096,
+        this,
+        2,
+        &taskHandle,
+        1
+    );
+}
+
+
+void SystemTask::taskEntry(void* parameter)
+{
+    SystemTask* task =
+        static_cast<SystemTask*>(parameter);
+
+    task->run();
+}
+
+
+void SystemTask::run()
+{
+    serviceManager.begin();
+
+
+    TickType_t lastWakeTime =
+        xTaskGetTickCount();
+
+
+    while(true)
+    {
+        serviceManager.update();
+
+
+        vTaskDelayUntil(
+            &lastWakeTime,
+            pdMS_TO_TICKS(10)
+        );
+    }
+}

src/tasks/system_task.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <Arduino.h>
+#include "../services/service_manager.h"
+
+
+class SystemTask
+{
+public:
+
+    SystemTask(ServiceManager& manager);
+
+    void start();
+
+
+private:
+
+    static void taskEntry(void* parameter);
+
+    void run();
+
+
+    ServiceManager& serviceManager;
+
+    TaskHandle_t taskHandle;
+};