diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 2905011582..7251471f82 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on win bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing bool cs_fake_pos # 32 - true when fake position measurements are being fused bool cs_fake_hgt # 33 - true when fake height measurements are being fused +bool cs_mag_field_ne_disturbed # 34 - true when the NE mag field does not match the expected strength # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index e1ae0dc117..9c79e68818 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -535,6 +535,7 @@ union filter_control_status_u { uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused + uint64_t mag_field_ne_disturbed : 1; ///< 34 - true when the NE mag field does not match the expected strength } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index b12ce5db08..f7ff4d551a 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -65,17 +65,39 @@ void Ekf::controlMagFusion() _control_status.flags.synthetic_mag_z = false; } - _control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag); - - // compute mag heading innovation (for estimator_aid_src_mag_heading logging) const Vector3f mag_observation = mag_sample.mag - _state.mag_B; const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); const Vector3f mag_earth_pred = R_to_earth * mag_observation; + // mag field disturbed + if (_params.check_mag_strength) { + + _control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_observation); + + // mag field NE disturbed + if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { + + // expected mag field from the world magnetic model in earth frame + const Vector3f wmm = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + const float wmm_ne_norm = Vector2f(wmm(0), wmm(1)).norm(); + + // norm magnetometer NE components + const float mag_ne_norm = Vector2f(mag_earth_pred(0), mag_earth_pred(1)).norm(); + + // magnetometer NE strength must be within ~20% of the expected value + const float gate_size = wmm_ne_norm * 0.2f; + _control_status.flags.mag_field_ne_disturbed = !isMeasuredMatchingExpected(mag_ne_norm, wmm_ne_norm, gate_size); + } + + } else { + _control_status.flags.mag_field_disturbed = false; + _control_status.flags.mag_field_ne_disturbed = false; + } + resetEstimatorAidStatus(_aid_src_mag_heading); _aid_src_mag_heading.timestamp_sample = mag_sample.time_us; - _aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();; + _aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); _aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation); // compute magnetometer innovations (for estimator_aid_src_mag logging) @@ -193,7 +215,7 @@ void Ekf::runOnGroundYawReset() bool Ekf::canResetMagHeading() const { - return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE); + return !_control_status.flags.mag_field_ne_disturbed && (_params.mag_fusion_type != MagFuseType::NONE); } void Ekf::runInAirYawReset() @@ -233,6 +255,8 @@ void Ekf::runInAirYawReset() ); resetMagCov(); + + has_realigned_yaw = true; } if (!has_realigned_yaw && canResetMagHeading()) { @@ -351,23 +375,19 @@ bool Ekf::shouldInhibitMag() const && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel; - return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed; + return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_ne_disturbed; } bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const { - if (_params.check_mag_strength - && ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) { + if (PX4_ISFINITE(_mag_strength_gps)) { + constexpr float wmm_gate_size = 0.2f; // +/- Gauss + return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size); - if (PX4_ISFINITE(_mag_strength_gps)) { - constexpr float wmm_gate_size = 0.2f; // +/- Gauss - return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size); - - } else { - constexpr float average_earth_mag_field_strength = 0.45f; // Gauss - constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss - return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); - } + } else { + constexpr float average_earth_mag_field_strength = 0.45f; // Gauss + constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss + return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); } return false; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bf4de3456f..18891f3e9f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1370,7 +1370,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); - status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; + status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_ne_disturbed; status.accel_device_id = _device_id_accel; status.baro_device_id = _device_id_baro; @@ -1446,6 +1446,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent; status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos; status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt; + status_flags.cs_mag_field_ne_disturbed = _ekf.control_status_flags().mag_field_ne_disturbed; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;