diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c
index 74e62749ab..0265fed5ed 100755
--- a/src/main/flight/navigation_rewrite.c
+++ b/src/main/flight/navigation_rewrite.c
@@ -699,7 +699,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 {
     UNUSED(previousState);
     /* All good for RTH */
-    if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) {
+    if (posControl.flags.hasValidPositionSensor &&  posControl.flags.hasValidHeadingSensor && STATE(GPS_FIX_HOME)) {
         // Switch between 2D and 3D RTH depending on altitude sensor availability
         if (posControl.flags.hasValidAltitudeSensor) {
             return NAV_FSM_EVENT_SWITCH_TO_RTH_3D;
@@ -739,7 +739,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
     UNUSED(previousState);
 
     // If no position sensor available - switch to NAV_STATE_RTH_2D_GPS_FAILING
-    if (!posControl.flags.hasValidPositionSensor) {
+    if (!posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
         return NAV_FSM_EVENT_ERROR;         // NAV_STATE_RTH_2D_GPS_FAILING
     }
 
@@ -765,7 +765,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navi
     UNUSED(previousState);
 
     /* Wait for GPS to be online again */
-    if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) {
+    if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor && STATE(GPS_FIX_HOME)) {
         return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_2D_HEAD_HOME
     }
     /* No HOME set or position sensor failure timeout - land */
@@ -831,7 +831,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
     UNUSED(previousState);
 
     // If no position sensor available - land immediately
-    if (!posControl.flags.hasValidPositionSensor && checkForPositionSensorTimeout()) {
+    if (!posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor && checkForPositionSensorTimeout()) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
@@ -856,7 +856,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga
     UNUSED(previousState);
 
     // If no position sensor available - land immediately
-    if (!posControl.flags.hasValidPositionSensor) {
+    if (!posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
         return NAV_FSM_EVENT_ERROR;         // NAV_STATE_RTH_3D_GPS_FAILING
     }
 
@@ -893,7 +893,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_L
     UNUSED(previousState);
 
     // If no position sensor available - land immediately
-    if (!posControl.flags.hasValidPositionSensor && checkForPositionSensorTimeout()) {
+    if (!posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor && checkForPositionSensorTimeout()) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
@@ -990,7 +990,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
     UNUSED(previousState);
 
     // If no position sensor available - land immediately
-    if (posControl.flags.hasValidPositionSensor) {
+    if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
         switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
             case NAV_WP_ACTION_WAYPOINT:
             case NAV_WP_ACTION_RTH:
@@ -1039,7 +1039,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
     UNUSED(previousState);
 
     // If no position sensor available - land immediately
-    if (posControl.flags.hasValidPositionSensor) {
+    if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
         return NAV_FSM_EVENT_NONE;
     }
     /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
@@ -1279,6 +1279,7 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
     posControl.actualState.vel.V.Y = newVelY;
 
     posControl.flags.hasValidPositionSensor = hasValidSensor;
+    posControl.flags.hasValidHeadingSensor = isImuHeadingValid();
 
     if (hasValidSensor) {
         posControl.flags.horizontalPositionNewData = 1;
@@ -1947,7 +1948,7 @@ static bool canActivateAltHoldMode(void)
 
 static bool canActivatePosHoldMode(void)
 {
-    return posControl.flags.hasValidPositionSensor;
+    return posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor;
 }
 
 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
@@ -2170,6 +2171,7 @@ void navigationInit(navConfig_t *initialnavConfig,
     posControl.flags.hasValidAltitudeSensor = 0;
     posControl.flags.hasValidPositionSensor = 0;
     posControl.flags.hasValidSurfaceSensor = 0;
+    posControl.flags.hasValidHeadingSensor = 0;
 
     posControl.flags.forcedRTHActivated = 0;
     posControl.waypointCount = 0;
diff --git a/src/main/flight/navigation_rewrite_pos_estimator.c b/src/main/flight/navigation_rewrite_pos_estimator.c
index 7709fcb9d9..6e5ce6a380 100755
--- a/src/main/flight/navigation_rewrite_pos_estimator.c
+++ b/src/main/flight/navigation_rewrite_pos_estimator.c
@@ -539,7 +539,7 @@ static void updateEstimatedTopic(uint32_t currentTime)
     }
 
     /* Estimate XY-axis only if heading is valid (X-Y acceleration is North-East)*/
-    if (((posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) || isGPSValid) && (isImuHeadingValid() || STATE(FIXED_WING))) {
+    if ((posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) || isGPSValid) {
         if (isImuHeadingValid()) {
             inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X);
             inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y);
diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h
index c2277409a4..adde07d6a6 100755
--- a/src/main/flight/navigation_rewrite_private.h
+++ b/src/main/flight/navigation_rewrite_private.h
@@ -54,6 +54,7 @@ typedef struct navigationFlags_s {
     bool hasValidAltitudeSensor;        // Indicates that we have a working altitude sensor (got at least one valid reading from it)
     bool hasValidPositionSensor;        // Indicates that GPS is working (or not)
     bool hasValidSurfaceSensor;
+    bool hasValidHeadingSensor;         // Indicate valid heading - wither mag or GPS at certain speed on airplane
 
     bool isAdjustingPosition;
     bool isAdjustingAltitude;