From d5876ab0d27b07861009abb9947f48d53735a7c7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 11 Jul 2024 12:25:14 -0400
Subject: [PATCH] ekf2: stop gps yaw fusion on numerical error

---
 src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp
index d66fbac7c6..987d2ae3fc 100644
--- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp
@@ -100,6 +100,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 		// we reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		ECL_ERR("GPS yaw numerical error - covariance reset");
+		stopGpsYawFusion();
 		return;
 	}