feat(ekf2): Fusion-Control of sensors over MAVLink

boards/holybro/kakutef7/default.px4board

@@ -30,6 +30,7 @@ CONFIG_MODULES_EKF2=y
 # CONFIG_EKF2_GNSS_YAW is not set
 # CONFIG_EKF2_MAGNETOMETER is not set
 # CONFIG_EKF2_RANGE_FINDER is not set
+# CONFIG_EKF2_OPTICAL_FLOW is not set
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y

msg/CMakeLists.txt

@@ -79,6 +79,7 @@ set(msg_files
 	EstimatorBias.msg
 	EstimatorBias3d.msg
 	EstimatorEventFlags.msg
+	EstimatorFusionControl.msg
 	EstimatorGpsStatus.msg
 	EstimatorInnovations.msg
 	EstimatorSelectorStatus.msg

msg/EstimatorFusionControl.msg

@@ -0,0 +1,23 @@
+uint64 timestamp                # time since system start (microseconds)
+
+# sensor intended for fusion (enabled via EKF2_SENS_EN AND CTRL param != disabled)
+bool[2] gps_intended
+bool of_intended
+bool ev_intended
+bool[4] agp_intended
+bool baro_intended
+bool rng_intended
+bool mag_intended
+bool aspd_intended
+bool rngbcn_intended
+
+# 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,6 +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_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_ESTIMATOR_SENSOR_ENABLE
+uint8 FUSION_SOURCE_GPS = 0   # GNSS (EKF2_GPS{i}_CTRL, use instance param)
+uint8 FUSION_SOURCE_OF = 1    # Optical Flow (EKF2_OF_CTRL)
+uint8 FUSION_SOURCE_EV = 2    # External Vision (EKF2_EV_CTRL)
+uint8 FUSION_SOURCE_AGP = 3   # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param)
+uint8 FUSION_SOURCE_BARO = 4  # Barometer (EKF2_BARO_CTRL)
+uint8 FUSION_SOURCE_RNG = 5   # Range Finder (EKF2_RNG_CTRL)
+uint8 FUSION_SOURCE_MAG = 6   # Magnetometer (EKF2_MAG_TYPE)
+uint8 FUSION_SOURCE_ASPD = 7  # Airspeed (EKF2_ARSP_THR)
+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,6 +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_ESTIMATOR_SENSOR_ENABLE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 

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

@@ -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 @@ class AuxGlobalPosition : public ModuleParams
 	 */
 	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 (!(_params.ctrl & 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 = (_params.ekf2_baro_ctrl == 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/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 = (_params.ekf2_ev_ctrl & 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 = (_params.ekf2_ev_ctrl & 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 = (_params.ekf2_ev_ctrl & 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 = (_params.ekf2_ev_ctrl & 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/gps_control.cpp

@@ -41,7 +41,9 @@
 
 void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 {
-	if (!_gps_buffer || (_params.ekf2_gps_ctrl == 0)) {
+	_fc.gps.available = (_params.ekf2_gps_ctrl != 0);
+
+	if (!_gps_buffer || !_fc.gps.intended()) {
 		stopGnssFusion();
 		return;
 	}

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 (_params.ekf2_mag_type == 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 = ((_params.ekf2_mag_type == MagFuseType::INIT)
-						      || (_params.ekf2_mag_type == MagFuseType::AUTO)
-						      || (_params.ekf2_mag_type == MagFuseType::HEADING))
+		bool continuing_conditions_passing = ((_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::INIT))
+						      || (_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::AUTO))
+						      || (_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
-						       && (_params.ekf2_mag_type == 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
-							&& ((_params.ekf2_mag_type == MagFuseType::HEADING)
-							    || (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
+							&& ((_params.ekf2_mag_type == static_cast<int32_t>(MagFuseType::HEADING))
+							    || (_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 || (_params.ekf2_of_ctrl != 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 = (_params.ekf2_of_ctrl == 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 = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
-				|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
+		const bool continuing_conditions_passing = _fc.rng.intended()
 				&& _control_status.flags.tilt_align
 				&& measurement_valid;
 

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

@@ -284,6 +284,27 @@ struct systemFlagUpdate {
 	bool in_transition{false};
 };
 
+// Runtime fusion control. Populated by EKF2 module, read by EKF core.
+static constexpr uint8_t MAX_AGP_INSTANCES = 4;
+
+struct FusionSensor {
+	bool enabled{false};   // runtime toggleable via MAVLink
+	bool available{false}; // CTRL-param != disabled-value (functions as factory-setting)
+	bool intended() const { return enabled && available; }
+};
+
+struct FusionControl {
+	FusionSensor gps;
+	FusionSensor of;
+	FusionSensor ev;
+	FusionSensor agp[MAX_AGP_INSTANCES];
+	FusionSensor baro;
+	FusionSensor rng;
+	FusionSensor mag;
+	FusionSensor aspd;
+	FusionSensor rngbcn;
+};
+
 struct parameters {
 
 	int32_t ekf2_predict_us{10000};         ///< filter update interval in microseconds
@@ -293,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)};
 
@@ -319,7 +341,7 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_BAROMETER)
 	int32_t ekf2_baro_ctrl {1};
-	float ekf2_baro_delay{0.0f};            ///< barometer height measurement delay relative to the IMU (mSec)
+	float ekf2_baro_delay {0.0f};           ///< barometer height measurement delay relative to the IMU (mSec)
 	float ekf2_baro_noise{2.0f};            ///< observation noise for barometric height fusion (m)
 	float baro_bias_nsd{0.13f};             ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
 	float ekf2_baro_gate{5.0f};             ///< barometric and GPS height innovation consistency gate size (STD)

src/modules/ekf2/EKF/ekf.h

@@ -137,6 +137,10 @@ class Ekf final : public EstimatorInterface
 	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 && (_params.ekf2_of_ctrl == 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;

src/modules/ekf2/EKF/estimator_interface.h

@@ -155,6 +155,7 @@ class EstimatorInterface
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() { return &_params; }
+	FusionControl *getFusionControlHandle() { return &_fc; }
 
 	// set vehicle landed status data
 	void set_in_air_status(bool in_air)
@@ -336,6 +337,7 @@ class EstimatorInterface
 	virtual bool init(uint64_t timestamp) = 0;
 
 	parameters _params{};		// filter parameters
+	FusionControl _fc{};
 
 	/*
 	 OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer