feat(ekf2): EKF2_SENS_EN param, SensEnBit enum, MAVLink fusion command

Add EKF2_SENS_EN bitmask parameter (replaces EKF2_EN_BOOT) with
per-sensor enable bits. initFusionControl reads SENS_EN while disarmed.
handleSensorFusionCommand sets FusionSensor.enabled via
VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE. syncSensEnParam writes back to
param on disarm. Update EstimatorFusionControl.msg to bool
intended/active fields. Update VehicleCommand.msg FUSION_SOURCE enum.

msg/EstimatorFusionControl.msg

@@ -1,13 +1,23 @@
 uint64 timestamp                # time since system start (microseconds)
 
-int32 gps_intended              # effective EKF2_GPS_CTRL value (0 = disabled)
+# sensor intended for fusion (enabled via EKF2_SENS_EN AND CTRL param != disabled)
-int32 of_intended               # effective EKF2_OF_CTRL value (0 = disabled)
+bool[2] gps_intended
-int32 ev_intended               # effective EKF2_EV_CTRL value (0 = disabled)
+bool of_intended
-int32[4] agp_intended           # effective EKF2_AGPx_CTRL values (0 = disabled)
+bool ev_intended
-int32 baro_intended             # effective EKF2_BARO_CTRL value (0 = disabled)
+bool[4] agp_intended
-int32 rng_intended              # effective EKF2_RNG_CTRL value (0 = disabled)
+bool baro_intended
-int32 drag_intended             # effective EKF2_DRAG_CTRL value (0 = disabled)
+bool rng_intended
-int32 mag_intended              # effective EKF2_MAG_TYPE value (5 = disabled/NONE)
+bool mag_intended
-int32 imu_intended              # effective EKF2_IMU_CTRL value (0 = disabled)
+bool aspd_intended
+bool rngbcn_intended
 
-uint8 agp_active                # bitmask of AGP instances actively fusing (bit i = instance i)
+# whether the estimator is actively fusing data from each source
+bool[2] gps_active
+bool of_active
+bool ev_active
+bool[4] agp_active
+bool baro_active
+bool rng_active
+bool mag_active
+bool aspd_active
+bool rngbcn_active

msg/versioned/VehicleCommand.msg

@@ -110,17 +110,17 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch.
 
 uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # External reset of estimator global position when dead reckoning.
 uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004
-uint16 VEHICLE_CMD_SET_EKF_SENSOR_FUSION = 43005 # Enable/disable EKF sensor fusion. |sensor (FUSION_SOURCE_*)|instance (0-based, for multi-instance types)|action (0=disable, 1=enable)|Unused|Unused|Unused|Unused|
+uint16 VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE = 43006 # Enable/disable estimator sensor fusion. |Source (FUSION_SOURCE_*)|Sensor instance (0-based)|Enable (1) or Disable (0)|Estimator Instance (NaN: not used)|Empty|Empty|Empty|
-# Sensor fusion source types for VEHICLE_CMD_SET_EKF_SENSOR_FUSION
+# Sensor fusion source types for VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE
-uint8 FUSION_SOURCE_GPS = 1   # GNSS (EKF2_GPS_CTRL)
+uint8 FUSION_SOURCE_GPS = 0   # GNSS (EKF2_GPS{i}_CTRL, use instance param)
-uint8 FUSION_SOURCE_OF = 2    # Optical Flow (EKF2_OF_CTRL)
+uint8 FUSION_SOURCE_OF = 1    # Optical Flow (EKF2_OF_CTRL)
-uint8 FUSION_SOURCE_EV = 3    # External Vision (EKF2_EV_CTRL)
+uint8 FUSION_SOURCE_EV = 2    # External Vision (EKF2_EV_CTRL)
-uint8 FUSION_SOURCE_AGP = 4   # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param)
+uint8 FUSION_SOURCE_AGP = 3   # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param)
-uint8 FUSION_SOURCE_BARO = 5  # Barometer (EKF2_BARO_CTRL)
+uint8 FUSION_SOURCE_BARO = 4  # Barometer (EKF2_BARO_CTRL)
-uint8 FUSION_SOURCE_RNG = 6   # Range Finder (EKF2_RNG_CTRL)
+uint8 FUSION_SOURCE_RNG = 5   # Range Finder (EKF2_RNG_CTRL)
-uint8 FUSION_SOURCE_DRAG = 7  # Drag (EKF2_DRAG_CTRL)
+uint8 FUSION_SOURCE_MAG = 6   # Magnetometer (EKF2_MAG_TYPE)
-uint8 FUSION_SOURCE_MAG = 8   # Magnetometer (EKF2_MAG_TYPE)
+uint8 FUSION_SOURCE_ASPD = 7  # Airspeed (EKF2_ARSP_THR)
-uint8 FUSION_SOURCE_IMU = 9   # IMU (EKF2_IMU_CTRL)
+uint8 FUSION_SOURCE_RNGBCN = 8 # Ranging Beacon
 
 # PX4 vehicle commands (beyond 16 bit MAVLink commands).
 uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX).

src/modules/commander/Commander.cpp

@@ -1613,7 +1613,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
 	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE:
 	case vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE:
-	case vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION:
+	case vehicle_command_s::VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 

src/modules/ekf2/EKF2.cpp

@@ -67,6 +67,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_delay_max(_params->ekf2_delay_max),
 	_param_ekf2_imu_ctrl(_params->ekf2_imu_ctrl),
 	_param_ekf2_vel_lim(_params->ekf2_vel_lim),
+	_param_ekf2_sens_en(_params->ekf2_sens_en),
 #if defined(CONFIG_EKF2_AUXVEL)
 	_param_ekf2_avel_delay(_params->ekf2_avel_delay),
 #endif // CONFIG_EKF2_AUXVEL
@@ -219,16 +220,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_abl_tau(_params->ekf2_abl_tau),
 	_param_ekf2_gyr_b_lim(_params->ekf2_gyr_b_lim)
 {
-#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	char param_name[20] {};
-
-	for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
-		snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", i);
-		_param_ekf2_agp_ctrl[i] = param_find(param_name);
-	}
-
-#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
-
 	initFusionControl();
 	AdvertiseTopics();
 }
@@ -470,7 +461,7 @@ void EKF2::Run()
 
 		// update parameters from storage
 		updateParams();
-		updateFusionIntended();
+		initFusionControl();
 
 		VerifyParams();
 
@@ -621,7 +612,7 @@ void EKF2::Run()
 				_vehicle_command_ack_pub.publish(command_ack);
 			}
 
-			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION) {
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE) {
 				handleSensorFusionCommand(vehicle_command, command_ack);
 				command_ack.timestamp = hrt_absolute_time();
 				_vehicle_command_ack_pub.publish(command_ack);
@@ -1006,194 +997,100 @@ void EKF2::VerifyParams()
 
 void EKF2::initFusionControl()
 {
-	_boot_param = param_find("EKF2_EN_BOOT");
+	if (!_prev_armed) {
-	int32_t boot_mask = 4095; // default: all enabled
+
-	if (_boot_param != PARAM_INVALID) {
+		const int32_t sens_en = _param_ekf2_sens_en.get();
-		param_get(_boot_param, &boot_mask);
+
+		_fc.gps.enabled    = sens_en & (1 << SENS_EN_GPS0);
+		_fc.of.enabled     = sens_en & (1 << SENS_EN_OF);
+		_fc.ev.enabled     = sens_en & (1 << SENS_EN_EV);
+
+		for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
+			_fc.agp[i].enabled = sens_en & (1 << (SENS_EN_AGP0 + i));
 		}
 
-	// bit layout: 0=GPS 1=OF 2=EV 3..6=AGP0..3 7=BARO 8=RNG 9=DRAG 10=MAG 11=IMU
+		_fc.baro.enabled   = sens_en & (1 << SENS_EN_BARO);
-#if defined(CONFIG_EKF2_GNSS)
+		_fc.rng.enabled    = sens_en & (1 << SENS_EN_RNG);
-	_fc.gps.exists = boot_mask & (1 << 0);
+		_fc.mag.enabled    = sens_en & (1 << SENS_EN_MAG);
-	_fc.gps.intended = _fc.gps.exists ? _params->ekf2_gps_ctrl : 0;
+		_fc.aspd.enabled   = sens_en & (1 << SENS_EN_ASPD);
-#endif
+		_fc.rngbcn.enabled = sens_en & (1 << SENS_EN_RNGBCN);
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_fc.of.exists   = boot_mask & (1 << 1);
-	_fc.of.intended = _fc.of.exists ? _params->ekf2_of_ctrl : 0;
-#endif
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_fc.ev.exists   = boot_mask & (1 << 2);
-	_fc.ev.intended = _fc.ev.exists ? _params->ekf2_ev_ctrl : 0;
-#endif
-#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
-		_fc.agp[i].exists = boot_mask & (1 << (3 + i));
-		int32_t agp_param_val = 0;
-		if (_param_ekf2_agp_ctrl[i] != PARAM_INVALID) {
-			param_get(_param_ekf2_agp_ctrl[i], &agp_param_val);
-		}
-		_fc.agp[i].intended = _fc.agp[i].exists ? agp_param_val : 0;
-	}
-#endif
-#if defined(CONFIG_EKF2_BAROMETER)
-	_fc.baro.exists = boot_mask & (1 << 7);
-	_fc.baro.intended = _fc.baro.exists ? _params->ekf2_baro_ctrl : 0;
-#endif
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_fc.rng.exists  = boot_mask & (1 << 8);
-	_fc.rng.intended = _fc.rng.exists ? _params->ekf2_rng_ctrl : 0;
-#endif
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	_fc.drag.exists = boot_mask & (1 << 9);
-	_fc.drag.intended = _fc.drag.exists ? _params->ekf2_drag_ctrl : 0;
-#endif
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	_fc.mag.exists  = boot_mask & (1 << 10);
-	_fc.mag.intended = _fc.mag.exists ? _params->ekf2_mag_type : 5;
-#endif
-	_fc.imu.exists  = boot_mask & (1 << 11);
-	_fc.imu.intended = _fc.imu.exists ? _params->ekf2_imu_ctrl : 0;
-}
-
-void EKF2::updateFusionIntended()
-{
-	if (_fc.gps.intended != 0) {
-		_fc.gps.intended = _params->ekf2_gps_ctrl;
-	}
-	if (_fc.of.intended != 0) {
-		_fc.of.intended = _params->ekf2_of_ctrl;
-	}
-	if (_fc.ev.intended != 0) {
-		_fc.ev.intended = _params->ekf2_ev_ctrl;
-	}
-#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
-		if (_fc.agp[i].intended != 0) {
-			int32_t agp_param_val = 0;
-			if (_param_ekf2_agp_ctrl[i] != PARAM_INVALID) {
-				param_get(_param_ekf2_agp_ctrl[i], &agp_param_val);
-			}
-			_fc.agp[i].intended = agp_param_val;
-		}
-	}
-#endif
-	if (_fc.baro.intended != 0) {
-		_fc.baro.intended = _params->ekf2_baro_ctrl;
-	}
-	if (_fc.rng.intended != 0) {
-		_fc.rng.intended = _params->ekf2_rng_ctrl;
-	}
-	if (_fc.drag.intended != 0) {
-		_fc.drag.intended = _params->ekf2_drag_ctrl;
-	}
-	if (_fc.mag.intended != 5) {
-		_fc.mag.intended = _params->ekf2_mag_type;
-	}
-	if (_fc.imu.intended != 0) {
-		_fc.imu.intended = _params->ekf2_imu_ctrl;
 	}
 }
 
-
 void EKF2::handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack)
 {
-	const uint8_t sensor = static_cast<uint8_t>(cmd.param1);
+	ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+	const uint8_t sensor_type = static_cast<uint8_t>(cmd.param1);
 	const uint8_t instance = PX4_ISFINITE(cmd.param2) ? static_cast<uint8_t>(cmd.param2) : 0;
-	const int32_t action = static_cast<int32_t>(cmd.param3);  // 0 = disable, 1 = enable
+	const bool enable = (static_cast<int32_t>(cmd.param3) == 1);
 
-	const bool enable = (action == 1);
+	FusionSensor *sensor = nullptr;
-	bool supported = false;
 
-	switch (sensor) {
+	switch (sensor_type) {
-	case vehicle_command_s::FUSION_SOURCE_GPS:
+	case vehicle_command_s::FUSION_SOURCE_GPS:    sensor = &_fc.gps;    break;
-		_fc.gps.exists = enable;
-		_fc.gps.intended = enable ? _param_ekf2_gps_ctrl.get() : 0;
-		updateBootParam(0, enable);
-		supported = true;
-		break;
 
-	case vehicle_command_s::FUSION_SOURCE_OF:
+	case vehicle_command_s::FUSION_SOURCE_OF:     sensor = &_fc.of;     break;
-		_fc.of.exists = enable;
-		_fc.of.intended = enable ? _param_ekf2_of_ctrl.get() : 0;
-		updateBootParam(1, enable);
-		supported = true;
-		break;
 
-	case vehicle_command_s::FUSION_SOURCE_EV:
+	case vehicle_command_s::FUSION_SOURCE_EV:     sensor = &_fc.ev;     break;
-		_fc.ev.exists = enable;
-		_fc.ev.intended = enable ? _param_ekf2_ev_ctrl.get() : 0;
-		updateBootParam(2, enable);
-		supported = true;
-		break;
 
 	case vehicle_command_s::FUSION_SOURCE_AGP:
-		if (instance < MAX_AGP_INSTANCES) {
+		if (instance < MAX_AGP_INSTANCES) { sensor = &_fc.agp[instance]; }
-			_fc.agp[instance].exists = enable;
+
-			int32_t agp_param_val = 0;
-			if (_param_ekf2_agp_ctrl[instance] != PARAM_INVALID) {
-				param_get(_param_ekf2_agp_ctrl[instance], &agp_param_val);
-			}
-			_fc.agp[instance].intended = enable ? agp_param_val : 0;
-			updateBootParam(3 + instance, enable);
-			supported = true;
-		}
 		break;
 
-	case vehicle_command_s::FUSION_SOURCE_BARO:
+	case vehicle_command_s::FUSION_SOURCE_BARO:   sensor = &_fc.baro;   break;
-		_fc.baro.exists = enable;
-		_fc.baro.intended = enable ? _param_ekf2_baro_ctrl.get() : 0;
-		updateBootParam(7, enable);
-		supported = true;
-		break;
 
-	case vehicle_command_s::FUSION_SOURCE_RNG:
+	case vehicle_command_s::FUSION_SOURCE_RNG:    sensor = &_fc.rng;    break;
-		_fc.rng.exists = enable;
-		_fc.rng.intended = enable ? _param_ekf2_rng_ctrl.get() : 0;
-		updateBootParam(8, enable);
-		supported = true;
-		break;
 
-	case vehicle_command_s::FUSION_SOURCE_DRAG:
+	case vehicle_command_s::FUSION_SOURCE_MAG:    sensor = &_fc.mag;    break;
-		_fc.drag.exists = enable;
-		_fc.drag.intended = enable ? _param_ekf2_drag_ctrl.get() : 0;
-		updateBootParam(9, enable);
-		supported = true;
-		break;
 
-	case vehicle_command_s::FUSION_SOURCE_MAG:
+	case vehicle_command_s::FUSION_SOURCE_ASPD:   sensor = &_fc.aspd;   break;
-		_fc.mag.exists = enable;
-		_fc.mag.intended = enable ? _param_ekf2_mag_type.get() : 5;
-		updateBootParam(10, enable);
-		supported = true;
-		break;
 
-	case vehicle_command_s::FUSION_SOURCE_IMU:
+	case vehicle_command_s::FUSION_SOURCE_RNGBCN: sensor = &_fc.rngbcn; break;
-		_fc.imu.exists = enable;
+
-		_fc.imu.intended = enable ? _param_ekf2_imu_ctrl.get() : 0;
+	default: break;
-		updateBootParam(11, enable);
-		supported = true;
-		break;
 	}
 
-	ack.result = supported ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	if (sensor) {
+		sensor->enabled = enable;
+
+		if (!_prev_armed) {
+			syncSensEnParam();
+		}
+
+		ack.result = (!enable || sensor->available)
+			     ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED
+			     : vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+	}
 }
 
-void EKF2::updateBootParam(uint8_t bit, bool enable)
+void EKF2::syncSensEnParam()
 {
-	if (_boot_param == PARAM_INVALID) {
+	int32_t sens_en = 0;
-		return;
+
+	if (_fc.gps.enabled)    { sens_en |= (1 << SENS_EN_GPS0); }
+
+	if (_fc.of.enabled)     { sens_en |= (1 << SENS_EN_OF); }
+
+	if (_fc.ev.enabled)     { sens_en |= (1 << SENS_EN_EV); }
+
+	for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
+		if (_fc.agp[i].enabled) { sens_en |= (1 << (SENS_EN_AGP0 + i)); }
 	}
 
-	int32_t boot_mask = 0;
+	if (_fc.baro.enabled)   { sens_en |= (1 << SENS_EN_BARO); }
-	param_get(_boot_param, &boot_mask);
 
-	if (enable) {
+	if (_fc.rng.enabled)    { sens_en |= (1 << SENS_EN_RNG); }
-		boot_mask |= (1 << bit);
-	} else {
-		boot_mask &= ~(1 << bit);
-	}
 
-	param_set_no_notification(_boot_param, &boot_mask);
+	if (_fc.mag.enabled)    { sens_en |= (1 << SENS_EN_MAG); }
+
+	if (_fc.aspd.enabled)   { sens_en |= (1 << SENS_EN_ASPD); }
+
+	if (_fc.rngbcn.enabled) { sens_en |= (1 << SENS_EN_RNGBCN); }
+
+	_param_ekf2_sens_en.set(sens_en);
+	_param_ekf2_sens_en.commit_no_notification();
 }
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
@@ -2118,19 +2015,40 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 void EKF2::PublishFusionControl(const hrt_abstime &timestamp)
 {
 	estimator_fusion_control_s msg{};
-	msg.gps_intended  = _fc.gps.intended;
+	msg.gps_intended[0] = _fc.gps.intended();
-	msg.of_intended   = _fc.of.intended;
+	msg.of_intended     = _fc.of.intended();
-	msg.ev_intended   = _fc.ev.intended;
+	msg.ev_intended     = _fc.ev.intended();
 
 	for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
-		msg.agp_intended[i] = _fc.agp[i].intended;
+		msg.agp_intended[i] = _fc.agp[i].intended();
 	}
 
-	msg.baro_intended = _fc.baro.intended;
+	msg.baro_intended   = _fc.baro.intended();
-	msg.rng_intended  = _fc.rng.intended;
+	msg.rng_intended    = _fc.rng.intended();
-	msg.drag_intended = _fc.drag.intended;
+	msg.mag_intended    = _fc.mag.intended();
-	msg.mag_intended  = _fc.mag.intended;
+	msg.aspd_intended   = _fc.aspd.intended();
-	msg.imu_intended  = _fc.imu.intended;
+	msg.rngbcn_intended = _fc.rngbcn.intended();
+
+	const auto &cs = _ekf.control_status_flags();
+	msg.gps_active[0] = cs.gnss_pos || cs.gps_hgt || cs.gnss_vel || cs.gnss_yaw;
+	msg.of_active     = cs.opt_flow;
+	msg.ev_active     = cs.ev_pos || cs.ev_hgt || cs.ev_vel || cs.ev_yaw;
+	msg.baro_active   = cs.baro_hgt;
+	msg.rng_active    = cs.rng_hgt;
+	msg.mag_active    = cs.mag;
+	msg.aspd_active   = cs.fuse_aspd;
+	// msg.rngbcn_active = cs.rngbcn_fusion; // waiting for RangeBeacon PR
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
+	{
+		const uint8_t agp_mask = _ekf.getAgpFusingBitmask();
+
+		for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) {
+			msg.agp_active[i] = agp_mask & (1u << i);
+		}
+	}
+#endif
+
 	msg.timestamp     = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_fc_pub.publish(msg);
 }
@@ -2883,13 +2801,18 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 		// vehicle_status
 		vehicle_status_s vehicle_status;
 
-		bool armed = false;
-
 		if (_status_sub.copy(&vehicle_status)
 		    && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) {
 
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			if (_prev_armed && !armed) {
+				syncSensEnParam();
+			}
+
+			_prev_armed = armed;
+
 			// initially set in_air from arming_state (will be overridden if land detector is available)
-			armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 			flags.in_air = armed;
 
 			// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)

src/modules/ekf2/EKF2.hpp

@@ -407,16 +407,24 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
-#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
+	enum SensEnBit : uint16_t {
-	param_t _param_ekf2_agp_ctrl[MAX_AGP_INSTANCES] {PARAM_INVALID, PARAM_INVALID, PARAM_INVALID, PARAM_INVALID};
+		SENS_EN_GPS0   = 0,
-#endif
+		SENS_EN_GPS1   = 1,
-
+		SENS_EN_OF     = 2,
-	param_t _boot_param{PARAM_INVALID};
+		SENS_EN_EV     = 3,
+		SENS_EN_AGP0   = 4,
+		// bit: 5-7 reserved for AGP1..3
+		SENS_EN_BARO   = 8,
+		SENS_EN_RNG    = 9,
+		SENS_EN_MAG    = 10,
+		SENS_EN_ASPD   = 11,
+		SENS_EN_RNGBCN = 12,
+	};
+	bool _prev_armed{false};
 
 	void initFusionControl();
-	void updateFusionIntended();
 	void handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack);
-	void updateBootParam(uint8_t bit, bool enable);
+	void syncSensEnParam();
 
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
@@ -519,6 +527,7 @@ private:
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
 		(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
 		(ParamBool<px4::params::EKF2_POS_LOCK>) _param_ekf2_pos_lock,
+		(ParamExtInt<px4::params::EKF2_SENS_EN>) _param_ekf2_sens_en,
 
 #if defined(CONFIG_EKF2_AUXVEL)
 		(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
@@ -568,7 +577,7 @@ private:
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_BAROMETER)
-		(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
+		(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,
 		(ParamExtFloat<px4::params::EKF2_BARO_DELAY>) _param_ekf2_baro_delay,
 		(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _param_ekf2_baro_noise,
 		(ParamExtFloat<px4::params::EKF2_BARO_GATE>) _param_ekf2_baro_gate,
@@ -690,7 +699,7 @@ private:
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		// optical flow fusion
 		(ParamExtInt<px4::params::EKF2_OF_CTRL>)
-		_param_ekf2_of_ctrl, ///< optical flow fusion selection
+		_param_ekf2_of_ctrl,
 		(ParamExtInt<px4::params::EKF2_OF_GYR_SRC>)
 		_param_ekf2_of_gyr_src,
 		(ParamExtFloat<px4::params::EKF2_OF_DELAY>)

src/modules/ekf2/module.yaml

@@ -193,27 +193,29 @@ parameters:
       type: boolean
       default: 0
 
-    EKF2_EN_BOOT:
+    EKF2_SENS_EN:
       description:
-        short: Sensor fusion enable at boot
+        short: Sensor fusion enable bitmask
-        long: 'Bitmask to control which sensor fusion sources are active at boot.
+        long: 'Bitmask to control which sensor fusion sources are enabled.
-          Sources whose bit is cleared will be disabled at startup.
+          Sources whose bit is cleared will be disabled.
-          They can be toggled at runtime via MAVLink command
+          Only applied while disarmed. For in-flight changes use the
-          (VEHICLE_CMD_SET_EKF_SENSOR_FUSION), which also updates this param.'
+          MAVLink command VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE or the
+          individual CTRL params (e.g. EKF2_GPS_CTRL, EKF2_BARO_CTRL).'
       type: bitmask
       bit:
-        0: GNSS
+        0: GNSS 0
-        1: Optical flow
+        1: GNSS 1
-        2: External vision
+        2: Optical flow
-        3: Aux global position 0
+        3: External vision
-        4: Aux global position 1
+        4: Aux global position 0
-        5: Aux global position 2
+        5: Aux global position 1
-        6: Aux global position 3
+        6: Aux global position 2
-        7: Barometer
+        7: Aux global position 3
-        8: Range finder
+        8: Barometer
-        9: Drag
+        9: Range finder
         10: Magnetometer
-        11: IMU
+        11: Airspeed
-      default: 4095
+        12: Ranging beacon
+      default: 8191
       min: 0
-      max: 4095
+      max: 8191