From aa560653d4dd2c69c88b0e08b9df673c0e32dddd Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 19 Jul 2013 09:53:44 +0300 Subject: [PATCH 1/2] Make baro altitude offset (relative to GPS alt) filter configurable --- flight/modules/StateEstimation/filterbaro.c | 25 +++++++++++++-------- shared/uavobjectdefinition/revosettings.xml | 2 ++ 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index e31454c70..fe276e468 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -32,19 +32,16 @@ #include "inc/stateestimation.h" +#include + // Private constants #define STACK_REQUIRED 64 -// low pass filter configuration to calculate offset -// of barometric altitude sensor -// reasoning: updates at: 10 Hz, tau= 300 s settle time -// exp(-(1/f) / tau ) ~=~ 0.9997 -#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f - // Private types struct data { float baroOffset; + float baroGPSOffsetCorrectionAlpha; float baroAlt; int16_t first_run; }; @@ -71,6 +68,17 @@ static int32_t init(stateFilter *self) this->baroOffset = 0.0f; this->first_run = 100; + + // Low pass filter configuration to calculate offset of barometric altitude sensor. + // Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062 + // Set BaroOffsetFilterTau = 0 to completely disable baro offset updates. + RevoSettingsData revoSettings; + RevoSettingsInitialize(); + RevoSettingsGet(&revoSettings); + + this->baroGPSOffsetCorrectionAlpha = (revoSettings.BaroGPSOffsetCorrectionTau < 0.0001f) ? + 1.0f : expf(-(1.0f / revoSettings.BaroGPSOffsetCorrectionFrequency) / revoSettings.BaroGPSOffsetCorrectionTau); + return 0; } @@ -90,9 +98,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Track barometric altitude offset with a low pass filter // based on GPS altitude if available if (IS_SET(state->updated, SENSORUPDATES_pos)) { - this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (this->baroAlt + state->pos[2]); + this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha + + (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]); } // calculate bias corrected altitude if (IS_SET(state->updated, SENSORUPDATES_baro)) { diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 26e129bdd..4480f0e56 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -2,6 +2,8 @@ Settings for the revo to control the algorithm and what is updated + + From 1bfa66c2b653a1aaf70d1952081dbf4ff1ab4b1e Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 26 Jul 2013 22:14:08 +0300 Subject: [PATCH 2/2] Change settings for baro altitude offset correction --- flight/modules/StateEstimation/filterbaro.c | 9 +-------- shared/uavobjectdefinition/revosettings.xml | 8 ++++++-- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index fe276e468..739fa5d72 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -69,15 +69,8 @@ static int32_t init(stateFilter *self) this->baroOffset = 0.0f; this->first_run = 100; - // Low pass filter configuration to calculate offset of barometric altitude sensor. - // Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062 - // Set BaroOffsetFilterTau = 0 to completely disable baro offset updates. - RevoSettingsData revoSettings; RevoSettingsInitialize(); - RevoSettingsGet(&revoSettings); - - this->baroGPSOffsetCorrectionAlpha = (revoSettings.BaroGPSOffsetCorrectionTau < 0.0001f) ? - 1.0f : expf(-(1.0f / revoSettings.BaroGPSOffsetCorrectionFrequency) / revoSettings.BaroGPSOffsetCorrectionTau); + RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); return 0; } diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 4480f0e56..0cf9a4561 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -2,8 +2,12 @@ Settings for the revo to control the algorithm and what is updated - - + + + +