flight/modules/StateEstimation/filterair.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       filterair.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Airspeed filter, calculates true airspeed based on indicated
  12  *             airspeed and uncorrected barometric altitude
  13  *             NOTE: This Sensor uses UNCORRECTED barometric altitude for
  14  *             correction --  run before barometric bias correction!
  15  *
  16  * @see        The GNU Public License (GPL) Version 3
  17  *
  18  ******************************************************************************/
  19 /*
  20  * This program is free software; you can redistribute it and/or modify
  21  * it under the terms of the GNU General Public License as published by
  22  * the Free Software Foundation; either version 3 of the License, or
  23  * (at your option) any later version.
  24  *
  25  * This program is distributed in the hope that it will be useful, but
  26  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  27  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  28  * for more details.
  29  *
  30  * You should have received a copy of the GNU General Public License along
  31  * with this program; if not, write to the Free Software Foundation, Inc.,
  32  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  33  */
  34
  35 #include "inc/stateestimation.h"
  36
  37 // Private constants
  38
  39 #define STACK_REQUIRED 64
  40
  41 // simple IAS to TAS aproximation - 2% increase per 1000ft
  42 // since we do not have flowing air temperature information
  43 #define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
  44
  45 // Private types
  46 struct data {
  47     float altitude;
  48 };
  49
  50 // Private functions
  51
  52 static int32_t init(stateFilter *self);
  53 static filterResult filter(stateFilter *self, stateEstimation *state);
  54
  55
  56 int32_t filterAirInitialize(stateFilter *handle)
  57 {
  58     handle->init      = &init;
  59     handle->filter    = &filter;
  60     handle->localdata = pios_malloc(sizeof(struct data));
  61     return STACK_REQUIRED;
  62 }
  63
  64 static int32_t init(stateFilter *self)
  65 {
  66     struct data *this = (struct data *)self->localdata;
  67
  68     this->altitude = 0.0f;
  69     return 0;
  70 }
  71
  72 static filterResult filter(stateFilter *self, stateEstimation *state)
  73 {
  74     struct data *this = (struct data *)self->localdata;
  75
  76     // take static pressure altitude estimation for
  77     if (IS_SET(state->updated, SENSORUPDATES_baro)) {
  78         this->altitude = state->baro[0];
  79     }
  80     // calculate true airspeed estimation
  81     if (IS_SET(state->updated, SENSORUPDATES_airspeed) && (state->airspeed[1] < 0.f)) {
  82         state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
  83     }
  84
  85     return FILTERRESULT_OK;
  86 }
  87
  88
  89 /**
  90  * @}
  91  * @}
  92  */