From b7efd4f94767a96ac87010e5d64fd7a95a3ec36c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 1 Dec 2021 20:22:34 -0500 Subject: [PATCH] ekf2: selector treat combined test ratio > 1 as a warning * ekf2: selector treat combined test ratio > 1 as a warning * ekf2: wait for 1s of constant warning to trigger an instance switch Co-authored-by: bresch --- src/modules/ekf2/EKF2Selector.cpp | 13 +++++++++++-- src/modules/ekf2/EKF2Selector.hpp | 2 ++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 1169aeff9a..b71ace2c9f 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -293,10 +293,15 @@ bool EKF2Selector::UpdateErrorScores() float combined_test_ratio = fmaxf(0.5f * (status.vel_test_ratio + status.pos_test_ratio), status.hgt_test_ratio); _instance[i].combined_test_ratio = combined_test_ratio; - _instance[i].healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f) && (combined_test_ratio < 1.f); + _instance[i].healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f); + _instance[i].warning = (combined_test_ratio >= 1.f); _instance[i].filter_fault = (status.filter_fault_flags != 0); _instance[i].timeout = false; + if (!_instance[i].warning) { + _instance[i].time_last_no_warning = status.timestamp_sample; + } + if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) { _instance[i].relative_test_ratio = 0; } @@ -733,7 +738,11 @@ void EKF2Selector::Run() SelectInstance(best_ekf); } - } else if (lower_error_available && (hrt_elapsed_time(&_last_instance_change) > 10_s)) { + } else if (lower_error_available + && ((hrt_elapsed_time(&_last_instance_change) > 10_s) + || (_instance[_selected_instance].warning + && (hrt_elapsed_time(&_instance[_selected_instance].time_last_no_warning) > 1_s)))) { + // if this instance has a significantly lower relative error to the active primary, we consider it as a // better instance and would like to switch to it even if the current primary is healthy SelectInstance(best_ekf_alternate); diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 992762a182..8963a9e8fa 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -125,11 +125,13 @@ private: uint32_t mag_device_id{0}; hrt_abstime time_last_selected{0}; + hrt_abstime time_last_no_warning{0}; float combined_test_ratio{NAN}; float relative_test_ratio{NAN}; bool healthy{false}; + bool warning{false}; bool filter_fault{false}; bool timeout{false};