flight/modules/StateEstimation/filtermag.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup State Estimation
   6  * @brief Acquires sensor data and computes state estimate
   7  * @{
   8  *
   9  * @file       filtermag.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Magnetometer drift compensation, uses previous cycles
  12  *             AttitudeState for estimation
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  ******************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 #include "inc/stateestimation.h"
  34 #include <attitudestate.h>
  35 #include <revocalibration.h>
  36 #include <revosettings.h>
  37 #include <systemalarms.h>
  38 #include <homelocation.h>
  39 #include <auxmagsettings.h>
  40 #include <CoordinateConversions.h>
  41 #include <mathmisc.h>
  42
  43 // Private constants
  44 //
  45 #define STACK_REQUIRED 256
  46
  47 // Private types
  48 struct data {
  49     RevoCalibrationData revoCalibration;
  50     RevoSettingsData    revoSettings;
  51     AuxMagSettingsUsageOptions auxMagUsage;
  52     uint8_t warningcount;
  53     uint8_t errorcount;
  54     float   homeLocationBe[3];
  55     float   magBe;
  56     float   invMagBe;
  57     float   magBias[3];
  58 };
  59
  60 // Private variables
  61
  62 // Private functions
  63
  64 static int32_t init(stateFilter *self);
  65 static filterResult filter(stateFilter *self, stateEstimation *state);
  66 static bool checkMagValidity(struct data *this, float error, bool setAlarms);
  67 static void magOffsetEstimation(struct data *this, float mag[3]);
  68 static float getMagError(struct data *this, float mag[3]);
  69
  70 int32_t filterMagInitialize(stateFilter *handle)
  71 {
  72     handle->init      = &init;
  73     handle->filter    = &filter;
  74     handle->localdata = pios_malloc(sizeof(struct data));
  75     return STACK_REQUIRED;
  76 }
  77
  78 static int32_t init(stateFilter *self)
  79 {
  80     struct data *this = (struct data *)self->localdata;
  81
  82     this->magBias[0]   = this->magBias[1] = this->magBias[2] = 0.0f;
  83     this->warningcount = this->errorcount = 0;
  84     HomeLocationBeGet(this->homeLocationBe);
  85     // magBe holds the magnetic vector length (expected)
  86     this->magBe    = vector_lengthf(this->homeLocationBe, 3);
  87     this->invMagBe = 1.0f / this->magBe;
  88     RevoCalibrationGet(&this->revoCalibration);
  89     RevoSettingsGet(&this->revoSettings);
  90     AuxMagSettingsUsageGet(&this->auxMagUsage);
  91     return 0;
  92 }
  93
  94 static filterResult filter(stateFilter *self, stateEstimation *state)
  95 {
  96     struct data *this   = (struct data *)self->localdata;
  97     float auxMagError;
  98     float boardMagError;
  99     float temp_mag[3]   = { 0 };
 100     uint8_t temp_status = MAGSTATUS_INVALID;
 101     uint8_t magSamples  = 0;
 102
 103     // Uses the external mag when available
 104     if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
 105         IS_SET(state->updated, SENSORUPDATES_auxMag)) {
 106         auxMagError = getMagError(this, state->auxMag);
 107         // Handles alarms only if it will rely on aux mag only
 108         bool auxMagValid = checkMagValidity(this, auxMagError, (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY));
 109         // if we are going to use Aux only, force the update even if mag is invalid
 110         if (auxMagValid || (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY)) {
 111             temp_mag[0] = state->auxMag[0];
 112             temp_mag[1] = state->auxMag[1];
 113             temp_mag[2] = state->auxMag[2];
 114             temp_status = MAGSTATUS_AUX;
 115             magSamples++;
 116         }
 117     }
 118
 119     if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
 120         IS_SET(state->updated, SENSORUPDATES_boardMag)) {
 121         // TODO:mag Offset estimation works with onboard mag only right now.
 122         if (this->revoCalibration.MagBiasNullingRate > 0) {
 123             magOffsetEstimation(this, state->boardMag);
 124         }
 125         boardMagError = getMagError(this, state->boardMag);
 126         // sets warning only if no mag data are available (aux is invalid or missing)
 127         bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
 128         // force it to be set to board mag value if no data has been feed to temp_mag yet.
 129         // this works also as a failsafe in case aux mag stops feeding data.
 130         if (boardMagValid || (temp_status == MAGSTATUS_INVALID)) {
 131             temp_mag[0] += state->boardMag[0];
 132             temp_mag[1] += state->boardMag[1];
 133             temp_mag[2] += state->boardMag[2];
 134             temp_status  = MAGSTATUS_OK;
 135             magSamples++;
 136         }
 137     }
 138
 139     if (magSamples > 1) {
 140         temp_mag[0] *= 0.5f;
 141         temp_mag[1] *= 0.5f;
 142         temp_mag[2] *= 0.5f;
 143     }
 144
 145     if (temp_status != MAGSTATUS_INVALID) {
 146         state->mag[0]   = temp_mag[0];
 147         state->mag[1]   = temp_mag[1];
 148         state->mag[2]   = temp_mag[2];
 149         state->updated |= SENSORUPDATES_mag;
 150     }
 151     state->magStatus = temp_status;
 152     return FILTERRESULT_OK;
 153 }
 154
 155 /**
 156  * check validity of magnetometers
 157  */
 158 static bool checkMagValidity(struct data *this, float error, bool setAlarms)
 159 {
 160     #define ALARM_THRESHOLD 5
 161
 162     // set errors
 163     if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
 164         this->warningcount = 0;
 165         this->errorcount   = 0;
 166         AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
 167         return true;
 168     }
 169
 170     if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
 171         this->errorcount = 0;
 172         if (this->warningcount > ALARM_THRESHOLD) {
 173             if (setAlarms) {
 174                 AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
 175             }
 176             return false;
 177         } else {
 178             this->warningcount++;
 179             return true;
 180         }
 181     }
 182
 183     if (this->errorcount > ALARM_THRESHOLD) {
 184         if (setAlarms) {
 185             AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
 186         }
 187         return false;
 188     } else {
 189         this->errorcount++;
 190     }
 191     // still in "grace period"
 192     return true;
 193 }
 194
 195 static float getMagError(struct data *this, float mag[3])
 196 {
 197     // vector norm
 198     float magnitude = vector_lengthf(mag, 3);
 199     // absolute value of relative error against Be
 200     float error     = fabsf(magnitude - this->magBe) * this->invMagBe;
 201
 202     return error;
 203 }
 204
 205 /**
 206  * Perform an update of the @ref MagBias based on
 207  * Magmeter Offset Cancellation: Theory and Implementation,
 208  * revisited William Premerlani, October 14, 2011
 209  */
 210 void magOffsetEstimation(struct data *this, float mag[3])
 211 {
 212 #if 0
 213     // Constants, to possibly go into a UAVO
 214     static const float MIN_NORM_DIFFERENCE = 50;
 215
 216     static float B2[3] = { 0, 0, 0 };
 217
 218     MagBiasData magBias;
 219     MagBiasGet(&magBias);
 220
 221     // Remove the current estimate of the bias
 222     mag->x -= magBias.x;
 223     mag->y -= magBias.y;
 224     mag->z -= magBias.z;
 225
 226     // First call
 227     if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
 228         B2[0] = mag->x;
 229         B2[1] = mag->y;
 230         B2[2] = mag->z;
 231         return;
 232     }
 233
 234     float B1[3]     = { mag->x, mag->y, mag->z };
 235     float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
 236     if (norm_diff > MIN_NORM_DIFFERENCE) {
 237         float norm_b1    = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
 238         float norm_b2    = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
 239         float scale      = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
 240         float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
 241
 242         magBias.x += b_error[0];
 243         magBias.y += b_error[1];
 244         magBias.z += b_error[2];
 245
 246         MagBiasSet(&magBias);
 247
 248         // Store this value to compare against next update
 249         B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
 250     }
 251 #else // if 0
 252
 253     const float Rxy  = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
 254     const float Rz   = this->homeLocationBe[2];
 255
 256     const float rate = this->revoCalibration.MagBiasNullingRate;
 257     float Rot[3][3];
 258     float B_e[3];
 259     float xy[2];
 260     float delta[3];
 261
 262     AttitudeStateData attitude;
 263     AttitudeStateGet(&attitude);
 264
 265     // Get the rotation matrix
 266     Quaternion2R(&attitude.q1, Rot);
 267
 268     // Rotate the mag into the NED frame
 269     B_e[0] = Rot[0][0] * mag[0] + Rot[1][0] * mag[1] + Rot[2][0] * mag[2];
 270     B_e[1] = Rot[0][1] * mag[0] + Rot[1][1] * mag[1] + Rot[2][1] * mag[2];
 271     B_e[2] = Rot[0][2] * mag[0] + Rot[1][2] * mag[1] + Rot[2][2] * mag[2];
 272
 273     float cy = cosf(DEG2RAD(attitude.Yaw));
 274     float sy = sinf(DEG2RAD(attitude.Yaw));
 275
 276     xy[0] = cy * B_e[0] + sy * B_e[1];
 277     xy[1] = -sy * B_e[0] + cy * B_e[1];
 278
 279     float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
 280
 281     delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
 282     delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
 283     delta[2] = -rate * (Rz - B_e[2]);
 284
 285     if (!isnan(delta[0]) && !isinf(delta[0]) &&
 286         !isnan(delta[1]) && !isinf(delta[1]) &&
 287         !isnan(delta[2]) && !isinf(delta[2])) {
 288         this->magBias[0] += delta[0];
 289         this->magBias[1] += delta[1];
 290         this->magBias[2] += delta[2];
 291     }
 292
 293     // Add bias to state estimation
 294     mag[0] += this->magBias[0];
 295     mag[1] += this->magBias[1];
 296     mag[2] += this->magBias[2];
 297
 298 #endif // if 0
 299 }
 300
 301 /**
 302  * @}
 303  * @}
 304  */