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: