From d62f11201780c4fdaf6b35e3ace55b70a1500e15 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Dec 2025 12:15:08 +0100 Subject: [PATCH] ekf2: prevent false mag fault detection A false positive could be triggered if velocity fusion started, then stopped after takeoff and only position fusion started again (because velocity fusion timed out and had a timestamp > time_last_on_ground). We now also check if the fusion timeout is due to the innovation being rejected (and not just a temporary check failure or data interruption). --- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 60ed3f74ce..6b523fad28 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -104,12 +104,17 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) bool do_vel_pos_reset = false; - if (!_control_status.flags.gnss_fault && (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)) { + if (!_control_status.flags.gnss_fault && _control_status.flags.in_air && isYawFailure()) { + const bool velocity_fusion_failure = _aid_src_gnss_vel.innovation_rejected + && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us); - if (_control_status.flags.in_air - && isYawFailure() - && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) - && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) { + const bool position_fusion_failure = _aid_src_gnss_pos.innovation_rejected + && isTimedOut(_time_last_hor_pos_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_pos_fuse > _time_last_on_ground_us); + + if ((_control_status.flags.gnss_vel && velocity_fusion_failure) + || (_control_status.flags.gnss_pos && position_fusion_failure)) { do_vel_pos_reset = tryYawEmergencyReset(); } }