From b9abef37c39336e45e6d67f3e89f89aab7c00de2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 3 May 2021 21:52:18 +1000 Subject: [PATCH] AP_NavEKF3: Reset to GPS yaw if fusion times out when on ground. --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 21 +++++++++++++------ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 3 ++- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 7d0978c2c2..d3bdfeea2c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -569,7 +569,7 @@ bool NavEKF3_core::using_external_yaw(void) const #endif if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { - return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; + return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } return false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 0a53ab3add..4bbc32bac8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -264,13 +264,18 @@ void NavEKF3_core::SelectMagFusion() if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { alignYawAngle(yawAngDataDelayed); yaw_source_reset = false; + have_fused_gps_yaw = true; + lastSynthYawTime_ms = imuSampleTime_ms; + last_gps_yaw_fuse_ms = imuSampleTime_ms; } else if (tiltAlignComplete && yawAlignComplete) { - fuseEulerYaw(yawFusionMethod::GPS); + have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS); + if (have_fused_gps_yaw) { + last_gps_yaw_fuse_ms = imuSampleTime_ms; + } } - have_fused_gps_yaw = true; - last_gps_yaw_fusion_ms = imuSampleTime_ms; + last_gps_yaw_ms = imuSampleTime_ms; } else if (tiltAlignComplete && !yawAlignComplete) { - // External yaw sources can take singificant time to start providing yaw data so + // External yaw sources can take significant time to start providing yaw data so // wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable if(imuSampleTime_ms - lastSynthYawTime_ms > 140) { yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); @@ -284,7 +289,11 @@ void NavEKF3_core::SelectMagFusion() } lastSynthYawTime_ms = imuSampleTime_ms; } + } else if (tiltAlignComplete && yawAlignComplete && onGround && imuSampleTime_ms - last_gps_yaw_fuse_ms > 10000) { + // handle scenario where were were using GPS yaw previously, but the yaw fusion has timed out. + yaw_source_reset = true; } + if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) { // no fallback return; @@ -305,7 +314,7 @@ void NavEKF3_core::SelectMagFusion() // we don't have GPS yaw data and are configured for // fallback. If we've only just lost GPS yaw - if (imuSampleTime_ms - last_gps_yaw_fusion_ms < 10000) { + if (imuSampleTime_ms - last_gps_yaw_ms < 10000) { // don't fallback to magnetometer fusion for 10s return; } @@ -315,7 +324,7 @@ void NavEKF3_core::SelectMagFusion() return; } if (!inFlight) { - // don't fall back if not flying + // don't fall back if not flying but reset to GPS yaw if it becomes available return; } if (!gps_yaw_mag_fallback_active) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index f62822321e..f847dbbbe5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1435,7 +1435,8 @@ private: */ bool learnMagBiasFromGPS(void); - uint32_t last_gps_yaw_fusion_ms; + uint32_t last_gps_yaw_ms; // last time the EKF attempted to use the GPS yaw + uint32_t last_gps_yaw_fuse_ms; // last time the EKF successfully fused the GPS yaw bool gps_yaw_mag_fallback_ok; bool gps_yaw_mag_fallback_active; uint8_t gps_yaw_fallback_good_counter;