From 38ac102566c00cfc7deefc772ff39ce110512ad8 Mon Sep 17 00:00:00 2001 From: Saibernard <112599512+Saibernard@users.noreply.github.com> Date: Sat, 29 Aug 2026 17:23:11 -0400 Subject: [PATCH] fix(ekf2): avoid failover to an instance with a failing test ratio (#28418) * fix(ekf2): keep evaluating instance selection while the primary is unhealthy The selection block only ran when UpdateErrorScores() reported a change: a new instance appearing or a health transition. A primary that stops publishing produces exactly one such transition, on the cycle its timeout is detected. If no switch happens on that cycle, nothing sets updated again: the stable alternatives do not count as primary updates and the timed out instance is skipped thereafter, so the fallback logic is never evaluated again even though the module keeps being scheduled. Today that single evaluation always resolves the situation, because the fallback switches unconditionally to the best healthy candidate on that same cycle. But any selection policy that can decline to switch on the transition cycle, for example one that waits out a transient fault, needs the decision re-evaluated while the primary remains unhealthy. Re-enter the selection block whenever the selected instance is unhealthy. The decisions inside are unchanged and switching is idempotent, so behaviour today is identical. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * fix(ekf2): do not fail over to an instance with a sustained test ratio failure When the primary EKF goes unhealthy the selector falls back to the best instance that is healthy, and healthy only requires zero filter fault flags and a positive combined test ratio. An instance whose test ratio has been failing for a long time therefore remains a first class switch target even though its state can be far from the truth. b7efd4f947 introduced this on purpose for the switch-away direction: a test ratio at or above one became a warning rather than ill health, so a transient ratio spike cannot hard fail an instance, and a warned primary is left through the lower relative error path once the warning has been sustained for one second. What that commit did not do is apply the same reasoning to the switch-to direction. The candidate loop only filters on healthy, so a brief hard fault on the primary, for example transient accelerometer clipping, sends the selector straight to a diverged instance. That is the mechanism behind the repeated altitude jumps in issue 27013: one instance had stopped fusing baro, its vertical state up to 155.6 m from the other instance while its combined test ratio sat pegged at 2, and each of the seven short clipping faults on the good instance bounced the selector back to it (15 instance switches in total counting the returns), the worst switch stepping the published altitude by 128.5 m and provoking a hard TECS reaction. Classify fallback candidates with the same sustained warning test the switch-away trigger already uses. When the primary goes unhealthy, fail over immediately to the best candidate without a sustained warning; the different IMU preference is kept within each tier, and a candidate without a sustained warning is preferred even over a warned candidate on a different IMU, since a warned instance is the one known to be diverging. A sustained warned candidate is accepted in two cases only: the primary has timed out entirely, where frozen attitude and position outputs are worse than any live alternative, or the primary has been continuously unhealthy for kWarnedFallbackDelay (five seconds), so a brief fault rides out on the current state while a persistently faulted primary still gets the least bad alternative rather than none. The re-evaluation of this decision while the primary stays unhealthy is provided by the previous commit. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran * test(ekf2): add a functional test for the instance selector The selector had no test at any level. This drives EKF2Selector through published multi instance estimator_status messages on the real work queue and observes estimator_selector_status, so the selection policy is exercised without a simulator. The scenarios encode the failure pattern from issue 27013 and the no-whipsaw property discussed there: a clean fallback on a hard primary fault stays immediate, the switch away from a degraded primary through the sustained warning path still works, three separate brief hard faults on the primary no longer bounce the selector to an instance whose test ratio has been failing for seconds, a primary that stops publishing falls back to the degraded instance without delay, and a primary that stays hard faulted for longer than the ride-out window still falls back rather than being kept forever. The scenario setup helpers run until the selector reaches the intended starting state rather than assuming fixed timings, since the health hysteresis and warning windows run on wall clock time. The fault-clear windows exceed the selector's one second healthy hysteresis so the faults are genuinely separate and the unhealthy-since tracking restarts between them. The harness waits until the work queue manager actually serves queues before constructing the selector: a fixed delay races the manager startup on a loaded runner. to run: make tests TESTFILTER=EKF2Selector Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran --------- Signed-off-by: Saibernard Yogendran (cherry picked from commit 436cc71c89d898390a90efe3a4e97f46ccbccb65) --- src/modules/ekf2/CMakeLists.txt | 2 + src/modules/ekf2/EKF2Selector.cpp | 50 +++- src/modules/ekf2/EKF2Selector.hpp | 5 + src/modules/ekf2/EKF2SelectorTest.cpp | 337 ++++++++++++++++++++++++++ 4 files changed, 389 insertions(+), 5 deletions(-) create mode 100644 src/modules/ekf2/EKF2SelectorTest.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 1f05add4fc7a..b608c7a4859c 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -295,4 +295,6 @@ px4_add_module( if(BUILD_TESTING) add_subdirectory(test) + + px4_add_functional_gtest(SRC EKF2SelectorTest.cpp LINKLIBS modules__ekf2) endif() diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index f5f67e2cded9..12c1d580c53b 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -132,6 +132,7 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) _instance_changed_count++; _last_instance_change = sensor_selection.timestamp; _instance[ekf_instance].time_last_selected = _last_instance_change; + _selected_unhealthy_since = 0; // reset all relative test ratios for (uint8_t i = 0; i < _available_instances; i++) { @@ -731,7 +732,9 @@ void EKF2Selector::Run() } } - if (updated) { + // keep re-evaluating while the primary is unhealthy: a silent primary + // produces no further updates and the fallback below is time based + if (updated || !_instance[_selected_instance].healthy.get_state()) { const uint8_t available_instances_prev = _available_instances; const uint8_t selected_instance_prev = _selected_instance; const uint32_t instance_changed_count_prev = _instance_changed_count; @@ -740,10 +743,19 @@ void EKF2Selector::Run() bool lower_error_available = false; float alternative_error = 0.f; // looking for instances that have error lower than the current primary float best_test_ratio = FLT_MAX; + float best_test_ratio_no_sustained_warning = FLT_MAX; uint8_t best_ekf = _selected_instance; uint8_t best_ekf_alternate = INVALID_INSTANCE; uint8_t best_ekf_different_imu = INVALID_INSTANCE; + uint8_t best_ekf_no_sustained_warning = INVALID_INSTANCE; + uint8_t best_ekf_different_imu_no_sustained_warning = INVALID_INSTANCE; + + // nominally healthy, but the test ratio has been failing for a while + const auto sustained_warning = [this](uint8_t i) { + return _instance[i].warning + && (hrt_elapsed_time(&_instance[i].time_last_no_warning) > 1_s); + }; // loop through all available instances to find if an alternative is available for (int i = 0; i < _available_instances; i++) { @@ -757,6 +769,7 @@ void EKF2Selector::Run() const float test_ratio = _instance[i].combined_test_ratio; const float relative_error = _instance[i].relative_test_ratio; + if (relative_error < alternative_error) { best_ekf_alternate = i; alternative_error = relative_error; @@ -776,14 +789,41 @@ void EKF2Selector::Run() best_ekf_different_imu = i; } } + + if (!sustained_warning(i) && (test_ratio > 0) && (test_ratio < best_test_ratio_no_sustained_warning)) { + best_ekf_no_sustained_warning = i; + best_test_ratio_no_sustained_warning = test_ratio; + + if (_instance[i].accel_device_id != _instance[_selected_instance].accel_device_id) { + best_ekf_different_imu_no_sustained_warning = i; + } + } } } + if (_instance[_selected_instance].healthy.get_state()) { + _selected_unhealthy_since = 0; + } + if (!_instance[_selected_instance].healthy.get_state()) { - // prefer the best healthy instance using a different IMU - if (!SelectInstance(best_ekf_different_imu)) { - // otherwise switch to the healthy instance with best overall test ratio - SelectInstance(best_ekf); + if (_selected_unhealthy_since == 0) { + _selected_unhealthy_since = hrt_absolute_time(); + } + + // ride out brief primary faults instead of switching to a diverged instance, + // but a timed out primary (frozen outputs) falls back without delay + const bool allow_sustained_warning_fallback = _instance[_selected_instance].timeout + || (hrt_elapsed_time(&_selected_unhealthy_since) > kWarnedFallbackDelay); + + // prefer candidates without a sustained warning, different IMU first + if (!SelectInstance(best_ekf_different_imu_no_sustained_warning)) { + if (!SelectInstance(best_ekf_no_sustained_warning)) { + if (allow_sustained_warning_fallback) { + if (!SelectInstance(best_ekf_different_imu)) { + SelectInstance(best_ekf); + } + } + } } } else if (lower_error_available diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 535af97e6d38..a3caae920873 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -78,6 +78,10 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem private: static constexpr uint8_t INVALID_INSTANCE{UINT8_MAX}; + + // unhealthy time before a sustained-warned instance becomes an acceptable + // fallback; a timed out primary bypasses this delay + static constexpr hrt_abstime kWarnedFallbackDelay{5_s}; static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms}; void Run() override; @@ -186,6 +190,7 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem uint32_t _instance_changed_count{0}; hrt_abstime _last_instance_change{0}; + hrt_abstime _selected_unhealthy_since{0}; ///< 0 while the selected instance is healthy hrt_abstime _last_status_publish{0}; bool _selector_status_publish{false}; diff --git a/src/modules/ekf2/EKF2SelectorTest.cpp b/src/modules/ekf2/EKF2SelectorTest.cpp new file mode 100644 index 000000000000..ce0f5b56c930 --- /dev/null +++ b/src/modules/ekf2/EKF2SelectorTest.cpp @@ -0,0 +1,337 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Functional test for the EKF2 instance selector, replaying the failure + * pattern from https://github.com/PX4/PX4-Autopilot/issues/27013. + * + * to run: make tests TESTFILTER=EKF2Selector + */ + +#include + +#include "EKF2Selector.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +// filter_fault_flags bit 10: bad vertical accelerometer data +static constexpr uint32_t kFaultBadAccVertical = (1u << 10); + +// Keep output unbuffered so a CI harness kill cannot swallow it, and stop the +// work queue manager at the end so the process exits instead of timing out. +class EKF2SelectorTestEnvironment : public ::testing::Environment +{ +public: + void SetUp() override { setvbuf(stdout, nullptr, _IONBF, 0); } + void TearDown() override { px4::WorkQueueManagerStop(); } +}; + +static const auto *global_env = ::testing::AddGlobalTestEnvironment(new EKF2SelectorTestEnvironment()); + +// PX4 destroys a work queue when its last WorkItem detaches, so deleting the +// selector between tests would race the next test's construction against the +// dying queue. One keeper item pins the queue for the whole process. +class WorkQueueKeeper : public px4::ScheduledWorkItem +{ +public: + WorkQueueKeeper() : ScheduledWorkItem("wq_keeper", px4::wq_configurations::nav_and_controllers) {} + +private: + void Run() override {} +}; + +class EKF2SelectorTest : public ::testing::Test +{ +public: + void SetUp() override + { + // the selector attaches to the work queue at construction + static bool wq_manager_started = false; + + if (!wq_manager_started) { + // the gtest harness starts no platform threads: the hrt callout + // worker must exist before the selector's delayed self-scheduling + // runs, or the work queue blocks on an uninitialized semaphore + hrt_work_queue_init(); + ASSERT_EQ(px4::WorkQueueManagerStart(), 0); + wq_manager_started = true; + } + + // the manager thread reports running asynchronously: wait for the + // selector's queue to be servable instead of sleeping a fixed time + const hrt_abstime wq_wait_start = hrt_absolute_time(); + + while (px4::WorkQueueFindOrCreate(px4::wq_configurations::nav_and_controllers) == nullptr) { + ASSERT_LT(hrt_elapsed_time(&wq_wait_start), 10_s) << "work queue manager did not start"; + px4_usleep(100_ms); + } + + static WorkQueueKeeper *wq_keeper = new WorkQueueKeeper(); + ASSERT_NE(wq_keeper, nullptr); + + // shared across tests: a fresh PublicationMulti per test would + // advertise new uORB instances while the selector watches 0..N + if (_status_pub[0] == nullptr) { + _status_pub[0] = new uORB::PublicationMulti(ORB_ID(estimator_status)); + _status_pub[1] = new uORB::PublicationMulti(ORB_ID(estimator_status)); + _status_pub[0]->advertise(); + _status_pub[1]->advertise(); + } + + _selector = new EKF2Selector(); + ASSERT_NE(_selector, nullptr); + + settleBaseline(); + } + + void TearDown() override + { + _selector->Stop(); + delete _selector; + } + + // publish one instance's estimator_status the way EKF2 would + void publishStatus(int instance, float test_ratio, uint32_t fault_flags) + { + estimator_status_s status{}; + status.timestamp_sample = hrt_absolute_time(); + status.accel_device_id = 1000 + instance; + status.gyro_device_id = 2000 + instance; + status.vel_test_ratio = test_ratio; + status.pos_test_ratio = test_ratio; + status.hgt_test_ratio = test_ratio; + status.filter_fault_flags = fault_flags; + status.timestamp = hrt_absolute_time(); + _status_pub[instance]->publish(status); + } + + // run the scenario for a wall-clock duration at ~100 Hz + void runFor(hrt_abstime duration, float ratio0, uint32_t faults0, float ratio1, uint32_t faults1) + { + const hrt_abstime start = hrt_absolute_time(); + + while (hrt_elapsed_time(&start) < duration) { + publishStatus(0, ratio0, faults0); + publishStatus(1, ratio1, faults1); + _selector->ScheduleNow(); + px4_usleep(5_ms); + } + } + + // settle on both instances healthy with instance 0 selected + void settleBaseline() + { + const hrt_abstime start = hrt_absolute_time(); + + hrt_abstime last_report = start; + + // generous timeout: a loaded machine can restart the 1 s health hysteresis + while (hrt_elapsed_time(&start) < 20_s) { + estimator_selector_status_s status{}; + _selector_status_sub.copy(&status); + + if (hrt_elapsed_time(&last_report) > 2_s) { + last_report = hrt_absolute_time(); + printf("settle: elapsed=%.1fs now=%llu selector status timestamp=%llu primary=%d healthy=%d/%d changed=%lu\n", + hrt_elapsed_time(&start) * 1e-6, (unsigned long long)last_report, + (unsigned long long)status.timestamp, status.primary_instance, + status.healthy[0], status.healthy[1], (unsigned long)status.instance_changed_count); + } + + // only trust a status this selector instance produced + const bool fresh = (status.timestamp > start); + + if (fresh && (status.primary_instance == 0) && (status.instances_available == 2) + && status.healthy[0] && status.healthy[1]) { + return; + } + + // a single request can be consumed during a transient + if (!fresh || (status.primary_instance != 0)) { + _selector->RequestInstance(0); + } + + publishStatus(0, 0.1f, 0); + publishStatus(1, 0.1f, 0); + _selector->ScheduleNow(); + px4_usleep(5_ms); + } + + estimator_selector_status_s status{}; + _selector_status_sub.copy(&status); + FAIL() << "selector did not settle on a healthy instance 0:" + << " primary=" << (int)status.primary_instance + << " available=" << (int)status.instances_available + << " healthy0=" << status.healthy[0] + << " healthy1=" << status.healthy[1] + << " changed=" << status.instance_changed_count; + } + + // peg instance 0's test ratio until the selector switches to instance 1 + void degradeUntilSwitched() + { + const hrt_abstime start = hrt_absolute_time(); + + while (hrt_elapsed_time(&start) < 15_s) { + publishStatus(0, 2.f, 0); + publishStatus(1, 0.1f, 0); + _selector->ScheduleNow(); + px4_usleep(5_ms); + + if (primaryInstance() == 1) { + return; + } + } + + FAIL() << "selector never switched away from the degraded primary"; + } + + uint8_t primaryInstance() + { + estimator_selector_status_s status{}; + _selector_status_sub.copy(&status); + return status.primary_instance; + } + + uint32_t instanceChangedCount() + { + estimator_selector_status_s status{}; + _selector_status_sub.copy(&status); + return status.instance_changed_count; + } + + EKF2Selector *_selector{nullptr}; + static uORB::PublicationMulti *_status_pub[2]; + uORB::Subscription _selector_status_sub{ORB_ID(estimator_selector_status)}; +}; + +uORB::PublicationMulti *EKF2SelectorTest::_status_pub[2] {nullptr, nullptr}; + +// two healthy instances: no churn +TEST_F(EKF2SelectorTest, staysOnHealthyInstance) +{ + const uint32_t switches = instanceChangedCount(); + + runFor(500_ms, 0.1f, 0, 0.1f, 0); + + EXPECT_EQ(primaryInstance(), 0); + EXPECT_EQ(instanceChangedCount(), switches); // no changes after settling +} + +// a hard fault on the primary fails over immediately to a clean alternative +TEST_F(EKF2SelectorTest, cleanFallbackIsImmediate) +{ + const hrt_abstime fault_start = hrt_absolute_time(); + hrt_abstime switched_after = 0; + + while (hrt_elapsed_time(&fault_start) < 5_s) { + publishStatus(0, 0.1f, kFaultBadAccVertical); + publishStatus(1, 0.1f, 0); + _selector->ScheduleNow(); + px4_usleep(5_ms); + + if (primaryInstance() == 1) { + switched_after = hrt_elapsed_time(&fault_start); + break; + } + } + + EXPECT_EQ(primaryInstance(), 1); + EXPECT_GT(switched_after, 0u); + EXPECT_LT(switched_after, 1_s); // immediate, not the warned-fallback delay +} + +// the switch-away half of b7efd4f947 still works +TEST_F(EKF2SelectorTest, switchesAwayFromDegradedPrimary) +{ + degradeUntilSwitched(); + EXPECT_EQ(primaryInstance(), 1); +} + +// the issue 27013 pattern: brief faults on the primary must not bounce the +// selector to a nominally healthy instance with a pegged test ratio +TEST_F(EKF2SelectorTest, noFallbackToSustainedWarnedInstance) +{ + degradeUntilSwitched(); + const uint32_t switches_before = instanceChangedCount(); + + // clear windows exceed the 1 s healthy hysteresis, so these are three + // separate faults, not one long unhealthy episode + for (int cycle = 0; cycle < 3; cycle++) { + runFor(800_ms, 2.f, 0, 0.1f, kFaultBadAccVertical); // faulted primary, diverged alternative + runFor(1600_ms, 2.f, 0, 0.1f, 0); // fault clears, primary re-heals + } + + EXPECT_EQ(primaryInstance(), 1); + EXPECT_EQ(instanceChangedCount(), switches_before); // no whipsaw +} + +// a timed out primary falls back to the warned instance without delay +TEST_F(EKF2SelectorTest, timedOutPrimaryFallsBackImmediately) +{ + runFor(2500_ms, 2.f, 0, 0.1f, 0); + ASSERT_EQ(primaryInstance(), 1); + + // primary goes silent, only the warned instance 0 keeps publishing + const hrt_abstime start = hrt_absolute_time(); + + while (hrt_elapsed_time(&start) < 3_s) { + publishStatus(0, 2.f, 0); + _selector->ScheduleNow(); + px4_usleep(5_ms); + + if (primaryInstance() == 0) { + break; + } + } + + EXPECT_EQ(primaryInstance(), 0); +} + +// a persistently faulted primary still falls back to the warned instance +TEST_F(EKF2SelectorTest, sustainedFaultFallsBackEventually) +{ + degradeUntilSwitched(); + + runFor(7000_ms, 2.f, 0, 0.1f, kFaultBadAccVertical); + EXPECT_EQ(primaryInstance(), 0); +}