ekf2: selector improve status reporting

- publish estimator_selector_status at minimal rate or immediately on change
 - log all estimator_selector_status updates

src/modules/ekf2/EKF2Selector.cpp

@@ -69,12 +69,12 @@ void EKF2Selector::Stop()
 
 bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
 {
-	if (ekf_instance != _selected_instance) {
+	if ((ekf_instance != INVALID_INSTANCE) && (ekf_instance != _selected_instance)) {
 
 		// update sensor_selection immediately
 		sensor_selection_s sensor_selection{};
-		sensor_selection.accel_device_id = _instance[ekf_instance].estimator_status.accel_device_id;
-		sensor_selection.gyro_device_id = _instance[ekf_instance].estimator_status.gyro_device_id;
+		sensor_selection.accel_device_id = _instance[ekf_instance].status.accel_device_id;
+		sensor_selection.gyro_device_id = _instance[ekf_instance].status.gyro_device_id;
 		sensor_selection.timestamp = hrt_absolute_time();
 		_sensor_selection_pub.publish(sensor_selection);
 
@@ -84,7 +84,8 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
 			_instance[_selected_instance].estimator_status_sub.unregisterCallback();
 
 			if (!_instance[_selected_instance].healthy) {
-				PX4_WARN("primary EKF changed %d (unhealthy) -> %d", _selected_instance, ekf_instance);
+				PX4_WARN("primary EKF changed %d (%s) -> %d", _selected_instance,
+					 _instance[_selected_instance].filter_fault ? "filter fault" : "unhealthy", ekf_instance);
 			}
 		}
 
@@ -223,9 +224,9 @@ bool EKF2Selector::UpdateErrorScores()
 	for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) {
 		const bool prev_healthy = _instance[i].healthy;
 
-		const estimator_status_s &status = _instance[i].estimator_status;
+		const estimator_status_s &status = _instance[i].status;
 
-		if (_instance[i].estimator_status_sub.update(&_instance[i].estimator_status)) {
+		if (_instance[i].estimator_status_sub.update(&_instance[i].status)) {
 
 			if ((i + 1) > _available_instances) {
 				_available_instances = i + 1;
@@ -248,12 +249,13 @@ bool EKF2Selector::UpdateErrorScores()
 
 			_instance[i].combined_test_ratio = combined_test_ratio;
 			_instance[i].healthy = tilt_align && yaw_align && (status.filter_fault_flags == 0);
+			_instance[i].filter_fault = (status.filter_fault_flags != 0);
 
 			if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) {
 				_instance[i].relative_test_ratio = 0;
 			}
 
-		} else if (hrt_elapsed_time(&status.timestamp) > 20_ms) {
+		} else if (hrt_elapsed_time(&status.timestamp) > (FILTER_UPDATE_PERIOD * 2)) {
 			_instance[i].healthy = false;
 		}
 
@@ -269,6 +271,7 @@ bool EKF2Selector::UpdateErrorScores()
 
 		if (prev_healthy != _instance[i].healthy) {
 			updated = true;
+			_selector_status_publish = true;
 		}
 	}
 
@@ -388,7 +391,7 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset)
 		_local_position_last = local_position;
 
 		// publish estimator's local position for system (vehicle_local_position) unless it's stale
-		if (local_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
+		if (local_position.timestamp >= _instance[_selected_instance].status.timestamp_sample) {
 			// republish with total reset count and current timestamp
 			local_position.xy_reset_counter = _xy_reset_counter;
 			local_position.z_reset_counter = _z_reset_counter;
@@ -448,7 +451,7 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
 		_global_position_last = global_position;
 
 		// publish estimator's global position for system (vehicle_global_position) unless it's stale
-		if (global_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
+		if (global_position.timestamp >= _instance[_selected_instance].status.timestamp_sample) {
 			// republish with total reset count and current timestamp
 			global_position.lat_lon_reset_counter = _lat_lon_reset_counter;
 			global_position.alt_reset_counter = _alt_reset_counter;
@@ -463,7 +466,7 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
 void EKF2Selector::Run()
 {
 	// re-schedule as backup timeout
-	ScheduleDelayed(10_ms);
+	ScheduleDelayed(FILTER_UPDATE_PERIOD);
 
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
@@ -478,11 +481,11 @@ void EKF2Selector::Run()
 	// update combined test ratio for all estimators
 	const bool updated = UpdateErrorScores();
 
-	// if no valid instance selected then select instance with valid IMU
+	// if no valid instance then force select first instance with valid IMU
 	if (_selected_instance == INVALID_INSTANCE) {
 		for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) {
-			if ((_instance[i].estimator_status.accel_device_id != 0)
-			    && (_instance[i].estimator_status.gyro_device_id != 0)) {
+			if ((_instance[i].status.accel_device_id != 0)
+			    && (_instance[i].status.gyro_device_id != 0)) {
 
 				if (SelectInstance(i)) {
 					break;
@@ -497,6 +500,12 @@ void EKF2Selector::Run()
 	}
 
 	if (updated) {
+
+		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;
+		const hrt_abstime last_instance_change_prev = _last_instance_change;
+
 		bool lower_error_available = false;
 		float alternative_error = 0.f; // looking for instances that have error lower than the current primary
 		uint8_t best_ekf_instance = _selected_instance;
@@ -554,60 +563,69 @@ void EKF2Selector::Run()
 			}
 		}
 
-		// publish selector status
-		estimator_selector_status_s selector_status{};
-		selector_status.primary_instance = _selected_instance;
-		selector_status.instances_available = _available_instances;
-		selector_status.instance_changed_count = _instance_changed_count;
-		selector_status.last_instance_change = _last_instance_change;
-		selector_status.accel_device_id = _instance[_selected_instance].estimator_status.accel_device_id;
-		selector_status.baro_device_id = _instance[_selected_instance].estimator_status.baro_device_id;
-		selector_status.gyro_device_id = _instance[_selected_instance].estimator_status.gyro_device_id;
-		selector_status.mag_device_id = _instance[_selected_instance].estimator_status.mag_device_id;
-		selector_status.gyro_fault_detected = _gyro_fault_detected;
-		selector_status.accel_fault_detected = _accel_fault_detected;
-
-		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;
-		}
+		// publish selector status at ~1 Hz or immediately on any change
+		if (_selector_status_publish || (hrt_elapsed_time(&_last_status_publish) > 1_s)
+		    || (available_instances_prev != _available_instances)
+		    || (selected_instance_prev != _selected_instance)
+		    || (instance_changed_count_prev != _instance_changed_count)
+		    || (last_instance_change_prev != _last_instance_change)
+		    || _accel_fault_detected || _gyro_fault_detected) {
+
+			estimator_selector_status_s selector_status{};
+			selector_status.primary_instance = _selected_instance;
+			selector_status.instances_available = _available_instances;
+			selector_status.instance_changed_count = _instance_changed_count;
+			selector_status.last_instance_change = _last_instance_change;
+			selector_status.accel_device_id = _instance[_selected_instance].status.accel_device_id;
+			selector_status.baro_device_id = _instance[_selected_instance].status.baro_device_id;
+			selector_status.gyro_device_id = _instance[_selected_instance].status.gyro_device_id;
+			selector_status.mag_device_id = _instance[_selected_instance].status.mag_device_id;
+			selector_status.gyro_fault_detected = _gyro_fault_detected;
+			selector_status.accel_fault_detected = _accel_fault_detected;
+
+			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;
+			}
 
-		for (int i = 0; i < IMU_STATUS_SIZE; i++) {
-			selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
-			selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
-		}
+			for (int i = 0; i < IMU_STATUS_SIZE; i++) {
+				selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
+				selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
+			}
 
-		selector_status.timestamp = hrt_absolute_time();
-		_estimator_selector_status_pub.publish(selector_status);
+			selector_status.timestamp = hrt_absolute_time();
+			_estimator_selector_status_pub.publish(selector_status);
+			_last_status_publish = selector_status.timestamp;
+			_selector_status_publish = false;
+		}
 	}
 
 	// republish selected estimator data for system
-	if (_selected_instance != INVALID_INSTANCE) {
-		// selected estimator_attitude -> vehicle_attitude
-		if (_instance[_selected_instance].estimator_attitude_sub.updated()) {
-			PublishVehicleAttitude();
-		}
 
-		// selected estimator_local_position -> vehicle_local_position
-		if (_instance[_selected_instance].estimator_local_position_sub.updated()) {
-			PublishVehicleLocalPosition();
-		}
+	// selected estimator_attitude -> vehicle_attitude
+	if (_instance[_selected_instance].estimator_attitude_sub.updated()) {
+		PublishVehicleAttitude();
+	}
 
-		// selected estimator_global_position -> vehicle_global_position
-		if (_instance[_selected_instance].estimator_global_position_sub.updated()) {
-			PublishVehicleGlobalPosition();
-		}
+	// selected estimator_local_position -> vehicle_local_position
+	if (_instance[_selected_instance].estimator_local_position_sub.updated()) {
+		PublishVehicleLocalPosition();
+	}
 
-		// selected estimator_odometry -> vehicle_odometry
-		if (_instance[_selected_instance].estimator_odometry_sub.updated()) {
-			vehicle_odometry_s vehicle_odometry;
+	// selected estimator_global_position -> vehicle_global_position
+	if (_instance[_selected_instance].estimator_global_position_sub.updated()) {
+		PublishVehicleGlobalPosition();
+	}
 
-			if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) {
-				if (vehicle_odometry.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
-					vehicle_odometry.timestamp = hrt_absolute_time();
-					_vehicle_odometry_pub.publish(vehicle_odometry);
-				}
+	// selected estimator_odometry -> vehicle_odometry
+	if (_instance[_selected_instance].estimator_odometry_sub.updated()) {
+		vehicle_odometry_s vehicle_odometry;
+
+		if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) {
+			if (vehicle_odometry.timestamp >= _instance[_selected_instance].status.timestamp_sample) {
+				vehicle_odometry.timestamp = hrt_absolute_time();
+				_vehicle_odometry_pub.publish(vehicle_odometry);
 			}
 		}
 	}
@@ -625,8 +643,8 @@ void EKF2Selector::PrintStatus()
 		const EstimatorInstance &inst = _instance[i];
 
 		PX4_INFO("%d: ACC: %d, GYRO: %d, MAG: %d, %s, test ratio: %.7f (%.5f) %s",
-			 inst.instance, inst.estimator_status.accel_device_id, inst.estimator_status.gyro_device_id,
-			 inst.estimator_status.mag_device_id,
+			 inst.instance, inst.status.accel_device_id, inst.status.gyro_device_id,
+			 inst.status.mag_device_id,
 			 inst.healthy ? "healthy" : "unhealthy",
 			 (double)inst.combined_test_ratio, (double)inst.relative_test_ratio,
 			 (_selected_instance == i) ? "*" : "");

src/modules/ekf2/EKF2Selector.hpp

@@ -56,6 +56,8 @@
 static constexpr uint8_t EKF2_MAX_INSTANCES{9};
 static_assert(EKF2_MAX_INSTANCES <= ORB_MULTI_MAX_INSTANCES, "EKF2_MAX_INSTANCES must be <= ORB_MULTI_MAX_INSTANCES");
 
+using namespace time_literals;
+
 class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -68,7 +70,9 @@ public:
 	void PrintStatus();
 
 private:
-	static constexpr uint8_t INVALID_INSTANCE = UINT8_MAX;
+	static constexpr uint8_t INVALID_INSTANCE{UINT8_MAX};
+	static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms};
+
 	void Run() override;
 	void PublishVehicleAttitude(bool reset = false);
 	void PublishVehicleLocalPosition(bool reset = false);
@@ -96,7 +100,7 @@ private:
 		uORB::Subscription estimator_global_position_sub;
 		uORB::Subscription estimator_odometry_sub;
 
-		estimator_status_s estimator_status{};
+		estimator_status_s status{};
 
 		hrt_abstime time_last_selected{0};
 
@@ -104,6 +108,7 @@ private:
 		float relative_test_ratio{NAN};
 
 		bool healthy{false};
+		bool filter_fault{false};
 
 		const uint8_t instance;
 	};
@@ -147,6 +152,9 @@ private:
 	uint32_t _instance_changed_count{0};
 	hrt_abstime _last_instance_change{0};
 
+	hrt_abstime _last_status_publish{0};
+	bool _selector_status_publish{false};
+
 	// vehicle_attitude: reset counters
 	vehicle_attitude_s _attitude_last{};
 	matrix::Quatf _delta_q_reset{};

src/modules/logger/logged_topics.cpp

@@ -114,7 +114,7 @@ void LoggedTopics::add_default_topics()
 
 	// EKF multi topics (currently max 9 estimators)
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4;
-	add_topic("estimator_selector_status", 200);
+	add_topic("estimator_selector_status");
 	add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);