From 2bafe9df083a9a338f7de40e47c1777f402afab6 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 22 Jun 2020 17:16:13 +0200
Subject: [PATCH] GPS Yaw: wait to fuse at yaw at least once before declaring
 it faulty

This fixes the cases where the yaw message from the GNSS receiver would
take more time than the vel/pos. The estimator should wait and not
immediately fall back to an other aiding source after 5sec.
If it never comes, it will never fall back, but this is ok since the
user wants to fly with GPS aiding an not with something else.
---
 EKF/control.cpp               | 4 +++-
 test/sensor_simulator/gps.cpp | 2 +-
 test/sensor_simulator/gps.h   | 2 +-
 test/test_EKF_gps_yaw.cpp     | 6 +++++-
 4 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index e9e6aa5ff4..df97aedd87 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -526,6 +526,7 @@ void Ekf::controlGpsFusion()
 
 		} else {
 			if (ISFINITE(_gps_sample_delayed.yaw)) {
+
 				if (!_control_status.flags.gps_yaw
 				    && _control_status.flags.tilt_align
 				    && !_gps_hgt_intermittent) {
@@ -548,7 +549,8 @@ void Ekf::controlGpsFusion()
 			// Check if the data is constantly fused by the estimator,
 			// if not, declare the sensor faulty and stop the fusion
 			// By doing this, another source of yaw aiding is allowed to start
-			if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
+			if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)
+			    && _control_status.flags.gps_yaw) {
 				_is_gps_yaw_faulty = true;
 				stopGpsYawFusion();
 			}
diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp
index 1d4fc1d1d2..263b16b91f 100644
--- a/test/sensor_simulator/gps.cpp
+++ b/test/sensor_simulator/gps.cpp
@@ -102,7 +102,7 @@ gps_message Gps::getDefaultGpsData()
 	gps_data.lat = 473566094;
 	gps_data.lon = 85190237;
 	gps_data.alt = 422056;
-	gps_data.yaw = 0.0f;
+	gps_data.yaw = NAN;
 	gps_data.yaw_offset = 0.0f;
 	gps_data.fix_type = 3;
 	gps_data.eph = 0.5f;
diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h
index 46016a0e22..4dcfecd702 100644
--- a/test/sensor_simulator/gps.h
+++ b/test/sensor_simulator/gps.h
@@ -66,7 +66,7 @@ public:
 	gps_message getDefaultGpsData();
 
 private:
-	gps_message _gps_data;
+	gps_message _gps_data{};
 	Vector3f _gps_pos_rate{};
 
 	void send(uint64_t time) override;
diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp
index 50629470e5..dee81174d2 100644
--- a/test/test_EKF_gps_yaw.cpp
+++ b/test/test_EKF_gps_yaw.cpp
@@ -58,8 +58,10 @@ class EkfGpsHeadingTest : public ::testing::Test {
 	void SetUp() override
 	{
 		_ekf->init(0);
+		_sensor_simulator._gps.setYaw(NAN);
 		_sensor_simulator.runSeconds(2);
 		_ekf_wrapper.enableGpsFusion();
+		_ekf_wrapper.enableGpsHeadingFusion();
 		_sensor_simulator.startGps();
 		_sensor_simulator.runSeconds(11);
 	}
@@ -121,11 +123,13 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 TEST_F(EkfGpsHeadingTest, fallBackToMag)
 {
 	// GIVEN: an initial GPS yaw, not aligned with the current one
+	// GPS yaw is expected to arrive a bit later, first feed some NANs
+	// to the filter
+	_sensor_simulator.runSeconds(6);
 	float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
 	_sensor_simulator._gps.setYaw(gps_heading);
 
 	// WHEN: the GPS yaw fusion is activated
-	_ekf_wrapper.enableGpsHeadingFusion();
 	_sensor_simulator.runSeconds(1);
 
 	// THEN: GPS heading fusion should have started, and mag