From c180846ca9ccb9ac688af92d5a7d0350f6cab97a Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Tue, 14 Jan 2014 15:42:45 +0100
Subject: [PATCH] OP-1161 discard magnetometer in filters if plausibility alarm
 is set

---
 flight/modules/StateEstimation/filtermag.c | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c
index 7997016e0..87506cbb8 100644
--- a/flight/modules/StateEstimation/filtermag.c
+++ b/flight/modules/StateEstimation/filtermag.c
@@ -33,6 +33,7 @@
 #include "inc/stateestimation.h"
 #include <attitudestate.h>
 #include <revocalibration.h>
+#include <systemalarms.h>
 #include <homelocation.h>
 
 #include <CoordinateConversions.h>
@@ -81,8 +82,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     struct data *this = (struct data *)self->localdata;
 
     if (IS_SET(state->updated, SENSORUPDATES_mag)) {
-        if (this->revoCalibration.MagBiasNullingRate > 0) {
-            magOffsetEstimation(this, state->mag);
+        SystemAlarmsAlarmData alarms;
+        SystemAlarmsAlarGet(&alarms);
+        if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) {
+            UNSET_MASK(state->updated, SENSORUPDATES_mag);
+        } else {
+            if (this->revoCalibration.MagBiasNullingRate > 0) {
+                magOffsetEstimation(this, state->mag);
+            }
         }
     }