flight/modules/StateEstimation/filtercf.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       filtercf.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Complementary filter to calculate Attitude from Accels and Gyros
  12  *             and optionally magnetometers:
  13  *             WARNING: Will drift if the mean acceleration force doesn't point
  14  *             to ground, unsafe on fixed wing, since position hold is
  15  *             implemented as continuous circular flying!
  16  *
  17  * @see        The GNU Public License (GPL) Version 3
  18  *
  19  ******************************************************************************/
  20 /*
  21  * This program is free software; you can redistribute it and/or modify
  22  * it under the terms of the GNU General Public License as published by
  23  * the Free Software Foundation; either version 3 of the License, or
  24  * (at your option) any later version.
  25  *
  26  * This program is distributed in the hope that it will be useful, but
  27  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  28  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  29  * for more details.
  30  *
  31  * You should have received a copy of the GNU General Public License along
  32  * with this program; if not, write to the Free Software Foundation, Inc.,
  33  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  34  */
  35
  36 #include "inc/stateestimation.h"
  37
  38 #include <attitudesettings.h>
  39 #include <attitudestate.h>
  40 #include <flightstatus.h>
  41 #include <homelocation.h>
  42 #include <revocalibration.h>
  43
  44 #include <CoordinateConversions.h>
  45 #include <pios_notify.h>
  46 // Private constants
  47
  48 #define STACK_REQUIRED          512
  49
  50 #define CALIBRATION_DELAY_MS    4000
  51 #define CALIBRATION_DURATION_MS 6000
  52
  53 // Private types
  54 struct data {
  55     AttitudeSettingsData attitudeSettings;
  56     HomeLocationData     homeLocation;
  57     bool    first_run;
  58     bool    useMag;
  59     float   currentAccel[3];
  60     float   currentMag[3];
  61     float   accels_filtered[3];
  62     float   grot_filtered[3];
  63     float   gyroBias[3];
  64     bool    accelUpdated;
  65     bool    magUpdated;
  66     float   accel_alpha;
  67     bool    accel_filter_enabled;
  68     float   rollPitchBiasRate;
  69     int32_t timeval;
  70     int32_t starttime;
  71     uint8_t init;
  72     bool    magCalibrated;
  73 };
  74
  75 // Private variables
  76 bool initialized = 0;
  77 static FlightStatusData flightStatus;
  78
  79 // Private functions
  80
  81 static int32_t initwithmag(stateFilter *self);
  82 static int32_t initwithoutmag(stateFilter *self);
  83 static int32_t maininit(stateFilter *self);
  84 static filterResult filter(stateFilter *self, stateEstimation *state);
  85 static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
  86
  87 static void flightStatusUpdatedCb(UAVObjEvent *ev);
  88
  89 static void globalInit(void);
  90
  91
  92 static void globalInit(void)
  93 {
  94     if (!initialized) {
  95         initialized = 1;
  96         FlightStatusInitialize();
  97         HomeLocationInitialize();
  98         RevoCalibrationInitialize();
  99         FlightStatusConnectCallback(&flightStatusUpdatedCb);
 100         flightStatusUpdatedCb(NULL);
 101     }
 102 }
 103
 104 int32_t filterCFInitialize(stateFilter *handle)
 105 {
 106     globalInit();
 107     handle->init      = &initwithoutmag;
 108     handle->filter    = &filter;
 109     handle->localdata = pios_malloc(sizeof(struct data));
 110     return STACK_REQUIRED;
 111 }
 112
 113 int32_t filterCFMInitialize(stateFilter *handle)
 114 {
 115     globalInit();
 116     handle->init      = &initwithmag;
 117     handle->filter    = &filter;
 118     handle->localdata = pios_malloc(sizeof(struct data));
 119     return STACK_REQUIRED;
 120 }
 121
 122 static int32_t initwithmag(stateFilter *self)
 123 {
 124     struct data *this = (struct data *)self->localdata;
 125
 126     this->useMag = 1;
 127     return maininit(self);
 128 }
 129
 130 static int32_t initwithoutmag(stateFilter *self)
 131 {
 132     struct data *this = (struct data *)self->localdata;
 133
 134     this->useMag = 0;
 135     return maininit(self);
 136 }
 137
 138 static int32_t maininit(stateFilter *self)
 139 {
 140     struct data *this = (struct data *)self->localdata;
 141
 142     this->first_run     = 1;
 143     this->accelUpdated  = 0;
 144     this->magCalibrated = true;
 145     AttitudeSettingsGet(&this->attitudeSettings);
 146     HomeLocationGet(&this->homeLocation);
 147
 148     const float fakeDt = 0.0025f;
 149     if (this->attitudeSettings.AccelTau < 0.0001f) {
 150         this->accel_alpha = 0; // not trusting this to resolve to 0
 151         this->accel_filter_enabled = false;
 152     } else {
 153         this->accel_alpha = expf(-fakeDt / this->attitudeSettings.AccelTau);
 154         this->accel_filter_enabled = true;
 155     }
 156
 157     // reset gyro Bias
 158     this->gyroBias[0] = 0.0f;
 159     this->gyroBias[1] = 0.0f;
 160     this->gyroBias[2] = 0.0f;
 161
 162     return 0;
 163 }
 164
 165 /**
 166  * Collect all required state variables, then run complementary filter
 167  */
 168 static filterResult filter(stateFilter *self, stateEstimation *state)
 169 {
 170     struct data *this   = (struct data *)self->localdata;
 171
 172     filterResult result = FILTERRESULT_OK;
 173
 174     if (IS_SET(state->updated, SENSORUPDATES_mag)) {
 175         this->magUpdated    = 1;
 176         this->currentMag[0] = state->mag[0];
 177         this->currentMag[1] = state->mag[1];
 178         this->currentMag[2] = state->mag[2];
 179     }
 180     if (IS_SET(state->updated, SENSORUPDATES_accel)) {
 181         this->accelUpdated    = 1;
 182         this->currentAccel[0] = state->accel[0];
 183         this->currentAccel[1] = state->accel[1];
 184         this->currentAccel[2] = state->accel[2];
 185     }
 186     if (IS_SET(state->updated, SENSORUPDATES_gyro)) {
 187         if (this->accelUpdated) {
 188             float attitude[4];
 189             result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
 190             if (result == FILTERRESULT_OK) {
 191                 state->attitude[0] = attitude[0];
 192                 state->attitude[1] = attitude[1];
 193                 state->attitude[2] = attitude[2];
 194                 state->attitude[3] = attitude[3];
 195                 state->updated    |= SENSORUPDATES_attitude;
 196             }
 197             this->accelUpdated = 0;
 198             this->magUpdated   = 0;
 199         }
 200     }
 201     return result;
 202 }
 203
 204
 205 static inline void apply_accel_filter(const struct data *this, const float *raw, float *filtered)
 206 {
 207     if (this->accel_filter_enabled) {
 208         filtered[0] = filtered[0] * this->accel_alpha + raw[0] * (1 - this->accel_alpha);
 209         filtered[1] = filtered[1] * this->accel_alpha + raw[1] * (1 - this->accel_alpha);
 210         filtered[2] = filtered[2] * this->accel_alpha + raw[2] * (1 - this->accel_alpha);
 211     } else {
 212         filtered[0] = raw[0];
 213         filtered[1] = raw[1];
 214         filtered[2] = raw[2];
 215     }
 216 }
 217
 218 static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
 219 {
 220     float dT;
 221
 222     // During initialization and
 223     if (this->first_run) {
 224 #if defined(PIOS_INCLUDE_HMC5X83)
 225         // wait until mags have been updated
 226         if (!this->magUpdated && this->useMag) {
 227             return FILTERRESULT_ERROR;
 228         }
 229 #else
 230         mag[0] = 100.0f;
 231         mag[1] = 0.0f;
 232         mag[2] = 0.0f;
 233 #endif
 234         float magBias[3];
 235         RevoCalibrationmag_biasArrayGet(magBias);
 236         // don't trust Mag for initial orientation if it has not been calibrated
 237         if (fabsf(magBias[0]) < 1e-6f && fabsf(magBias[1]) < 1e-6f && fabsf(magBias[2]) < 1e-6f) {
 238             this->magCalibrated = false;
 239             mag[0] = 100.0f;
 240             mag[1] = 0.0f;
 241             mag[2] = 0.0f;
 242         }
 243
 244         AttitudeStateData attitudeState; // base on previous state
 245         AttitudeStateGet(&attitudeState);
 246         this->init = 0;
 247
 248         // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
 249         // so pseudo "north" vector can be estimated even if the board is not level
 250         attitudeState.Roll = atan2f(-accel[1], -accel[2]);
 251         float zn  = cosf(attitudeState.Roll) * mag[2] + sinf(attitudeState.Roll) * mag[1];
 252         float yn  = cosf(attitudeState.Roll) * mag[1] - sinf(attitudeState.Roll) * mag[2];
 253
 254         // rotate accels z vector according to roll
 255         float azn = cosf(attitudeState.Roll) * accel[2] + sinf(attitudeState.Roll) * accel[1];
 256         attitudeState.Pitch = atan2f(accel[0], -azn);
 257
 258         float xn  = cosf(attitudeState.Pitch) * mag[0] + sinf(attitudeState.Pitch) * zn;
 259
 260         attitudeState.Yaw = atan2f(-yn, xn);
 261         // TODO: This is still a hack
 262         // Put this in a proper generic function in CoordinateConversion.c
 263         // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
 264         // should calculate the rotation in 3d space using proper cross product math
 265         // SUBTODO: formulate the math required
 266
 267         attitudeState.Roll  = RAD2DEG(attitudeState.Roll);
 268         attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
 269         attitudeState.Yaw   = RAD2DEG(attitudeState.Yaw);
 270
 271         RPY2Quaternion(&attitudeState.Roll, attitude);
 272
 273         this->first_run = 0;
 274         this->accels_filtered[0] = 0.0f;
 275         this->accels_filtered[1] = 0.0f;
 276         this->accels_filtered[2] = 0.0f;
 277         this->grot_filtered[0]   = 0.0f;
 278         this->grot_filtered[1]   = 0.0f;
 279         this->grot_filtered[2]   = 0.0f;
 280         this->timeval   = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing
 281         this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals
 282
 283         return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion
 284     }
 285
 286     if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) {
 287         // wait 4 seconds for the user to get his hands off in case the board was just powered
 288         this->timeval = PIOS_DELAY_GetRaw();
 289         return FILTERRESULT_ERROR;
 290     } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) {
 291         // For first 6 seconds use accels to get gyro bias
 292         this->attitudeSettings.AccelKp     = 1.0f;
 293         this->attitudeSettings.AccelKi     = 0.0f;
 294         this->attitudeSettings.YawBiasRate = 0.23f;
 295         this->accel_filter_enabled   = false;
 296         this->rollPitchBiasRate      = 0.01f;
 297         this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
 298         PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
 299     } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
 300         this->attitudeSettings.AccelKp     = 1.0f;
 301         this->attitudeSettings.AccelKi     = 0.0f;
 302         this->attitudeSettings.YawBiasRate = 0.23f;
 303         this->accel_filter_enabled   = false;
 304         this->rollPitchBiasRate      = 0.01f;
 305         this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
 306         this->init = 0;
 307         PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
 308     } else if (this->init == 0) {
 309         // Reload settings (all the rates)
 310         AttitudeSettingsGet(&this->attitudeSettings);
 311         this->rollPitchBiasRate = 0.0f;
 312         if (this->accel_alpha > 0.0f) {
 313             this->accel_filter_enabled = true;
 314         }
 315         this->init = 1;
 316     }
 317
 318     // Compute the dT using the cpu clock
 319     dT = PIOS_DELAY_DiffuS(this->timeval) / 1000000.0f;
 320     this->timeval = PIOS_DELAY_GetRaw();
 321     if (dT < 0.001f) { // safe bounds
 322         dT = 0.001f;
 323     }
 324
 325     AttitudeStateData attitudeState; // base on previous state
 326     AttitudeStateGet(&attitudeState);
 327
 328     // Get the current attitude estimate
 329     quat_copy(&attitudeState.q1, attitude);
 330
 331     // Apply smoothing to accel values, to reduce vibration noise before main calculations.
 332     apply_accel_filter(this, accel, this->accels_filtered);
 333
 334     // Rotate gravity to body frame and cross with accels
 335     float grot[3];
 336     grot[0] = -(2.0f * (attitude[1] * attitude[3] - attitude[0] * attitude[2]));
 337     grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
 338     grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
 339
 340     float accel_err[3];
 341     apply_accel_filter(this, grot, this->grot_filtered);
 342
 343     CrossProduct((const float *)this->accels_filtered, (const float *)this->grot_filtered, accel_err);
 344
 345     // Account for accel magnitude
 346     float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]);
 347     if (accel_mag < 1.0e-3f) {
 348         return FILTERRESULT_CRITICAL; // safety feature copied from CC
 349     }
 350
 351     // Account for filtered gravity vector magnitude
 352     float grot_mag;
 353     if (this->accel_filter_enabled) {
 354         grot_mag = sqrtf(this->grot_filtered[0] * this->grot_filtered[0] + this->grot_filtered[1] * this->grot_filtered[1] + this->grot_filtered[2] * this->grot_filtered[2]);
 355     } else {
 356         grot_mag = 1.0f;
 357     }
 358     if (grot_mag < 1.0e-3f) {
 359         return FILTERRESULT_CRITICAL;
 360     }
 361
 362     accel_err[0] /= (accel_mag * grot_mag);
 363     accel_err[1] /= (accel_mag * grot_mag);
 364     accel_err[2] /= (accel_mag * grot_mag);
 365
 366     float mag_err[3] = { 0.0f };
 367     if (this->magUpdated && this->useMag) {
 368         // Rotate gravity to body frame and cross with accels
 369         float brot[3];
 370         float Rbe[3][3];
 371
 372         Quaternion2R(attitude, Rbe);
 373
 374         rot_mult(Rbe, this->homeLocation.Be, brot);
 375
 376         float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
 377         mag[0]  /= mag_len;
 378         mag[1]  /= mag_len;
 379         mag[2]  /= mag_len;
 380
 381         float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
 382         brot[0] /= bmag;
 383         brot[1] /= bmag;
 384         brot[2] /= bmag;
 385
 386         // Only compute if neither vector is null
 387         if (bmag < 1.0f || mag_len < 1.0f) {
 388             mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
 389         } else {
 390             CrossProduct((const float *)mag, (const float *)brot, mag_err);
 391         }
 392     } else {
 393         mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
 394     }
 395
 396     // Correct rates based on integral coefficient
 397     gyro[0] -= this->gyroBias[0];
 398     gyro[1] -= this->gyroBias[1];
 399     gyro[2] -= this->gyroBias[2];
 400
 401     // Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
 402     this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate;
 403     this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;
 404     if (this->useMag) {
 405         this->gyroBias[2] -= -mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
 406     } else {
 407         this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
 408     }
 409
 410     float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
 411     // Correct rates based on proportional coefficient
 412     gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
 413     gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
 414     if (this->useMag) {
 415         gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT;
 416     } else {
 417         gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;
 418     }
 419
 420     // Work out time derivative from INSAlgo writeup
 421     // Also accounts for the fact that gyros are in deg/s
 422     float qdot[4];
 423     qdot[0]     = DEG2RAD(-attitude[1] * gyrotmp[0] - attitude[2] * gyrotmp[1] - attitude[3] * gyrotmp[2]) * dT / 2;
 424     qdot[1]     = DEG2RAD(attitude[0] * gyrotmp[0] - attitude[3] * gyrotmp[1] + attitude[2] * gyrotmp[2]) * dT / 2;
 425     qdot[2]     = DEG2RAD(attitude[3] * gyrotmp[0] + attitude[0] * gyrotmp[1] - attitude[1] * gyrotmp[2]) * dT / 2;
 426     qdot[3]     = DEG2RAD(-attitude[2] * gyrotmp[0] + attitude[1] * gyrotmp[1] + attitude[0] * gyrotmp[2]) * dT / 2;
 427
 428     // Take a time step
 429     attitude[0] = attitude[0] + qdot[0];
 430     attitude[1] = attitude[1] + qdot[1];
 431     attitude[2] = attitude[2] + qdot[2];
 432     attitude[3] = attitude[3] + qdot[3];
 433
 434     if (attitude[0] < 0.0f) {
 435         attitude[0] = -attitude[0];
 436         attitude[1] = -attitude[1];
 437         attitude[2] = -attitude[2];
 438         attitude[3] = -attitude[3];
 439     }
 440
 441     // Renomalize
 442     float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
 443     attitude[0] = attitude[0] / qmag;
 444     attitude[1] = attitude[1] / qmag;
 445     attitude[2] = attitude[2] / qmag;
 446     attitude[3] = attitude[3] / qmag;
 447
 448     // If quaternion has become inappropriately short or is nan reinit.
 449     // THIS SHOULD NEVER ACTUALLY HAPPEN
 450     if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
 451         this->first_run = 1;
 452         return FILTERRESULT_WARNING;
 453     }
 454
 455     if (this->init) {
 456         return FILTERRESULT_OK;
 457     } else {
 458         return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later
 459     }
 460 }
 461
 462 static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 463 {
 464     FlightStatusGet(&flightStatus);
 465 }
 466
 467 /**
 468  * @}
 469  * @}
 470  */