diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 0bea2411f..3b4a047f8 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -38,6 +38,12 @@ // Private constants +// Acceldrift ki value used at startup to let it quickly converge +// to its target value +#define INITIALIZATION_ACCELDRIFT_KI 0.2f +// lenght of accel bias initialization phase +#define INITIALIZATION_DURATION 5000 + #define STACK_REQUIRED 128 #define DT_ALPHA 1e-2f @@ -146,9 +152,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // low pass filter accelerometers this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; - if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < 5000) { + if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION) { // allow the offset to reach quickly the target value in case of small AccelDriftKi - this->accelBiasState = (1.0f - 0.2f) * this->accelBiasState + 0.2f * this->accelState; + this->accelBiasState = (1.0f - INITIALIZATION_ACCELDRIFT_KI) * this->accelBiasState + INITIALIZATION_ACCELDRIFT_KI * this->accelState; } else { // correct accel offset (low pass zeroing) this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;