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 #include +#include #include #include @@ -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); + } } }