From b7d54b54772a2a2e17eeac8165fa3a5504f4fdb4 Mon Sep 17 00:00:00 2001
From: RomanBapst <bapstroman@gmail.com>
Date: Thu, 4 Jun 2020 15:52:43 +0300
Subject: [PATCH] gps: fixed filter initialization bug around gps - do not gps
 reference altitude to zero in case gps checks pass before the filter
 initialized - reset the filtered gps position and position derivative filters
 in case we are in air or there is movement on the ground

Signed-off-by: RomanBapst <bapstroman@gmail.com>
---
 EKF/ekf.cpp        | 8 ++++----
 EKF/ekf.h          | 1 +
 EKF/ekf_helper.cpp | 6 ++++++
 EKF/gps_checks.cpp | 3 +++
 4 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index b6e820b48a..770dee41fa 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -93,6 +93,10 @@ void Ekf::reset()
 	_accel_magnitude_filt = 0.0f;
 	_ang_rate_magnitude_filt = 0.0f;
 	_prev_dvel_bias_var.zero();
+
+	_gps_alt_ref = 0.0f;
+
+	resetGpsDriftCheckFilters();
 }
 
 bool Ekf::update()
@@ -189,10 +193,6 @@ bool Ekf::initialiseFilter()
 		// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
 		setControlBaroHeight();
 
-		// reset variables that are shared with post alignment GPS checks
-		_gps_pos_deriv_filt(2) = 0.0f;
-		_gps_alt_ref = 0.0f;
-
 		if(!initialiseTilt()){
 			return false;
 		}
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 2f381fee26..854b2d52d2 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -893,4 +893,5 @@ private:
 	// Returns true if the reset was successful
 	bool resetYawToEKFGSF();
 
+	void resetGpsDriftCheckFilters();
 };
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index c064a5a4f0..ec7c2ec8a6 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -1866,3 +1866,9 @@ void Ekf::runYawEKFGSF()
 		yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
 	}
 }
+
+void Ekf::resetGpsDriftCheckFilters()
+{
+	_gps_velNE_filt.setZero();
+	_gps_pos_deriv_filt.setZero();
+}
diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp
index 737fcf4b4e..e0ccb8beb5 100644
--- a/EKF/gps_checks.cpp
+++ b/EKF/gps_checks.cpp
@@ -194,10 +194,13 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		_gps_check_fail_status.flags.hspeed = false;
 		_gps_drift_updated = false;
 
+		resetGpsDriftCheckFilters();
+
 	} else {
 		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
 		_gps_drift_updated = true;
 
+		resetGpsDriftCheckFilters();
 	}
 
 	// save GPS fix for next time