Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: improve switch-over to use of mag field states 

Set initial variances to measurement uncertainties
Always perform a field  and yaw reset
This commit is contained in:
Paul Riseborough 2016-05-24 10:23:49 +10:00 committed by Andrew Tridgell
parent fe9ddfdfeb
commit 6d34ac5ceb
1 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -67,7 +67,18 @@ void NavEKF2_core::setWindMagStateLearningMode()
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4); bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied // Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied); bool setMagInhibit = !magCalRequested || magCalDenied;
if (!inhibitMagStates && setMagInhibit) {
inhibitMagStates = true;
} else if (inhibitMagStates && !setMagInhibit) {
inhibitMagStates = false;
// when commencing use of magnetic field states, set the variances equal to the observation uncertainty
for (uint8_t index=16; index<=21; index++) {
P[index][index] = sq(frontend->_magNoise);
}
// let the magnetometer fusion know it needs to reset the yaw and field states
firstMagYawInit = false;
}
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff // because we want it re-done for each takeoff