Merge remote-tracking branch 'origin/corvuscorax/OP-1161_magnetometer-alarm' into next

flight/modules/StateEstimation/filterekf.c

@@ -36,6 +36,7 @@
 #include <ekfconfiguration.h>
 #include <ekfstatevariance.h>
 #include <attitudestate.h>
+#include <systemalarms.h>
 #include <homelocation.h>
 
 #include <insgps.h>
@@ -348,7 +349,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     INSCovariancePrediction(dT);
 
     if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
-        sensors |= MAG_SENSORS;
+        SystemAlarmsAlarmData alarms;
+        SystemAlarmsAlarmGet(&alarms);
+        if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) {
+            sensors |= MAG_SENSORS;
+        }
     }
 
     if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {

flight/modules/StateEstimation/filtermag.c

@@ -33,6 +33,8 @@
 #include "inc/stateestimation.h"
 #include <attitudestate.h>
 #include <revocalibration.h>
+#include <revosettings.h>
+#include <systemalarms.h>
 #include <homelocation.h>
 
 #include <CoordinateConversions.h>
@@ -43,9 +45,13 @@
 
 // Private types
 struct data {
-    HomeLocationData    homeLocation;
     RevoCalibrationData revoCalibration;
-    float magBias[3];
+    RevoSettingsData    revoSettings;
+    uint8_t warningcount;
+    uint8_t errorcount;
+    float   homeLocationBe[3];
+    float   magBe2;
+    float   magBias[3];
 };
 
 // Private variables
@@ -54,6 +60,7 @@ struct data {
 
 static int32_t init(stateFilter *self);
 static int32_t filter(stateFilter *self, stateEstimation *state);
+static void checkMagValidity(struct data *this, float mag[3]);
 static void magOffsetEstimation(struct data *this, float mag[3]);
 
 
@@ -70,9 +77,13 @@ 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->magBias[0]   = this->magBias[1] = this->magBias[2] = 0.0f;
-    HomeLocationGet(&this->homeLocation);
+    this->warningcount = this->errorcount = 0;
+    HomeLocationBeGet(this->homeLocationBe);
+    // magBe2 holds the squared magnetic vector length (extpected)
+    this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
     RevoCalibrationGet(&this->revoCalibration);
+    RevoSettingsGet(&this->revoSettings);
     return 0;
 }
 
@@ -81,6 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     struct data *this = (struct data *)self->localdata;
 
     if (IS_SET(state->updated, SENSORUPDATES_mag)) {
+        checkMagValidity(this, state->mag);
         if (this->revoCalibration.MagBiasNullingRate > 0) {
             magOffsetEstimation(this, state->mag);
         }
@@ -89,6 +101,53 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     return 0;
 }
 
+/**
+ * check validity of magnetometers
+ */
+static void checkMagValidity(struct data *this, float mag[3])
+{
+        #define ALARM_THRESHOLD 5
+
+    // mag2 holds the actual magnetic vector length (squared)
+    float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
+
+    // warning and error thresholds
+    // avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
+    //
+    // actual = |mag|
+    // minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
+    // maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
+    // minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
+    // maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
+    //
+
+    float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
+    float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
+    float minError2   = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
+    float maxError2   = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
+
+    // set errors
+    if (minWarning2 < mag2 && mag2 < maxWarning2) {
+        this->warningcount = 0;
+        this->errorcount   = 0;
+        AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
+    } else if (minError2 < mag2 && mag2 < maxError2) {
+        this->errorcount = 0;
+        if (this->warningcount > ALARM_THRESHOLD) {
+            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
+        } else {
+            this->warningcount++;
+        }
+    } else {
+        if (this->errorcount > ALARM_THRESHOLD) {
+            AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
+        } else {
+            this->errorcount++;
+        }
+    }
+}
+
+
 /**
  * Perform an update of the @ref MagBias based on
  * Magmeter Offset Cancellation: Theory and Implementation,
@@ -137,8 +196,8 @@ static void magOffsetEstimation(struct data *this, float mag[3])
     }
 #else // if 0
 
-    const float Rxy  = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
+    const float Rxy  = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
-    const float Rz   = this->homeLocation.Be[2];
+    const float Rz   = this->homeLocationBe[2];
 
     const float rate = this->revoCalibration.MagBiasNullingRate;
     float Rot[3][3];

flight/modules/StateEstimation/stateestimation.c

@@ -161,17 +161,14 @@ static float gyroDelta[3];
 
 // preconfigured filter chains selectable via revoSettings.FusionAlgorithm
 static const filterPipeline *cfQueue = &(filterPipeline) {
-    .filter = &magFilter,
+    .filter = &airFilter,
     .next   = &(filterPipeline) {
-        .filter = &airFilter,
+        .filter = &baroiFilter,
         .next   = &(filterPipeline) {
-            .filter = &baroiFilter,
+            .filter = &altitudeFilter,
             .next   = &(filterPipeline) {
-                .filter = &altitudeFilter,
+                .filter = &cfFilter,
-                .next   = &(filterPipeline) {
+                .next   = NULL,
-                    .filter = &cfFilter,
-                    .next   = NULL,
-                }
             }
         }
     }

ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg

@@ -27,10 +27,10 @@
      borderopacity="1.0"
      inkscape:pageopacity="0.0"
      inkscape:pageshadow="2"
-     inkscape:zoom="7.4629556"
+     inkscape:zoom="29.851822"
-     inkscape:cx="32.393349"
+     inkscape:cx="82.604436"
-     inkscape:cy="37.450437"
+     inkscape:cy="54.252252"
-     inkscape:current-layer="foreground"
+     inkscape:current-layer="background"
      id="namedview3608"
      showgrid="true"
      inkscape:window-width="1366"
@@ -1002,12 +1002,13 @@
        inkscape:label="#rect4550-8-1-4-21-5-13" />
     <rect
        style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
-       id="rect4550-8-1-4-21-5-8"
+       id="Magnetometer"
        width="18.966711"
        height="10.202584"
        x="570.72327"
        y="361.65591"
-       ry="1" />
+       ry="1"
+       inkscape:label="#rect4550-8-1-4-21-5-8" />
     <rect
        style="fill:#241c1c;fill-opacity:1;stroke:none;display:inline"
        id="Battery"
@@ -1123,6 +1124,21 @@
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer42"
+     inkscape:label="Magnetometer-OK"
+     style="display:none">
+    <rect
+       style="fill:#008000;fill-opacity:1;stroke:none;display:inline"
+       id="Magnetometer-OK"
+       width="18.966711"
+       height="10.202584"
+       x="73.057671"
+       y="17.391815"
+       ry="1"
+       inkscape:label="#rect4550-8-1-4-21-5-33" />
+  </g>
   <g
      inkscape:groupmode="layer"
      id="layer10"
@@ -1399,7 +1415,7 @@
      inkscape:groupmode="layer"
      id="g5044"
      inkscape:label="Airspeed-Warning"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
        id="Airspeed-Warning"
@@ -1410,6 +1426,21 @@
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer41"
+     inkscape:label="Magnetometer-Warning"
+     style="display:none">
+    <rect
+       style="fill:#ff6600;fill-opacity:1;stroke:none;display:inline"
+       id="Magnetometer-Warning"
+       width="18.966711"
+       height="10.202584"
+       x="73.091171"
+       y="17.425316"
+       ry="1"
+       inkscape:label="#rect4550-8-1-4-21-5-33" />
+  </g>
   <g
      inkscape:groupmode="layer"
      id="g5047"
@@ -1686,7 +1717,7 @@
      inkscape:groupmode="layer"
      id="g5420"
      inkscape:label="Airspeed-Critical"
-     style="display:inline">
+     style="display:none">
     <rect
        style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
        id="Airspeed-Critical"
@@ -1697,6 +1728,21 @@
        ry="1"
        inkscape:label="#rect4550-8-1-4-21-5-33" />
   </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer40"
+     inkscape:label="Magnetometer-Critical"
+     style="display:none">
+    <rect
+       style="fill:#d40000;fill-opacity:1;stroke:none;display:inline"
+       id="Magnetometer-Critical"
+       width="18.966711"
+       height="10.202584"
+       x="73.02417"
+       y="17.391815"
+       ry="1"
+       inkscape:label="#rect4550-8-1-4-21-5-33" />
+  </g>
   <g
      inkscape:groupmode="layer"
      id="g5423"
@@ -2055,6 +2101,28 @@
          style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
     </g>
   </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer4"
+     inkscape:label="Magnetometer-Error"
+     style="display:none">
+    <g
+       style="display:inline"
+       id="Magnetometer-Error"
+       inkscape:label="#g3398"
+       transform="matrix(1.0309004,0,0,1,20.190366,0.03602308)">
+      <path
+         inkscape:connector-curvature="0"
+         id="path6233-9-9-79-9"
+         d="m 51.59088,18.071902 18.268111,8.770366"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19476616;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+      <path
+         inkscape:connector-curvature="0"
+         id="path6233-9-9-4-3-4"
+         d="M 51.580734,26.818576 69.902677,18.119165"
+         style="fill:#ff0000;stroke:#ff0000;stroke-width:1.19167542;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" />
+    </g>
+  </g>
   <g
      inkscape:groupmode="layer"
      id="layer2"
@@ -2450,15 +2518,15 @@
     <text
        xml:space="preserve"
        style="font-size:4px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"
-       x="93.336571"
+       x="94.605125"
        y="20.116049"
        id="text5330-7-7-8"
        sodipodi:linespacing="125%"
        transform="scale(0.83127393,1.2029729)"><tspan
          sodipodi:role="line"
-         id="tspan4295"
+         id="tspan3835"
-         x="93.336571"
+         x="94.605125"
-         y="20.116049">BARO</tspan></text>
+         y="20.116049">MAG</tspan></text>
     <text
        xml:space="preserve"
        style="font-size:3px;font-style:normal;font-variant:normal;font-weight:bold;font-stretch:normal;text-align:start;line-height:125%;letter-spacing:0px;word-spacing:0px;writing-mode:lr-tb;text-anchor:start;fill:#ffffff;fill-opacity:1;stroke:none;display:inline;font-family:Arial;-inkscape-font-specification:Arial Bold"

shared/uavobjectdefinition/revosettings.xml

@@ -7,6 +7,10 @@
         Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
         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.05,0.15" />
+
         <!--  Cooefficients for the polynomial that models the barometer pressure bias as a function of temperature 
               bias = a + b * temp + c * temp^2 + d * temp^3 --> 
         <field name="BaroTempCorrectionPolynomial" units="" type="float" elements="4" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>

shared/uavobjectdefinition/systemalarms.xml

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