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); +}