From 03b5436a213ca53fc2a39077cc77917fb735b7cc Mon Sep 17 00:00:00 2001
From: ErinTUDelft <e.lucassen@student.tudelft.nl>
Date: Wed, 30 Oct 2024 21:48:59 +0100
Subject: [PATCH] Put EKF and accelerometer on the realtime loop

---
 src/main/fc/core.c             | 32 ++++++++++++++++++++++++++++++++
 src/main/fc/core.h             |  5 +++++
 src/main/fc/tasks.c            | 26 ++++++--------------------
 src/main/scheduler/scheduler.c | 11 +++++++++++
 src/main/scheduler/scheduler.h |  5 ++---
 5 files changed, 56 insertions(+), 23 deletions(-)

diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index bc49fe55c62..81f6198364f 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -1457,6 +1457,38 @@ FAST_CODE bool gyroFilterReady(void)
     }
 }
 
+FAST_CODE bool accelerometerReady(void)
+{
+    if (pidUpdateCounter % 8  == 0) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+FAST_CODE void taskUpdateAccelerometer(timeUs_t currentTimeUs)
+{
+    accUpdate(currentTimeUs);
+#ifdef USE_THROW_TO_ARM
+    updateThrowFallStateMachine(currentTimeUs);
+#endif
+}
+
+FAST_CODE bool EKFReady(void)
+{
+    if (pidUpdateCounter % 16 == 0) { // todo, get correct denom
+        return true;
+    } else {
+        return false;
+    }
+}
+
+FAST_CODE void taskEKF(timeUs_t currentTimeUs)
+{
+    updateEkf(currentTimeUs);
+}
+
+
 FAST_CODE bool innerLoopReady(void)
 {
     if ((pidUpdateCounter % activePidLoopDenom) == (activePidLoopDenom / 2)) {
diff --git a/src/main/fc/core.h b/src/main/fc/core.h
index d3bad566ca2..5b4eae89236 100644
--- a/src/main/fc/core.h
+++ b/src/main/fc/core.h
@@ -85,7 +85,12 @@ void updateArmingStatus(void);
 
 void resetInnerLoopCounter(void);
 void taskGyroSample(timeUs_t currentTimeUs);
+
+void taskUpdateAccelerometer(timeUs_t currentTimeUs);
+void taskEKF(timeUs_t currentTimeUs);
 bool gyroFilterReady(void);
+bool accelerometerReady(void);
+bool EKFReady(void);
 bool innerLoopReady(void);
 void taskFiltering(timeUs_t currentTimeUs);
 void taskMainInnerLoop(timeUs_t currentTimeUs);
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index fb718384a1d..ed14ef2242a 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -174,15 +174,6 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
     batteryUpdateAlarms();
 }
 
-#ifdef USE_ACC
-static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
-{
-    accUpdate(currentTimeUs);
-#ifdef USE_THROW_TO_ARM
-    updateThrowFallStateMachine(currentTimeUs);
-#endif
-}
-#endif
 
 typedef enum {
     RX_STATE_CHECK,
@@ -370,12 +361,6 @@ static void taskPosCtl(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_EKF
-static void taskEkf(timeUs_t currentTimeUs)
-{
-    updateEkf(currentTimeUs);
-}
-#endif
 
 #ifdef USE_CAMERA_CONTROL
 static void taskCameraControl(uint32_t currentTime)
@@ -422,9 +407,13 @@ task_attribute_t task_attributes[TASK_COUNT] = {
     [TASK_FILTER] = DEFINE_TASK("FILTER", NULL, NULL, taskFiltering, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
     [TASK_INNER_LOOP] = DEFINE_TASK("INNER LOOP", NULL, NULL, taskMainInnerLoop, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 
+#ifdef USE_EKF
+    [TASK_EKF] = DEFINE_TASK("EKF", NULL, NULL, taskEKF, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
+#endif
+
 #ifdef USE_ACC
-    [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
-    [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
+    [TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
+    [TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 #endif
 
     [TASK_RX] = DEFINE_TASK("RX", NULL, rxUpdateCheck, taskUpdateRxMain, TASK_PERIOD_HZ(33), TASK_PRIORITY_HIGH), // If event-based scheduling doesn't work, fallback to periodic scheduling
@@ -482,9 +471,6 @@ task_attribute_t task_attributes[TASK_COUNT] = {
     [TASK_POS_CTL] = DEFINE_TASK("POS_CTL", NULL, NULL, taskPosCtl, TASK_PERIOD_HZ(500), TASK_PRIORITY_MEDIUM),
 #endif
 
-#ifdef USE_EKF
-    [TASK_EKF] = DEFINE_TASK("EKF", NULL, NULL, taskEkf, TASK_PERIOD_HZ(500), TASK_PRIORITY_MEDIUM),
-#endif
 
 #ifdef USE_LED_STRIP
     [TASK_LEDSTRIP] = DEFINE_TASK("LEDSTRIP", NULL, NULL, ledStripUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index 39ae82b2a5a..55c00fa8e4e 100644
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -500,6 +500,17 @@ FAST_CODE void scheduler(void)
             if (gyroFilterReady()) {
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_FILTER), currentTimeUs);
             }
+
+            if (accelerometerReady()) {
+                taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_ACCEL), currentTimeUs);
+                #ifndef USE_EKF
+                taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_ATTITUDE), currentTimeUs);
+                #endif
+            }
+            if (EKFReady()) {
+                taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_EKF), currentTimeUs);
+            }
+
             if (innerLoopReady()) {
                 taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_INNER_LOOP), currentTimeUs);
             }
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 4a6207abcf4..c8f90922cc3 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -101,6 +101,7 @@ typedef enum {
     TASK_MAIN,
     TASK_GYRO,
     TASK_FILTER,
+    TASK_EKF,
     TASK_INNER_LOOP,
     TASK_ACCEL,
     TASK_ATTITUDE,
@@ -149,9 +150,7 @@ typedef enum {
 #ifdef USE_POS_CTL
     TASK_POS_CTL,
 #endif
-#ifdef USE_EKF
-    TASK_EKF,
-#endif
+
 #ifdef USE_LED_STRIP
     TASK_LEDSTRIP,
 #endif