ekf2: add new kconfig option for multi-EKF

- disabled if flash constrained or memory constrained

boards/px4/fmu-v5x/default.px4board

@@ -39,7 +39,6 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y

src/modules/ekf2/EKF2.cpp

@@ -42,9 +42,9 @@ using matrix::Vector3f;
 
 pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
-#if !defined(CONSTRAINED_FLASH)
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
-#endif // !CONSTRAINED_FLASH
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 
 EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	ModuleParams(nullptr),
@@ -204,6 +204,7 @@ EKF2::~EKF2()
 	perf_free(_msg_missed_optical_flow_perf);
 }
 
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 bool EKF2::multi_init(int imu, int mag)
 {
 	// advertise all topics to ensure consistent uORB instance numbering
@@ -295,6 +296,7 @@ bool EKF2::multi_init(int imu, int mag)
 
 	return false;
 }
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 
 int EKF2::print_status()
 {
@@ -391,10 +393,14 @@ void EKF2::Run()
 	}
 
 	if (!_callback_registered) {
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
+
 		if (_multi_mode) {
 			_callback_registered = _vehicle_imu_sub.registerCallback();
 
-		} else {
+		} else
+#endif // CONFIG_EKF2_MULTI_INSTANCE
+		{
 			_callback_registered = _sensor_combined_sub.registerCallback();
 		}
 
@@ -433,6 +439,8 @@ void EKF2::Run()
 
 	hrt_abstime imu_dt = 0; // for tracking time slip later
 
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
+
 	if (_multi_mode) {
 		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
 		vehicle_imu_s imu;
@@ -492,7 +500,9 @@ void EKF2::Run()
 			}
 		}
 
-	} else {
+	} else
+#endif // CONFIG_EKF2_MULTI_INSTANCE
+	{
 		const unsigned last_generation = _sensor_combined_sub.get_last_generation();
 		sensor_combined_s sensor_combined;
 		imu_updated = _sensor_combined_sub.update(&sensor_combined);
@@ -2325,7 +2335,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 		replay_mode = true;
 	}
 
-#if !defined(CONSTRAINED_FLASH)
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 	bool multi_mode = false;
 	int32_t imu_instances = 0;
 	int32_t mag_instances = 0;
@@ -2464,7 +2474,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 	} else
 
-#endif // !CONSTRAINED_FLASH
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 
 	{
 		// otherwise launch regular
@@ -2502,10 +2512,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)
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 	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
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 	return 0;
 }
 
@@ -2528,7 +2538,7 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 		EKF2::unlock_module();
 		return ret;
 
-#if !defined(CONSTRAINED_FLASH)
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 	} else if (strcmp(argv[1], "select_instance") == 0) {
 
 		if (EKF2::trylock_module()) {
@@ -2552,14 +2562,14 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 		}
 
 		return 0;
-#endif // !CONSTRAINED_FLASH
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 	} else if (strcmp(argv[1], "status") == 0) {
 		if (EKF2::trylock_module()) {
-#if !defined(CONSTRAINED_FLASH)
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 			if (_ekf2_selector.load()) {
 				_ekf2_selector.load()->PrintStatus();
 			}
-#endif // !CONSTRAINED_FLASH
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 
 			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
 				if (_objects[i].load()) {
@@ -2600,7 +2610,7 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 			// otherwise stop everything
 			bool was_running = false;
 
-#if !defined(CONSTRAINED_FLASH)
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 			if (_ekf2_selector.load()) {
 				PX4_INFO("stopping ekf2 selector");
 				_ekf2_selector.load()->Stop();
@@ -2608,7 +2618,7 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 				_ekf2_selector.store(nullptr);
 				was_running = true;
 			}
-#endif // !CONSTRAINED_FLASH
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 
 			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
 				EKF2 *inst = _objects[i].load();

src/modules/ekf2/EKF2.hpp

@@ -125,7 +125,9 @@ public:
 	static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); }
 	static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); }
 
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
 	bool multi_init(int imu, int mag);
+#endif // CONFIG_EKF2_MULTI_INSTANCE
 
 	int instance() const { return _instance; }
 

src/modules/ekf2/Kconfig

@@ -4,6 +4,15 @@ menuconfig MODULES_EKF2
 	---help---
 		Enable support for ekf2
 
+menuconfig EKF2_MULTI_INSTANCE
+depends on MODULES_EKF2
+        bool "multi-EKF support"
+        default y
+	depends on !BOARD_CONSTRAINED_MEMORY
+	depends on !BOARD_CONSTRAINED_FLASH
+	---help---
+		EKF2 support multiple instances and selector.
+
 menuconfig USER_EKF2
 	bool "ekf2 running as userspace module"
 	default n