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-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

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

@ -45,14 +45,13 @@
// 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 homeLocationBe[3];
float magAverage[3]; float magBe2;
float magBias[3]; float magBias[3];
}; };
// Private variables // Private variables
@ -78,10 +77,11 @@ 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,56 +106,43 @@ 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 // set errors
float warning2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2]; if (minWarning2 < mag2 && mag2 < maxWarning2) {
float error2 = this->revoSettings.MagnetometerMaxDeviation.Error * this->revoSettings.MagnetometerMaxDeviation.Error * warning2; this->warningcount = 0;
warning2 = this->revoSettings.MagnetometerMaxDeviation.Warning * this->revoSettings.MagnetometerMaxDeviation.Warning * warning2; this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
// calculate difference } else if (minError2 < mag2 && mag2 < maxError2) {
expected[0] = expected[0] - this->magAverage[0]; this->errorcount = 0;
expected[1] = expected[1] - this->magAverage[1]; if (this->warningcount > ALARM_THRESHOLD) {
expected[2] = expected[2] - this->magAverage[2]; AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
float deviation2 = expected[0] * expected[0] + expected[1] * expected[1] + expected[2] * expected[2];
// set errors
if (deviation2 < warning2) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
} else if (deviation2 < error2) {
this->errorcount = 0;
if (this->warningcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
} else {
this->warningcount++;
}
} else { } else {
if (this->errorcount > ALARM_THRESHOLD) { this->warningcount++;
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); }
} else { } else {
this->errorcount++; if (this->errorcount > ALARM_THRESHOLD) {
} AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL);
} else {
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];