From 85eee31510efa74147ea096551f27f1afa683df9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 2 Oct 2014 13:43:46 +0900
Subject: [PATCH] AHRS: rename ekfNotStarted method to initialised

Also created default implementation in AP_AHRS class so AP_AHRS_DCM does
not need to implement it.
---
 libraries/AP_AHRS/AP_AHRS.h          | 4 ++--
 libraries/AP_AHRS/AP_AHRS_DCM.cpp    | 5 -----
 libraries/AP_AHRS/AP_AHRS_DCM.h      | 3 ---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 8 +++++---
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   | 5 +++--
 5 files changed, 10 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index e3c90f658c..5689781b89 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -338,8 +338,8 @@ public:
     // is the AHRS subsystem healthy?
     virtual bool healthy(void) = 0;
 
-    // is the EKF waiting to start?
-    virtual bool ekfNotStarted(void) = 0;
+    // true if the AHRS has completed initialisation
+    virtual bool initialised(void) const { return true; };
 
 protected:
     AHRS_VehicleClass _vehicle_class;
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 00dc769f77..9f79bcd45c 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -937,8 +937,3 @@ 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 e1056e9ecc..76160c09e5 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -106,9 +106,6 @@ 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 1e749ee84c..2930194bd3 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -266,10 +266,12 @@ bool AP_AHRS_NavEKF::healthy(void)
     return AP_AHRS_DCM::healthy();    
 }
 
-bool AP_AHRS_NavEKF::ekfNotStarted(void)
+// true if the AHRS has completed initialisation
+bool AP_AHRS_NavEKF::initialised(void) const
 {
-    return !ekf_started;
-}
+    // initialisation complete 10sec after ekf has started
+    return (ekf_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS));
+};
 
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index e3246abee1..d03ffdc173 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -28,6 +28,7 @@
 #include <AP_NavEKF.h>
 
 #define AP_AHRS_NAVEKF_AVAILABLE 1
+#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000     // time in milliseconds the ekf needs to settle after being started
 
 class AP_AHRS_NavEKF : public AP_AHRS_DCM
 {
@@ -95,8 +96,8 @@ public:
     // is the AHRS subsystem healthy?
     bool healthy(void);
 
-    // is the EKF waiting to start?
-    bool ekfNotStarted(void);
+    // true if the AHRS has completed initialisation
+    bool initialised(void) const;
 
 private:
     bool using_EKF(void) const;