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

AP_NavEKF: Add public function for estimated magnetometer offsets

Browse Source
This commit is contained in:
Paul Riseborough 2015-03-16 12:05:45 -07:00 committed by Andrew Tridgell
parent 1c244af3d8
commit 14795719f6
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3650,6 +3650,20 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
magXYZ = state.body_magfield*1000.0f;
}
// return magnetometer offsets
// return true if offsets are valid
bool NavEKF::getMagOffsets(Vector3f &magOffsets) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited
if (secondMagYawInit && (_magCal != 2)) {
magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f;
return true;
} else {
magOffsets = _ahrs->get_compass()->get_offsets();
return false;
}
}
// return the last calculated latitude, longitude and height
bool NavEKF::getLLH(struct Location &loc) const
{

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -145,6 +145,10 @@ public:
// return body magnetic field estimates in measurement units / 1000
void getMagXYZ(Vector3f &magXYZ) const;
// Return estimated magnetometer offsets
// Return true if magnetometer offsets are valid
bool getMagOffsets(Vector3f &magOffsets) const;
// return the last calculated latitude, longitude and height
bool getLLH(struct Location &loc) const;