From 0546f4082b8204b6cea38c990204c5e7214a884c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 31 Mar 2022 17:45:50 -0400 Subject: [PATCH] ekf2: replace mag reset request flags with timeout checks --- src/modules/ekf2/EKF/control.cpp | 3 +- src/modules/ekf2/EKF/ekf.h | 6 +-- src/modules/ekf2/EKF/ekf_helper.cpp | 15 ++---- src/modules/ekf2/EKF/gps_checks.cpp | 17 ------ src/modules/ekf2/EKF/gps_control.cpp | 2 + src/modules/ekf2/EKF/mag_control.cpp | 78 +++++++++++++--------------- 6 files changed, 45 insertions(+), 76 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 54313c2322..b3f2882321 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1048,13 +1048,12 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer && - !_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { + !_using_synthetic_position && _control_status.flags.in_air) { if (!_control_status.flags.wind) { // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; resetWind(); - } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 72c4919678..7a01b7b303 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -422,11 +422,8 @@ private: bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset - 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 @@ -537,6 +534,8 @@ private: Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2) + float _mag_heading_last_declination{NAN}; + gps_check_fail_status_u _gps_check_fail_status{}; // variables used to inhibit accel bias learning @@ -859,7 +858,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/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 366c67b697..96164bd91b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -481,6 +481,7 @@ bool Ekf::resetMagHeading() // record the time for the magnetic field alignment event _flt_mag_align_start_time = _imu_sample_delayed.time_us; + _time_last_mag_heading_fuse = _time_last_imu; return true; } @@ -1556,18 +1557,11 @@ void Ekf::startGpsFusion() yaw_reset_needed = true; } - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - if (_mag_inhibit_yaw_reset_req || _mag_yaw_reset_req) { - yaw_reset_needed = true; - } - } - if (isYawEmergencyEstimateAvailable() && isYawError(math::radians(10.f))) { yaw_reset_needed = true; } - // Do not use external vision for yaw if using GPS because yaw needs to be - // defined relative to an NED reference frame + // yaw needs to be defined relative to an NED reference frame if (yaw_reset_needed) { if (resetYawToEKFGSF()) { ECL_INFO("starting GPS, reset yaw using yaw estimator"); @@ -1583,8 +1577,7 @@ void Ekf::startGpsFusion() } else { // all failed - ECL_WARN("starting GPS, yaw reset failed"); - _mag_yaw_reset_req = true; + ECL_ERR("starting GPS, yaw reset failed"); } } @@ -1769,6 +1762,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) // capture the reset event _state_reset_status.quat_counter++; + + _time_last_heading_fuse = _time_last_imu; } // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index c451ee2c5b..993c2aede2 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -84,19 +84,11 @@ bool Ekf::collect_gps(const gps_message &gps) _earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat())); _last_gps_origin_time_us = _time_last_imu; - const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps); - // set the magnetic field data returned by the geo library using the current GPS position _mag_declination_gps = get_mag_declination_radians(lat, lon); _mag_inclination_gps = get_mag_inclination_radians(lat, lon); _mag_strength_gps = get_mag_strength_gauss(lat, lon); - // request a reset of the yaw using the new declination - if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) - && !declination_was_valid) { - _mag_yaw_reset_req = true; - } - // save the horizontal and vertical position uncertainty of the origin _gps_origin_eph = gps.eph; _gps_origin_epv = gps.epv; @@ -108,8 +100,6 @@ bool Ekf::collect_gps(const gps_message &gps) // a rough 2D fix is still sufficient to lookup declination if ((gps.fix_type >= 2) && (gps.eph < 1000)) { - const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps); - // If we have good GPS data set the origin's WGS-84 position to the last gps fix const double lat = gps.lat * 1.0e-7; const double lon = gps.lon * 1.0e-7; @@ -119,13 +109,6 @@ bool Ekf::collect_gps(const gps_message &gps) _mag_inclination_gps = get_mag_inclination_radians(lat, lon); _mag_strength_gps = get_mag_strength_gauss(lat, lon); - // request mag yaw reset if there's a mag declination for the first time - if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) { - if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) { - _mag_yaw_reset_req = true; - } - } - _earth_rate_NED = calcEarthRateNED((float)math::radians(lat)); } } diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index fd2c72bfd4..5885ac52ed 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -97,10 +97,12 @@ void Ekf::controlGpsFusion() // and cause another navigation failure _control_status.flags.mag_fault = true; _warning_events.flags.emergency_yaw_reset_mag_stopped = true; + stopMagFusion(); } else if (_control_status.flags.gps_yaw) { _control_status.flags.gps_yaw_fault = true; _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; + stopGpsYawFusion(); } else if (_control_status.flags.ev_yaw) { stopEvYawFusion(); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 264fac9717..b197447adb 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -126,21 +126,31 @@ void Ekf::controlMagFusion() const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D; + const bool declination_changed = _control_status.flags.mag_hdg && (fabsf(_mag_heading_last_declination - getMagDeclination()) > math::radians(1.f)); + + bool mag_fuse_recent = !isTimedOut(_time_last_mag_heading_fuse, (uint64_t)5e6) || !isTimedOut(_time_last_mag_3d_fuse, (uint64_t)5e6); + if (!_control_status.flags.yaw_align - || _mag_yaw_reset_req || _mag_inhibit_yaw_reset_req + || !mag_fuse_recent + || declination_changed || haglYawResetReq() || (!mag_enabled_previously && mag_enabled)) { runYawReset(); } + if (_control_status.flags.mag_hdg) { + _mag_heading_last_declination = getMagDeclination(); + } + if (!_control_status.flags.yaw_align) { // Having the yaw aligned is mandatory to continue return; } checkMagDeclRequired(); - checkMagInhibition(); + + _is_yaw_fusion_inhibited = shouldInhibitMag(); runMagAndMagDeclFusions(mag_sample.mag); } @@ -165,34 +175,34 @@ bool Ekf::haglYawResetReq() const void Ekf::runYawReset() { - if (!_is_yaw_fusion_inhibited) { - bool has_realigned_yaw = false; + bool has_realigned_yaw = false; - if (_control_status.flags.gps && _control_status.flags.fixed_wing) { - has_realigned_yaw = realignYawGPS(); + if (_control_status.flags.gps && _control_status.flags.fixed_wing) { + has_realigned_yaw = realignYawGPS(); + } + + if (!has_realigned_yaw) { + has_realigned_yaw = resetMagHeading(); + } + + if (has_realigned_yaw) { + _control_status.flags.yaw_align = true; + + if (_control_status.flags.in_air) { + _control_status.flags.mag_aligned_in_flight = true; } - if (!has_realigned_yaw) { - has_realigned_yaw = resetMagHeading(); - } - - if (has_realigned_yaw) { - _mag_yaw_reset_req = false; - _control_status.flags.yaw_align = true; - - if (_control_status.flags.in_air) { - _control_status.flags.mag_aligned_in_flight = 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)); - } + // 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. + bool mag_fuse_recent = !isTimedOut(_time_last_mag_heading_fuse, (uint64_t)5e6) || !isTimedOut(_time_last_mag_3d_fuse, (uint64_t)5e6); + + if (!mag_fuse_recent) { + // 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)); } + // reset + _time_last_mag_heading_fuse = _time_last_imu; } } @@ -261,20 +271,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 @@ -405,10 +401,6 @@ bool Ekf::resetMagStates() if (reset) { resetMagCov(); - // clear any pending resets - _mag_yaw_reset_req = false; - _mag_inhibit_yaw_reset_req = false; - if (mag_available) { // record the start time for the magnetic field alignment _flt_mag_align_start_time = _imu_sample_delayed.time_us;