refactor(ekf2): FusionSensor available/enabled/intended() data model

Split FusionSensor into available (CTRL param != disabled) and enabled
(runtime-toggleable). intended() = enabled && available. EKF core aid
sources now set available themselves and use intended() or _params
directly for CTRL-level checks. Remove drag/imu from FusionControl,
add aspd/rngbcn. Add AGP sourceFusingBitmask() for active-status.

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 	const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
 
-	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_fc.drag.intended == 0))) {
+	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) {
 		_control_status.flags.wind = false;
 	}
 
@@ -82,7 +82,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 #endif // CONFIG_EKF2_GNSS
 
-	if (_params.ekf2_arsp_thr <= 0.f) {
+	_fc.aspd.available = (_params.ekf2_arsp_thr > 0.f);
+
+	if (!_fc.aspd.intended()) {
 		stopAirspeedFusion();
 		return;
 	}

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -132,6 +132,19 @@ bool AuxGlobalPosition::anySourceFusing() const
 	return false;
 }
 
+uint8_t AuxGlobalPosition::sourceFusingBitmask() const
+{
+	uint8_t mask = 0;
+
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i] && _sources[i]->isFusing()) {
+			mask |= (1u << i);
+		}
+	}
+
+	return mask;
+}
+
 int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instance) const
 {
 	char param_name[20] {};

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp

@@ -67,6 +67,7 @@ public:
 	 */
 	float testRatioFiltered() const;
 	bool anySourceFusing() const;
+	uint8_t sourceFusingBitmask() const;
 	int32_t getIdParam(int instance);
 	void setIdParam(int instance, int32_t sensor_id);
 	int mapSensorIdToSlot(int32_t sensor_id);

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp

@@ -99,11 +99,13 @@ void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::im
 
 bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
+	ekf._fc.agp[_slot].available = (_params.ctrl != 0);
+
 	AuxGlobalPositionSample sample;
 
 	if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
 
-		if (!(ekf._fc.agp[_slot].intended & static_cast<int32_t>(Ctrl::kHPos))) {
+		if (!ekf._fc.agp[_slot].intended()) {
 			return true;
 		}
 

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -40,6 +40,8 @@
 
 void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 {
+	_fc.baro.available = (_params.ekf2_baro_ctrl != 0);
+
 	static constexpr const char *HGT_SRC_NAME = "baro";
 
 	auto &aid_src = _aid_src_baro_hgt;
@@ -111,7 +113,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		}
 
 		// determine if we should use height aiding
-		const bool continuing_conditions_passing = (_fc.baro.intended == 1)
+		const bool continuing_conditions_passing = _fc.baro.intended()
 				&& measurement_valid
 				&& (_baro_counter > _obs_buffer_length)
 				&& !_control_status.flags.baro_fault;

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -45,7 +45,7 @@
 
 void Ekf::controlDragFusion(const imuSample &imu_delayed)
 {
-	if ((_fc.drag.intended > 0) && _drag_buffer) {
+	if ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) {
 
 		if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
 			_control_status.flags.wind = true;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -41,6 +41,8 @@
 
 void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 {
+	_fc.ev.available = (_params.ekf2_ev_ctrl != 0);
+
 	_ev_pos_b_est.predict(_dt_ekf_avg);
 	_ev_hgt_b_est.predict(_dt_ekf_avg);
 

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -102,7 +102,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 		bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 	}
 
-	const bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::VPOS))
+	const bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
 			&& measurement_valid;
 
 	const bool starting_conditions_passing = common_starting_conditions_passing

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -49,7 +49,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
 
 	// determine if we should use EV position aiding
-	bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::HPOS))
+	bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
 					     && _control_status.flags.tilt_align
 					     && PX4_ISFINITE(ev_sample.pos(0))
 					     && PX4_ISFINITE(ev_sample.pos(1));

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -51,7 +51,7 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
 
 	// determine if we should use EV velocity aiding
-	bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::VEL))
+	bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
 					     && _control_status.flags.tilt_align
 					     && ev._sample.vel.isAllFinite()
 					     && !ev._sample.vel.longerThan(_params.ekf2_vel_lim);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -66,7 +66,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 	}
 
 	// determine if we should use EV yaw aiding
-	bool continuing_conditions_passing = (_fc.ev.intended & static_cast<int32_t>(EvCtrl::YAW))
+	bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
 					     && _control_status.flags.tilt_align
 					     && !_control_status.flags.ev_yaw_fault
 					     && PX4_ISFINITE(aid_src.observation)

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 						       && _gnss_checks.passed()
 						       && !_control_status.flags.gnss_fault;
 
-		const bool continuing_conditions_passing = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VPOS))
+		const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& common_conditions_passing;
 
 		const bool starting_conditions_passing = continuing_conditions_passing

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -47,7 +47,7 @@
 
 void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 {
-	if (!(_fc.gps.intended & static_cast<int32_t>(GnssCtrl::YAW))
+	if (!(_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
 	    || _control_status.flags.gnss_yaw_fault) {
 
 		stopGnssYawFusion();

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -41,7 +41,9 @@
 
 void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
-	if (!_gps_buffer || (_fc.gps.intended == 0)) {
+	_fc.gps.available = (_params.ekf2_gps_ctrl != 0);
+
+	if (!_gps_buffer || !_fc.gps.intended()) {
 		stopGnssFusion();
 		return;
 	}
@@ -126,7 +128,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
 {
-	const bool continuing_conditions_passing = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VEL))
+	const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
 			&& _control_status.flags.tilt_align
 			&& _control_status.flags.yaw_align
 			&& !_control_status.flags.gnss_fault
@@ -182,7 +184,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
 
 void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
 {
-	const bool gnss_pos_enabled = (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::HPOS));
+	const bool gnss_pos_enabled = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
 
 	const bool continuing_conditions_passing = gnss_pos_enabled
 			&& _control_status.flags.tilt_align
@@ -385,8 +387,8 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
 		_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
 
 		// Try to align yaw using estimate if available
-		if (((_fc.gps.intended & static_cast<int32_t>(GnssCtrl::VEL))
+		if (((_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
-		     || (_fc.gps.intended & static_cast<int32_t>(GnssCtrl::HPOS)))
+		     || (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
 		    && !_control_status.flags.yaw_align
 		    && _control_status.flags.tilt_align) {
 			if (resetYawToEKFGSF()) {

src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp

@@ -59,7 +59,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 					 && (accel_lpf_norm_sq < sq(upper_accel_limit));
 
 	// fuse gravity observation if our overall acceleration isn't too big
-	_control_status.flags.gravity_vector = (_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GravityVector))
+	_control_status.flags.gravity_vector = (_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
 					       && (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest)
 					       && !isHorizontalAidingActive()
 					       && _control_status.flags.tilt_align; // Let fake position do the initial alignment (more robust before takeoff)

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -54,7 +54,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	if (_fc.mag.intended == MagFuseType::NONE) {
+	_fc.mag.available = _params.ekf2_mag_type != static_cast<int32_t>(MagFuseType::NONE);
+
+	if (!_fc.mag.intended()) {
 		stopMagFusion();
 		return;
 	}
@@ -150,9 +152,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 				      math::max(_params.ekf2_mag_gate, 1.f)); // innovation gate
 
 		// determine if we should use mag fusion
-		bool continuing_conditions_passing = ((_fc.mag.intended == MagFuseType::INIT)
+		bool continuing_conditions_passing = ((_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::INIT))
-						      || (_fc.mag.intended == MagFuseType::AUTO)
+						      || (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO))
-						      || (_fc.mag.intended == MagFuseType::HEADING))
+						      || (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::HEADING)))
 						     && _control_status.flags.tilt_align
 						     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
 						     && mag_sample.mag.longerThan(0.f)
@@ -187,12 +189,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 							       && (!_control_status.flags.yaw_manual || _control_status.flags.mag_aligned_in_flight);
 
 			_control_status.flags.mag_3D = common_conditions_passing
-						       && (_fc.mag.intended == MagFuseType::AUTO)
+						       && (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO))
 						       && _control_status.flags.mag_aligned_in_flight;
 
 			_control_status.flags.mag_hdg = common_conditions_passing
-							&& ((_fc.mag.intended == MagFuseType::HEADING)
+							&& ((_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::HEADING))
-							    || (_fc.mag.intended == MagFuseType::AUTO && !_control_status.flags.mag_3D));
+							    || (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO) && !_control_status.flags.mag_3D));
 		}
 
 		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -42,7 +42,9 @@
 
 void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 {
-	if (!_flow_buffer || (_fc.of.intended != 1)) {
+	_fc.of.available = (_params.ekf2_of_ctrl != 0);
+
+	if (!_flow_buffer || !_fc.of.intended()) {
 		stopFlowFusion();
 		return;
 	}
@@ -146,7 +148,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					       && !flow_sample.flow_rate.longerThan(_flow_max_rate)
 					       && !flow_compensated.longerThan(_flow_max_rate);
 
-		const bool continuing_conditions_passing = (_fc.of.intended == 1)
+		const bool continuing_conditions_passing = _fc.of.intended()
 				&& _control_status.flags.tilt_align
 				&& is_within_sensor_dist;
 

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -42,6 +42,8 @@
 
 void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
+	_fc.rng.available = (_params.ekf2_rng_ctrl != static_cast<int32_t>(RngCtrl::DISABLED));
+
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
 	bool rng_data_ready = false;
@@ -127,8 +129,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			aid_src.innovation_rejected = false;
 		}
 
-		const bool continuing_conditions_passing = ((_fc.rng.intended == static_cast<int32_t>(RngCtrl::ENABLED))
+		const bool continuing_conditions_passing = _fc.rng.intended()
-				|| (_fc.rng.intended == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
 				&& _control_status.flags.tilt_align
 				&& measurement_valid;
 
@@ -138,11 +139,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _range_sensor.isDataHealthy();
 
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
-						      && (_fc.rng.intended == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
 
 		const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
-					  && (_fc.rng.intended == static_cast<int32_t>(RngCtrl::ENABLED));
+					  && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
 
 		if (_control_status.flags.rng_hgt) {
 			if (!(do_conditional_range_aid || do_range_aid)) {

src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp

@@ -42,7 +42,9 @@
 
 void Ekf::controlRangingBeaconFusion(const imuSample &imu_delayed)
 {
-	if (!_ranging_beacon_buffer || (_params.ekf2_rngbc_ctrl == 0)) {
+	_fc.rngbcn.available = (_params.ekf2_rngbc_ctrl != 0);
+
+	if (!_ranging_beacon_buffer || !_fc.rngbcn.intended()) {
 		stopRangingBeaconFusion();
 		return;
 	}

src/modules/ekf2/EKF/common.h

@@ -287,10 +287,10 @@ struct systemFlagUpdate {
 // Runtime fusion control. Populated by EKF2 module, read by EKF core.
 static constexpr uint8_t MAX_AGP_INSTANCES = 4;
 
-
 struct FusionSensor {
-	bool exists{false};
+	bool enabled{false};   // runtime toggleable via MAVLink
-	int32_t intended{0};
+	bool available{false}; // CTRL-param != disabled-value (functions as factory-setting)
+	bool intended() const { return enabled && available; }
 };
 
 struct FusionControl {
@@ -300,9 +300,9 @@ struct FusionControl {
 	FusionSensor agp[MAX_AGP_INSTANCES];
 	FusionSensor baro;
 	FusionSensor rng;
-	FusionSensor drag;
+	FusionSensor mag;
-	FusionSensor mag{false, 5};  // MagFuseType::NONE
+	FusionSensor aspd;
-	FusionSensor imu;
+	FusionSensor rngbcn;
 };
 
 struct parameters {
@@ -314,6 +314,7 @@ struct parameters {
 	float ekf2_vel_lim{100.f};              ///< velocity state limit (m/s)
 
 	// measurement source control
+	int32_t ekf2_sens_en{8191};             ///< sensor fusion enable bitmask (EKF2_SENS_EN)
 	int32_t ekf2_hgt_ref{static_cast<int32_t>(HeightSensor::BARO)};
 	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
 

src/modules/ekf2/EKF/control.cpp

@@ -160,7 +160,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 	_zero_velocity_update.update(*this, imu_delayed);
 
-	if (_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GyroBias)) {
+	if (_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias)) {
 		_zero_gyro_update.update(*this, imu_delayed);
 	}
 

src/modules/ekf2/EKF/ekf.h

@@ -137,6 +137,10 @@ public:
 	const Vector3f &getFlowRefBodyRate() const { return _ref_body_rate; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+	uint8_t getAgpFusingBitmask() const { return _aux_global_position.sourceFusingBitmask(); }
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+
 	float getHeadingInnov() const;
 	float getHeadingInnovVar() const;
 	float getHeadingInnovRatio() const;

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -826,7 +826,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 		inertial_dead_reckoning = false;
 
 	} else {
-		if (!_control_status.flags.in_air && (_fc.of.intended == 1)
+		if (!_control_status.flags.in_air && _fc.of.intended()
 		    && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
 		   ) {
 			// currently landed, but optical flow aiding should be possible once in air
@@ -853,7 +853,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 
 		if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
 		    && (_params.ekf2_fuse_beta == 1)
-		    && (_params.ekf2_arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
+		    && _fc.aspd.intended() && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
 		   ) {
 			// currently landed, but air data aiding should be possible once in air
 			aiding_expected_in_air = true;
@@ -997,7 +997,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 
 
 	// gyro bias inhibit
-	const bool do_inhibit_all_gyro_axes = !(_fc.imu.intended & static_cast<int32_t>(ImuCtrl::GyroBias));
+	const bool do_inhibit_all_gyro_axes = !(_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
 
 	for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
 		bool is_bias_observable = true; // TODO: gyro bias conditions
@@ -1005,7 +1005,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	}
 
 	// accel bias inhibit
-	const bool do_inhibit_all_accel_axes = !(_fc.imu.intended & static_cast<int32_t>(ImuCtrl::AccelBias))
+	const bool do_inhibit_all_accel_axes = !(_params.ekf2_imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
 					       || is_manoeuvre_level_high
 					       || _fault_status.flags.bad_acc_vertical;
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -519,7 +519,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if (_fc.drag.intended > 0) {
+	if (_params.ekf2_drag_ctrl > 0) {
 
 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {