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

OP-1235 cleanup "magic numbers"

Browse Source
This commit is contained in:
Alessio Morale 2014-04-08 20:51:16 +02:00
parent 7f6175d817
commit 4764186a5f
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,6 +38,12 @@
// Private constants // 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 STACK_REQUIRED 128
#define DT_ALPHA 1e-2f #define DT_ALPHA 1e-2f
@ -146,9 +152,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// 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) { if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION) {
// allow the offset to reach quickly the target value in case of small AccelDriftKi // 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 { } else {
// correct accel offset (low pass zeroing) // correct accel offset (low pass zeroing)
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;