ekf2: selector use hysteresis for healthy flag

- require that an instance is healthy continuously for >= 1s before
allowing it to be used

src/modules/ekf2/CMakeLists.txt

@@ -75,6 +75,7 @@ px4_add_module(
 
 	DEPENDS
 		geo
+		hysteresis
 		perf
 		EKF2Utility
 		px4_work_queue

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) ? "*" : "");
 	}

src/modules/ekf2/EKF2Selector.hpp

@@ -39,6 +39,7 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/time.h>
+#include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Subscription.hpp>
@@ -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};