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