rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

OP-1161 changed the way magnetometer-errors are handled - and defaults

Browse Source 
* ekf will ignore magnetometer errors for initialization
 (errors might be caused by not yet existing attitude estimation)
* defaults changed for filtering
* magnetometer sensor will be available for all filters, alarms need to
  be checked separately
This commit is contained in:
Corvus Corax 2014-01-14 21:09:14 +01:00
parent 75b1a7ff5f
commit 3b635693a5
3 changed files with 36 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/modules/StateEstimation/filterekf.c
View File

@ -36,6 +36,7 @@
#include <ekfconfiguration.h> #include <ekfconfiguration.h>
#include <ekfstatevariance.h> #include <ekfstatevariance.h>
#include <attitudestate.h> #include <attitudestate.h>
#include <systemalarms.h>
#include <homelocation.h> #include <homelocation.h>
#include <insgps.h> #include <insgps.h>
@ -358,7 +359,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSCovariancePrediction(this->dTa); INSCovariancePrediction(this->dTa);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS; SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) {
sensors |= MAG_SENSORS;
}
} }
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) { if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {

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

@ -48,8 +48,9 @@ struct data {
HomeLocationData homeLocation; HomeLocationData homeLocation;
RevoCalibrationData revoCalibration; RevoCalibrationData revoCalibration;
RevoSettingsData revoSettings; RevoSettingsData revoSettings;
uint16_t counter; uint16_t idlecounter;
bool validity; uint8_t warningcount;
uint8_t errorcount;
float magAverage[3]; float magAverage[3];
float magBias[3]; float magBias[3];
}; };
@ -60,7 +61,7 @@ struct data {
static int32_t init(stateFilter *self); static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state); static int32_t filter(stateFilter *self, stateEstimation *state);
static bool checkMagValidity(struct data *this, float mag[3]); static void checkMagValidity(struct data *this, float mag[3]);
static void magOffsetEstimation(struct data *this, float mag[3]); static void magOffsetEstimation(struct data *this, float mag[3]);
@ -79,8 +80,7 @@ static int32_t init(stateFilter *self)
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->magAverage[0] = this->magAverage[1] = this->magAverage[2] = 0.0f;
this->counter = 0; this->idlecounter = this->warningcount = this->errorcount = 0;
this->validity = true;
HomeLocationGet(&this->homeLocation); HomeLocationGet(&this->homeLocation);
RevoCalibrationGet(&this->revoCalibration); RevoCalibrationGet(&this->revoCalibration);
RevoSettingsGet(&this->revoSettings); RevoSettingsGet(&this->revoSettings);
@ -92,12 +92,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata; struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) { if (IS_SET(state->updated, SENSORUPDATES_mag)) {
if (!checkMagValidity(this, state->mag)) { checkMagValidity(this, state->mag);
UNSET_MASK(state->updated, SENSORUPDATES_mag); if (this->revoCalibration.MagBiasNullingRate > 0) {
} else { magOffsetEstimation(this, state->mag);
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
} }
} }
@ -107,19 +104,20 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
/** /**
* check validity of magnetometers * check validity of magnetometers
*/ */
static bool checkMagValidity(struct data *this, float mag[3]) static void checkMagValidity(struct data *this, float mag[3])
{ {
#define MAG_LOW_PASS_ALPHA 0.05f #define MAG_LOW_PASS_ALPHA 0.2f
#define IDLE_COUNT 20 #define IDLE_COUNT 10
#define ALARM_THRESHOLD 3
// low pass filter sensor to not give warnings due to noise // 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[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[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[0]; 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 // throttle this check, thanks to low pass filter it is not necessary every iteration
if (!this->counter--) { if (!this->idlecounter--) {
this->counter = IDLE_COUNT; this->idlecounter = IDLE_COUNT;
// calculate expected Be vector // calculate expected Be vector
AttitudeStateData attitudeState; AttitudeStateData attitudeState;
@ -142,17 +140,24 @@ static bool checkMagValidity(struct data *this, float mag[3])
// set errors // set errors
if (deviation2 < warning2) { if (deviation2 < warning2) {
this->warningcount = 0;
this->errorcount = 0;
AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER); AlarmsClear(SYSTEMALARMS_ALARM_MAGNETOMETER);
this->validity = true;
} else if (deviation2 < error2) { } else if (deviation2 < error2) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING); this->errorcount = 0;
this->validity = false; if (this->warningcount > ALARM_THRESHOLD) {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_WARNING);
} else {
this->warningcount++;
}
} else { } else {
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR); if (this->errorcount > ALARM_THRESHOLD) {
this->validity = false; AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_ERROR);
} else {
this->errorcount++;
}
} }
} }
return this->validity;
} }

2
shared/uavobjectdefinition/revosettings.xml
View File

@ -9,7 +9,7 @@
<field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/> <field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
<!-- Configuration for magnetometer vector validity check --> <!-- Configuration for magnetometer vector validity check -->
<field name="MagnetometerMaxDeviation" units="%" type="float" elementnames="Warning,Error" defaultvalue="0.2,0.5" /> <field name="MagnetometerMaxDeviation" units="%" type="float" elementnames="Warning,Error" defaultvalue="0.15,0.5" />
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>