Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Reset mag and heading states to try and pass pre-flight checks

Browse Source
This commit is contained in:
Paul Riseborough 2015-08-19 12:32:09 +10:00 committed by Randy Mackay
parent 72ab60a19e
commit efce10b6cd
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4376,6 +4376,9 @@ void NavEKF::readMagData()
// read compass data and scale to improve numerical conditioning
magData = _ahrs->get_compass()->get_field() * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// get states stored at time closest to measurement time after allowance for measurement delay
RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - msecMagDelay));
@ -4686,6 +4689,7 @@ void NavEKF::InitialiseVariables()
ekfStartTime_ms = imuSampleTime_ms;
lastGpsVelFail_ms = 0;
lastGpsAidBadTime_ms = 0;
magYawResetTimer_ms = imuSampleTime_ms;
// initialise other variables
gpsNoiseScaler = 1.0f;
@ -5218,6 +5222,18 @@ bool NavEKF::calcGpsGoodToAlign(void)
"GPS horiz error %.1f", (double)hAcc);
}
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
// This enables us to handle large changes to the external magnetic field environment that occur before arming
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f) || !consistentMagData) {
magYawResetTimer_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
// reset heading and field states
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
}
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -685,6 +685,8 @@ private:
bool useGpsVertVel; // true if GPS vertical velocity should be used
float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
bool yawResetAngleWaiting; // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
bool consistentMagData; // true when the magnetometers are passing consistency checks
// Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement