diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 163fc5b77b..e458fe2438 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -200,7 +200,7 @@ public:
     }
 
     // return true if we will use compass for yaw
-    virtual bool use_compass(void) const { return _compass && _compass->use_for_yaw(); }
+    virtual bool use_compass(void) { return _compass && _compass->use_for_yaw(); }
 
     // return true if yaw has been initialised
     bool yaw_initialised(void) const {
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index c41dcd4cff..a48e33c686 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -289,7 +289,7 @@ bool AP_AHRS_DCM::have_gps(void) const
 }
 
 // return true if we should use the compass for yaw correction
-bool AP_AHRS_DCM::use_compass(void) const
+bool AP_AHRS_DCM::use_compass(void)
 {
     if (!_compass || !_compass->use_for_yaw()) {
         // no compass available
@@ -310,8 +310,13 @@ bool AP_AHRS_DCM::use_compass(void) const
     // prevent flyaways with very bad compass offsets
     int32_t error = abs(wrap_180_cd(yaw_sensor - _gps->ground_course_cd));
     if (error > 4500 && _wind.length() < _gps->ground_speed_cm*0.008f) {
-        // start using the GPS for heading
-        return false;
+        if (hal.scheduler->millis() - _last_consistent_heading > 2000) {
+            // start using the GPS for heading if the compass has been
+            // inconsistent with the GPS for 2 seconds
+            return false;
+        }
+    } else {
+        _last_consistent_heading = hal.scheduler->millis();
     }
 
     // use the compass
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index af309ee6ee..4884cbe74a 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -71,7 +71,7 @@ public:
     // if we have an estimate
     bool airspeed_estimate(float *airspeed_ret);
 
-    bool            use_compass(void) const;
+    bool            use_compass(void);
 
 private:
     float _ki;
@@ -147,6 +147,7 @@ private:
     Vector3f _last_vel;
     uint32_t _last_wind_time;
     float _last_airspeed;
+    uint32_t _last_consistent_heading;
 
     // estimated wind in m/s
     Vector3f _wind;