Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug that could publish bad compass offsets

Browse Source 
Magnetometer bias states will subject to larger errors early in flight before flight motion makes the offsets observable and the state variances reduce.
Adds a check on state variances.
Replaces the parameter check with a check of the actual filter fusion method being used.
This commit is contained in:
Paul Riseborough 2016-06-16 20:41:15 +10:00 committed by Andrew Tridgell
parent 165335b9e3
commit 5c59922574
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -349,8 +349,15 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
// return true if offsets are valid
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (mag_idx == magSelectIndex && finalInflightMagInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
// compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
// primary compass is valid and state variances have converged
const float maxMagVar = 5E-6f;
bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
if ((mag_idx == magSelectIndex) &&
finalInflightMagInit &&
!inhibitMagStates &&
_ahrs->get_compass()->healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true;
} else {