From 8cc85923d76b18037bef165c4fabb0a77b754897 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 26 Nov 2017 11:54:37 +0100 Subject: [PATCH] LP-560 Add better filtering before set the baroOffset value --- flight/modules/StateEstimation/filterbaro.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index bb1150b34..6aefdb757 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -7,7 +7,8 @@ * @{ * * @file filterbaro.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. * @brief Barometric altitude filter, calculates altitude offset based on * GPS altitude offset if available * @@ -36,8 +37,9 @@ // Private constants -#define STACK_REQUIRED 128 -#define INIT_CYCLES 100 +#define STACK_REQUIRED 128 +#define INIT_CYCLES 500 +#define BARO_OFFSET_ALPHA 0.02f // Private types struct data { @@ -119,7 +121,11 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (this->first_run < INIT_CYCLES || !this->useGPS) { - this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt); + if (this->first_run > INIT_CYCLES - 2) { + this->baroOffset = (state->baro[0] + this->gpsAlt); + } + // Set baroOffset using filtering, this allow better altitude zeroing + this->baroOffset = ((1.0f - BARO_OFFSET_ALPHA) * this->baroOffset) + (BARO_OFFSET_ALPHA * (state->baro[0] + this->gpsAlt)); this->baroAlt = state->baro[0]; this->first_run--; }