rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1235 make Initialization Acceldrift KI configurable

Browse Source
This commit is contained in:
Alessio Morale 2014-04-14 10:58:46 +02:00
parent 4764186a5f
commit 5763ed3d8d
2 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,11 +38,8 @@
// 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
// duration of accel bias initialization phase
#define INITIALIZATION_DURATION_MS 5000
#define STACK_REQUIRED 128
@ -152,9 +149,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) < INITIALIZATION_DURATION) {
if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
// allow the offset to reach quickly the target value in case of small AccelDriftKi
this->accelBiasState = (1.0f - INITIALIZATION_ACCELDRIFT_KI) * this->accelBiasState + INITIALIZATION_ACCELDRIFT_KI * this->accelState;
this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
} else {
// correct accel offset (low pass zeroing)
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;

1
shared/uavobjectdefinition/altitudefiltersettings.xml
View File

@ -3,6 +3,7 @@
<description>Settings for the @ref State Estimator module plugin altitudeFilter</description>
<field name="AccelLowPassKp" units="m/s^2" type="float" elements="1" defaultvalue="0.04"/>
<field name="AccelDriftKi" units="m/s^2" type="float" elements="1" defaultvalue="0.0005"/>
<field name="InitializationAccelDriftKi" units="m/s^2" type="float" elements="1" defaultvalue="0.2"/>
<field name="BaroKp" units="m" type="float" elements="1" defaultvalue="0.04"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>