Skip to content

Commit 45d2913

Browse files
f5sohfilnet
authored andcommitted
Merged in f5soh/librepilot/LP-560_BaroOffset (pull request #479)
LP-560 Add better filtering before set the baroOffset value Approved-by: Lalanne Laurent <f5soh@free.fr> Approved-by: Philippe Renon <philippe_renon@yahoo.fr> Approved-by: Alessio Morale <alessiomorale@gmail.com>
2 parents 606cf8c + 8cc8592 commit 45d2913

1 file changed

Lines changed: 10 additions & 4 deletions

File tree

flight/modules/StateEstimation/filterbaro.c

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,8 @@
77
* @{
88
*
99
* @file filterbaro.c
10-
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
10+
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
11+
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
1112
* @brief Barometric altitude filter, calculates altitude offset based on
1213
* GPS altitude offset if available
1314
*
@@ -36,8 +37,9 @@
3637

3738
// Private constants
3839

39-
#define STACK_REQUIRED 128
40-
#define INIT_CYCLES 100
40+
#define STACK_REQUIRED 128
41+
#define INIT_CYCLES 500
42+
#define BARO_OFFSET_ALPHA 0.02f
4143

4244
// Private types
4345
struct data {
@@ -119,7 +121,11 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
119121
// Initialize to current altitude reading at initial location
120122
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
121123
if (this->first_run < INIT_CYCLES || !this->useGPS) {
122-
this->baroOffset = (((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)) * this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * (state->baro[0] + this->gpsAlt);
124+
if (this->first_run > INIT_CYCLES - 2) {
125+
this->baroOffset = (state->baro[0] + this->gpsAlt);
126+
}
127+
// Set baroOffset using filtering, this allow better altitude zeroing
128+
this->baroOffset = ((1.0f - BARO_OFFSET_ALPHA) * this->baroOffset) + (BARO_OFFSET_ALPHA * (state->baro[0] + this->gpsAlt));
123129
this->baroAlt = state->baro[0];
124130
this->first_run--;
125131
}

0 commit comments

Comments
 (0)