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

OP-1161 discard magnetometer in filters if plausibility alarm is set

Browse Source
This commit is contained in:
Corvus Corax 2014-01-14 15:42:45 +01:00
parent 3d1cc31c3b
commit c180846ca9
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,6 +33,7 @@
#include "inc/stateestimation.h"
#include <attitudestate.h>
#include <revocalibration.h>
#include <systemalarms.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
@ -81,8 +82,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
struct data *this = (struct data *)self->localdata;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarGet(&alarms);
if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) {
UNSET_MASK(state->updated, SENSORUPDATES_mag);
} else {
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
}
}