fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add function to calculate the magnetic declination to use

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-14 22:01:53 +01:00
committed by Roman
parent 9017e077f8
commit 83945581ed
3 changed files with 28 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+22
View File
@@ -124,6 +124,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
return true;
}
// Calculate the magnetic declination to be used by the alignment and fusion processing
void Ekf::calcMagDeclination()
{
// set source of magnetic declination for internal use
if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) {
_mag_declination = _mag_declination_gps;
_mag_declination_to_save_deg = math::degrees(_mag_declination);
} else {
_mag_declination = math::radians(_params.mag_declination_deg);
_mag_declination_to_save_deg = _params.mag_declination_deg;
}
} else {
// always use the parameter value
_mag_declination = math::radians(_params.mag_declination_deg);
_mag_declination_to_save_deg = _params.mag_declination_deg;
}
}
// This function forces the covariance matrix to be symmetric
void Ekf::makeSymmetrical()
{