Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: move getMagOffsets into outputs

Browse Source
This commit is contained in:
Jonathan Challinger 2016-03-29 12:56:16 -07:00 committed by Andrew Tridgell
parent acfaafe276
commit 1185cd1be7
2 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -142,20 +142,6 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
* MAGNETOMETER * * MAGNETOMETER *
********************************************************/ ********************************************************/
// return magnetometer offsets
// return true if offsets are valid
bool NavEKF2_core::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
if (firstMagYawInit && (frontend->_magCal != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true;
} else {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
return false;
}
}
// check for new magnetometer data and update store measurements if available // check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData() void NavEKF2_core::readMagData()
{ {

14
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -345,6 +345,20 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
magXYZ = stateStruct.body_magfield*1000.0f; magXYZ = stateStruct.body_magfield*1000.0f;
} }
// return magnetometer offsets
// return true if offsets are valid
bool NavEKF2_core::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
if (firstMagYawInit && (frontend->_magCal != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true;
} else {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
return false;
}
}
// return the index for the active magnetometer // return the index for the active magnetometer
uint8_t NavEKF2_core::getActiveMag() const uint8_t NavEKF2_core::getActiveMag() const
{ {