ekf2: add command line option to manually select instance

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("<instance>", "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)

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

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<uint8_t> _request_instance{INVALID_INSTANCE};
 
 	uint32_t _instance_changed_count{0};
 	hrt_abstime _last_instance_change{0};