rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

OP-1161 reimplemented magnetometer plausibikity check using vector

Browse Source 
length only
This commit is contained in:
Corvus Corax 2014-05-07 23:47:14 +02:00
parent 2da1f2aa09
commit 1add404d80
1 changed files with 44 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
flight/modules/StateEstimation/filtermag.c
View File

@ -45,13 +45,12 @@
// Private types // Private types
struct data { struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration; RevoCalibrationData revoCalibration;
RevoSettingsData revoSettings; RevoSettingsData revoSettings;
uint16_t idlecounter;
uint8_t warningcount; uint8_t warningcount;
uint8_t errorcount; uint8_t errorcount;
float magAverage[3]; float homeLocationBe[3];
float magBe2;
float magBias[3]; float magBias[3];
}; };
@ -79,9 +78,10 @@ static int32_t init(stateFilter *self)
struct data *this = (struct data *)self->localdata; struct data *this = (struct data *)self->localdata;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
this->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f; this->warningcount = this->errorcount = 0;
this->idlecounter = this->warningcount = this->errorcount = 0; HomeLocationBeGet(this->homeLocationBe);
HomeLocationGet(&this->homeLocation); // magBe2 holds the squared magnetic vector length (extpected)
this->magBe2 = this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1] + this->homeLocationBe[2] * this->homeLocationBe[2];
RevoCalibrationGet(&this->revoCalibration); RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings); RevoSettingsGet(&this->revoSettings);
return 0; return 0;
@ -106,44 +106,32 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
*/ */
static void checkMagValidity(struct data *this, float mag[3]) static void checkMagValidity(struct data *this, float mag[3])
{ {
#define MAG_LOW_PASS_ALPHA 0.2f #define ALARM_THRESHOLD 5
#define IDLE_COUNT 10
#define ALARM_THRESHOLD 3
// low pass filter sensor to not give warnings due to noise // mag2 holds the actual magnetic vector length (squared)
this->magAverage[0] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[0] + MAG_LOW_PASS_ALPHA * mag[0]; float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2];
this->magAverage[1] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[1] + MAG_LOW_PASS_ALPHA * mag[1];
this->magAverage[2] = (1.0f - MAG_LOW_PASS_ALPHA) * this->magAverage[2] + MAG_LOW_PASS_ALPHA * mag[2];
// throttle this check, thanks to low pass filter it is not necessary every iteration // warning and error thresholds
if (!this->idlecounter--) { // avoud sqrt() : minlimit<actual<maxlimit === minlimit²<actual²<maxlimit²
this->idlecounter = IDLE_COUNT; //
// actual = |mag|
// minlimit = |Be| - maxDeviation*|Be| = |Be| * (1 - maxDeviation)
// maxlimit = |Be| + maxDeviation*|Be| = |Be| * (1 + maxDeviation)
// minlimit² = |Be|² * ( 1 - 2*maxDeviation + maxDeviation²)
// maxlimit² = |Be|² * ( 1 + 2*maxDeviation + maxDeviation²)
//
// calculate expected Be vector float minWarning2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
AttitudeStateData attitudeState; float maxWarning2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Warning + this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning);
AttitudeStateGet(&attitudeState); float minError2 = this->magBe2 * (1.0f - 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
float Rot[3][3]; float maxError2 = this->magBe2 * (1.0f + 2.0f * this->revoSettings.MagnetometerMaxDeviation.Error + this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error);
float expected[3];
Quaternion2R(&attitudeState.q1, Rot);
rot_mult(Rot, this->homeLocation.Be, expected);
// calculate maximum allowed deviation
float warning2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
float error2 = this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error * warning2;
warning2 = this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning * warning2;
// calculate difference
expected[0] = expected[0] - this->magAverage[0];
expected[1] = expected[1] - this->magAverage[1];
expected[2] = expected[2] - this->magAverage[2];
float deviation2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
// set errors // set errors
if (deviation2 < warning2) { if (minWarning2 < mag2 && mag2 < maxWarning2) {
this->warningcount = 0; this->warningcount = 0;
this->errorcount = 0; this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (deviation2 < error2) { } else if (minError2 < mag2 && mag2 < maxError2) {
this->errorcount = 0; this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) { if (this->warningcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
@ -157,7 +145,6 @@ static void checkMagValidity(struct data *this, float mag[3])
this->errorcount++; this->errorcount++;
} }
} }
}
} }
@ -209,8 +196,8 @@ static void magOffsetEstimation(struct data *this, float mag[3])
} }
#else // if 0 #else // if 0
const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]); const float Rxy = sqrtf(this->homeLocationBe[0] * this->homeLocationBe[0] + this->homeLocationBe[1] * this->homeLocationBe[1]);
const float Rz = this->homeLocation.Be[2]; const float Rz = this->homeLocationBe[2];
const float rate = this->revoCalibration.MagBiasNullingRate; const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3]; float Rot[3][3];