diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 2a1f8ecf5d009bec19e137033d0cc2dc2ce0bfd9..39e7309566268af7bebe6ac0da04c22a3cc0bc1e 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -324,6 +324,9 @@ public:
     // return the active accelerometer instance
     uint8_t get_active_accel_instance(void) const { return _active_accel_instance; }
 
+    // is the AHRS subsystem healthy?
+    virtual bool healthy(void) = 0;
+
 protected:
     AHRS_VehicleClass _vehicle_class;
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index aba87bd3236e34bb73440f8fc7f24e1cb980330b..10a5738cd761a23120cbcc3ecf299baf1143b898 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -241,6 +241,7 @@ AP_AHRS_DCM::normalize(void)
         !renorm(t2, _dcm_matrix.c)) {
         // Our solution is blowing up and we will force back
         // to last euler angles
+        _last_failure_ms = hal.scheduler->millis();
         reset(true);
     }
 }
@@ -598,6 +599,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
         GA_e.normalize();
         if (GA_e.is_inf()) {
             // wait for some non-zero acceleration information
+            _last_failure_ms = hal.scheduler->millis();
             return;
         }
         using_gps_corrections = true;
@@ -648,6 +650,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
 
     if (besti == -1) {
         // no healthy accelerometers!
+        _last_failure_ms = hal.scheduler->millis();
         return;
     }
 
@@ -697,6 +700,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     if (error[besti].is_nan() || error[besti].is_inf()) {
         // don't allow bad values
         check_matrix();
+        _last_failure_ms = hal.scheduler->millis();
         return;
     }
 
@@ -918,3 +922,11 @@ void AP_AHRS_DCM::set_home(const Location &loc)
     _home.options = 0;
 }
 
+/*
+  check if the AHRS subsystem is healthy
+*/
+bool AP_AHRS_DCM::healthy(void)
+{
+    // consider ourselves healthy if there have been no failures for 5 seconds
+    return (_last_failure_ms == 0 || hal.scheduler->millis() - _last_failure_ms > 5000);
+}
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index e8709f7997499a06c86e5237ba7044669190e06b..4179c7cf3b77dcc9827731a8f673b388409b0fed 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -81,6 +81,9 @@ public:
     void set_home(const Location &loc);
     void estimate_wind(void);
 
+    // is the AHRS subsystem healthy?
+    bool healthy(void);
+
 private:
     float _ki;
     float _ki_yaw;
@@ -165,6 +168,9 @@ private:
 
     // estimated wind in m/s
     Vector3f _wind;
+
+    // last time AHRS failed in milliseconds
+    uint32_t _last_failure_ms;
 };
 
 #endif // __AP_AHRS_DCM_H__
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 3bd7d5793b896d5c61659cc22b6b996e40e0bb76..ffb263e8e3cef8cb92712240c89143240ec9185f 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -228,5 +228,16 @@ bool AP_AHRS_NavEKF::using_EKF(void) const
     return ekf_started && _ekf_use && EKF.healthy();
 }
 
+/*
+  check if the AHRS subsystem is healthy
+*/
+bool AP_AHRS_NavEKF::healthy(void)
+{
+    if (_ekf_use) {
+        return ekf_started && EKF.healthy();
+    }
+    return AP_AHRS_DCM::healthy();    
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 20775fdc813985a67f541dce3684a2849028b72c..52426fba6146af3f33c7281de19ab7c4d7172562 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -92,6 +92,9 @@ public:
 
     void set_ekf_use(bool setting) { _ekf_use.set(setting); }
 
+    // is the AHRS subsystem healthy?
+    bool healthy(void);
+
 private:
     bool using_EKF(void) const;