From b425253a54ec002f338059847aa2be5118ec48f6 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 11 Jul 2024 12:19:50 -0400
Subject: [PATCH] ekf2: stop mag fusion on numerical error

---
 src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
index acd16578c5..b6065867ec 100644
--- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
@@ -101,6 +101,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 
 			resetMagCov();
 
+			stopMagFusion();
+
 			return false;
 		}