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

AP_NavEKF: check mag instance id when returning mag offsets

Browse Source
This commit is contained in:
Jonathan Challinger 2016-03-29 13:05:19 -07:00 committed by Andrew Tridgell
parent 1185cd1be7
commit 6938e3d57b
4 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -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

4
libraries/AP_NavEKF/AP_NavEKF_core.cpp
View File

@ -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 {

2
libraries/AP_NavEKF/AP_NavEKF_core.h
View File

@ -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