From b51d01f97949e5c8cba2ddb2cf9b981bf9618e71 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 29 Sep 2014 18:37:14 +1000
Subject: [PATCH] AP_AHRS : add method to report if EKF is waiting to start

---
 libraries/AP_AHRS/AP_AHRS.h          | 3 +++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp    | 5 +++++
 libraries/AP_AHRS/AP_AHRS_DCM.h      | 3 +++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 5 +++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   | 3 +++
 5 files changed, 19 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index a1f96a482b..e3c90f658c 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -338,6 +338,9 @@ public:
     // is the AHRS subsystem healthy?
     virtual bool healthy(void) = 0;
 
+    // is the EKF waiting to start?
+    virtual bool ekfNotStarted(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 9f79bcd45c..00dc769f77 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -937,3 +937,8 @@ 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);
 }
+
+bool AP_AHRS_DCM::ekfNotStarted(void)
+{
+    return false;
+}
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 76160c09e5..e1056e9ecc 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -106,6 +106,9 @@ public:
     // is the AHRS subsystem healthy?
     bool healthy(void);
 
+    // is the EKF waiting to start?
+    bool ekfNotStarted(void);
+
 private:
     float _ki;
     float _ki_yaw;
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 71ff44b17a..1e749ee84c 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -266,5 +266,10 @@ bool AP_AHRS_NavEKF::healthy(void)
     return AP_AHRS_DCM::healthy();    
 }
 
+bool AP_AHRS_NavEKF::ekfNotStarted(void)
+{
+    return !ekf_started;
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 563535bcae..e3246abee1 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -95,6 +95,9 @@ public:
     // is the AHRS subsystem healthy?
     bool healthy(void);
 
+    // is the EKF waiting to start?
+    bool ekfNotStarted(void);
+
 private:
     bool using_EKF(void) const;