From 982692f5ed148ea01c17b9bd42b430da8aa8a13b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Jun 2021 10:22:04 -0400 Subject: [PATCH] ekf2: selector handle NAN test ratios as uninitilaized - requires https://github.com/PX4/PX4-ECL/pull/1020 --- src/modules/ekf2/EKF2Selector.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 13398c9d39..bacdc4ea0d 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -283,15 +283,15 @@ bool EKF2Selector::UpdateErrorScores() } // test ratios are invalid when 0, >= 1 is a failure - if (status.vel_test_ratio <= 0.f) { + if (!PX4_ISFINITE(status.vel_test_ratio) || (status.vel_test_ratio <= 0.f)) { status.vel_test_ratio = 1.f; } - if (status.pos_test_ratio <= 0.f) { + if (!PX4_ISFINITE(status.pos_test_ratio) || (status.pos_test_ratio <= 0.f)) { status.pos_test_ratio = 1.f; } - if (status.hgt_test_ratio <= 0.f) { + if (!PX4_ISFINITE(status.hgt_test_ratio) || (status.hgt_test_ratio <= 0.f)) { status.hgt_test_ratio = 1.f; } @@ -306,7 +306,7 @@ bool EKF2Selector::UpdateErrorScores() _instance[i].relative_test_ratio = 0; } - } else if (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout) { + } else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) { _instance[i].healthy = false; _instance[i].timeout = true; }