flight/modules/StateEstimation/filterbaro.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       filterbaro.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Barometric altitude filter, calculates altitude offset based on
  12  *             GPS altitude offset if available
  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
  35 #include <revosettings.h>
  36
  37 // Private constants
  38
  39 #define STACK_REQUIRED 128
  40 #define INIT_CYCLES    100
  41
  42 // Private types
  43 struct data {
  44     float   baroOffset;
  45     float   baroGPSOffsetCorrectionAlpha;
  46     float   baroAlt;
  47     float   gpsAlt;
  48     int16_t first_run;
  49     bool    useGPS;
  50 };
  51
  52 // Private variables
  53
  54 // Private functions
  55
  56 static int32_t initwithgps(stateFilter *self);
  57 static int32_t initwithoutgps(stateFilter *self);
  58 static int32_t maininit(stateFilter *self);
  59 static filterResult filter(stateFilter *self, stateEstimation *state);
  60
  61
  62 int32_t filterBaroInitialize(stateFilter *handle)
  63 {
  64     handle->init      = &initwithgps;
  65     handle->filter    = &filter;
  66     handle->localdata = pios_malloc(sizeof(struct data));
  67     return STACK_REQUIRED;
  68 }
  69
  70 int32_t filterBaroiInitialize(stateFilter *handle)
  71 {
  72     handle->init      = &initwithoutgps;
  73     handle->filter    = &filter;
  74     handle->localdata = pios_malloc(sizeof(struct data));
  75     return STACK_REQUIRED;
  76 }
  77
  78 static int32_t initwithgps(stateFilter *self)
  79 {
  80     struct data *this = (struct data *)self->localdata;
  81
  82     this->useGPS = 1;
  83     return maininit(self);
  84 }
  85
  86 static int32_t initwithoutgps(stateFilter *self)
  87 {
  88     struct data *this = (struct data *)self->localdata;
  89
  90     this->useGPS = 0;
  91     return maininit(self);
  92 }
  93
  94 static int32_t maininit(stateFilter *self)
  95 {
  96     struct data *this = (struct data *)self->localdata;
  97
  98     this->baroOffset = 0.0f;
  99     this->gpsAlt     = 0.0f;
 100     this->first_run  = INIT_CYCLES;
 101
 102     RevoSettingsInitialize();
 103     RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
 104
 105     return 0;
 106 }
 107
 108 static filterResult filter(stateFilter *self, stateEstimation *state)
 109 {
 110     struct data *this = (struct data *)self->localdata;
 111
 112     if (this->first_run) {
 113         // Make sure initial location is initialized properly before continuing
 114         if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
 115             if (this->first_run == INIT_CYCLES) {
 116                 this->gpsAlt = state->pos[2];
 117                 this->first_run--;
 118             }
 119         }
 120         // Initialize to current altitude reading at initial location
 121         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 122             if (this->first_run < INIT_CYCLES || !this->useGPS) {
 123                 this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt);
 124                 this->baroAlt    = state->baro[0];
 125                 this->first_run--;
 126             }
 127             UNSET_MASK(state->updated, SENSORUPDATES_baro);
 128         }
 129         // make sure we raise an error until properly initialized - would not be good if people arm and
 130         // use altitudehold without initialized barometer filter
 131         // Here, the filter is not initialized, return ERROR
 132         return FILTERRESULT_CRITICAL;
 133     } else {
 134         // Track barometric altitude offset with a low pass filter
 135         // based on GPS altitude if available
 136         if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
 137             this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
 138                                (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
 139         }
 140         // calculate bias corrected altitude
 141         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 142             this->baroAlt   = state->baro[0];
 143             state->baro[0] -= this->baroOffset;
 144         }
 145         return FILTERRESULT_OK;
 146     }
 147 }
 148
 149
 150 /**
 151  * @}
 152  * @}
 153  */