OP-1161 Add alarm for magnetometer plausibility check

flight/modules/Sensors/sensors.c

@@ -55,6 +55,8 @@
 #include <attitudestate.h>
 #include <attitudesettings.h>
 #include <revocalibration.h>
+#include <revosettings.h>
+#include <homelocation.h>
 #include <flightstatus.h>
 #include <taskinfo.h>
 
@@ -73,6 +75,7 @@
 // Private functions
 static void SensorsTask(void *parameters);
 static void settingsUpdatedCb(UAVObjEvent *objEv);
+static void checkMagValidity(MagSensorData *mag);
 // static void magOffsetEstimation(MagSensorData *mag);
 
 // Private variables
@@ -92,6 +95,8 @@ static float R[3][3] = {
     { 0 }
 };
 static int8_t rotate = 0;
+static RevoSettingsMagnetometerMaxDeviationData magMaxDev;
+static float Be[3]   = { 0 };
 
 /**
  * API for sensor fusion algorithms:
@@ -112,12 +117,17 @@ int32_t SensorsInitialize(void)
     AccelSensorInitialize();
     MagSensorInitialize();
     RevoCalibrationInitialize();
+    RevoSettingsInitialize();
     AttitudeSettingsInitialize();
+    AttitudeStateInitialize();
+    HomeLocationInitialize();
 
     rotate = 0;
 
     RevoCalibrationConnectCallback(&settingsUpdatedCb);
     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
+    RevoSettingsConnectCallback(&settingsUpdatedCb);
+    HomeLocationConnectCallback(&settingsUpdatedCb);
 
     return 0;
 }
@@ -404,6 +414,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 
             MagSensorSet(&mag);
             mag_update_time = PIOS_DELAY_GetRaw();
+            checkMagValidity(&mag);
         }
 #endif /* if defined(PIOS_INCLUDE_HMC5883) */
 
@@ -415,6 +426,57 @@ 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
  */
@@ -458,6 +520,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
         Quaternion2R(rotationQuat, R);
         rotate = 1;
     }
+
+    RevoSettingsMagnetometerMaxDeviationGet(&magMaxDev);
+    HomeLocationBeGet(Be);
 }
 /**
  * @}

shared/uavobjectdefinition/revosettings.xml

@@ -8,6 +8,9 @@
         Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
         <field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
 
+	<!-- Configuration for magnetometer vector validity check -->
+	<field name="MagnetometerMaxDeviation" units="%" type="float" elementnames="Warning,Error" defaultvalue="0.2,0.5" />
+
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/systemalarms.xml

@@ -14,6 +14,7 @@
 			<elementname>Actuator</elementname>
 			<elementname>Attitude</elementname>
 			<elementname>Sensors</elementname>
+			<elementname>Magnetometer</elementname>
 			<elementname>Stabilization</elementname>
 			<elementname>Guidance</elementname>
 			<elementname>Battery</elementname>