rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

small fix to baro state filter - more init cycles

Browse Source
This commit is contained in:
Corvus Corax 2013-12-08 13:35:00 +01:00
parent 5cc8dedadf
commit 66b0ffd682
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,6 +37,7 @@
// Private constants // Private constants
#define STACK_REQUIRED 64 #define STACK_REQUIRED 64
#define INIT_CYCLES 1000
// Private types // Private types
struct data { struct data {
@ -67,7 +68,7 @@ static int32_t init(stateFilter *self)
struct data *this = (struct data *)self->localdata; struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f; this->baroOffset = 0.0f;
this->first_run = 100; this->first_run = INIT_CYCLES;
RevoSettingsInitialize(); RevoSettingsInitialize();
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
@ -82,7 +83,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->first_run) { if (this->first_run) {
// 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)) {
this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0]; this->baroOffset = (INIT_CYCLES - this->first_run) / INIT_CYCLES * this->baroOffset + (this->first_run / INIT_CYCLES) * state->baro[0];
this->baroAlt = this->baroOffset; this->baroAlt = this->baroOffset;
this->first_run--; this->first_run--;
UNSET_MASK(state->updated, SENSORUPDATES_baro); UNSET_MASK(state->updated, SENSORUPDATES_baro);