From 57a380d8ec614ccd9d2651ca6424344d986dd195 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 26 Feb 2026 22:33:46 +0100 Subject: [PATCH] ekf2: fix silent pass of preflt heading check if no heading src active --- .../commander/HealthAndArmingChecks/checks/estimatorCheck.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 010cbacf7f..b1bce851a6 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -695,7 +695,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f bool v_xy_valid = lpos.v_xy_valid; if (!context.isArmed()) { - if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) { + if (pre_flt_fail_innov_pos_horiz) { xy_valid = false; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 98e2be7828..db1014f52a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1853,7 +1853,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.time_slip = _last_time_slip_us * 1e-6f; static constexpr float kMinTestRatioPreflight = 0.5f; - status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.hdg_test_ratio); + status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.hdg_test_ratio) || !_ekf.control_status_flags().yaw_align; status.pre_flt_fail_innov_height = (kMinTestRatioPreflight < status.hgt_test_ratio); status.pre_flt_fail_innov_pos_horiz = (kMinTestRatioPreflight < status.pos_test_ratio); status.pre_flt_fail_innov_vel_horiz = (kMinTestRatioPreflight < vel_xy_test_ratio);