From 96e6b794307e7aab3090ae89f70c219308f33ee8 Mon Sep 17 00:00:00 2001
From: Steve Evans <SteveCEvans@users.noreply.github.com>
Date: Tue, 7 Nov 2023 05:37:27 +0000
Subject: [PATCH] Fix compass loop time (#13125)

---
 src/main/fc/tasks.c        |  1 +
 src/main/sensors/compass.c | 19 ++++++++++++++++---
 src/main/sensors/compass.h |  2 +-
 3 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 096a332ba..d3924036a 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -31,6 +31,7 @@
 #include "cms/cms.h"
 
 #include "common/color.h"
+#include "common/time.h"
 #include "common/utils.h"
 
 #include "config/feature.h"
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 5d4d4c69f..75eb9b53d 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -71,6 +71,8 @@ mag_t mag;
 
 PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
 
+#define COMPASS_READ_US 500 // Allow 500us for compass data read
+
 void pgResetFn_compassConfig(compassConfig_t *compassConfig)
 {
     compassConfig->mag_alignment = ALIGN_DEFAULT;
@@ -373,10 +375,16 @@ bool compassIsCalibrationComplete(void)
 
 uint32_t compassUpdate(timeUs_t currentTimeUs)
 {
+    static uint8_t busyCount = 0;
+    uint32_t nextPeriod = COMPASS_READ_US;
+
     if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
         // No action was taken as the read has not completed
-        schedulerIgnoreTaskExecRate();
-        return 500; // Wait 500us between states
+        schedulerIgnoreTaskStateTime();
+
+        busyCount++;
+
+        return nextPeriod; // Wait COMPASS_READ_US between states
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -419,6 +427,11 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
         }
     }
 
-    return TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ);
+    nextPeriod = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ) - COMPASS_READ_US * busyCount;
+
+    // Reset the busy count
+    busyCount = 0;
+
+    return nextPeriod;
 }
 #endif // USE_MAG
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 40203c47b..1d30d62e7 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -27,7 +27,7 @@
 #include "pg/pg.h"
 #include "sensors/sensors.h"
 
-#define TASK_COMPASS_RATE_HZ 200
+#define TASK_COMPASS_RATE_HZ 300
 
 // Type of magnetometer used/detected
 typedef enum {
-- 
2.11.4.GIT