OP-1406 - Improve handling of external magnetometer, add additional settings

flight/libraries/inc/CoordinateConversions.h

@@ -112,5 +112,73 @@ inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
     result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
 }
 
+inline void matrix_inline_scale_3f(float a[3][3], float scale)
+{
+    a[0][0] *= scale;
+    a[0][1] *= scale;
+    a[0][2] *= scale;
+
+    a[1][0] *= scale;
+    a[1][1] *= scale;
+    a[1][2] *= scale;
+
+    a[2][0] *= scale;
+    a[2][1] *= scale;
+    a[2][2] *= scale;
+}
+
+inline void rot_about_axis_x(const float rotation, float R[3][3])
+{
+    float s = sinf(rotation);
+    float c = cosf(rotation);
+
+    R[0][0] = 1;
+    R[0][1] = 0;
+    R[0][2] = 0;
+
+    R[1][0] = 0;
+    R[1][1] = c;
+    R[1][2] = -s;
+
+    R[2][0] = 0;
+    R[2][1] = s;
+    R[2][2] = c;
+}
+
+inline void rot_about_axis_y(const float rotation, float R[3][3])
+{
+    float s = sinf(rotation);
+    float c = cosf(rotation);
+
+    R[0][0] = c;
+    R[0][1] = 0;
+    R[0][2] = s;
+
+    R[1][0] = 0;
+    R[1][1] = 1;
+    R[1][2] = 0;
+
+    R[2][0] = -s;
+    R[2][1] = 0;
+    R[2][2] = c;
+}
+
+inline void rot_about_axis_z(const float rotation, float R[3][3])
+{
+    float s = sinf(rotation);
+    float c = cosf(rotation);
+
+    R[0][0] = c;
+    R[0][1] = -s;
+    R[0][2] = 0;
+
+    R[1][0] = s;
+    R[1][1] = c;
+    R[1][2] = 0;
+
+    R[2][0] = 0;
+    R[2][1] = 0;
+    R[2][2] = 1;
+}
 
 #endif // COORDINATECONVERSIONS_H_

flight/modules/StateEstimation/filtermag.c

@@ -36,7 +36,7 @@
 #include <revosettings.h>
 #include <systemalarms.h>
 #include <homelocation.h>
-
+#include <auxmagsettings.h>
 #include <CoordinateConversions.h>
 
 // Private constants
@@ -47,10 +47,12 @@
 struct data {
     RevoCalibrationData revoCalibration;
     RevoSettingsData    revoSettings;
+    AuxMagSettingsUsageOptions auxMagUsage;
     uint8_t warningcount;
     uint8_t errorcount;
     float   homeLocationBe[3];
-    float   magBe2;
+    float   magBe;
+    float   invMagBe;
     float   magBias[3];
 };
 
@@ -60,9 +62,9 @@ struct data {
 
 static int32_t init(stateFilter *self);
 static filterResult filter(stateFilter *self, stateEstimation *state);
-static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms);
+static bool checkMagValidity(struct data *this, float error, bool setAlarms);
-static void magOffsetEstimation(struct data *this, float mag[3]);
+// static void magOffsetEstimation(struct data *this, float mag[3]);
-
+static float getMagError(struct data *this, float mag[3]);
 
 int32_t filterMagInitialize(stateFilter *handle)
 {
@@ -80,105 +82,131 @@ static int32_t init(stateFilter *self)
     this->magBias[0]   = this->magBias[1] = this->magBias[2] = 0.0f;
     this->warningcount = this->errorcount = 0;
     HomeLocationBeGet(this->homeLocationBe);
-    // magBe2 holds the squared magnetic vector length (extpected)
+    // magBe 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];
+    this->magBe    = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2]);
+    this->invMagBe = 1.0f / this->magBe;
     RevoCalibrationGet(&this->revoCalibration);
     RevoSettingsGet(&this->revoSettings);
+    AuxMagSettingsUsageGet(&this->auxMagUsage);
     return 0;
 }
 
 static filterResult filter(stateFilter *self, stateEstimation *state)
 {
     struct data *this   = (struct data *)self->localdata;
+    float auxMagError;
+    float boardMagError;
+    float temp_mag[3]   = { 0 };
+    uint8_t temp_status = MAGSTATUS_INVALID;
+    uint8_t magSamples  = 0;
 
     // Uses the external mag when available
-    if (IS_SET(state->updated, SENSORUPDATES_auxMag) && checkMagValidity(this, state->auxMag, false)) {
+    if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
-        state->mag[0]    = state->auxMag[0];
+        IS_SET(state->updated, SENSORUPDATES_auxMag)) {
-        state->mag[1]    = state->auxMag[1];
+        auxMagError = getMagError(this, state->auxMag);
-        state->mag[2]    = state->auxMag[2];
+        // Handles alarms only if it will rely on aux mag only
-        state->magStatus = MAGSTATUS_AUX;
+        if (checkMagValidity(this, auxMagError,
-        return FILTERRESULT_OK;
+                             (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY))) {
-    }
+            temp_mag[0] = state->auxMag[0];
-
+            temp_mag[1] = state->auxMag[1];
-    // Defaults to board magnetometer
+            temp_mag[2] = state->auxMag[2];
-    state->mag[0]    = state->boardMag[0];
+            temp_status = MAGSTATUS_AUX;
-    state->mag[1]    = state->boardMag[1];
+            magSamples++;
-    state->mag[2]    = state->boardMag[2];
-    state->magStatus = MAGSTATUS_ONBOARD;
-    if (IS_SET(state->updated, SENSORUPDATES_mag)) {
-        // set alarm as aux sensor is invalid too
-        if (!checkMagValidity(this, state->mag, true)) {
-            state->magStatus = MAGSTATUS_INVALID;
-        }
-        if (this->revoCalibration.MagBiasNullingRate > 0) {
-            magOffsetEstimation(this, state->mag);
         }
     }
 
+    if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
+        IS_SET(state->updated, SENSORUPDATES_boardMag)) {
+        // TODO!
+        /*if (this->revoCalibration.MagBiasNullingRate > 0) {
+            magOffsetEstimation(this, state->boardMag);
+           }*/
+        boardMagError = getMagError(this, state->boardMag);
+        // sets warning only if no mag data are available (aux is invalid or missing)
+        if (checkMagValidity(this, boardMagError, temp_status == MAGSTATUS_INVALID)) {
+            temp_mag[0] += state->boardMag[0];
+            temp_mag[1] += state->boardMag[1];
+            temp_mag[2] += state->boardMag[2];
+            temp_status  = MAGSTATUS_OK;
+            magSamples++;
+        }
+    }
+
+    if (magSamples > 1) {
+        temp_mag[0] *= 0.5f;
+        temp_mag[1] *= 0.5f;
+        temp_mag[2] *= 0.5f;
+    }
+
+    if (temp_status != MAGSTATUS_INVALID) {
+        temp_mag[0]    *= this->invMagBe;
+        temp_mag[1]    *= this->invMagBe;
+        temp_mag[2]    *= this->invMagBe;
+        state->mag[0]   = temp_mag[0];
+        state->mag[1]   = temp_mag[1];
+        state->mag[2]   = temp_mag[2];
+        state->updated |= SENSORUPDATES_mag;
+    }
+    state->magStatus = temp_status;
     return FILTERRESULT_OK;
 }
 
 /**
  * check validity of magnetometers
  */
-static bool checkMagValidity(struct data *this, float mag[3], bool setAlarms)
+static bool checkMagValidity(struct data *this, float error, bool setAlarms)
 {
     #define ALARM_THRESHOLD 5
-    bool valid = true;
-    // 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) {
+    if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
         this->warningcount = 0;
         this->errorcount   = 0;
-        valid = true;
         AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
-    } else if (minError2 < mag2 && mag2 < maxError2) {
+        return true;
+    }
+
+    if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
         this->errorcount = 0;
         if (this->warningcount > ALARM_THRESHOLD) {
-            valid = false;
             if (setAlarms) {
                 AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
             }
+            return false;
         } else {
             this->warningcount++;
+            return true;
         }
-    } else {
+    }
+
     if (this->errorcount > ALARM_THRESHOLD) {
-            valid = false;
         if (setAlarms) {
             AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
         }
+        return false;
     } else {
         this->errorcount++;
     }
-    }
+    // still in "grace period"
-    return valid;
+    return true;
 }
 
+static float getMagError(struct data *this, float mag[3])
+{
+    // vector norm
+    float magnitude = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
+    // absolute value of relative error against Be
+    float error     = fabsf(magnitude - this->magBe) * this->invMagBe;
 
+    return error;
+}
+#if 0
 /**
  * Perform an update of the @ref MagBias based on
  * Magmeter Offset Cancellation: Theory and Implementation,
  * revisited William Premerlani, October 14, 2011
  */
-static void magOffsetEstimation(struct data *this, float mag[3])
+void magOffsetEstimation(struct data *this, float mag[3])
 {
 #if 0
     // Constants, to possibly go into a UAVO
@@ -269,7 +297,7 @@ static void magOffsetEstimation(struct data *this, float mag[3])
 #endif // if 0
 }
 
-
+#endif /* if 0 */
 /**
  * @}
  * @}

flight/modules/StateEstimation/inc/stateestimation.h

@@ -47,8 +47,8 @@ typedef enum {
     SENSORUPDATES_gyro         = 1 << 0,
         SENSORUPDATES_accel    = 1 << 1,
         SENSORUPDATES_mag      = 1 << 2,
-        SENSORUPDATES_auxMag   = 1 << 2,
+        SENSORUPDATES_boardMag = 1 << 10,
-        SENSORUPDATES_boardMag = 1 << 2,
+        SENSORUPDATES_auxMag   = 1 << 9,
         SENSORUPDATES_attitude = 1 << 3,
         SENSORUPDATES_pos      = 1 << 4,
         SENSORUPDATES_vel      = 1 << 5,
@@ -57,7 +57,7 @@ typedef enum {
         SENSORUPDATES_lla      = 1 << 8,
 } sensorUpdates;
 
-#define MAGSTATUS_ONBOARD 1
+#define MAGSTATUS_OK      1
 #define MAGSTATUS_AUX     2
 #define MAGSTATUS_INVALID 0
 typedef struct {

flight/modules/StateEstimation/stateestimation.c

@@ -292,6 +292,7 @@ int32_t StateEstimationInitialize(void)
     MagSensorConnectCallback(&sensorUpdatedCb);
     BaroSensorConnectCallback(&sensorUpdatedCb);
     AirspeedSensorConnectCallback(&sensorUpdatedCb);
+    AuxMagSensorConnectCallback(&sensorUpdatedCb);
     GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
     GPSPositionSensorConnectCallback(&sensorUpdatedCb);
 
@@ -470,8 +471,7 @@ static void StateEstimationCb(void)
             gyroDelta[2] = states.gyro[2] - gyroRaw[2];
         }
         EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
-
+        if (IS_SET(states.updated, SENSORUPDATES_mag)) {
-        {
             MagStateData s;
 
             MagStateGet(&s);
@@ -479,8 +479,8 @@ static void StateEstimationCb(void)
             s.y = states.mag[1];
             s.z = states.mag[2];
             switch (states.magStatus) {
-            case MAGSTATUS_ONBOARD:
+            case MAGSTATUS_OK:
-                s.Source = MAGSTATE_SOURCE_ONBOARD;
+                s.Source = MAGSTATE_SOURCE_OK;
                 break;
             case MAGSTATUS_AUX:
                 s.Source = MAGSTATE_SOURCE_AUX;
@@ -590,7 +590,11 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
     }
 
     if (ev->obj == MagSensorHandle()) {
-        updatedSensors |= SENSORUPDATES_mag;
+        updatedSensors |= SENSORUPDATES_boardMag;
+    }
+
+    if (ev->obj == AuxMagSensorHandle()) {
+        updatedSensors |= SENSORUPDATES_auxMag;
     }
 
     if (ev->obj == GPSPositionSensorHandle()) {

flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc

@@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
 UAVOBJSRCFILENAMES += auxmagsensor
-UAVOBJSRCFILENAMES += auxmagcalibration
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor

flight/targets/boards/revolution/firmware/UAVObjects.inc

@@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
 UAVOBJSRCFILENAMES += auxmagsensor
-UAVOBJSRCFILENAMES += auxmagcalibration
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor

flight/targets/boards/revoproto/firmware/UAVObjects.inc

@@ -32,7 +32,7 @@ UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
 UAVOBJSRCFILENAMES += auxmagsensor
-UAVOBJSRCFILENAMES += auxmagcalibration
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor

flight/targets/boards/simposix/firmware/UAVObjects.inc

@@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accelstate
 UAVOBJSRCFILENAMES += accelsensor
 UAVOBJSRCFILENAMES += magsensor
 UAVOBJSRCFILENAMES += auxmagsensor
-UAVOBJSRCFILENAMES += auxmagcalibration
+UAVOBJSRCFILENAMES += auxmagsettings
 UAVOBJSRCFILENAMES += magstate
 UAVOBJSRCFILENAMES += barosensor
 UAVOBJSRCFILENAMES += airspeedsensor

shared/uavobjectdefinition/auxmagsettings.xml

@@ -1,10 +1,13 @@
 <xml>
-    <object name="AuxMagCalibration" singleinstance="true" settings="true" category="Sensors">
+    <object name="AuxMagSettings" singleinstance="true" settings="true" category="Sensors">
         <description>Settings for auxiliary magnetometer calibration</description>
         <field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
         <field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
          defaultvalue="1,0,0,0,1,0,0,0,1"/>
         <field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
+        <field name="Orientation" units="degrees" type="float" elements="1" defaultvalue="0"/>
+        <field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext" defaultvalue="GPSV9"/>
+        <field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/magstate.xml

@@ -4,7 +4,7 @@
         <field name="x" units="mGa" type="float" elements="1"/>
         <field name="y" units="mGa" type="float" elements="1"/>
         <field name="z" units="mGa" type="float" elements="1"/>
-        <field name="Source" units="" type="enum" elements="1" options="Invalid,OnBoard,Aux" defaultvalue="Invalid"/>
+        <field name="Source" units="" type="enum" elements="1" options="Invalid,Ok,Aux" defaultvalue="Invalid"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>