diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 13caff5ca2..ee7c7e1e7f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -163,21 +163,22 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) && mag_sample.mag.isAllFinite(); const bool starting_conditions_passing = continuing_conditions_passing - && checkMagField(mag_sample.mag) && (_mag_counter > 3) // wait until we have more than a few samples through the filter && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); - checkMagHeadingConsistency(mag_sample); + checkMagHeadingConsistency(mag_sample.mag - _state.mag_B); const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; { const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; - const bool common_conditions_passing = _control_status.flags.mag + + const bool common_conditions_passing = checkMagField(mag_sample.mag - _state.mag_B) + && _control_status.flags.mag && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) && !_control_status.flags.mag_fault @@ -194,8 +195,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); } - // TODO: allow clearing mag_fault if mag_3d is good? - if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { ECL_INFO("starting mag 3D fusion"); @@ -455,23 +454,15 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) } } -void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) +void Ekf::checkMagHeadingConsistency(const Vector3f &mag) { - // use mag bias if variance good - Vector3f mag_bias{0.f, 0.f, 0.f}; - const Vector3f mag_bias_var = getMagBiasVariance(); - - if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) { - mag_bias = _state.mag_B; - } - // calculate mag heading // Rotate the measurements into earth frame using the zero yaw angle const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); // the angle of the projection onto the horizontal gives the yaw angle // calculate the yaw innovation and wrap to the interval between +-pi - const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias); + const Vector3f mag_earth_pred = R_to_earth * mag; const float declination = getMagDeclination(); const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination; @@ -497,17 +488,21 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) } } -bool Ekf::checkMagField(const Vector3f &mag_sample) +bool Ekf::checkMagField(const Vector3f &mag) { _control_status.flags.mag_field_disturbed = false; + const Vector3f mag_earth = _R_to_earth * mag; + + _mag_strength = mag.length(); + _mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); + if (_params.mag_check == 0) { // skip all checks return true; } bool is_check_failing = false; - _mag_strength = mag_sample.length(); if (_params.mag_check & static_cast(MagCheckMask::STRENGTH)) { if (PX4_ISFINITE(_wmm_field_strength_gauss)) { @@ -523,16 +518,13 @@ bool Ekf::checkMagField(const Vector3f &mag_sample) constexpr float average_earth_mag_field_strength = 0.45f; // Gauss constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss - if (!isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) { + if (!isMeasuredMatchingExpected(mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) { _control_status.flags.mag_field_disturbed = true; is_check_failing = true; } } } - const Vector3f mag_earth = _R_to_earth * mag_sample; - _mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); - if (_params.mag_check & static_cast(MagCheckMask::INCLINATION)) { if (PX4_ISFINITE(_wmm_inclination_rad)) { const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg); @@ -648,7 +640,7 @@ bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longi || strength_changed ) { - ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)", + ECL_INFO("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)", (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad), (double)latitude_deg, (double)longitude_deg ); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a7d59b47ef..9958c45a90 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1063,7 +1063,7 @@ private: void resetMagStates(const Vector3f &mag, bool reset_heading = true); bool haglYawResetReq(); - void checkMagHeadingConsistency(const magSample &mag_sample); + void checkMagHeadingConsistency(const Vector3f &mag); bool checkMagField(const Vector3f &mag); static bool isMeasuredMatchingExpected(float measured, float expected, float gate); diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index ca4f440c62..3e6086c9fd 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -112,14 +112,14 @@ TEST_F(EkfMagTest, noInitLargeStrength) const Vector3f mag_data(1.f, 1.f, 1.f); _sensor_simulator._mag.setData(mag_data); - const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + //const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); _sensor_simulator.runSeconds(_init_duration_s); - // THEN: the fusion shouldn't start + // THEN: mag heading and mag 3d fusion shouldn't start EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); - EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); + //EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); + //EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); } TEST_F(EkfMagTest, suddenLargeStrength) @@ -161,11 +161,11 @@ TEST_F(EkfMagTest, noInitLargeInclination) const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); _sensor_simulator.runSeconds(_init_duration_s + 10.f); // live some extra time fo GNSS checks to pass - // THEN: the fusion shouldn't start + // THEN: mag heading and mag 3d fusion shouldn't start EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); - EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); + //EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); + //EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); // BUT then: as soon as there is some meaningful data const float mag_heading = -M_PI_F / 7.f;