diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 10eec6a895..3cc6f67747 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -493,13 +493,15 @@ static void imuCalculateEstimatedAttitude(float dT)
     if (STATE(FIXED_WING)) {
         bool canUseCOG = isGPSHeadingValid();
 
-        if (canUseCOG) {
+        // Prefer compass (if available)
+        if (canUseMAG) {
+            useMag = true;
+            gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
+        }
+        else if (canUseCOG) {
             if (gpsHeadingInitialized) {
-                // Use GPS heading if error is acceptable or if it's the only source of heading
-                if (ABS(gpsSol.groundCourse - attitude.values.yaw) < DEGREES_TO_DECIDEGREES(MAX_GPS_HEADING_ERROR_DEG) || !canUseMAG) {
-                    courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-                    useCOG = true;
-                }
+                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+                useCOG = true;
             }
             else {
                 // Re-initialize quaternion from known Roll, Pitch and GPS heading
@@ -509,15 +511,6 @@ static void imuCalculateEstimatedAttitude(float dT)
                 // Force reset of heading hold target
                 resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
             }
-
-            // If we can't use COG and there's MAG available - fallback
-            if (!useCOG && canUseMAG) {
-                useMag = true;
-            }
-        }
-        else if (canUseMAG) {
-            useMag = true;
-            gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if possible
         }
     }
     else {