flight/modules/StateEstimation/filteraltitude.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       filteraltitude.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Barometric altitude filter, calculates vertical speed and true
  12  *             altitude based on Barometric altitude and accelerometers
  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 <altitudefiltersettings.h>
  36 #include <homelocation.h>
  37
  38 #include <CoordinateConversions.h>
  39
  40 // Private constants
  41
  42 // duration of accel bias initialization phase
  43 #define INITIALIZATION_DURATION_MS 5000
  44
  45 #define STACK_REQUIRED             128
  46
  47 #define DT_ALPHA                   1e-2f
  48 #define DT_MIN                     1e-6f
  49 #define DT_MAX                     1.0f
  50 #define DT_AVERAGE                 1e-3f
  51 static volatile bool reloadSettings;
  52
  53 // Private types
  54 struct data {
  55     float altitudeState; // state = altitude,velocity,accel_offset,accel
  56     float velocityState;
  57     float accelBiasState;
  58     float accelState;
  59     float pos[3]; // position updates from other filters
  60     float vel[3]; // position updates from other filters
  61
  62     PiOSDeltatimeConfig dt1config;
  63     PiOSDeltatimeConfig dt2config;
  64     float accelLast;
  65     float baroLast;
  66     bool  first_run;
  67     portTickType initTimer;
  68     AltitudeFilterSettingsData settings;
  69     float gravity;
  70 };
  71
  72 // Private variables
  73
  74 // Private functions
  75
  76 static int32_t init(stateFilter *self);
  77 static filterResult filter(stateFilter *self, stateEstimation *state);
  78 static void settingsUpdatedCb(UAVObjEvent *ev);
  79
  80
  81 int32_t filterAltitudeInitialize(stateFilter *handle)
  82 {
  83     handle->init      = &init;
  84     handle->filter    = &filter;
  85     handle->localdata = pios_malloc(sizeof(struct data));
  86     AttitudeStateInitialize();
  87     AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
  88     reloadSettings    = true;
  89     return STACK_REQUIRED;
  90 }
  91
  92 static int32_t init(stateFilter *self)
  93 {
  94     struct data *this = (struct data *)self->localdata;
  95
  96     this->altitudeState  = 0.0f;
  97     this->velocityState  = 0.0f;
  98     this->accelBiasState = 0.0f;
  99     this->accelState     = 0.0f;
 100     this->pos[0]    = 0.0f;
 101     this->pos[1]    = 0.0f;
 102     this->pos[2]    = 0.0f;
 103     this->vel[0]    = 0.0f;
 104     this->vel[1]    = 0.0f;
 105     this->vel[2]    = 0.0f;
 106     PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
 107     PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
 108     this->baroLast  = 0.0f;
 109     this->accelLast = 0.0f;
 110     this->first_run = 1;
 111     HomeLocationg_eGet(&this->gravity);
 112     return 0;
 113 }
 114
 115 static filterResult filter(stateFilter *self, stateEstimation *state)
 116 {
 117     struct data *this = (struct data *)self->localdata;
 118
 119     if (reloadSettings) {
 120         reloadSettings = false;
 121         AltitudeFilterSettingsGet(&this->settings);
 122     }
 123
 124     if (this->first_run) {
 125         // Initialize to current altitude reading at initial location
 126         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 127             this->first_run = 0;
 128             this->initTimer = xTaskGetTickCount();
 129         }
 130     } else {
 131         // save existing position and velocity updates so GPS will still work
 132         if (IS_SET(state->updated, SENSORUPDATES_pos)) {
 133             this->pos[0]  = state->pos[0];
 134             this->pos[1]  = state->pos[1];
 135             this->pos[2]  = state->pos[2];
 136             state->pos[2] = -this->altitudeState;
 137         }
 138         if (IS_SET(state->updated, SENSORUPDATES_vel)) {
 139             this->vel[0]  = state->vel[0];
 140             this->vel[1]  = state->vel[1];
 141             this->vel[2]  = state->vel[2];
 142             state->vel[2] = -this->velocityState;
 143         }
 144         if (IS_SET(state->updated, SENSORUPDATES_accel)) {
 145             // rotate accels into global coordinate frame
 146             AttitudeStateData att;
 147             AttitudeStateGet(&att);
 148             float Rbe[3][3];
 149             Quaternion2R(&att.q1, Rbe);
 150             float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
 151
 152             // low pass filter accelerometers
 153             this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
 154             if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
 155                 // allow the offset to reach quickly the target value in case of small AccelDriftKi
 156                 this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
 157             } else {
 158                 // correct accel offset (low pass zeroing)
 159                 this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
 160             }
 161             // correct velocity and position state (integration)
 162             // low pass for average dT, compensate timing jitter from scheduler
 163             //
 164             float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
 165             float speedLast = this->velocityState;
 166
 167             this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
 168             this->accelLast      = this->accelState - this->accelBiasState;
 169
 170             this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT;
 171
 172
 173             state->pos[0]   = this->pos[0];
 174             state->pos[1]   = this->pos[1];
 175             state->pos[2]   = -this->altitudeState;
 176             state->updated |= SENSORUPDATES_pos;
 177
 178             state->vel[0]   = this->vel[0];
 179             state->vel[1]   = this->vel[1];
 180             state->vel[2]   = -this->velocityState;
 181             state->updated |= SENSORUPDATES_vel;
 182         }
 183         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 184             // correct the altitude state (simple low pass)
 185             this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0];
 186
 187             // correct the velocity state (low pass differentiation)
 188             // low pass for average dT, compensate timing jitter from scheduler
 189             float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
 190             this->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
 191             this->baroLast  = state->baro[0];
 192
 193             state->pos[0]   = this->pos[0];
 194             state->pos[1]   = this->pos[1];
 195             state->pos[2]   = -this->altitudeState;
 196             state->updated |= SENSORUPDATES_pos;
 197
 198             state->vel[0]   = this->vel[0];
 199             state->vel[1]   = this->vel[1];
 200             state->vel[2]   = -this->velocityState;
 201             state->updated |= SENSORUPDATES_vel;
 202         }
 203     }
 204
 205     return FILTERRESULT_OK;
 206 }
 207
 208 void settingsUpdatedCb(UAVObjEvent *ev)
 209 {
 210     if (ev->obj == AltitudeFilterSettingsHandle()) {
 211         reloadSettings = true;
 212     }
 213 }
 214
 215 /**
 216  * @}
 217  * @}
 218  */