From 1a05c27bbb89937867cba8020606df4a2a28bdc7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 15 May 2014 17:09:18 +1000
Subject: [PATCH] AP_AHRS: added healthy() function

this will be used to report when the AHRS subsystem becomes unhealthy
---
 libraries/AP_AHRS/AP_AHRS.h          |  3 +++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp    | 12 ++++++++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h      |  6 ++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 11 +++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  3 +++
 5 files changed, 35 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 2a1f8ecf5..39e730956 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 aba87bd32..10a5738cd 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 e8709f799..4179c7cf3 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 3bd7d5793..ffb263e8e 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 20775fdc8..52426fba6 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;
 
-- 
GitLab