diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 85e1805627..7fb5da77a9 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -75,6 +75,7 @@ px4_add_module( DEPENDS geo + hysteresis perf EKF2Utility px4_work_queue diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index b71ace2c9f..75ad802ca5 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -82,7 +82,7 @@ void EKF2Selector::PrintInstanceChange(const uint8_t old_instance, uint8_t new_i } else if (_accel_fault_detected) { old_reason = " (accel fault)"; - } else if (!_instance[_selected_instance].healthy && (_instance[_selected_instance].healthy_count > 0)) { + } else if (!_instance[_selected_instance].healthy.get_state() && (_instance[_selected_instance].healthy_count > 0)) { // skipped if previous instance was never healthy in the first place (eg initialization) old_reason = " (unhealthy)"; } @@ -255,7 +255,7 @@ bool EKF2Selector::UpdateErrorScores() // calculate individual error scores for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { - const bool prev_healthy = _instance[i].healthy; + const bool prev_healthy = _instance[i].healthy.get_state(); estimator_status_s status; @@ -293,7 +293,10 @@ 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); + + const bool healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f); + _instance[i].healthy.set_state_and_update(healthy, status.timestamp); + _instance[i].warning = (combined_test_ratio >= 1.f); _instance[i].filter_fault = (status.filter_fault_flags != 0); _instance[i].timeout = false; @@ -307,21 +310,21 @@ bool EKF2Selector::UpdateErrorScores() } } else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) { - _instance[i].healthy = false; + _instance[i].healthy.set_state_and_update(false, hrt_absolute_time()); _instance[i].timeout = true; } // if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay if (_gyro_fault_detected && (faulty_gyro_id != 0) && (_instance[i].gyro_device_id == faulty_gyro_id)) { - _instance[i].healthy = false; + _instance[i].healthy.set_state_and_update(false, hrt_absolute_time()); } // if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay if (_accel_fault_detected && (faulty_accel_id != 0) && (_instance[i].accel_device_id == faulty_accel_id)) { - _instance[i].healthy = false; + _instance[i].healthy.set_state_and_update(false, hrt_absolute_time()); } - if (prev_healthy != _instance[i].healthy) { + if (prev_healthy != _instance[i].healthy.get_state()) { updated = true; _selector_status_publish = true; @@ -705,7 +708,7 @@ void EKF2Selector::Run() // (has relative error less than selected instance and has not been the selected instance for at least 10 seconds // OR // selected instance has stopped updating - if (_instance[i].healthy && (i != _selected_instance)) { + if (_instance[i].healthy.get_state() && (i != _selected_instance)) { const float test_ratio = _instance[i].combined_test_ratio; const float relative_error = _instance[i].relative_test_ratio; @@ -731,7 +734,7 @@ void EKF2Selector::Run() } } - if (!_instance[_selected_instance].healthy) { + 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 @@ -798,7 +801,7 @@ void EKF2Selector::PublishEstimatorSelectorStatus() for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio; selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio; - selector_status.healthy[i] = _instance[i].healthy; + selector_status.healthy[i] = _instance[i].healthy.get_state(); } for (int i = 0; i < IMU_STATUS_SIZE; i++) { @@ -824,7 +827,7 @@ void EKF2Selector::PrintStatus() PX4_INFO("%" PRIu8 ": ACC: %" PRIu32 ", GYRO: %" PRIu32 ", MAG: %" PRIu32 ", %s, test ratio: %.7f (%.5f) %s", inst.instance, inst.accel_device_id, inst.gyro_device_id, inst.mag_device_id, - inst.healthy ? "healthy" : "unhealthy", + inst.healthy.get_state() ? "healthy" : "unhealthy", (double)inst.combined_test_ratio, (double)inst.relative_test_ratio, (_selected_instance == i) ? "*" : ""); } diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 8963a9e8fa..628fa1a090 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -107,7 +108,9 @@ private: estimator_odometry_sub{ORB_ID(estimator_odometry), i}, estimator_wind_sub{ORB_ID(estimator_wind), i}, instance(i) - {} + { + healthy.set_hysteresis_time_from(false, 1_s); + } uORB::SubscriptionCallbackWorkItem estimator_attitude_sub; uORB::SubscriptionCallbackWorkItem estimator_status_sub; @@ -130,7 +133,8 @@ private: float combined_test_ratio{NAN}; float relative_test_ratio{NAN}; - bool healthy{false}; + systemlib::Hysteresis healthy{false}; + bool warning{false}; bool filter_fault{false}; bool timeout{false};