From fd9a1a8b1c1351b371a515b37d905fc734b4b681 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 21 Apr 2022 11:22:54 -0400 Subject: [PATCH] ekf2: magless fixes and improvements - periodically fuse yaw estimator yaw if valid and no other heading sources available - initial yaw align from yaw estimator also include GPS vel/pos reset - fix resetYawToEKFGSF() logic to prevent excessive resets - GPS fusion timeout use lower threshold for yaw estimator heading reset if no other heading source available --- src/modules/ekf2/EKF/ekf_helper.cpp | 4 ++-- src/modules/ekf2/EKF/gps_control.cpp | 32 ++++++++++++++++++++++------ 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 4096756d3f..6deae1cd45 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1779,8 +1779,8 @@ bool Ekf::resetYawToEKFGSF() // The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time // to improve its estimate if the previous reset was not successful. if (!isYawEmergencyEstimateAvailable() - && isTimedOut(_ekfgsf_yaw_reset_time, 5000000) - && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) { + || !isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000) + || _ekfgsf_yaw_reset_count > _params.EKFGSF_reset_count_limit) { return false; } diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 2062b5f892..26bb8d2cf0 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -56,14 +56,25 @@ void Ekf::controlGpsFusion() if (gps_checks_passing && !gps_checks_failing && isYawEmergencyEstimateAvailable()) { bool perform_yaw_align = !_control_status.flags.yaw_align && _control_status.flags.tilt_align; - bool no_yaw_aid_src = !yawAidingAvailable(); - bool yaw_error = isYawError(math::radians(10.f)); // 10 degrees yaw error - if (perform_yaw_align || (no_yaw_aid_src && yaw_error)) { + if (perform_yaw_align) { if (resetYawToEKFGSF()) { ECL_INFO("Yaw aligned using IMU and GPS"); + resetVelocityToGps(_gps_sample_delayed); + resetHorizontalPositionToGps(_gps_sample_delayed); } } + + // periodically fuse yaw estimate if necessary + if (isYawEmergencyEstimateAvailable() + && _control_status.flags.yaw_align + && isTimedOut(_ekfgsf_yaw_reset_time, 1'000'000) + && isTimedOut(_time_last_heading_fuse, (uint64_t)1'000'000)) { + + float innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yawEstimator.getYaw()); + float obs_var = _yawEstimator.getYawVar(); + fuseYaw(innovation, obs_var); + } } // Determine if we should use GPS aiding for velocity and horizontal position @@ -89,13 +100,22 @@ void Ekf::controlGpsFusion() * because the timeout could have been caused by bad GPS. * The total number of resets allowed per boot cycle is limited. */ - if (isYawFailure() + if (isYawEmergencyEstimateAvailable() + && !yawAidingAvailable() + && isYawError(math::radians(10.f)) && _control_status.flags.in_air && !was_gps_signal_lost && resetYawToEKFGSF()) { ECL_WARN("GPS emergency yaw reset"); + } else if (isYawFailure() + && _control_status.flags.in_air + && !was_gps_signal_lost + && resetYawToEKFGSF()) { + + ECL_WARN("GPS emergency yaw reset"); + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around // and cause another navigation failure @@ -113,7 +133,6 @@ void Ekf::controlGpsFusion() } } else { - _warning_events.flags.gps_fusion_timout = true; ECL_WARN("GPS fusion timeout - resetting"); // use GPS velocity data to check and correct yaw angle if a FW vehicle @@ -123,6 +142,7 @@ void Ekf::controlGpsFusion() } } + _warning_events.flags.gps_fusion_timout = true; resetVelocityToGps(_gps_sample_delayed); resetHorizontalPositionToGps(_gps_sample_delayed); } @@ -173,7 +193,7 @@ bool Ekf::shouldResetGpsFusion() const || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); /* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of - * navigation casued by a bad yaw estimate. + * navigation caused by a bad yaw estimate. * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,