From 0cb210d045d005d0775bc7b4c1631addb128c8e3 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 1 Jan 2016 11:56:38 +1100
Subject: [PATCH] EKF: Changes required to enter POSCTL mode

---
 EKF/estimator_base.cpp | 30 +++++++++++++++++++++++++-----
 EKF/estimator_base.h   | 11 ++++++++---
 2 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp
index ed88c1f8a8..fe2dd95ee4 100644
--- a/EKF/estimator_base.cpp
+++ b/EKF/estimator_base.cpp
@@ -170,6 +170,8 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
 		_time_last_gps = gps_sample_new.time_us;
 
+		_last_valid_gps_time_us = hrt_absolute_time();
+
 		gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
 
 
@@ -261,8 +263,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 	_params.baro_delay_ms = 0;
 	_params.gps_delay_ms = 200;
 	_params.airspeed_delay_ms = 0;
-	_params.requiredEph = 200;
-	_params.requiredEpv = 200;
+	_params.requiredEph = 500;
+	_params.requiredEpv = 800;
 	_params.gyro_noise = 1e-3f;
 	_params.accel_noise = 1e-1f;
 	_params.gyro_bias_p_noise = 1e-5f;
@@ -335,20 +337,38 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
 		map_projection_init(&_posRef, lat, lon);
 		_gps_alt_ref = gps->alt / 1e3f;
 		_gps_initialised = true;
+		_last_gps_origin_time_us = hrt_absolute_time();
 	}
 }
 
 bool EstimatorBase::gps_is_good(struct gps_message *gps)
 {
 	// go through apm implementation of calcGpsGoodToAlign for fancier checks
-	if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
-		return true;
+	// Use a stricter check for initialisation than during flight to avoid complete loss of GPS
+	if (_gps_initialised) {
+		if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph * 2) && (gps->epv < _params.requiredEpv * 2)) {
+			return true;
 
+		} else {
+			return false;
+		}
 	} else {
-		return false;
+		if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
+			return true;
+
+		} else {
+			return false;
+		}
 	}
 }
 
+bool EstimatorBase::position_is_valid()
+{
+	// return true if the position estimate is valid
+	// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
+	return _gps_initialised &&  (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6;
+}
+
 void EstimatorBase::printStoredIMU()
 {
 	printf("---------Printing IMU data buffer------------\n");
diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h
index 98a9e8a1fa..ee606c1143 100644
--- a/EKF/estimator_base.h
+++ b/EKF/estimator_base.h
@@ -190,6 +190,8 @@ protected:
 	float _dt_imu_avg;
 	uint64_t _imu_time_last;
 
+	uint64_t _last_valid_gps_time_us;
+
 	imuSample _imu_sample_delayed;
 	imuSample _imu_down_sampled;
 	Quaternion
@@ -206,9 +208,6 @@ protected:
 	outputSample _output_new;
 	imuSample _imu_sample_new;
 
-	struct map_projection_reference_s _posRef;
-	float _gps_alt_ref;
-
 
 	uint64_t _imu_ticks;
 
@@ -266,6 +265,8 @@ public:
 	void printGps(struct gpsSample *data);
 	void printStoredGps();
 
+	bool position_is_valid();
+
 	void copy_quaternion(float *quat) {
 		for (unsigned i = 0; i < 4; i++) {
 			quat[i] = _output_new.quat_nominal(i);
@@ -284,4 +285,8 @@ public:
 	void copy_timestamp(uint64_t *time_us) {
 		*time_us = _imu_time_last;
 	}
+
+	uint64_t _last_gps_origin_time_us;
+	struct map_projection_reference_s _posRef;
+	float _gps_alt_ref;
 };