diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c
index 213e4d399b..509b2d1ed5 100644
--- a/src/main/drivers/compass/compass_ak8963.c
+++ b/src/main/drivers/compass/compass_ak8963.c
@@ -382,7 +382,6 @@ void ak8963BusInit(const busDevice_t *busdev)
 #endif
 
 #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
-#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
     case BUSTYPE_MPU_SLAVE:
         rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
 
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index a7b9c6bc68..286e5cd7be 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -671,8 +671,11 @@ bool processRx(timeUs_t currentTimeUs)
 
     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
         LED1_ON;
+        // increase frequency of attitude task to reduce drift when in angle or horizon mode
+        rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
     } else {
         LED1_OFF;
+        rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
     }
 
     if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
diff --git a/src/main/fc/fc_tasks.h b/src/main/fc/fc_tasks.h
index 1033f2f9d7..16db100bb6 100644
--- a/src/main/fc/fc_tasks.h
+++ b/src/main/fc/fc_tasks.h
@@ -19,8 +19,4 @@
 
 #define LOOPTIME_SUSPEND_TIME 3  // Prevent too long busy wait times
 
-#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
-#define TASK_PERIOD_MS(ms) ((ms) * 1000)
-#define TASK_PERIOD_US(us) (us)
-
 void fcTasksInit(void);
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 1f8167691b..e7e20cc4f3 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -19,6 +19,11 @@
 
 #include "common/time.h"
 
+#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
+#define TASK_PERIOD_MS(ms) ((ms) * 1000)
+#define TASK_PERIOD_US(us) (us)
+
+
 typedef enum {
     TASK_PRIORITY_IDLE = 0,     // Disables dynamic scheduling, task is executed only if no other task is active this cycle
     TASK_PRIORITY_LOW = 1,
diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc
index 5d60dbe3d1..ccca516628 100644
--- a/src/test/unit/arming_prevention_unittest.cc
+++ b/src/test/unit/arming_prevention_unittest.cc
@@ -669,4 +669,5 @@ extern "C" {
     void dashboardEnablePageCycling(void) {}
     void dashboardDisablePageCycling(void) {}
     bool imuQuaternionHeadfreeOffsetSet(void) { return true; }
+    void rescheduleTask(cfTaskId_e, uint32_t) {}
 }