rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-560 Add better filtering before set the baroOffset value

Browse Source
This commit is contained in:
Laurent Lalanne 2017-11-26 11:54:37 +01:00
parent 8935906fca
commit 8cc85923d7
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -7,7 +7,8 @@
* @{ * @{
* *
* @file filterbaro.c * @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 * @brief Barometric altitude filter, calculates altitude offset based on
* GPS altitude offset if available * GPS altitude offset if available
* *
@ -37,7 +38,8 @@
// Private constants // Private constants
#define STACK_REQUIRED 128 #define STACK_REQUIRED 128
#define INIT_CYCLES 100 #define INIT_CYCLES 500
#define BARO_OFFSET_ALPHA 0.02f
// Private types // Private types
struct data { struct data {
@ -119,7 +121,11 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
// Initialize to current altitude reading at initial location // Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (IS_SET(state->updated, SENSORUPDATES_baro)) {
if (this->first_run < INIT_CYCLES || !this->useGPS) { 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->baroAlt = state->baro[0];
this->first_run--; this->first_run--;
} }