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 LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  12  * @brief      Barometric altitude filter, calculates altitude offset based on
  13  *             GPS altitude offset if available
  14  *
  15  * @see        The GNU Public License (GPL) Version 3
  16  *
  17  ******************************************************************************/
  18 /*
  19  * This program is free software; you can redistribute it and/or modify
  20  * it under the terms of the GNU General Public License as published by
  21  * the Free Software Foundation; either version 3 of the License, or
  22  * (at your option) any later version.
  23  *
  24  * This program is distributed in the hope that it will be useful, but
  25  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  26  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  27  * for more details.
  28  *
  29  * You should have received a copy of the GNU General Public License along
  30  * with this program; if not, write to the Free Software Foundation, Inc.,
  31  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  32  */
  33
  34 #include "inc/stateestimation.h"
  35
  36 #include <revosettings.h>
  37
  38 // Private constants
  39
  40 #define STACK_REQUIRED    128
  41 #define INIT_CYCLES       500
  42 #define BARO_OFFSET_ALPHA 0.02f
  43
  44 // Private types
  45 struct data {
  46     float   baroOffset;
  47     float   baroGPSOffsetCorrectionAlpha;
  48     float   baroAlt;
  49     float   gpsAlt;
  50     int16_t first_run;
  51     bool    useGPS;
  52 };
  53
  54 // Private variables
  55
  56 // Private functions
  57
  58 static int32_t initwithgps(stateFilter *self);
  59 static int32_t initwithoutgps(stateFilter *self);
  60 static int32_t maininit(stateFilter *self);
  61 static filterResult filter(stateFilter *self, stateEstimation *state);
  62
  63
  64 int32_t filterBaroInitialize(stateFilter *handle)
  65 {
  66     handle->init      = &initwithgps;
  67     handle->filter    = &filter;
  68     handle->localdata = pios_malloc(sizeof(struct data));
  69     return STACK_REQUIRED;
  70 }
  71
  72 int32_t filterBaroiInitialize(stateFilter *handle)
  73 {
  74     handle->init      = &initwithoutgps;
  75     handle->filter    = &filter;
  76     handle->localdata = pios_malloc(sizeof(struct data));
  77     return STACK_REQUIRED;
  78 }
  79
  80 static int32_t initwithgps(stateFilter *self)
  81 {
  82     struct data *this = (struct data *)self->localdata;
  83
  84     this->useGPS = 1;
  85     return maininit(self);
  86 }
  87
  88 static int32_t initwithoutgps(stateFilter *self)
  89 {
  90     struct data *this = (struct data *)self->localdata;
  91
  92     this->useGPS = 0;
  93     return maininit(self);
  94 }
  95
  96 static int32_t maininit(stateFilter *self)
  97 {
  98     struct data *this = (struct data *)self->localdata;
  99
 100     this->baroOffset = 0.0f;
 101     this->gpsAlt     = 0.0f;
 102     this->first_run  = INIT_CYCLES;
 103
 104     RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
 105
 106     return 0;
 107 }
 108
 109 static filterResult filter(stateFilter *self, stateEstimation *state)
 110 {
 111     struct data *this = (struct data *)self->localdata;
 112
 113     if (this->first_run) {
 114         // Make sure initial location is initialized properly before continuing
 115         if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
 116             if (this->first_run == INIT_CYCLES) {
 117                 this->gpsAlt = state->pos[2];
 118                 this->first_run--;
 119             }
 120         }
 121         // Initialize to current altitude reading at initial location
 122         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 123             if (this->first_run < INIT_CYCLES || !this->useGPS) {
 124                 if (this->first_run > INIT_CYCLES - 2) {
 125                     this->baroOffset = (state->baro[0] + this->gpsAlt);
 126                 }
 127                 // Set baroOffset using filtering, this allow better altitude zeroing
 128                 this->baroOffset = ((1.0f - BARO_OFFSET_ALPHA) * this->baroOffset) + (BARO_OFFSET_ALPHA * (state->baro[0] + this->gpsAlt));
 129                 this->baroAlt    = state->baro[0];
 130                 this->first_run--;
 131             }
 132             UNSET_MASK(state->updated, SENSORUPDATES_baro);
 133         }
 134         // make sure we raise an error until properly initialized - would not be good if people arm and
 135         // use altitudehold without initialized barometer filter
 136         // Here, the filter is not initialized, return ERROR
 137         return FILTERRESULT_CRITICAL;
 138     } else {
 139         // Track barometric altitude offset with a low pass filter
 140         // based on GPS altitude if available
 141         if (this->useGPS && IS_SET(state->updated, SENSORUPDATES_pos)) {
 142             this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
 143                                (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
 144         }
 145         // calculate bias corrected altitude
 146         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
 147             this->baroAlt   = state->baro[0];
 148             state->baro[0] -= this->baroOffset;
 149         }
 150         return FILTERRESULT_OK;
 151     }
 152 }
 153
 154
 155 /**
 156  * @}
 157  * @}
 158  */