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

ekf2: add kconfig for magnetometer support (enabled by default)

Browse Source
This commit is contained in:
Daniel Agar
2023-09-15 10:02:09 -04:00
parent d1266c856f
commit 845b01a00d
16 changed files with 404 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_3d_control.cpp
+19
View File
@@ -217,3 +217,22 @@ void Ekf::stopMagFusion()
}
}
void Ekf::saveMagCovData()
{
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
}
void Ekf::loadMagCovData()
{
// re-instate the NED axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
// re-instate the XYZ body axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
}