fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: only save mag declination if changed

Browse Source
This commit is contained in:
Daniel Agar 2021-11-15 17:10:53 -05:00
parent 96bf3aa5d0
commit 4e8c234a55
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/EKF2.cpp
View File

@ -1873,12 +1873,14 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
float declination_deg;
if (_ekf.get_mag_decl_deg(&declination_deg)) {
_param_ekf2_mag_decl.set(declination_deg);
_mag_decl_saved = true;
_param_ekf2_mag_decl.update();
if (!_multi_mode || (_multi_mode && _instance == 0)) {
if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {
_param_ekf2_mag_decl.set(declination_deg);
_param_ekf2_mag_decl.commit_no_notification();
}
_mag_decl_saved = true;
}
}
}