From 6938e3d57b9561fdfabe0c3a568bec4923bf8c89 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 29 Mar 2016 13:05:19 -0700 Subject: [PATCH] AP_NavEKF: check mag instance id when returning mag offsets --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- libraries/AP_NavEKF/AP_NavEKF.h | 2 +- libraries/AP_NavEKF/AP_NavEKF_core.cpp | 4 ++-- libraries/AP_NavEKF/AP_NavEKF_core.h | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 0a3c97ebdf..0a7947b8b8 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -600,12 +600,12 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid -bool NavEKF::getMagOffsets(Vector3f &magOffsets) const +bool NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const { if (!core) { return false; } - return core->getMagOffsets(magOffsets); + return core->getMagOffsets(mag_idx, magOffsets); } // Return the last calculated latitude, longitude and height in WGS-84 diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index f051767c08..7cedc66fbc 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -129,7 +129,7 @@ public: // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid - bool getMagOffsets(Vector3f &magOffsets) const; + bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; // Return the last calculated latitude, longitude and height in WGS-84 // If a calculated location isn't available, return a raw GPS measurement diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.cpp b/libraries/AP_NavEKF/AP_NavEKF_core.cpp index 811bf229fb..24ba679d66 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_core.cpp @@ -3518,10 +3518,10 @@ void NavEKF_core::getMagXYZ(Vector3f &magXYZ) const // return magnetometer offsets // return true if offsets are valid -bool NavEKF_core::getMagOffsets(Vector3f &magOffsets) const +bool NavEKF_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 (secondMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy()) { + if (mag_idx == _ahrs->get_compass()->get_primary() && secondMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy()) { magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f; return true; } else { diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.h b/libraries/AP_NavEKF/AP_NavEKF_core.h index 467a95b9e0..35ae5781a0 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core.h +++ b/libraries/AP_NavEKF/AP_NavEKF_core.h @@ -172,7 +172,7 @@ public: // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid - bool getMagOffsets(Vector3f &magOffsets) const; + bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; // Return the last calculated latitude, longitude and height in WGS-84 // If a calculated location isn't available, return a raw GPS measurement