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-1412 delay ekf initialization until magnetometers are reporting valid data

Browse Source
This commit is contained in:
Corvus Corax 2014-07-22 23:49:41 +02:00
parent eec6c482ea
commit 95799cf11b
1 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -205,6 +205,15 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
this->work.updated |= state->updated;
// check magnetometer alarm, discard any magnetometer readings if not OK
// this will also delay initialization
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Magnetometer != SYSTEMALARMS_ALARM_OK) {
UNSET_MASK(state->updated, SENSORUPDATES_mag);
UNSET_MASK(this->work.updated, SENSORUPDATES_mag);
}
// Get most recent data
IMPORT_SENSOR_IF_UPDATED(gyro, 3);
IMPORT_SENSOR_IF_UPDATED(accel, 3);
@ -349,11 +358,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
INSCovariancePrediction(dT);
if (IS_SET(this->work.updated, SENSORUPDATES_mag)) {
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.Magnetometer == SYSTEMALARMS_ALARM_OK) {
sensors |= MAG_SENSORS;
}
sensors |= MAG_SENSORS;
}
if (IS_SET(this->work.updated, SENSORUPDATES_baro)) {