rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

bugfix in baro alt filter plugin

Browse Source
This commit is contained in:
Corvus Corax 2013-05-25 02:02:53 +02:00
parent 902bf29c92
commit 87b1b4873c
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/StateEstimation/filterbaro.c
View File

@ -45,6 +45,7 @@
// Private types
struct data {
float baroOffset;
float baroAlt;
bool first_run;
};
@ -82,6 +83,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroOffset = state->baro[0];
this->baroAlt = state->baro[0];
}
} else {
// Track barometric altitude offset with a low pass filter
@ -89,10 +91,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-state->pos[2] - state->baro[0]);
* (this->baroAlt + state->pos[2]);
}
// calculate bias corrected altitude
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
}