delete SYS_MC_EST_GROUP

- introduce per module parameters (EKF2_EN, LPE_EN, ATT_EN)
 - add basic checks to prevent EKF2 + LPE running simultaneously

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -187,10 +187,6 @@ param set-default SYS_FAILURE_EN 1
 # does not go below 50% by default, but failure injection can trigger failsafes.
 param set-default COM_LOW_BAT_ACT 2
 
-# Allow to override SYS_MC_EST_GROUP via env
-if [ -n "$SYS_MC_EST_GROUP" ]; then
-	param set SYS_MC_EST_GROUP $SYS_MC_EST_GROUP
-fi
 
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
@@ -330,7 +326,7 @@ fi
 [ -e "$autostart_file".post ] && . "$autostart_file".post
 
 # Run script to start logging
-if param compare SYS_MC_EST_GROUP 2
+if param compare EKF2_EN 1
 then
 	set LOGGER_ARGS "-p ekf2_timestamps"
 else

ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21

@@ -18,7 +18,6 @@
 . ${R}etc/init.d/rc.mc_defaults
 
 
-param set-default SYS_MC_EST_GROUP 2
 param set-default SYS_HAS_MAG 0
 param set-default EKF2_OF_CTRL 1
 param set-default EKF2_GPS_CTRL 0

ROMFS/px4fmu_common/init.d/rc.airship_apps

@@ -5,49 +5,6 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
 
-###############################################################################
-#                       Begin Estimator Group Selection                       #
-###############################################################################
-
-#
-# LPE
-#
-if param compare SYS_MC_EST_GROUP 1
-then
-	#
-	# Try to start LPE. If it fails, start EKF2 as a default.
-	# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
-	#
-	if attitude_estimator_q start
-	then
-		echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
-		local_position_estimator start
-	else
-		echo "ERROR [init] Estimator LPE not available. Using EKF2"
-		param set SYS_MC_EST_GROUP 2
-		param save
-		reboot
-	fi
-else
-	#
-	# Q estimator (attitude estimation only)
-	#
-	if param compare SYS_MC_EST_GROUP 3
-	then
-		attitude_estimator_q start
-	else
-		#
-		# EKF2
-		#
-		param set SYS_MC_EST_GROUP 2
-		ekf2 start &
-	fi
-fi
-
-###############################################################################
-#                        End Estimator Group Selection                        #
-###############################################################################
-
 #
 # Start Control Allocator
 #

ROMFS/px4fmu_common/init.d/rc.balloon_apps

@@ -4,39 +4,3 @@
 #
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
-
-#
-# Start the attitude and position estimator.
-#
-
-if param compare SYS_MC_EST_GROUP 1
-then
-	#
-	# Try to start LPE. If it fails, start EKF2 as a default.
-	# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
-	#
-	if attitude_estimator_q start
-	then
-		echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
-		local_position_estimator start
-	else
-		echo "ERROR [init] Estimator LPE not available. Using EKF2"
-		param set SYS_MC_EST_GROUP 2
-		param save
-		reboot
-	fi
-else
-	#
-	# Q estimator (attitude estimation only)
-	#
-	if param compare SYS_MC_EST_GROUP 3
-	then
-		attitude_estimator_q start
-	else
-		#
-		# EKF2
-		#
-		param set SYS_MC_EST_GROUP 2
-		ekf2 start &
-	fi
-fi

ROMFS/px4fmu_common/init.d/rc.balloon_defaults

@@ -13,4 +13,6 @@ param set-default MAV_TYPE 8
 #
 # Default parameters for balloon UAVs.
 #
-param set-default SYS_MC_EST_GROUP 1
+param set-default LPE_EN 1
+param set-default ATT_EN 1
+param set-default EKF2_EN 0

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -5,49 +5,6 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
 
-###############################################################################
-#                       Begin Estimator Group Selection                       #
-###############################################################################
-
-#
-# LPE
-#
-if param compare SYS_MC_EST_GROUP 1
-then
-	#
-	# Try to start LPE. If it fails, start EKF2 as a default.
-	# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
-	#
-	if attitude_estimator_q start
-	then
-		echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
-		local_position_estimator start
-	else
-		echo "ERROR [init] Estimator LPE not available. Using EKF2"
-		param set SYS_MC_EST_GROUP 2
-		param save
-		reboot
-	fi
-else
-	#
-	# Q estimator (attitude estimation only)
-	#
-	if param compare SYS_MC_EST_GROUP 3
-	then
-		attitude_estimator_q start
-	else
-		#
-		# EKF2
-		#
-		param set SYS_MC_EST_GROUP 2
-		ekf2 start &
-	fi
-fi
-
-###############################################################################
-#                        End Estimator Group Selection                        #
-###############################################################################
-
 #
 # Start Control Allocator
 #

ROMFS/px4fmu_common/init.d/rcS

@@ -391,6 +391,23 @@ else
 		pwm_out start
 	fi
 
+	#
+	# state estimator selection
+	if param compare -s EKF2_EN 1
+	then
+		ekf2 start &
+	fi
+
+	if param compare -s LPE_EN 1
+	then
+		local_position_estimator start
+	fi
+
+	if param compare -s ATT_EN 1
+	then
+		attitude_estimator_q start
+	fi
+
 	#
 	# Configure vehicle type specific parameters.
 	# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.

boards/diatone/mamba-f405-mk2/init/rc.board_defaults

@@ -9,9 +9,4 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Disable safety switch by default
 param set-default CBRK_IO_SAFETY 22027
 
-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_W_ACC 0.4
-param set-default ATT_W_GYRO_BIAS 0
-
 param set-default SYS_HAS_MAG 0

boards/flywoo/gn-f405/init/rc.board_defaults

@@ -12,8 +12,6 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Disable safety switch by default
 param set-default CBRK_IO_SAFETY 22027
 
-# EKF2 can be enabled when baro is avaialble and EKF2_MAG_TYPE is set to 5
-param set-default SYS_MC_EST_GROUP 2
 param set-default EKF2_MAG_TYPE 5
 param set-default SENS_BOARD_ROT 6
 

boards/omnibus/f4sd/init/rc.board_defaults

@@ -12,9 +12,4 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Disable safety switch by default
 param set-default CBRK_IO_SAFETY 22027
 
-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_W_ACC 0.4
-param set-default ATT_W_GYRO_BIAS 0
-
 param set-default SYS_HAS_MAG 0

boards/spracing/h7extreme/init/rc.board_defaults

@@ -12,9 +12,4 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050
 
-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_W_ACC 0.4
-param set-default ATT_W_GYRO_BIAS 0
-
 param set-default SYS_HAS_MAG 0

src/drivers/ins/vectornav/VectorNav.cpp

@@ -62,10 +62,6 @@ VectorNav::VectorNav(const char *port) :
 		v = 0;
 		param_set(param_find("EKF2_EN"), &v);
 
-		// SYS_MC_EST_GROUP 0 (disabled)
-		v = 0;
-		param_set(param_find("SYS_MC_EST_GROUP"), &v);
-
 		// SENS_IMU_MODE (VN handles sensor selection)
 		v = 0;
 		param_set(param_find("SENS_IMU_MODE"), &v);

src/lib/parameters/param_translation.cpp

@@ -43,6 +43,70 @@
 bool param_modify_on_import(bson_node_t node)
 {
 
+	// 2023-12-18 SYS_MC_EST_GROUP removed
+	if ((node->type == bson_type_t::BSON_INT32) && (strcmp("SYS_MC_EST_GROUP", node->name) == 0)) {
+
+		int32_t value = node->i32;
+
+		// value 1 local_position_estimator, attitude_estimator_q (unsupported)
+		if (value == 1) {
+			// enable local_position_estimator
+			int32_t lpe_en_val = 1;
+			int lpe_en_ret = param_set(param_find("LPE_EN"), &lpe_en_val);
+
+			// enable attitude_estimator_q
+			int32_t att_en_val = 1;
+			int att_en_ret = param_set(param_find("ATT_EN"), &att_en_val);
+
+			// disable ekf2 (only if enabling lpe and att_w was successful)
+			if (lpe_en_ret == PX4_OK && att_en_ret == PX4_OK) {
+				int32_t ekf2_en_val = 0;
+				param_set(param_find("EKF2_EN"), &ekf2_en_val);
+
+			} else {
+				int32_t ekf2_en_val = 1;
+				param_set(param_find("EKF2_EN"), &ekf2_en_val);
+			}
+		}
+
+		// value 2 ekf2 (recommended)
+		if (value == 2) {
+			// disable local_position_estimator
+			int32_t lpe_en_val = 0;
+			param_set(param_find("LPE_EN"), &lpe_en_val);
+
+			// disable attitude_estimator_q
+			int32_t att_en_val = 0;
+			param_set(param_find("ATT_EN"), &att_en_val);
+
+			// enable ekf2
+			int32_t ekf2_en_val = 1;
+			param_set(param_find("EKF2_EN"), &ekf2_en_val);
+		}
+
+		// value 3 Q attitude estimator (no position)
+		if (value == 3) {
+			// disable local_position_estimator
+			int32_t lpe_en_val = 0;
+			param_set(param_find("LPE_EN"), &lpe_en_val);
+
+			// enable attitude_estimator_q
+			int32_t att_en_val = 1;
+			int att_en_ret = param_set(param_find("ATT_EN"), &att_en_val);
+
+			// disable ekf2 (only if enabling att_w was successful)
+			if (att_en_ret == PX4_OK) {
+				int32_t ekf2_en_val = 0;
+				param_set(param_find("EKF2_EN"), &ekf2_en_val);
+
+			} else {
+				int32_t ekf2_en_val = 1;
+				param_set(param_find("EKF2_EN"), &ekf2_en_val);
+			}
+		}
+
+		return true;
+	}
 
 	return false;
 }

src/lib/systemlib/system_params.c

@@ -83,20 +83,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
  */
 PARAM_DEFINE_INT32(SYS_HITL, 0);
 
-/**
- * Set multicopter estimator group
- *
- * Set the group of estimators used for multicopters and VTOLs
- *
- * @value 1 local_position_estimator, attitude_estimator_q (unsupported)
- * @value 2 ekf2 (recommended)
- * @value 3 Q attitude estimator (no position)
- *
- * @reboot_required true
- * @group System
- */
-PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 2);
-
 /**
  * Enable auto start of rate gyro thermal calibration at the next power up.
  *

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -180,6 +180,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 
 bool AttitudeEstimatorQ::init()
 {
+	uORB::SubscriptionData<vehicle_attitude_s> vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	vehicle_attitude_sub.update();
+
+	if (vehicle_attitude_sub.advertised() && (hrt_elapsed_time(&vehicle_attitude_sub.get().timestamp) < 1_s)) {
+		PX4_ERR("init failed, vehicle_attitude already advertised");
+		return false;
+	}
+
 	if (!_sensors_sub.registerCallback()) {
 		PX4_ERR("callback registration failed");
 		return false;

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -39,6 +39,16 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+/**
+ * standalone attitude estimator enable (unsupported)
+ *
+ * Enable standalone quaternion based attitude estimator.
+ *
+ * @group Attitude Q estimator
+ * @boolean
+ */
+PARAM_DEFINE_INT32(ATT_EN, 0);
+
 /**
  * Complimentary filter accelerometer weight
  *

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -99,7 +99,10 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}
 
-	if (missing_data && _param_sys_mc_est_group.get() == 2) {
+	int32_t param_ekf2_en = 0;
+	param_get(param_find_no_notification("EKF2_EN"), &param_ekf2_en);
+
+	if (missing_data && (param_ekf2_en == 1)) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -106,7 +106,6 @@ private:
 	bool _nav_failure_imminent_warned{false};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
 					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
 					(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
 					(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,

src/modules/ekf2/EKF2.cpp

@@ -258,7 +258,6 @@ EKF2::~EKF2()
 bool EKF2::multi_init(int imu, int mag)
 {
 	// advertise all topics to ensure consistent uORB instance numbering
-	_ekf2_timestamps_pub.advertise();
 	_estimator_event_flags_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();

src/modules/ekf2/ekf2_params.c

@@ -39,6 +39,14 @@
  *
  */
 
+/**
+ * EKF2 enable
+ *
+ * @group EKF2
+ * @boolean
+ */
+PARAM_DEFINE_INT32(EKF2_EN, 1);
+
 /**
  * EKF prediction period
  *

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -137,6 +137,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 bool
 BlockLocalPositionEstimator::init()
 {
+	uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	vehicle_local_position_sub.update();
+
+	if (vehicle_local_position_sub.advertised() && (hrt_elapsed_time(&vehicle_local_position_sub.get().timestamp) < 1_s)) {
+		PX4_ERR("init failed, vehicle_local_position already advertised");
+		return false;
+	}
+
 	if (!_sensors_sub.registerCallback()) {
 		PX4_ERR("callback registration failed");
 		return false;

src/modules/local_position_estimator/params.c

@@ -1,6 +1,12 @@
 #include <parameters/param.h>
 
-// 16 is max name length
+/**
+ * Local position estimator enable (unsupported)
+ *
+ * @group Local Position Estimator
+ * @boolean
+ */
+PARAM_DEFINE_INT32(LPE_EN, 0);
 
 /**
  * Optical flow z offset from center

test/mavsdk_tests/configs/sitl.json

@@ -14,10 +14,7 @@
             "model": "iris",
             "vehicle": "iris",
             "test_filter": "[offboard_attitude]",
-            "timeout_min": 10,
+            "timeout_min": 10
-            "env": {
-                "SYS_MC_EST_GROUP": 3
-            }
         },
         {
             "model": "standard_vtol",