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     HomeLocationInitialize();
  87     AttitudeStateInitialize();
  88     AltitudeFilterSettingsInitialize();
  89     AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
  90     reloadSettings = true;
  91     return STACK_REQUIRED;
  92 }
  93
  94 static int32_t init(stateFilter *self)
  95 {
  96     struct data *this = (struct data *)self->localdata;
  97
  98     this->altitudeState  = 0.0f;
  99     this->velocityState  = 0.0f;
 100     this->accelBiasState = 0.0f;
 101     this->accelState     = 0.0f;
 102     this->pos[0]    = 0.0f;
 103     this->pos[1]    = 0.0f;
 104     this->pos[2]    = 0.0f;
 105     this->vel[0]    = 0.0f;
 106     this->vel[1]    = 0.0f;
 107     this->vel[2]    = 0.0f;
 108     PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
 109     PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA);
 110     this->baroLast  = 0.0f;
 111     this->accelLast = 0.0f;
 112     this->first_run = 1;
 113     HomeLocationg_eGet(&this->gravity);
 114     return 0;
 115 }
 116
 117 static filterResult filter(stateFilter *self, stateEstimation *state)
 118 {
 119     struct data *this = (struct data *)self->localdata;
 120
 121     if (reloadSettings) {
 122         reloadSettings = false;
 123         AltitudeFilterSettingsGet(&this->settings);
 124     }
 125
 126     if (this->first_run) {
 127         // Initialize to current altitude reading at initial location
 128         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 129             this->first_run = 0;
 130             this->initTimer = xTaskGetTickCount();
 131         }
 132     } else {
 133         // save existing position and velocity updates so GPS will still work
 134         if (IS_SET(state->updated, SENSORUPDATES_pos)) {
 135             this->pos[0]  = state->pos[0];
 136             this->pos[1]  = state->pos[1];
 137             this->pos[2]  = state->pos[2];
 138             state->pos[2] = -this->altitudeState;
 139         }
 140         if (IS_SET(state->updated, SENSORUPDATES_vel)) {
 141             this->vel[0]  = state->vel[0];
 142             this->vel[1]  = state->vel[1];
 143             this->vel[2]  = state->vel[2];
 144             state->vel[2] = -this->velocityState;
 145         }
 146         if (IS_SET(state->updated, SENSORUPDATES_accel)) {
 147             // rotate accels into global coordinate frame
 148             AttitudeStateData att;
 149             AttitudeStateGet(&att);
 150             float Rbe[3][3];
 151             Quaternion2R(&att.q1, Rbe);
 152             float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
 153
 154             // low pass filter accelerometers
 155             this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
 156             if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
 157                 // allow the offset to reach quickly the target value in case of small AccelDriftKi
 158                 this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
 159             } else {
 160                 // correct accel offset (low pass zeroing)
 161                 this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
 162             }
 163             // correct velocity and position state (integration)
 164             // low pass for average dT, compensate timing jitter from scheduler
 165             //
 166             float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
 167             float speedLast = this->velocityState;
 168
 169             this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
 170             this->accelLast      = this->accelState - this->accelBiasState;
 171
 172             this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT;
 173
 174
 175             state->pos[0]   = this->pos[0];
 176             state->pos[1]   = this->pos[1];
 177             state->pos[2]   = -this->altitudeState;
 178             state->updated |= SENSORUPDATES_pos;
 179
 180             state->vel[0]   = this->vel[0];
 181             state->vel[1]   = this->vel[1];
 182             state->vel[2]   = -this->velocityState;
 183             state->updated |= SENSORUPDATES_vel;
 184         }
 185         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 186             // correct the altitude state (simple low pass)
 187             this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0];
 188
 189             // correct the velocity state (low pass differentiation)
 190             // low pass for average dT, compensate timing jitter from scheduler
 191             float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
 192             this->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
 193             this->baroLast  = state->baro[0];
 194
 195             state->pos[0]   = this->pos[0];
 196             state->pos[1]   = this->pos[1];
 197             state->pos[2]   = -this->altitudeState;
 198             state->updated |= SENSORUPDATES_pos;
 199
 200             state->vel[0]   = this->vel[0];
 201             state->vel[1]   = this->vel[1];
 202             state->vel[2]   = -this->velocityState;
 203             state->updated |= SENSORUPDATES_vel;
 204         }
 205     }
 206
 207     return FILTERRESULT_OK;
 208 }
 209
 210 void settingsUpdatedCb(UAVObjEvent *ev)
 211 {
 212     if (ev->obj == AltitudeFilterSettingsHandle()) {
 213         reloadSettings = true;
 214     }
 215 }
 216
 217 /**
 218  * @}
 219  * @}
 220  */