AP_AHRS: added healthy() function

this will be used to report when the AHRS subsystem becomes unhealthy

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;
 

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);
+}

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__

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
 

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;