ekf2: selector increase status rate before potential instance change

src/modules/ekf2/EKF2Selector.cpp

@@ -315,6 +315,11 @@ bool EKF2Selector::UpdateErrorScores()
 				if (error_delta > 0 || error_delta < -threshold) {
 					_instance[i].relative_test_ratio += error_delta;
 					_instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim);
+
+					if ((error_delta < -threshold) && (_instance[i].relative_test_ratio < 1.f)) {
+						// increase status publication rate if there's movement towards a potential instance change
+						_selector_status_publish = true;
+					}
 				}
 			}
 		}
@@ -650,7 +655,6 @@ 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;
@@ -719,6 +723,21 @@ void EKF2Selector::Run()
 		    || (last_instance_change_prev != _last_instance_change)
 		    || _accel_fault_detected || _gyro_fault_detected) {
 
+			PublishEstimatorSelectorStatus();
+			_selector_status_publish = false;
+		}
+	}
+
+	// republish selected estimator data for system
+	PublishVehicleAttitude();
+	PublishVehicleLocalPosition();
+	PublishVehicleGlobalPosition();
+	PublishVehicleOdometry();
+	PublishWindEstimate();
+}
+
+void EKF2Selector::PublishEstimatorSelectorStatus()
+{
 	estimator_selector_status_s selector_status{};
 	selector_status.primary_instance = _selected_instance;
 	selector_status.instances_available = _available_instances;
@@ -745,16 +764,6 @@ void EKF2Selector::Run()
 	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
-	PublishVehicleAttitude();
-	PublishVehicleLocalPosition();
-	PublishVehicleGlobalPosition();
-	PublishVehicleOdometry();
-	PublishWindEstimate();
 }
 
 void EKF2Selector::PrintStatus()

src/modules/ekf2/EKF2Selector.hpp

@@ -78,6 +78,7 @@ private:
 	static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms};
 
 	void Run() override;
+	void PublishEstimatorSelectorStatus();
 	void PublishVehicleAttitude();
 	void PublishVehicleLocalPosition();
 	void PublishVehicleGlobalPosition();