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

OP-1048 fix CF stateestimation bug using AccelTau.

Browse Source 
move *_filtered to stateestimator data struct and initialize them at init.
they were previously used uninitialized thus causing estimation to fail validations, restarting the filter continously

+review OPReview
This commit is contained in:
Alessio Morale 2013-07-26 19:04:27 +02:00 committed by Oleg Semyonov
parent 50f82b3768
commit b4d2423680
1 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
flight/modules/StateEstimation/filtercf.c
View File

@ -55,6 +55,8 @@ struct data {
bool useMag; bool useMag;
float currentAccel[3]; float currentAccel[3];
float currentMag[3]; float currentMag[3];
float accels_filtered[3];
float grot_filtered[3];
float gyroBias[3]; float gyroBias[3];
bool accelUpdated; bool accelUpdated;
bool magUpdated; bool magUpdated;
@ -266,7 +268,12 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
RPY2Quaternion(&attitudeState.Roll, attitude); RPY2Quaternion(&attitudeState.Roll, attitude);
this->first_run = 0; this->first_run = 0;
this->accels_filtered[0] = 0.0f;
this->accels_filtered[1] = 0.0f;
this->accels_filtered[2] = 0.0f;
this->grot_filtered[0] = 0.0f;
this->grot_filtered[1] = 0.0f;
this->grot_filtered[2] = 0.0f;
this->timeval = PIOS_DELAY_GetRaw(); this->timeval = PIOS_DELAY_GetRaw();
this->starttime = this->timeval; this->starttime = this->timeval;
@ -316,9 +323,8 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
// Get the current attitude estimate // Get the current attitude estimate
quat_copy(&attitudeState.q1, attitude); quat_copy(&attitudeState.q1, attitude);
float accels_filtered[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations. // Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(this, accel, accels_filtered); apply_accel_filter(this, accel, this->accels_filtered);
// Rotate gravity to body frame and cross with accels // Rotate gravity to body frame and cross with accels
float grot[3]; float grot[3];
@ -326,13 +332,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1])); grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]); grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
float grot_filtered[3];
float accel_err[3]; float accel_err[3];
apply_accel_filter(this, grot, grot_filtered); apply_accel_filter(this, grot, this->grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
CrossProduct((const float *)this->accels_filtered, (const float *)this->grot_filtered, accel_err);
// Account for accel magnitude // Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]);
if (accel_mag < 1.0e-3f) { if (accel_mag < 1.0e-3f) {
return 2; // safety feature copied from CC return 2; // safety feature copied from CC
} }
@ -340,7 +346,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
// Account for filtered gravity vector magnitude // Account for filtered gravity vector magnitude
float grot_mag; float grot_mag;
if (this->accel_filter_enabled) { if (this->accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); grot_mag = sqrtf(this->grot_filtered[0] * this->grot_filtered[0] + this->grot_filtered[1] * this->grot_filtered[1] + this->grot_filtered[2] * this->grot_filtered[2]);
} else { } else {
grot_mag = 1.0f; grot_mag = 1.0f;
} }