feat(ekf2): add airspeed fusion ctrl, uint8 intended types, prepare dual-GPS msg

- Replace IMU with airspeed (ASPD) in fusion control
- Change all intended fields from int32 to uint8
- EKF2_ARSP_THR param changed from float to int32
- Add drag_active, test_ratio NaN init, MagFuseType casts
- Prepare EstimatorFusionControl msg and SENS_EN bitmask for
  dual-GPS (gps_intended[2], gps_active bitmask), OF stays single
- ack.result pre-initialized in handleSensorFusionCommand

Author: haumarco

msg/EstimatorFusionControl.msg

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

msg/versioned/VehicleCommand.msg

@@ -111,15 +111,15 @@ uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # External reset of estima
 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|
 # Sensor fusion source types for VEHICLE_CMD_SET_EKF_SENSOR_FUSION
-uint8 FUSION_SOURCE_GPS = 1   # GNSS (EKF2_GPS_CTRL)
+uint8 FUSION_SOURCE_GPS = 1   # GNSS (EKF2_GPS{i}_CTRL, use instance param)
 uint8 FUSION_SOURCE_OF = 2    # Optical Flow (EKF2_OF_CTRL)
 uint8 FUSION_SOURCE_EV = 3    # External Vision (EKF2_EV_CTRL)
 uint8 FUSION_SOURCE_AGP = 4   # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param)
 uint8 FUSION_SOURCE_BARO = 5  # Barometer (EKF2_BARO_CTRL)
 uint8 FUSION_SOURCE_RNG = 6   # Range Finder (EKF2_RNG_CTRL)
 uint8 FUSION_SOURCE_DRAG = 7  # Drag (EKF2_DRAG_CTRL)
 uint8 FUSION_SOURCE_MAG = 8   # Magnetometer (EKF2_MAG_TYPE)
-uint8 FUSION_SOURCE_IMU = 9   # IMU (EKF2_IMU_CTRL)
+uint8 FUSION_SOURCE_ASPD = 9  # Airspeed (EKF2_ARSP_THR)
 
 # 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/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -82,7 +82,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 #endif // CONFIG_EKF2_GNSS
 
-	if (_params.ekf2_arsp_thr <= 0.f) {
+	if (_fc.aspd.intended <= 0) {
 		stopAirspeedFusion();
 		return;
 	}
@@ -97,7 +97,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 				|| _control_status.flags.in_transition)
 				&& !_control_status.flags.fake_pos;
 
-		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr;
+		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _fc.aspd.intended;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& is_airspeed_significant

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,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	if (_fc.mag.intended == MagFuseType::NONE) {
+	if (_fc.mag.intended == static_cast<int32_t>(MagFuseType::NONE)) {
 		stopMagFusion();
 		return;
 	}
@@ -150,9 +150,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)
-						      || (_fc.mag.intended == MagFuseType::AUTO)
-						      || (_fc.mag.intended == MagFuseType::HEADING))
+		bool continuing_conditions_passing = ((_fc.mag.intended == static_cast<int32_t>(MagFuseType::INIT))
+						      || (_fc.mag.intended == static_cast<int32_t>(MagFuseType::AUTO))
+						      || (_fc.mag.intended == 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 +187,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)
+						       && (_fc.mag.intended == 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)
-							    || (_fc.mag.intended == MagFuseType::AUTO && !_control_status.flags.mag_3D));
+							&& ((_fc.mag.intended == static_cast<int32_t>(MagFuseType::HEADING))
+							    || (_fc.mag.intended == 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/common.h

@@ -274,10 +274,9 @@ struct systemFlagUpdate {
 // Runtime fusion control. Populated by EKF2 module, read by EKF core.
 static constexpr uint8_t MAX_AGP_INSTANCES = 4;
 
-
 struct FusionSensor {
 	bool enabled{false};
-	int32_t intended{0};
+	uint8_t intended{0};
 };
 
 struct FusionControl {
@@ -289,7 +288,7 @@ struct FusionControl {
 	FusionSensor rng;
 	FusionSensor drag;
 	FusionSensor mag{false, 5};  // MagFuseType::NONE
-	FusionSensor imu;
+	FusionSensor aspd;
 };
 
 struct parameters {
@@ -413,7 +412,7 @@ struct parameters {
 	float ekf2_asp_delay{100.0f};           ///< airspeed measurement delay relative to the IMU (mSec)
 	float ekf2_tas_gate{5.0f};              ///< True Airspeed innovation consistency gate size (STD)
 	float ekf2_eas_noise{1.4f};             ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
-	float ekf2_arsp_thr{2.0f};              ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
+	int32_t ekf2_arsp_thr{2};               ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)

src/modules/ekf2/EKF/control.cpp

@@ -158,7 +158,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_helper.cpp

@@ -852,7 +852,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 > 0) && 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;
@@ -996,15 +996,15 @@ 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
 		_gyro_bias_inhibit[index] = do_inhibit_all_gyro_axes || !is_bias_observable;
 	}
 
 	// 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/EKF2.cpp

@@ -265,7 +265,7 @@ void EKF2::AdvertiseTopics()
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 
-		if (_param_ekf2_arsp_thr.get() > 0.f) {
+		if (_param_ekf2_arsp_thr.get() > 0) {
 			_estimator_aid_src_airspeed_pub.advertise();
 		}
 
@@ -987,7 +987,7 @@ void EKF2::initFusionControl()
 	_num_sensor_table = 0;
 
 	auto add = [this, sens_en](FusionSensor & sensor, uint8_t sens_en_bit, uint8_t sensor_id,
-	int8_t instance, param_t param, int32_t disabled_val = 0) {
+	int8_t instance, param_t param, uint8_t disabled_val = 0) {
 		if (_num_sensor_table < MAX_SENSOR_TABLE) {
 			sensor.enabled = sens_en & (1 << sens_en_bit);
 			int32_t param_val = disabled_val;
@@ -996,20 +996,21 @@ void EKF2::initFusionControl()
 				param_get(param, &param_val);
 			}
 
-			sensor.intended = sensor.enabled ? param_val : disabled_val;
+			sensor.intended = sensor.enabled ? static_cast<uint8_t>(param_val) : disabled_val;
 			_sensor_table[_num_sensor_table++] = {&sensor, param, sens_en_bit, sensor_id, instance, disabled_val};
 		}
 	};
 
-	// bit layout: 0=GPS 1=OF 2=EV 3..6=AGP0..3 7=BARO 8=RNG 9=DRAG 10=MAG 11=IMU
+	// bit layout: 0..1=GPS0..1 2=OF 3=EV 4..7=AGP0..3 8=BARO 9=RNG 10=DRAG 11=MAG 12=ASPD
 #if defined(CONFIG_EKF2_GNSS)
-	add(_fc.gps,  0, vehicle_command_s::FUSION_SOURCE_GPS,  -1, _param_ekf2_gps_ctrl.handle());
+	add(_fc.gps,   0, vehicle_command_s::FUSION_SOURCE_GPS, -1, _param_ekf2_gps_ctrl.handle());
+	// TODO: gps[1] reserved — no param yet, add second GPS instance here when multi-GPS CTRL is implemented
 #endif
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	add(_fc.of,   1, vehicle_command_s::FUSION_SOURCE_OF,   -1, _param_ekf2_of_ctrl.handle());
+	add(_fc.of,    2, vehicle_command_s::FUSION_SOURCE_OF,  -1, _param_ekf2_of_ctrl.handle());
 #endif
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	add(_fc.ev,   2, vehicle_command_s::FUSION_SOURCE_EV,   -1, _param_ekf2_ev_ctrl.handle());
+	add(_fc.ev,    3, vehicle_command_s::FUSION_SOURCE_EV,  -1, _param_ekf2_ev_ctrl.handle());
 #endif
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
 
@@ -1020,24 +1021,26 @@ void EKF2::initFusionControl()
 		const param_t param_agp_ctrl = param_find_no_notification(param_name);
 
 		if (param_used(param_agp_ctrl) > 0) {
-			add(_fc.agp[i], 3 + i, vehicle_command_s::FUSION_SOURCE_AGP, static_cast<int8_t>(i), param_agp_ctrl);
+			add(_fc.agp[i], 4 + i, vehicle_command_s::FUSION_SOURCE_AGP, static_cast<int8_t>(i), param_agp_ctrl);
 		}
 	}
 
 #endif
 #if defined(CONFIG_EKF2_BAROMETER)
-	add(_fc.baro, 7, vehicle_command_s::FUSION_SOURCE_BARO, -1, _param_ekf2_baro_ctrl.handle());
+	add(_fc.baro,  8, vehicle_command_s::FUSION_SOURCE_BARO, -1, _param_ekf2_baro_ctrl.handle());
 #endif
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	add(_fc.rng,  8, vehicle_command_s::FUSION_SOURCE_RNG,  -1, _param_ekf2_rng_ctrl.handle());
+	add(_fc.rng,   9, vehicle_command_s::FUSION_SOURCE_RNG,  -1, _param_ekf2_rng_ctrl.handle());
 #endif
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	add(_fc.drag, 9, vehicle_command_s::FUSION_SOURCE_DRAG, -1, _param_ekf2_drag_ctrl.handle());
+	add(_fc.drag, 10, vehicle_command_s::FUSION_SOURCE_DRAG, -1, _param_ekf2_drag_ctrl.handle());
 #endif
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	add(_fc.mag, 10, vehicle_command_s::FUSION_SOURCE_MAG,  -1, _param_ekf2_mag_type.handle(), 5);
+	add(_fc.mag,  11, vehicle_command_s::FUSION_SOURCE_MAG,  -1, _param_ekf2_mag_type.handle(), 5);
+#endif
+#if defined(CONFIG_EKF2_AIRSPEED)
+	add(_fc.aspd, 12, vehicle_command_s::FUSION_SOURCE_ASPD, -1, _param_ekf2_arsp_thr.handle());
 #endif
-	add(_fc.imu, 11, vehicle_command_s::FUSION_SOURCE_IMU,  -1, _param_ekf2_imu_ctrl.handle());
 }
 
 void EKF2::updateFusionIntended()
@@ -1050,12 +1053,14 @@ void EKF2::updateFusionIntended()
 			param_get(e.param, &param_val);
 		}
 
-		e.sensor->intended = e.sensor->enabled ? param_val : e.disabled_val;
+		e.sensor->intended = e.sensor->enabled ? static_cast<uint8_t>(param_val) : e.disabled_val;
 	}
 }
 
 void EKF2::handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack)
 {
+	ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+
 	const uint8_t sensor = static_cast<uint8_t>(cmd.param1);
 	const uint8_t instance = PX4_ISFINITE(cmd.param2) ? static_cast<uint8_t>(cmd.param2) : 0;
 	const bool enable = (static_cast<int32_t>(cmd.param3) == 1);
@@ -1071,14 +1076,12 @@ void EKF2::handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_comma
 				param_get(e.param, &param_val);
 			}
 
-			e.sensor->intended = enable ? param_val : e.disabled_val;
+			e.sensor->intended = enable ? static_cast<uint8_t>(param_val) : e.disabled_val;
 			updateSensEnParam(e.sens_en_bit, enable);
 			ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 			return;
 		}
 	}
-
-	ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 }
 
 void EKF2::updateSensEnParam(uint8_t bit, bool enable)
@@ -2016,7 +2019,7 @@ 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.ev_intended   = _fc.ev.intended;
 
@@ -2028,10 +2031,20 @@ void EKF2::PublishFusionControl(const hrt_abstime &timestamp)
 	msg.rng_intended  = _fc.rng.intended;
 	msg.drag_intended = _fc.drag.intended;
 	msg.mag_intended  = _fc.mag.intended;
-	msg.imu_intended  = _fc.imu.intended;
+	msg.aspd_intended = _fc.aspd.intended;
+
+	const auto &cs = _ekf.control_status_flags();
+	msg.gps_active  = (cs.gnss_pos || cs.gps_hgt || cs.gnss_vel || cs.gnss_yaw) ? 1u : 0u; // TODO: per-instance active tracking
+	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.drag_active = (_fc.drag.intended > 0) && cs.wind && cs.in_air && !cs.fake_pos;
+	msg.mag_active  = cs.mag;
+	msg.aspd_active = cs.fuse_aspd;
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	msg.agp_active    = _ekf.getAgpFusingBitmask();
+	msg.agp_active  = _ekf.getAgpFusingBitmask();
 #endif
 
 	msg.timestamp     = _replay_mode ? timestamp : hrt_absolute_time();

src/modules/ekf2/EKF2.hpp

@@ -402,7 +402,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		uint8_t sens_en_bit;
 		uint8_t sensor_id;
 		int8_t instance;     // -1 = any instance
-		int32_t disabled_val;
+		uint8_t disabled_val;
 	};
 
 	static constexpr uint8_t MAX_SENSOR_TABLE = 9 + (MAX_AGP_INSTANCES - 1); // 12
@@ -594,7 +594,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
 
 		// control of airspeed fusion
-		(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
+		(ParamExtInt<px4::params::EKF2_ARSP_THR>)
 		_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
 #endif // CONFIG_EKF2_AIRSPEED