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

AP_NavEKF2: check mag instance id when returning mag offsets

This commit is contained in:
Jonathan Challinger 2016-03-29 13:06:42 -07:00 committed by Andrew Tridgell
parent 6938e3d57b
commit 97112ccd44
4 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -740,12 +740,21 @@ uint8_t NavEKF2::getActiveMag(int8_t instance)
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool NavEKF2::getMagOffsets(Vector3f &magOffsets) const
bool NavEKF2::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
if (!core) {
return false;
}
return core[primary].getMagOffsets(magOffsets);
// try the primary first, else loop through all of the cores and return when one has offsets for this mag instance
if (core[primary].getMagOffsets(mag_idx, magOffsets)) {
return true;
}
for (uint8_t i=0; i<num_cores; i++) {
if(core[i].getMagOffsets(mag_idx, magOffsets)) {
return true;
}
}
return false;
}
// Return the last calculated latitude, longitude and height in WGS-84

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -134,7 +134,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_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -347,10 +347,10 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
// return magnetometer offsets
// return true if offsets are valid
bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
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 (firstMagYawInit && (frontend->_magCal != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
if (mag_idx == magSelectIndex && firstMagYawInit && (frontend->_magCal != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true;
} else {

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -130,7 +130,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