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     HomeLocationInitialize();
  76     return STACK_REQUIRED;
  77 }
  78
  79 static int32_t init(stateFilter *self)
  80 {
  81     struct data *this = (struct data *)self->localdata;
  82
  83     this->magBias[0]   = this->magBias[1] = this->magBias[2] = 0.0f;
  84     this->warningcount = this->errorcount = 0;
  85     HomeLocationBeGet(this->homeLocationBe);
  86     // magBe holds the magnetic vector length (expected)
  87     this->magBe    = vector_lengthf(this->homeLocationBe, 3);
  88     this->invMagBe = 1.0f / this->magBe;
  89     RevoCalibrationGet(&this->revoCalibration);
  90     RevoSettingsGet(&this->revoSettings);
  91     AuxMagSettingsUsageGet(&this->auxMagUsage);
  92     return 0;
  93 }
  94
  95 static filterResult filter(stateFilter *self, stateEstimation *state)
  96 {
  97     struct data *this   = (struct data *)self->localdata;
  98     float auxMagError;
  99     float boardMagError;
 100     float temp_mag[3]   = { 0 };
 101     uint8_t temp_status = MAGSTATUS_INVALID;
 102     uint8_t magSamples  = 0;
 103
 104     // Uses the external mag when available
 105     if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_ONBOARDONLY) &&
 106         IS_SET(state->updated, SENSORUPDATES_auxMag)) {
 107         auxMagError = getMagError(this, state->auxMag);
 108         // Handles alarms only if it will rely on aux mag only
 109         bool auxMagValid = checkMagValidity(this, auxMagError, (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY));
 110         // if we are going to use Aux only, force the update even if mag is invalid
 111         if (auxMagValid || (this->auxMagUsage == AUXMAGSETTINGS_USAGE_AUXONLY)) {
 112             temp_mag[0] = state->auxMag[0];
 113             temp_mag[1] = state->auxMag[1];
 114             temp_mag[2] = state->auxMag[2];
 115             temp_status = MAGSTATUS_AUX;
 116             magSamples++;
 117         }
 118     }
 119
 120     if ((this->auxMagUsage != AUXMAGSETTINGS_USAGE_AUXONLY) &&
 121         IS_SET(state->updated, SENSORUPDATES_boardMag)) {
 122         // TODO:mag Offset estimation works with onboard mag only right now.
 123         if (this->revoCalibration.MagBiasNullingRate > 0) {
 124             magOffsetEstimation(this, state->boardMag);
 125         }
 126         boardMagError = getMagError(this, state->boardMag);
 127         // sets warning only if no mag data are available (aux is invalid or missing)
 128         bool boardMagValid = checkMagValidity(this, boardMagError, (temp_status == MAGSTATUS_INVALID));
 129         // force it to be set to board mag value if no data has been feed to temp_mag yet.
 130         // this works also as a failsafe in case aux mag stops feeding data.
 131         if (boardMagValid || (temp_status == MAGSTATUS_INVALID)) {
 132             temp_mag[0] += state->boardMag[0];
 133             temp_mag[1] += state->boardMag[1];
 134             temp_mag[2] += state->boardMag[2];
 135             temp_status  = MAGSTATUS_OK;
 136             magSamples++;
 137         }
 138     }
 139
 140     if (magSamples > 1) {
 141         temp_mag[0] *= 0.5f;
 142         temp_mag[1] *= 0.5f;
 143         temp_mag[2] *= 0.5f;
 144     }
 145
 146     if (temp_status != MAGSTATUS_INVALID) {
 147         state->mag[0]   = temp_mag[0];
 148         state->mag[1]   = temp_mag[1];
 149         state->mag[2]   = temp_mag[2];
 150         state->updated |= SENSORUPDATES_mag;
 151     }
 152     state->magStatus = temp_status;
 153     return FILTERRESULT_OK;
 154 }
 155
 156 /**
 157  * check validity of magnetometers
 158  */
 159 static bool checkMagValidity(struct data *this, float error, bool setAlarms)
 160 {
 161     #define ALARM_THRESHOLD 5
 162
 163     // set errors
 164     if (error < this->revoSettings.MagnetometerMaxDeviation.Warning) {
 165         this->warningcount = 0;
 166         this->errorcount   = 0;
 167         AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
 168         return true;
 169     }
 170
 171     if (error < this->revoSettings.MagnetometerMaxDeviation.Error) {
 172         this->errorcount = 0;
 173         if (this->warningcount > ALARM_THRESHOLD) {
 174             if (setAlarms) {
 175                 AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
 176             }
 177             return false;
 178         } else {
 179             this->warningcount++;
 180             return true;
 181         }
 182     }
 183
 184     if (this->errorcount > ALARM_THRESHOLD) {
 185         if (setAlarms) {
 186             AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
 187         }
 188         return false;
 189     } else {
 190         this->errorcount++;
 191     }
 192     // still in "grace period"
 193     return true;
 194 }
 195
 196 static float getMagError(struct data *this, float mag[3])
 197 {
 198     // vector norm
 199     float magnitude = vector_lengthf(mag, 3);
 200     // absolute value of relative error against Be
 201     float error     = fabsf(magnitude - this->magBe) * this->invMagBe;
 202
 203     return error;
 204 }
 205
 206 /**
 207  * Perform an update of the @ref MagBias based on
 208  * Magmeter Offset Cancellation: Theory and Implementation,
 209  * revisited William Premerlani, October 14, 2011
 210  */
 211 void magOffsetEstimation(struct data *this, float mag[3])
 212 {
 213 #if 0
 214     // Constants, to possibly go into a UAVO
 215     static const float MIN_NORM_DIFFERENCE = 50;
 216
 217     static float B2[3] = { 0, 0, 0 };
 218
 219     MagBiasData magBias;
 220     MagBiasGet(&magBias);
 221
 222     // Remove the current estimate of the bias
 223     mag->x -= magBias.x;
 224     mag->y -= magBias.y;
 225     mag->z -= magBias.z;
 226
 227     // First call
 228     if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
 229         B2[0] = mag->x;
 230         B2[1] = mag->y;
 231         B2[2] = mag->z;
 232         return;
 233     }
 234
 235     float B1[3]     = { mag->x, mag->y, mag->z };
 236     float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
 237     if (norm_diff > MIN_NORM_DIFFERENCE) {
 238         float norm_b1    = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
 239         float norm_b2    = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
 240         float scale      = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
 241         float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
 242
 243         magBias.x += b_error[0];
 244         magBias.y += b_error[1];
 245         magBias.z += b_error[2];
 246
 247         MagBiasSet(&magBias);
 248
 249         // Store this value to compare against next update
 250         B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
 251     }
 252 #else // if 0
 253
 254     const float Rxy  = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
 255     const float Rz   = this->homeLocationBe[2];
 256
 257     const float rate = this->revoCalibration.MagBiasNullingRate;
 258     float Rot[3][3];
 259     float B_e[3];
 260     float xy[2];
 261     float delta[3];
 262
 263     AttitudeStateData attitude;
 264     AttitudeStateGet(&attitude);
 265
 266     // Get the rotation matrix
 267     Quaternion2R(&attitude.q1, Rot);
 268
 269     // Rotate the mag into the NED frame
 270     B_e[0] = Rot[0][0] * mag[0] + Rot[1][0] * mag[1] + Rot[2][0] * mag[2];
 271     B_e[1] = Rot[0][1] * mag[0] + Rot[1][1] * mag[1] + Rot[2][1] * mag[2];
 272     B_e[2] = Rot[0][2] * mag[0] + Rot[1][2] * mag[1] + Rot[2][2] * mag[2];
 273
 274     float cy = cosf(DEG2RAD(attitude.Yaw));
 275     float sy = sinf(DEG2RAD(attitude.Yaw));
 276
 277     xy[0] = cy * B_e[0] + sy * B_e[1];
 278     xy[1] = -sy * B_e[0] + cy * B_e[1];
 279
 280     float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
 281
 282     delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
 283     delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
 284     delta[2] = -rate * (Rz - B_e[2]);
 285
 286     if (!isnan(delta[0]) && !isinf(delta[0]) &&
 287         !isnan(delta[1]) && !isinf(delta[1]) &&
 288         !isnan(delta[2]) && !isinf(delta[2])) {
 289         this->magBias[0] += delta[0];
 290         this->magBias[1] += delta[1];
 291         this->magBias[2] += delta[2];
 292     }
 293
 294     // Add bias to state estimation
 295     mag[0] += this->magBias[0];
 296     mag[1] += this->magBias[1];
 297     mag[2] += this->magBias[2];
 298
 299 #endif // if 0
 300 }
 301
 302 /**
 303  * @}
 304  * @}
 305  */