diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c new file mode 100644 index 000000000..01d38a1b8 --- /dev/null +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -0,0 +1,210 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup State Estimation + * @brief Acquires sensor data and computes state estimate + * @{ + * + * @file filteraltitude.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief Barometric altitude filter, calculates vertical speed and true + * altitude based on Barometric altitude and accelerometers + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/stateestimation.h" +#include +#include + +#include + +// Private constants + +#define STACK_REQUIRED 128 + +#define DT_ALPHA 1e-3f + +// Private types +struct data { + float state[4]; // state = altitude,velocity,accel_offset,accel + float pos[3]; // position updates from other filters + float vel[3]; // position updates from other filters + float dTA; + float dTA2; + int32_t lastTime; + float accelLast; + float baroLast; + int32_t baroLastTime; + bool first_run; + AltitudeFilterSettingsData settings; +}; + +// Private variables + +// Private functions + +static int32_t init(stateFilter *self); +static int32_t filter(stateFilter *self, stateEstimation *state); + + +int32_t filterAltitudeInitialize(stateFilter *handle) +{ + handle->init = &init; + handle->filter = &filter; + handle->localdata = pvPortMalloc(sizeof(struct data)); + AttitudeStateInitialize(); + AltitudeFilterSettingsInitialize(); + return STACK_REQUIRED; +} + +static int32_t init(stateFilter *self) +{ + struct data *this = (struct data *)self->localdata; + + this->state[0] = 0.0f; + this->state[1] = 0.0f; + this->state[2] = 0.0f; + this->state[3] = 0.0f; + this->pos[0] = 0.0f; + this->pos[1] = 0.0f; + this->pos[2] = 0.0f; + this->vel[0] = 0.0f; + this->vel[1] = 0.0f; + this->vel[2] = 0.0f; + this->dTA = -1.0f; + this->dTA2 = -1.0f; + this->baroLast = 0.0f; + this->accelLast = 0.0f; + this->first_run = 1; + AltitudeFilterSettingsGet(&this->settings); + return 0; +} + +static int32_t filter(stateFilter *self, stateEstimation *state) +{ + struct data *this = (struct data *)self->localdata; + + if (this->first_run) { + // Initialize to current altitude reading at initial location + if (IS_SET(state->updated, SENSORUPDATES_accel)) { + this->lastTime = PIOS_DELAY_GetRaw(); + } + if (IS_SET(state->updated, SENSORUPDATES_baro)) { + this->first_run = 0; + this->baroLastTime = PIOS_DELAY_GetRaw(); + } + } else { + // save existing position and velocity updates so GPS will still work + if (IS_SET(state->updated, SENSORUPDATES_pos)) { + this->pos[0] = state->pos[0]; + this->pos[1] = state->pos[1]; + this->pos[2] = state->pos[2]; + state->pos[2] = -this->state[0]; + } + if (IS_SET(state->updated, SENSORUPDATES_vel)) { + this->vel[0] = state->vel[0]; + this->vel[1] = state->vel[1]; + this->vel[2] = state->vel[2]; + state->vel[2] = -this->state[1]; + } + if (IS_SET(state->updated, SENSORUPDATES_accel)) { + // rotate accels into global coordinate frame + AttitudeStateData att; + AttitudeStateGet(&att); + float Rbe[3][3]; + Quaternion2R(&att.q1, Rbe); + float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); + + // low pass filter accelerometers + this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current; + + // correct accel offset (low pass zeroing) + this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3]; + + // correct velocity and position state (integration) + // low pass for average dT, compensate timing jitter from scheduler + float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f; + this->lastTime = PIOS_DELAY_GetRaw(); + if (dT < 0.001f) { + dT = 0.001f; + } + if (this->dTA < 0) { + this->dTA = dT; + } else { + this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA; + } + float speedLast = this->state[1]; + + this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA; + this->accelLast = this->state[3] - this->state[2]; + + this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA; + + + state->pos[0] = this->pos[0]; + state->pos[1] = this->pos[1]; + state->pos[2] = -this->state[0]; + state->updated |= SENSORUPDATES_pos; + + state->vel[0] = this->vel[0]; + state->vel[1] = this->vel[1]; + state->vel[2] = -this->state[1]; + state->updated |= SENSORUPDATES_vel; + } + if (IS_SET(state->updated, SENSORUPDATES_baro)) { + // correct the altitude state (simple low pass) + this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0]; + + // correct the velocity state (low pass differentiation) + // low pass for average dT, compensate timing jitter from scheduler + float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f; + this->baroLastTime = PIOS_DELAY_GetRaw(); + if (dT < 0.001f) { + dT = 0.001f; + } + if (this->dTA2 < 0) { + this->dTA2 = dT; + } else { + this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA; + } + this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2; + this->baroLast = state->baro[0]; + + state->pos[0] = this->pos[0]; + state->pos[1] = this->pos[1]; + state->pos[2] = -this->state[0]; + state->updated |= SENSORUPDATES_pos; + + state->vel[0] = this->vel[0]; + state->vel[1] = this->vel[1]; + state->vel[2] = -this->state[1]; + state->updated |= SENSORUPDATES_vel; + } + } + + return 0; +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 7c5793717..e31454c70 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -44,9 +44,9 @@ // Private types struct data { - float baroOffset; - float baroAlt; - bool first_run; + float baroOffset; + float baroAlt; + int16_t first_run; }; // Private variables @@ -70,7 +70,7 @@ static int32_t init(stateFilter *self) struct data *this = (struct data *)self->localdata; this->baroOffset = 0.0f; - this->first_run = 1; + this->first_run = 100; return 0; } @@ -81,9 +81,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->first_run = 0; - this->baroOffset = state->baro[0]; - this->baroAlt = state->baro[0]; + this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0]; + this->baroAlt = this->baroOffset; + this->first_run--; + UNSET_MASK(state->updated, SENSORUPDATES_baro); } } else { // Track barometric altitude offset with a low pass filter diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index 9b954874a..e80aa3148 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -64,6 +64,7 @@ typedef struct stateFilterStruct { int32_t filterMagInitialize(stateFilter *handle); int32_t filterBaroInitialize(stateFilter *handle); +int32_t filterAltitudeInitialize(stateFilter *handle); int32_t filterAirInitialize(stateFilter *handle); int32_t filterStationaryInitialize(stateFilter *handle); int32_t filterCFInitialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 5ed371844..8fa673c2d 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -123,6 +123,7 @@ static filterPipeline *filterChain = NULL; // different filters available to state estimation static stateFilter magFilter; static stateFilter baroFilter; +static stateFilter altitudeFilter; static stateFilter airFilter; static stateFilter stationaryFilter; static stateFilter cfFilter; @@ -138,9 +139,12 @@ static filterPipeline *cfQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &baroFilter, .next = &(filterPipeline) { - .filter = &cfFilter, - .next = NULL, - }, + .filter = &altitudeFilter, + .next = &(filterPipeline) { + .filter = &cfFilter, + .next = NULL, + } + } } } }; @@ -151,8 +155,11 @@ static const filterPipeline *cfmQueue = &(filterPipeline) { .next = &(filterPipeline) { .filter = &baroFilter, .next = &(filterPipeline) { - .filter = &cfmFilter, - .next = NULL, + .filter = &altitudeFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } } } } @@ -237,6 +244,7 @@ int32_t StateEstimationInitialize(void) // Initialize Filters stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter)); stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter)); + stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter)); stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter)); stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter)); stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter)); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 2381e7c65..fe3e9d05a 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -93,7 +93,7 @@ UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRCFILENAMES += oplinkstatus - +UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 996708520..ff47a51f2 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -94,7 +94,8 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 58ad986ca..d9dcd6116 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -95,6 +95,7 @@ UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudefiltersettings UAVOBJSRCFILENAMES += revosettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += ekfconfiguration diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 89b83c23b..4a151c31b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -33,6 +33,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \ + $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \ $$UAVOBJECT_SYNTHETICS/ekfconfiguration.h \ $$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \ $$UAVOBJECT_SYNTHETICS/revocalibration.h \ @@ -119,6 +120,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \ $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.cpp \ $$UAVOBJECT_SYNTHETICS/ekfconfiguration.cpp \ $$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \ $$UAVOBJECT_SYNTHETICS/revocalibration.cpp \ diff --git a/shared/uavobjectdefinition/altitudefiltersettings.xml b/shared/uavobjectdefinition/altitudefiltersettings.xml new file mode 100644 index 000000000..bc6222ad1 --- /dev/null +++ b/shared/uavobjectdefinition/altitudefiltersettings.xml @@ -0,0 +1,12 @@ + + + Settings for the @ref State Estimator module plugin altitudeFilter + + + + + + + + +