flight/modules/StateEstimation/filterlla.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       filterlla.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  11  * @brief      Computes NED position from GPS LLA data
  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 #include <CoordinateConversions.h>
  34
  35 #include <homelocation.h>
  36 #include <gpssettings.h>
  37 #include <gpspositionsensor.h>
  38
  39 // Private constants
  40
  41 #define STACK_REQUIRED 256
  42
  43 // Private types
  44 struct data {
  45     GPSSettingsData  settings;
  46     HomeLocationData home;
  47     float HomeECEF[3];
  48     float HomeRne[3][3];
  49 };
  50
  51 // Private variables
  52
  53 // Private functions
  54
  55 static int32_t init(stateFilter *self);
  56 static filterResult filter(stateFilter *self, stateEstimation *state);
  57
  58
  59 int32_t filterLLAInitialize(stateFilter *handle)
  60 {
  61     handle->init      = &init;
  62     handle->filter    = &filter;
  63     handle->localdata = pios_malloc(sizeof(struct data));
  64     GPSSettingsInitialize();
  65     GPSPositionSensorInitialize();
  66     HomeLocationInitialize();
  67     return STACK_REQUIRED;
  68 }
  69
  70 static int32_t init(__attribute__((unused)) stateFilter *self)
  71 {
  72     struct data *this = (struct data *)self->localdata;
  73
  74     GPSSettingsGet(&this->settings);
  75     HomeLocationGet(&this->home);
  76     if (this->home.Set == HOMELOCATION_SET_TRUE) {
  77         // calculate home location coordinate reference
  78         int32_t LLAi[3] = {
  79             this->home.Latitude,
  80             this->home.Longitude,
  81             (int32_t)(this->home.Altitude * 1e4f),
  82         };
  83         LLA2ECEF(LLAi, this->HomeECEF);
  84         RneFromLLA(LLAi, this->HomeRne);
  85     }
  86     return 0;
  87 }
  88
  89 static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
  90 {
  91     struct data *this = (struct data *)self->localdata;
  92
  93     // cannot update local NED if home location is unset
  94     if (this->home.Set != HOMELOCATION_SET_TRUE) {
  95         return FILTERRESULT_WARNING;
  96     }
  97
  98     // only do stuff if we have a valid GPS update
  99     if (IS_SET(state->updated, SENSORUPDATES_lla)) {
 100         // LLA information is not part of the state blob, due to its non standard layout (fixed point representation, quality of signal, ...)
 101         // this filter deals with the gory details of interpreting it and storing it in a standard Cartesian position state
 102         GPSPositionSensorData gpsdata;
 103         GPSPositionSensorGet(&gpsdata);
 104
 105         // check if we have a valid GPS signal (not checked by StateEstimation istelf)
 106         if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) &&
 107             (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
 108             (gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) {
 109             int32_t LLAi[3] = {
 110                 gpsdata.Latitude,
 111                 gpsdata.Longitude,
 112                 (int32_t)((gpsdata.Altitude + gpsdata.GeoidSeparation) * 1e4f),
 113             };
 114             LLA2Base(LLAi, this->HomeECEF, this->HomeRne, state->pos);
 115             state->updated |= SENSORUPDATES_pos;
 116         }
 117     }
 118
 119     return FILTERRESULT_OK;
 120 }
 121
 122 /**
 123  * @}
 124  * @}
 125  */