OP-1161 moved mag validity check function from sensors to state estimation

flight/modules/Sensors/sensors.c

@@ -55,8 +55,6 @@
 #include <attitudestate.h>
 #include <attitudesettings.h>
 #include <revocalibration.h>
-#include <revosettings.h>
-#include <homelocation.h>
 #include <flightstatus.h>
 #include <taskinfo.h>
 
@@ -75,7 +73,6 @@
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent *objEv);
-static void checkMagValidity(MagSensorData *mag);
 // static void magOffsetEstimation(MagSensorData *mag);
 
 // Private variables
@@ -95,8 +92,6 @@ static float R[3][3] = {
     { 0 }
 };
 static int8_t rotate = 0;
-static RevoSettingsMagnetometerMaxDeviationData magMaxDev;
-static float Be[3]   = { 0 };
 
 /**
  * API for sensor fusion algorithms:
@@ -117,17 +112,12 @@ int32_t SensorsInitialize(void)
     AccelSensorInitialize();
     MagSensorInitialize();
     RevoCalibrationInitialize();
-    RevoSettingsInitialize();
     AttitudeSettingsInitialize();
-    AttitudeStateInitialize();
-    HomeLocationInitialize();
 
     rotate = 0;
 
     RevoCalibrationConnectCallback(&settingsUpdatedCb);
     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
-    RevoSettingsConnectCallback(&settingsUpdatedCb);
-    HomeLocationConnectCallback(&settingsUpdatedCb);
 
     return 0;
 }
@@ -414,7 +404,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 
             MagSensorSet(&mag);
             mag_update_time = PIOS_DELAY_GetRaw();
-            checkMagValidity(&mag);
         }
 #endif /* if defined(PIOS_INCLUDE_HMC5883) */
 
@@ -426,57 +415,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
     }
 }
 
-/**
- * check validity of magnetometers
- */
-static void checkMagValidity(MagSensorData *mag)
-{
-        #define lowPassAlpha 0.01f
-        #define idleCount    100
-    static float average[3]     = { 0 };
-    static uint8_t noteverytime = idleCount;
-
-    // low pass filter sensor to not give warnings due to noise
-    average[0] = (1.0f - lowPassAlpha) * average[0] + lowPassAlpha * mag->x;
-    average[1] = (1.0f - lowPassAlpha) * average[1] + lowPassAlpha * mag->y;
-    average[2] = (1.0f - lowPassAlpha) * average[2] + lowPassAlpha * mag->z;
-
-    // throttle this check, thanks to low pass filter it is not necessary every iteration
-    if (!noteverytime--) {
-        noteverytime = idleCount;
-
-        // calculate expected Be vector
-        AttitudeStateData attitudeState;
-        AttitudeStateGet(&attitudeState);
-        float Rot[3][3];
-        float expected[3];
-        Quaternion2R(&attitudeState.q1, Rot);
-        rot_mult(Rot, Be, expected);
-
-        // calculate maximum allowed deviation
-        float warning = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
-        float error   = magMaxDev.Error * magMaxDev.Error * warning;
-        warning = magMaxDev.Warning * magMaxDev.Warning * warning;
-
-
-        // calculate difference
-        expected[0] = expected[0] - average[0];
-        expected[1] = expected[1] - average[1];
-        expected[2] = expected[2] - average[2];
-        float deviation = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
-
-        // set errors
-        if (deviation < warning) {
-            AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
-        } else if (deviation < error) {
-            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
-        } else {
-            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
-        }
-    }
-}
-
-
 /**
  * Locally cache some variables from the AtttitudeSettings object
  */
@@ -520,9 +458,6 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
         Quaternion2R(rotationQuat, R);
         rotate = 1;
     }
-
-    RevoSettingsMagnetometerMaxDeviationGet(&magMaxDev);
-    HomeLocationBeGet(Be);
 }
 /**
  * @}

flight/modules/StateEstimation/filtermag.c

@@ -33,6 +33,7 @@
 #include "inc/stateestimation.h"
 #include <attitudestate.h>
 #include <revocalibration.h>
+#include <revosettings.h>
 #include <systemalarms.h>
 #include <homelocation.h>
 
@@ -46,6 +47,10 @@
 struct data {
     HomeLocationData    homeLocation;
     RevoCalibrationData revoCalibration;
+    RevoSettingsData    revoSettings;
+    uint16_t counter;
+    bool     validity;
+    float    magAverage[3];
     float    magBias[3];
 };
 
@@ -55,6 +60,7 @@ struct data {
 
 static int32_t init(stateFilter *self);
 static int32_t filter(stateFilter *self, stateEstimation *state);
+static bool checkMagValidity(struct data *this, float mag[3]);
 static void magOffsetEstimation(struct data *this, float mag[3]);
 
 
@@ -72,8 +78,12 @@ static int32_t init(stateFilter *self)
     struct data *this = (struct data *)self->localdata;
 
     this->magBias[0]    = this->magBias[1] = this->magBias[2] = 0.0f;
+    this->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f;
+    this->counter  = 0;
+    this->validity = true;
     HomeLocationGet(&this->homeLocation);
     RevoCalibrationGet(&this->revoCalibration);
+    RevoSettingsGet(&this->revoSettings);
     return 0;
 }
 
@@ -82,9 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     struct data *this = (struct data *)self->localdata;
 
     if (IS_SET(state->updated, SENSORUPDATES_mag)) {
-        SystemAlarmsAlarmData alarms;
+        if (!checkMagValidity(this, state->mag)) {
-        SystemAlarmsAlarGet(&alarms);
-        if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) {
             UNSET_MASK(state->updated, SENSORUPDATES_mag);
         } else {
             if (this->revoCalibration.MagBiasNullingRate > 0) {
@@ -96,6 +104,58 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     return 0;
 }
 
+/**
+ * check validity of magnetometers
+ */
+static bool checkMagValidity(struct data *this, float mag[3])
+{
+        #define MAG_LOW_PASS_ALPHA 0.05f
+        #define IDLE_COUNT         20
+
+    // low pass filter sensor to not give warnings due to noise
+    this->magAverage[0] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[0] + MAG_LOW_PASS_ALPHA * mag[0];
+    this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[0];
+    this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[0];
+
+    // throttle this check, thanks to low pass filter it is not necessary every iteration
+    if (!this->counter--) {
+        this->counter = IDLE_COUNT;
+
+        // calculate expected Be vector
+        AttitudeStateData attitudeState;
+        AttitudeStateGet(&attitudeState);
+        float Rot[3][3];
+        float expected[3];
+        Quaternion2R(&attitudeState.q1, Rot);
+        rot_mult(Rot, this->homeLocation.Be, expected);
+
+        // calculate maximum allowed deviation
+        float warning2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
+        float error2   = this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error * warning2;
+        warning2    = this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning * warning2;
+
+        // calculate difference
+        expected[0] = expected[0] - this->magAverage[0];
+        expected[1] = expected[1] - this->magAverage[1];
+        expected[2] = expected[2] - this->magAverage[2];
+        float deviation2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
+
+        // set errors
+        if (deviation2 < warning2) {
+            AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
+            this->validity = true;
+        } else if (deviation2 < error2) {
+            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
+            this->validity = false;
+        } else {
+            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
+            this->validity = false;
+        }
+    }
+    return this->validity;
+}
+
+
 /**
  * Perform an update of the @ref MagBias based on
  * Magmeter Offset Cancellation: Theory and Implementation,