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