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 #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;