From 9ef71a9dec2e695af29314690bca8c82dd1fff92 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 2 Jan 2014 16:16:16 +1100
Subject: [PATCH] AP_NavEKF: added healthy() API

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 16 ++++++++++++++++
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 +++
 2 files changed, 19 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 187e0c3977..c772bb81cd 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -68,6 +68,22 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
     dtIMUAvgInv = 1.0f/dtIMUAvg;
 }
 
+bool NavEKF::healthy(void)
+{
+    if (!statesInitialised) {
+        return false;
+    }
+    Quaternion q(states[0],states[1],states[2],states[3]);
+    if (q.is_nan()) {
+        return false;
+    }
+    if (isnan(states[4]) || isnan(states[5]) || isnan(states[6])) {
+        return false;
+    }
+    // all OK
+    return true;
+}
+
 void NavEKF::InitialiseFilter(void)
 {
     // Calculate initial filter quaternion states from ahrs solution
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index df8527dbc9..c5545640c1 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -76,6 +76,9 @@ public:
     // Update Filter States - this should be called whenever new IMU data is available
     void UpdateFilter(void);
 
+    // return true if the filter is healthy
+    bool healthy(void);
+
     // fill in latitude, longitude and height of the reference point
     void getRefLLH(struct Location &loc) const;