aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

NavEKF: use primary compass instead of first instance

Browse Source 
Thanks very much to staroselskii for finding this bug
This commit is contained in:
Randy Mackay 2015-07-16 17:57:16 +09:00
parent cedc9a8670
commit 0acfcbfa81
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3831,11 +3831,11 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
bool NavEKF::getMagOffsets(Vector3f &magOffsets) const bool NavEKF::getMagOffsets(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 // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (secondMagYawInit && (_magCal != 2) && _ahrs->get_compass()->healthy(0)) { if (secondMagYawInit && (_magCal != 2) && _ahrs->get_compass()->healthy()) {
magOffsets = _ahrs->get_compass()->get_offsets(0) - state.body_magfield*1000.0f; magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f;
return true; return true;
} else { } else {
magOffsets = _ahrs->get_compass()->get_offsets(0); magOffsets = _ahrs->get_compass()->get_offsets();
return false; return false;
} }
} }
@ -4340,8 +4340,8 @@ void NavEKF::readMagData()
newDataMag = true; newDataMag = true;
// check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations // check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations
if (_ahrs->get_compass()->healthy(0)) { if (_ahrs->get_compass()->healthy()) {
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0); Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets();
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z)); bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration // Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
if (changeDetected && secondMagYawInit) { if (changeDetected && secondMagYawInit) {