diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 3fbf72b32..6618b18fc 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -348,7 +349,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) INSCovariancePrediction(dT); 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)) { diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index 7997016e0..8adaee6a1 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -33,6 +33,8 @@ #include "inc/stateestimation.h" #include #include +#include +#include #include #include @@ -43,9 +45,13 @@ // Private types struct data { - HomeLocationData homeLocation; RevoCalibrationData revoCalibration; - float magBias[3]; + RevoSettingsData revoSettings; + uint8_t warningcount; + uint8_t errorcount; + float homeLocationBe[3]; + float magBe2; + float magBias[3]; }; // Private variables @@ -54,6 +60,7 @@ struct data { static int32_t init(stateFilter *self); static int32_t filter(stateFilter *self, stateEstimation *state); +static void checkMagValidity(struct data *this, float mag[3]); static void magOffsetEstimation(struct data *this, float mag[3]); @@ -70,9 +77,13 @@ static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f; - 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; } @@ -81,6 +92,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) struct data *this = (struct data *)self->localdata; if (IS_SET(state->updated, SENSORUPDATES_mag)) { + checkMagValidity(this, state->mag); if (this->revoCalibration.MagBiasNullingRate > 0) { magOffsetEstimation(this, state->mag); } @@ -89,6 +101,53 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } +/** + * check validity of magnetometers + */ +static void checkMagValidity(struct data *this, float mag[3]) +{ + #define ALARM_THRESHOLD 5 + + // mag2 holds the actual magnetic vector length (squared) + float mag2 = mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]; + + // warning and error thresholds + // avoud sqrt() : minlimitmagBe2 * (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); + + // 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 { + this->warningcount++; + } + } else { + if (this->errorcount > ALARM_THRESHOLD) { + AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_CRITICAL); + } else { + this->errorcount++; + } + } +} + + /** * Perform an update of the @ref MagBias based on * Magmeter Offset Cancellation: Theory and Implementation, @@ -137,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]; diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3f77a1d8f..3b5f4b9b3 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -161,17 +161,14 @@ static float gyroDelta[3]; // preconfigured filter chains selectable via revoSettings.FusionAlgorithm static const filterPipeline *cfQueue = &(filterPipeline) { - .filter = &magFilter, + .filter = &airFilter, .next = &(filterPipeline) { - .filter = &airFilter, + .filter = &baroiFilter, .next = &(filterPipeline) { - .filter = &baroiFilter, + .filter = &altitudeFilter, .next = &(filterPipeline) { - .filter = &altitudeFilter, - .next = &(filterPipeline) { - .filter = &cfFilter, - .next = NULL, - } + .filter = &cfFilter, + .next = NULL, } } } diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index 40039bb47..884c78717 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -27,10 +27,10 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="7.4629556" - inkscape:cx="32.393349" - inkscape:cy="37.450437" - inkscape:current-layer="foreground" + inkscape:zoom="29.851822" + inkscape:cx="82.604436" + inkscape:cy="54.252252" + inkscape:current-layer="background" id="namedview3608" showgrid="true" inkscape:window-width="1366" @@ -1002,12 +1002,13 @@ inkscape:label="#rect4550-8-1-4-21-5-13" /> + ry="1" + inkscape:label="#rect4550-8-1-4-21-5-8" /> + + style="display:none"> + + style="display:none"> + + BARO + id="tspan3835" + x="94.605125" + y="20.116049">MAG + + + + diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index e1fcf6d32..6a7b690ce 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -15,6 +15,7 @@ Actuator Attitude Sensors + Magnetometer Airspeed Stabilization Guidance