flight/modules/StateEstimation/filterstationary.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       filterstationary.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Provides "fake" stationary state data for indoor mode
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  ******************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "inc/stateestimation.h"
  33 // Fake pos rate in uS
  34 #define FILTERSTATIONARY_FAKEPOSRATE 100000
  35 // Private constants
  36
  37 #define STACK_REQUIRED               64
  38
  39 // Private types
  40 // Private types
  41 struct data {
  42     uint32_t lastUpdate;
  43 };
  44 // Private variables
  45
  46 // Private functions
  47
  48 static int32_t init(stateFilter *self);
  49 static filterResult filter(stateFilter *self, stateEstimation *state);
  50
  51
  52 int32_t filterStationaryInitialize(stateFilter *handle)
  53 {
  54     handle->init      = &init;
  55     handle->filter    = &filter;
  56     handle->localdata = pios_malloc(sizeof(struct data));
  57     return STACK_REQUIRED;
  58 }
  59
  60 static int32_t init(stateFilter *self)
  61 {
  62     struct data *this = (struct data *)self->localdata;
  63
  64     this->lastUpdate = PIOS_DELAY_GetRaw();
  65     return 0;
  66 }
  67
  68 static filterResult filter(stateFilter *self, stateEstimation *state)
  69 {
  70     struct data *this = (struct data *)self->localdata;
  71
  72     if (PIOS_DELAY_DiffuS(this->lastUpdate) > FILTERSTATIONARY_FAKEPOSRATE) {
  73         this->lastUpdate = PIOS_DELAY_GetRaw();
  74         state->pos[0]    = 0.0f;
  75         state->pos[1]    = 0.0f;
  76         state->pos[2]    = 0.0f;
  77         state->updated  |= SENSORUPDATES_pos;
  78
  79         state->vel[0]    = 0.0f;
  80         state->vel[1]    = 0.0f;
  81         state->vel[2]    = 0.0f;
  82         state->updated  |= SENSORUPDATES_vel;
  83     }
  84     return FILTERRESULT_OK;
  85 }
  86
  87 /**
  88  * @}
  89  * @}
  90  */