fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 05:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag: add inclination check

Browse Source
This commit is contained in:
bresch
2023-07-17 12:21:27 +02:00
committed by Daniel Agar
parent e4a16bfc80
commit 357bf024f6
9 changed files with 122 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+2 -1
View File
@@ -191,6 +191,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
#endif // CONFIG_EKF2_BARO_COMPENSATION
_param_ekf2_mag_check(_params->mag_check),
_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
{
@@ -2532,7 +2533,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
if (!_mag_decl_saved) {
float declination_deg;
if (_ekf.get_mag_decl_deg(&declination_deg)) {
if (_ekf.get_mag_decl_deg(declination_deg)) {
_param_ekf2_mag_decl.update();
if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {