From 32b03819ef0c004d1624071620622449915d086e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 7 Mar 2016 20:21:04 +1100
Subject: [PATCH] EKF: Add function to calculate global position validity

---
 EKF/ekf_helper.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 2a7a5e85b3..8b4ac5484f 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -393,3 +393,10 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first
 		memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first));
 	}
 }
+
+bool Ekf::global_position_is_valid()
+{
+	// return true if the position estimate is valid
+	// TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status
+	return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps);
+}