From 1d9beed42f40fc8b966565da9e1571e06553725d Mon Sep 17 00:00:00 2001
From: Arthur Benemann <arthur.benemann@gmail.com>
Date: Thu, 5 Mar 2015 15:23:14 -0800
Subject: [PATCH] AP_SmallEKF: add function to report if the EKF is stable

---
 libraries/AP_NavEKF/AP_SmallEKF.cpp | 7 +++++++
 libraries/AP_NavEKF/AP_SmallEKF.h   | 3 +++
 2 files changed, 10 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_SmallEKF.cpp b/libraries/AP_NavEKF/AP_SmallEKF.cpp
index 472f006c50..a87737ef52 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.cpp
+++ b/libraries/AP_NavEKF/AP_SmallEKF.cpp
@@ -925,4 +925,11 @@ void SmallEKF::getQuat(Quaternion &quat) const
     quat = state.quat;
 }
 
+// get filter status - true is aligned
+bool SmallEKF::getStatus() const
+{
+    float run_time = hal.scheduler->millis() - StartTime_ms;
+    return  YawAligned && (run_time > 10000);
+}
+
 #endif // HAL_CPU_CLASS
diff --git a/libraries/AP_NavEKF/AP_SmallEKF.h b/libraries/AP_NavEKF/AP_SmallEKF.h
index 73a6e9932c..4d95d27c11 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.h
+++ b/libraries/AP_NavEKF/AP_SmallEKF.h
@@ -92,6 +92,9 @@ public:
     // get quaternion data
     void getQuat(Quaternion &quat) const;
 
+    // get filter alignment status - true is aligned
+    bool getStatus(void) const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private: