diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 6205ed270f..8bfa692a5b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -392,7 +392,7 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer && - !_control_status.flags.fake_pos && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { + !_control_status.flags.fake_pos && _control_status.flags.in_air) { if (!_control_status.flags.wind) { // reset the wind states and covariances when starting drag accel fusion diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 4dd5bdb1e9..afdd54a98a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -525,11 +525,9 @@ private: uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited float _last_static_yaw{NAN}; ///< last yaw angle recorded when on ground motion checks were passing (rad) - bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve. bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements - bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited SquareMatrix24f P{}; ///< state covariance matrix @@ -870,7 +868,6 @@ private: float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } void runOnGroundYawReset(); - bool canResetMagHeading() const; void runInAirYawReset(); void selectMagAuto(); @@ -880,7 +877,6 @@ private: bool canUse3DMagFusion() const; void checkMagDeclRequired(); - void checkMagInhibition(); bool shouldInhibitMag() const; bool magFieldStrengthDisturbed(const Vector3f &mag) const; static bool isMeasuredMatchingExpected(float measured, float expected, float gate); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 31018211a2..f03ad0ee32 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -125,10 +125,21 @@ void Ekf::controlMagFusion() } _mag_yaw_reset_req |= !_control_status.flags.yaw_align; - _mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req; if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) { + if (shouldInhibitMag()) { + if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) { + // If magnetometer use has been inhibited continuously then stop the fusion + stopMagFusion(); + } + + return; + + } else { + _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; + } + const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D; // Determine if we should use simple magnetic heading fusion which works better when @@ -172,7 +183,6 @@ void Ekf::controlMagFusion() } checkMagDeclRequired(); - checkMagInhibition(); runMagAndMagDeclFusions(mag_sample.mag); } @@ -193,29 +203,16 @@ void Ekf::checkHaglYawResetReq() void Ekf::runOnGroundYawReset() { - if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) { - const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false; + if (_mag_yaw_reset_req) { + const bool has_realigned_yaw = resetMagHeading(); if (has_realigned_yaw) { _mag_yaw_reset_req = false; _control_status.flags.yaw_align = true; - - // Handle the special case where we have not been constraining yaw drift or learning yaw bias due - // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. - if (_mag_inhibit_yaw_reset_req) { - _mag_inhibit_yaw_reset_req = false; - // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty - P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg)); - } } } } -bool Ekf::canResetMagHeading() const -{ - return !_control_status.flags.mag_field_disturbed && (_params.mag_fusion_type != MagFuseType::NONE); -} - void Ekf::runInAirYawReset() { // prevent a reset being performed more than once on the same frame @@ -224,7 +221,7 @@ void Ekf::runInAirYawReset() return; } - if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) { + if (_mag_yaw_reset_req) { bool has_realigned_yaw = false; // use yaw estimator if available @@ -262,7 +259,7 @@ void Ekf::runInAirYawReset() has_realigned_yaw = true; } - if (!has_realigned_yaw && canResetMagHeading()) { + if (!has_realigned_yaw) { has_realigned_yaw = resetMagHeading(); } @@ -273,16 +270,7 @@ void Ekf::runInAirYawReset() // record the time for the magnetic field alignment event _flt_mag_align_start_time = _imu_sample_delayed.time_us; - - // Handle the special case where we have not been constraining yaw drift or learning yaw bias due - // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. - if (_mag_inhibit_yaw_reset_req) { - _mag_inhibit_yaw_reset_req = false; - // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty - P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * _dt_ekf_avg)); - } } - } } @@ -356,20 +344,6 @@ void Ekf::checkMagDeclRequired() _control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected)); } -void Ekf::checkMagInhibition() -{ - _is_yaw_fusion_inhibited = shouldInhibitMag(); - - if (!_is_yaw_fusion_inhibited) { - _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; - } - - // If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading - if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) { - _mag_inhibit_yaw_reset_req = true; - } -} - bool Ekf::shouldInhibitMag() const { // If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer @@ -414,7 +388,7 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) if (_control_status.flags.mag_3D) { run3DMagAndDeclFusions(mag); - } else if (_control_status.flags.mag_hdg && !_is_yaw_fusion_inhibited) { + } else if (_control_status.flags.mag_hdg) { // Rotate the measurements into earth frame using the zero yaw angle Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); @@ -437,8 +411,7 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) { // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise // before they are used to constrain heading drift - const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6) - && !_control_status.flags.mag_fault && !_control_status.flags.mag_field_disturbed; + const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6); if (!_mag_decl_cov_reset) { // After any magnetic field covariance reset event the earth field state