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 #include <mathmisc.h>
  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 #define VARIANCE_WINDOW_SIZE    40
  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     pw_variance_t gyro_var[3];
  74 };
  75
  76 // Private variables
  77 bool initialized = 0;
  78 static FlightStatusData flightStatus;
  79
  80 // Private functions
  81
  82 static int32_t initwithmag(stateFilter *self);
  83 static int32_t initwithoutmag(stateFilter *self);
  84 static int32_t maininit(stateFilter *self);
  85 static filterResult filter(stateFilter *self, stateEstimation *state);
  86 static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
  87
  88 static void flightStatusUpdatedCb(UAVObjEvent *ev);
  89
  90 static void globalInit(void);
  91
  92
  93 static void globalInit(void)
  94 {
  95     if (!initialized) {
  96         initialized = 1;
  97         FlightStatusInitialize();
  98         HomeLocationInitialize();
  99         RevoCalibrationInitialize();
 100         FlightStatusConnectCallback(&flightStatusUpdatedCb);
 101         flightStatusUpdatedCb(NULL);
 102     }
 103 }
 104
 105 int32_t filterCFInitialize(stateFilter *handle)
 106 {
 107     globalInit();
 108     handle->init      = &initwithoutmag;
 109     handle->filter    = &filter;
 110     handle->localdata = pios_malloc(sizeof(struct data));
 111     return STACK_REQUIRED;
 112 }
 113
 114 int32_t filterCFMInitialize(stateFilter *handle)
 115 {
 116     globalInit();
 117     handle->init      = &initwithmag;
 118     handle->filter    = &filter;
 119     handle->localdata = pios_malloc(sizeof(struct data));
 120     return STACK_REQUIRED;
 121 }
 122
 123 static int32_t initwithmag(stateFilter *self)
 124 {
 125     struct data *this = (struct data *)self->localdata;
 126
 127     this->useMag = 1;
 128     return maininit(self);
 129 }
 130
 131 static int32_t initwithoutmag(stateFilter *self)
 132 {
 133     struct data *this = (struct data *)self->localdata;
 134
 135     this->useMag = 0;
 136     return maininit(self);
 137 }
 138
 139 static int32_t maininit(stateFilter *self)
 140 {
 141     struct data *this = (struct data *)self->localdata;
 142
 143     this->first_run     = 1;
 144     this->accelUpdated  = 0;
 145     this->magCalibrated = true;
 146     AttitudeSettingsGet(&this->attitudeSettings);
 147     HomeLocationGet(&this->homeLocation);
 148
 149     const float fakeDt = 0.0025f;
 150     if (this->attitudeSettings.AccelTau < 0.0001f) {
 151         this->accel_alpha = 0; // not trusting this to resolve to 0
 152         this->accel_filter_enabled = false;
 153     } else {
 154         this->accel_alpha = expf(-fakeDt / this->attitudeSettings.AccelTau);
 155         this->accel_filter_enabled = true;
 156     }
 157
 158     // reset gyro Bias
 159     this->gyroBias[0] = 0.0f;
 160     this->gyroBias[1] = 0.0f;
 161     this->gyroBias[2] = 0.0f;
 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
 235         pseudo_windowed_variance_init(&this->gyro_var[0], VARIANCE_WINDOW_SIZE);
 236         pseudo_windowed_variance_init(&this->gyro_var[1], VARIANCE_WINDOW_SIZE);
 237         pseudo_windowed_variance_init(&this->gyro_var[2], VARIANCE_WINDOW_SIZE);
 238
 239         float magBias[3];
 240         RevoCalibrationmag_biasArrayGet(magBias);
 241         // don't trust Mag for initial orientation if it has not been calibrated
 242         if (fabsf(magBias[0]) < 1e-6f && fabsf(magBias[1]) < 1e-6f && fabsf(magBias[2]) < 1e-6f) {
 243             this->magCalibrated = false;
 244             mag[0] = 100.0f;
 245             mag[1] = 0.0f;
 246             mag[2] = 0.0f;
 247         }
 248
 249         AttitudeStateData attitudeState; // base on previous state
 250         AttitudeStateGet(&attitudeState);
 251         this->init = 0;
 252
 253         // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
 254         // so pseudo "north" vector can be estimated even if the board is not level
 255         attitudeState.Roll = atan2f(-accel[1], -accel[2]);
 256         float zn  = cosf(attitudeState.Roll) * mag[2] + sinf(attitudeState.Roll) * mag[1];
 257         float yn  = cosf(attitudeState.Roll) * mag[1] - sinf(attitudeState.Roll) * mag[2];
 258
 259         // rotate accels z vector according to roll
 260         float azn = cosf(attitudeState.Roll) * accel[2] + sinf(attitudeState.Roll) * accel[1];
 261         attitudeState.Pitch = atan2f(accel[0], -azn);
 262
 263         float xn  = cosf(attitudeState.Pitch) * mag[0] + sinf(attitudeState.Pitch) * zn;
 264
 265         attitudeState.Yaw = atan2f(-yn, xn);
 266         // TODO: This is still a hack
 267         // Put this in a proper generic function in CoordinateConversion.c
 268         // 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)
 269         // should calculate the rotation in 3d space using proper cross product math
 270         // SUBTODO: formulate the math required
 271
 272         attitudeState.Roll  = RAD2DEG(attitudeState.Roll);
 273         attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
 274         attitudeState.Yaw   = RAD2DEG(attitudeState.Yaw);
 275
 276         RPY2Quaternion(&attitudeState.Roll, attitude);
 277
 278         this->first_run = 0;
 279         this->accels_filtered[0] = 0.0f;
 280         this->accels_filtered[1] = 0.0f;
 281         this->accels_filtered[2] = 0.0f;
 282         this->grot_filtered[0]   = 0.0f;
 283         this->grot_filtered[1]   = 0.0f;
 284         this->grot_filtered[2]   = 0.0f;
 285         this->timeval   = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing
 286         this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals
 287
 288         return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion
 289     }
 290     // check whether copter is steady
 291     if (this->init == 0 && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) {
 292         pseudo_windowed_variance_push_sample(&this->gyro_var[0], gyro[0]);
 293         pseudo_windowed_variance_push_sample(&this->gyro_var[1], gyro[1]);
 294         pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]);
 295         float const gyrovarx = pseudo_windowed_variance_get(&this->gyro_var[0]);
 296         float const gyrovary = pseudo_windowed_variance_get(&this->gyro_var[1]);
 297         float const gyrovarz = pseudo_windowed_variance_get(&this->gyro_var[2]);
 298
 299         if ((fabsf(gyrovarx) + fabsf(gyrovary) + fabsf(gyrovarz)) > this->attitudeSettings.BoardSteadyMaxVariance) {
 300             this->starttime = xTaskGetTickCount();
 301             this->first_run = 1;
 302             return FILTERRESULT_WARNING;
 303         }
 304     }
 305
 306
 307     if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) {
 308         // wait 4 seconds for the user to get his hands off in case the board was just powered
 309         this->timeval = PIOS_DELAY_GetRaw();
 310         return FILTERRESULT_ERROR;
 311     } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) {
 312         // For first 6 seconds use accels to get gyro bias
 313         this->attitudeSettings.AccelKp     = 1.0f;
 314         this->attitudeSettings.AccelKi     = 0.0f;
 315         this->attitudeSettings.YawBiasRate = 0.23f;
 316         this->accel_filter_enabled   = false;
 317         this->rollPitchBiasRate      = 0.01f;
 318         this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
 319         PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
 320     } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
 321         this->attitudeSettings.AccelKp     = 1.0f;
 322         this->attitudeSettings.AccelKi     = 0.0f;
 323         this->attitudeSettings.YawBiasRate = 0.23f;
 324         this->accel_filter_enabled   = false;
 325         this->rollPitchBiasRate      = 0.01f;
 326         this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
 327         this->init = 0;
 328         PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
 329     } else if (this->init == 0) {
 330         // Reload settings (all the rates)
 331         AttitudeSettingsGet(&this->attitudeSettings);
 332         this->rollPitchBiasRate = 0.0f;
 333         if (this->accel_alpha > 0.0f) {
 334             this->accel_filter_enabled = true;
 335         }
 336         this->init = 1;
 337     }
 338
 339     // Compute the dT using the cpu clock
 340     dT = PIOS_DELAY_DiffuS(this->timeval) / 1000000.0f;
 341     this->timeval = PIOS_DELAY_GetRaw();
 342     if (dT < 0.001f) { // safe bounds
 343         dT = 0.001f;
 344     }
 345
 346     AttitudeStateData attitudeState; // base on previous state
 347     AttitudeStateGet(&attitudeState);
 348
 349     // Get the current attitude estimate
 350     quat_copy(&attitudeState.q1, attitude);
 351
 352     // Apply smoothing to accel values, to reduce vibration noise before main calculations.
 353     apply_accel_filter(this, accel, this->accels_filtered);
 354
 355     // Rotate gravity to body frame and cross with accels
 356     float grot[3];
 357     grot[0] = -(2.0f * (attitude[1] * attitude[3] - attitude[0] * attitude[2]));
 358     grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
 359     grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
 360
 361     float accel_err[3];
 362     apply_accel_filter(this, grot, this->grot_filtered);
 363
 364     CrossProduct((const float *)this->accels_filtered, (const float *)this->grot_filtered, accel_err);
 365
 366     // Account for accel magnitude
 367     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]);
 368     if (accel_mag < 1.0e-3f) {
 369         return FILTERRESULT_CRITICAL; // safety feature copied from CC
 370     }
 371
 372     // Account for filtered gravity vector magnitude
 373     float grot_mag;
 374     if (this->accel_filter_enabled) {
 375         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]);
 376     } else {
 377         grot_mag = 1.0f;
 378     }
 379     if (grot_mag < 1.0e-3f) {
 380         return FILTERRESULT_CRITICAL;
 381     }
 382
 383     accel_err[0] /= (accel_mag * grot_mag);
 384     accel_err[1] /= (accel_mag * grot_mag);
 385     accel_err[2] /= (accel_mag * grot_mag);
 386
 387     float mag_err[3] = { 0.0f };
 388     if (this->magUpdated && this->useMag) {
 389         // Rotate gravity to body frame and cross with accels
 390         float brot[3];
 391         float Rbe[3][3];
 392
 393         Quaternion2R(attitude, Rbe);
 394
 395         rot_mult(Rbe, this->homeLocation.Be, brot);
 396
 397         float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
 398         mag[0]  /= mag_len;
 399         mag[1]  /= mag_len;
 400         mag[2]  /= mag_len;
 401
 402         float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
 403         brot[0] /= bmag;
 404         brot[1] /= bmag;
 405         brot[2] /= bmag;
 406
 407         // Only compute if neither vector is null
 408         if (bmag < 1.0f || mag_len < 1.0f) {
 409             mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
 410         } else {
 411             CrossProduct((const float *)mag, (const float *)brot, mag_err);
 412         }
 413     } else {
 414         mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
 415     }
 416
 417     // Correct rates based on integral coefficient
 418     gyro[0] -= this->gyroBias[0];
 419     gyro[1] -= this->gyroBias[1];
 420     gyro[2] -= this->gyroBias[2];
 421
 422     // Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
 423     this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate;
 424     this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;
 425     if (this->useMag) {
 426         this->gyroBias[2] -= -mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
 427     } else {
 428         this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
 429     }
 430
 431     float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
 432     // Correct rates based on proportional coefficient
 433     gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
 434     gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
 435     if (this->useMag) {
 436         gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT;
 437     } else {
 438         gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;
 439     }
 440
 441     // Work out time derivative from INSAlgo writeup
 442     // Also accounts for the fact that gyros are in deg/s
 443     float qdot[4];
 444     qdot[0]     = DEG2RAD(-attitude[1] * gyrotmp[0] - attitude[2] * gyrotmp[1] - attitude[3] * gyrotmp[2]) * dT / 2;
 445     qdot[1]     = DEG2RAD(attitude[0] * gyrotmp[0] - attitude[3] * gyrotmp[1] + attitude[2] * gyrotmp[2]) * dT / 2;
 446     qdot[2]     = DEG2RAD(attitude[3] * gyrotmp[0] + attitude[0] * gyrotmp[1] - attitude[1] * gyrotmp[2]) * dT / 2;
 447     qdot[3]     = DEG2RAD(-attitude[2] * gyrotmp[0] + attitude[1] * gyrotmp[1] + attitude[0] * gyrotmp[2]) * dT / 2;
 448
 449     // Take a time step
 450     attitude[0] = attitude[0] + qdot[0];
 451     attitude[1] = attitude[1] + qdot[1];
 452     attitude[2] = attitude[2] + qdot[2];
 453     attitude[3] = attitude[3] + qdot[3];
 454
 455     if (attitude[0] < 0.0f) {
 456         attitude[0] = -attitude[0];
 457         attitude[1] = -attitude[1];
 458         attitude[2] = -attitude[2];
 459         attitude[3] = -attitude[3];
 460     }
 461
 462     // Renormalize
 463     float inv_qmag = invsqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
 464     attitude[0] = attitude[0] * inv_qmag;
 465     attitude[1] = attitude[1] * inv_qmag;
 466     attitude[2] = attitude[2] * inv_qmag;
 467     attitude[3] = attitude[3] * inv_qmag;
 468
 469     // If quaternion has become inappropriately short or is nan reinit.
 470     // THIS SHOULD NEVER ACTUALLY HAPPEN
 471     if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
 472         this->first_run = 1;
 473         return FILTERRESULT_WARNING;
 474     }
 475
 476     if (this->init) {
 477         return FILTERRESULT_OK;
 478     } else {
 479         return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later
 480     }
 481 }
 482
 483 static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 484 {
 485     FlightStatusGet(&flightStatus);
 486 }
 487
 488 /**
 489  * @}
 490  * @}
 491  */