AP_SmallEKF: add function to report if the EKF is stable

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

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: