From 2ccd86102b84f4aaf86ab13b01d268969730f9c8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 3 Jun 2021 11:54:53 -0400 Subject: [PATCH] ekf2: add command line option to manually select instance --- src/modules/ekf2/EKF2.cpp | 30 +++++++++++- src/modules/ekf2/EKF2Selector.cpp | 80 ++++++++++++++++++++++--------- src/modules/ekf2/EKF2Selector.hpp | 9 ++++ 3 files changed, 96 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e9131a0ece..6781f64a7b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1856,7 +1856,10 @@ timestamps from the sensor topics. PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - +#if !defined(CONSTRAINED_FLASH) + PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance"); + PRINT_MODULE_USAGE_ARG("", "Specify desired estimator instance", false); +#endif // !CONSTRAINED_FLASH return 0; } @@ -1879,6 +1882,31 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[]) EKF2::unlock_module(); return ret; +#if !defined(CONSTRAINED_FLASH) + } else if (strcmp(argv[1], "select_instance") == 0) { + + if (EKF2::trylock_module()) { + if (_ekf2_selector.load()) { + if (argc > 2) { + int instance = atoi(argv[2]); + _ekf2_selector.load()->RequestInstance(instance); + } else { + EKF2::unlock_module(); + return EKF2::print_usage("instance required"); + } + + } else { + PX4_ERR("multi-EKF not active, unable to select instance"); + } + + EKF2::unlock_module(); + + } else { + PX4_WARN("module locked, try again later"); + } + + return 0; +#endif // !CONSTRAINED_FLASH } else if (strcmp(argv[1], "status") == 0) { if (EKF2::trylock_module()) { #if !defined(CONSTRAINED_FLASH) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 4141883b91..d1e97ccbf3 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -66,10 +66,49 @@ void EKF2Selector::Stop() ScheduleClear(); } +void EKF2Selector::PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance) +{ + const char *old_reason = nullptr; + + if (_instance[old_instance].filter_fault) { + old_reason = " (filter fault)"; + + } else if (_instance[old_instance].timeout) { + old_reason = " (timeout)"; + + } else if (_gyro_fault_detected) { + old_reason = " (gyro fault)"; + + } else if (_accel_fault_detected) { + old_reason = " (accel fault)"; + + } else if (!_instance[_selected_instance].healthy && (_instance[_selected_instance].healthy_count > 0)) { + // skipped if previous instance was never healthy in the first place (eg initialization) + old_reason = " (unhealthy)"; + } + + const char *new_reason = nullptr; + + if (_request_instance.load() == new_instance) { + new_reason = " (user selected)"; + } + + if (old_reason || new_reason) { + if (old_reason == nullptr) { + old_reason = ""; + } + + if (new_reason == nullptr) { + new_reason = ""; + } + + PX4_WARN("primary EKF changed %d%s -> %d%s", old_instance, old_reason, new_instance, new_reason); + } +} + bool EKF2Selector::SelectInstance(uint8_t ekf_instance) { - if ((ekf_instance != INVALID_INSTANCE) && (ekf_instance != _selected_instance)) { - + if ((ekf_instance != _selected_instance) && (ekf_instance < _available_instances)) { // update sensor_selection immediately sensor_selection_s sensor_selection{}; sensor_selection.accel_device_id = _instance[ekf_instance].accel_device_id; @@ -82,26 +121,7 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) _instance[_selected_instance].estimator_attitude_sub.unregisterCallback(); _instance[_selected_instance].estimator_status_sub.unregisterCallback(); - if (!_instance[_selected_instance].healthy) { - const char *reason = nullptr; - - if (_instance[_selected_instance].filter_fault) { - reason = "filter fault"; - - } else if (_instance[_selected_instance].timeout) { - reason = "timeout"; - - } else if (_gyro_fault_detected) { - reason = "gyro fault"; - - } else if (_accel_fault_detected) { - reason = "accel fault"; - } - - if (reason) { - PX4_WARN("primary EKF changed %d (%s) -> %d", _selected_instance, reason, ekf_instance); - } - } + PrintInstanceChange(_selected_instance, ekf_instance); } _instance[ekf_instance].estimator_attitude_sub.registerCallback(); @@ -299,6 +319,10 @@ bool EKF2Selector::UpdateErrorScores() if (prev_healthy != _instance[i].healthy) { updated = true; _selector_status_publish = true; + + if (!prev_healthy) { + _instance[i].healthy_count++; + } } } @@ -713,6 +737,18 @@ void EKF2Selector::Run() // 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); + + } else if (_request_instance.load() != INVALID_INSTANCE) { + + const uint8_t new_instance = _request_instance.load(); + + // attempt to switch to user manually selected instance + if (!SelectInstance(new_instance)) { + PX4_ERR("unable to switch to user selected instance %d", new_instance); + } + + // reset + _request_instance.store(INVALID_INSTANCE); } // publish selector status at ~1 Hz or immediately on any change diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 879b3ab1d4..a37f5e592c 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -73,17 +73,23 @@ public: void PrintStatus(); + void RequestInstance(uint8_t instance) { _request_instance.store(instance); } + private: static constexpr uint8_t INVALID_INSTANCE{UINT8_MAX}; static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms}; void Run() override; + + void PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance); + void PublishEstimatorSelectorStatus(); void PublishVehicleAttitude(); void PublishVehicleLocalPosition(); void PublishVehicleGlobalPosition(); void PublishVehicleOdometry(); void PublishWindEstimate(); + bool SelectInstance(uint8_t instance); // Update the error scores for all available instances @@ -126,6 +132,8 @@ private: bool filter_fault{false}; bool timeout{false}; + uint8_t healthy_count{0}; + const uint8_t instance; }; @@ -168,6 +176,7 @@ private: uint8_t _available_instances{0}; uint8_t _selected_instance{INVALID_INSTANCE}; + px4::atomic _request_instance{INVALID_INSTANCE}; uint32_t _instance_changed_count{0}; hrt_abstime _last_instance_change{0};