rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1235 allows AccelBiasValue to quickly reach the target value at initialization

Browse Source
This commit is contained in:
Alessio Morale 2014-02-23 12:57:59 +01:00
parent a2386f548b
commit 7f6175d817
1 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/modules/StateEstimation/filteraltitude.c
View File

@ -60,6 +60,7 @@ struct data {
float accelLast; float accelLast;
float baroLast; float baroLast;
bool first_run; bool first_run;
portTickType initTimer;
AltitudeFilterSettingsData settings; AltitudeFilterSettingsData settings;
}; };
@ -119,6 +120,7 @@ static int32_t 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)) {
this->first_run = 0; this->first_run = 0;
this->initTimer = xTaskGetTickCount();
} }
} else { } else {
// save existing position and velocity updates so GPS will still work // save existing position and velocity updates so GPS will still work
@ -143,11 +145,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
// low pass filter accelerometers // low pass filter accelerometers
this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < 5000) {
// correct accel offset (low pass zeroing) // allow the offset to reach quickly the target value in case of small AccelDriftKi
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; this->accelBiasState = (1.0f - 0.2f) * this->accelBiasState + 0.2f * this->accelState;
} else {
// correct accel offset (low pass zeroing)
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
}
// correct velocity and position state (integration) // correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler // low pass for average dT, compensate timing jitter from scheduler
// //