ekf2: add kconfig option to enable/disable drag fusion

boards/px4/fmu-v2/default.px4board

@@ -23,13 +23,13 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_DRAG_FUSION is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y

src/modules/ekf2/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -66,6 +66,51 @@ add_subdirectory(Utility)
 # 	add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES})
 # endif()
 
+set(EKF_SRCS)
+list(APPEND EKF_SRCS
+	EKF/airspeed_fusion.cpp
+	EKF/auxvel_fusion.cpp
+	EKF/baro_height_control.cpp
+	EKF/bias_estimator.cpp
+	EKF/control.cpp
+	EKF/covariance.cpp
+	EKF/ekf.cpp
+	EKF/ekf_helper.cpp
+	EKF/EKFGSF_yaw.cpp
+	EKF/estimator_interface.cpp
+	EKF/ev_control.cpp
+	EKF/ev_height_control.cpp
+	EKF/ev_pos_control.cpp
+	EKF/ev_vel_control.cpp
+	EKF/ev_yaw_control.cpp
+	EKF/fake_height_control.cpp
+	EKF/fake_pos_control.cpp
+	EKF/gnss_height_control.cpp
+	EKF/gps_checks.cpp
+	EKF/gps_control.cpp
+	EKF/gps_yaw_fusion.cpp
+	EKF/gravity_fusion.cpp
+	EKF/height_control.cpp
+	EKF/imu_down_sampler.cpp
+	EKF/mag_control.cpp
+	EKF/mag_fusion.cpp
+	EKF/optical_flow_control.cpp
+	EKF/optflow_fusion.cpp
+	EKF/output_predictor.cpp
+	EKF/range_finder_consistency_check.cpp
+	EKF/range_height_control.cpp
+	EKF/sensor_range_finder.cpp
+	EKF/sideslip_fusion.cpp
+	EKF/terrain_estimator.cpp
+	EKF/vel_pos_fusion.cpp
+	EKF/zero_innovation_heading_update.cpp
+	EKF/zero_velocity_update.cpp
+)
+
+if(CONFIG_EKF2_DRAG_FUSION)
+	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
+endif()
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -81,44 +126,7 @@ px4_add_module(
 	STACK_MAX
 		3600
 	SRCS
-		EKF/airspeed_fusion.cpp
-		EKF/auxvel_fusion.cpp
-		EKF/baro_height_control.cpp
-		EKF/bias_estimator.cpp
-		EKF/control.cpp
-		EKF/covariance.cpp
-		EKF/drag_fusion.cpp
-		EKF/ekf.cpp
-		EKF/ekf_helper.cpp
-		EKF/EKFGSF_yaw.cpp
-		EKF/estimator_interface.cpp
-		EKF/ev_control.cpp
-		EKF/ev_height_control.cpp
-		EKF/ev_pos_control.cpp
-		EKF/ev_vel_control.cpp
-		EKF/ev_yaw_control.cpp
-		EKF/fake_height_control.cpp
-		EKF/fake_pos_control.cpp
-		EKF/gnss_height_control.cpp
-		EKF/gps_checks.cpp
-		EKF/gps_control.cpp
-		EKF/gps_yaw_fusion.cpp
-		EKF/gravity_fusion.cpp
-		EKF/height_control.cpp
-		EKF/imu_down_sampler.cpp
-		EKF/mag_control.cpp
-		EKF/mag_fusion.cpp
-		EKF/optical_flow_control.cpp
-		EKF/optflow_fusion.cpp
-		EKF/output_predictor.cpp
-		EKF/range_finder_consistency_check.cpp
-		EKF/range_height_control.cpp
-		EKF/sensor_range_finder.cpp
-		EKF/sideslip_fusion.cpp
-		EKF/terrain_estimator.cpp
-		EKF/vel_pos_fusion.cpp
-		EKF/zero_innovation_heading_update.cpp
-		EKF/zero_velocity_update.cpp
+		${EKF_SRCS}
 
 		EKF2.cpp
 		EKF2.hpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -31,14 +31,14 @@
 #
 ############################################################################
 
-add_library(ecl_EKF
+set(EKF_SRCS)
+list(APPEND EKF_SRCS
 	airspeed_fusion.cpp
 	auxvel_fusion.cpp
 	baro_height_control.cpp
 	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
-	drag_fusion.cpp
 	ekf.cpp
 	ekf_helper.cpp
 	EKFGSF_yaw.cpp
@@ -72,6 +72,14 @@ add_library(ecl_EKF
 	zero_velocity_update.cpp
 )
 
+if(CONFIG_EKF2_DRAG_FUSION)
+	list(APPEND EKF_SRCS drag_fusion.cpp)
+endif()
+
+add_library(ecl_EKF
+	${EKF_SRCS}
+)
+
 add_dependencies(ecl_EKF prebuild_targets)
 target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model)
 target_compile_options(ecl_EKF PRIVATE -fno-associative-math)

src/modules/ekf2/EKF/common.h

@@ -252,10 +252,12 @@ struct extVisionSample {
 	int8_t     quality{};     ///< quality indicator between 0 and 100
 };
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 struct dragSample {
 	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
 	Vector2f    accelXY{};     ///< measured specific force along the X and Y body axes (m/sec**2)
 };
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 struct auxVelSample {
 	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
@@ -446,11 +448,13 @@ struct parameters {
 	// upper limit on airspeed used for correction  (m/s**2)
 	float max_correction_airspeed {20.0f};
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	// multi-rotor drag specific force fusion
 	float drag_noise{2.5f};                 ///< observation noise variance for drag specific force measurements (m/sec**2)**2
 	float bcoef_x{100.0f};                  ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
 	float bcoef_y{100.0f};                  ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
 	float mcoef{0.1f};                      ///< rotor momentum drag coefficient for the X and Y axes (1/s)
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 	// control of accel error detection and mitigation (IMU clipping)
 	const float vert_innov_test_lim{3.0f};          ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure

src/modules/ekf2/EKF/control.cpp

@@ -108,7 +108,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlGpsFusion(imu_delayed);
 	controlAirDataFusion(imu_delayed);
 	controlBetaFusion(imu_delayed);
+
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	controlDragFusion();
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 	controlHeightFusion(imu_delayed);
 	controlGravityFusion(imu_delayed);
 

src/modules/ekf2/EKF/ekf.h

@@ -171,9 +171,11 @@ public:
 	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
 	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
 	void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
 	void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 	void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; }
 	void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
@@ -537,8 +539,10 @@ private:
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
 	Vector3f _delta_vel_bias_var_accum{};   ///< kahan summation algorithm accumulator for delta velocity bias variance
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
@@ -685,8 +689,13 @@ private:
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
 	void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	// control fusion of multi-rotor drag specific force observations
+	void controlDragFusion();
+
 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag(const dragSample &drag_sample);
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 	// fuse single velocity and position measurement
 	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
@@ -914,9 +923,6 @@ private:
 	// control fusion of synthetic sideslip observations
 	void controlBetaFusion(const imuSample &imu_delayed);
 
-	// control fusion of multi-rotor drag specific force observations
-	void controlDragFusion();
-
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -53,7 +53,9 @@ EstimatorInterface::~EstimatorInterface()
 	delete _airspeed_buffer;
 	delete _flow_buffer;
 	delete _ext_vision_buffer;
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	delete _drag_buffer;
+#endif // CONFIG_EKF2_DRAG_FUSION
 	delete _auxvel_buffer;
 }
 
@@ -85,7 +87,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 		_min_obs_interval_us = (imu_sample.time_us - _time_delayed_us) / (_obs_buffer_length - 1);
 	}
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	setDragData(imu_sample);
+#endif // CONFIG_EKF2_DRAG_FUSION
 }
 
 void EstimatorInterface::setMagData(const magSample &mag_sample)
@@ -452,6 +456,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 	}
 }
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
@@ -502,6 +507,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 		}
 	}
 }
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
@@ -682,9 +688,11 @@ void EstimatorInterface::print_status()
 		printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
 	}
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	if (_drag_buffer) {
 		printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 	_output_predictor.print_status();
 }

src/modules/ekf2/EKF/estimator_interface.h

@@ -294,8 +294,6 @@ protected:
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
 	extVisionSample _ev_sample_prev{};
-	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
-
 	RangeFinderConsistencyCheck _rng_consistency_check;
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
@@ -320,7 +318,13 @@ protected:
 	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 
 	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
+
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	RingBuffer<dragSample> *_drag_buffer{nullptr};
+	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 	Vector2f _drag_test_ratio{};		// drag innovation consistency check ratio
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 	innovation_fault_status_u _innov_check_fail_status{};
 
 	bool _horizontal_deadreckon_time_exceeded{true};
@@ -345,7 +349,6 @@ protected:
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
 	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
-	RingBuffer<dragSample> *_drag_buffer{nullptr};
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
@@ -380,16 +383,18 @@ protected:
 
 private:
 
-	inline void setDragData(const imuSample &imu);
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	void setDragData(const imuSample &imu);
+
+	// Used by the multi-rotor specific drag force fusion
+	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
+	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 	void printBufferAllocationFailed(const char *buffer_name);
 
 	ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
 
 	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
-
-	// Used by the multi-rotor specific drag force fusion
-	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
-	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
 };
 #endif // !EKF_ESTIMATOR_INTERFACE_H

src/modules/ekf2/EKF2.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -161,10 +161,12 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
 	_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
 	_param_ekf2_gyr_b_lim(_params->gyro_bias_lim),
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	_param_ekf2_drag_noise(_params->drag_noise),
 	_param_ekf2_bcoef_x(_params->bcoef_x),
 	_param_ekf2_bcoef_y(_params->bcoef_y),
 	_param_ekf2_mcoef(_params->mcoef),
+#endif // CONFIG_EKF2_DRAG_FUSION
 	_param_ekf2_aspd_max(_params->max_correction_airspeed),
 	_param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
 	_param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
@@ -1118,7 +1120,9 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getFlowInnov(innovations.flow);
 	_ekf.getHeadingInnov(innovations.heading);
 	_ekf.getMagInnov(innovations.mag_field);
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	_ekf.getDragInnov(innovations.drag);
+#endif // CONFIG_EKF2_DRAG_FUSION
 	_ekf.getAirspeedInnov(innovations.airspeed);
 	_ekf.getBetaInnov(innovations.beta);
 	_ekf.getHaglInnov(innovations.hagl);
@@ -1169,7 +1173,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getFlowInnovRatio(test_ratios.flow[0]);
 	_ekf.getHeadingInnovRatio(test_ratios.heading);
 	_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	_ekf.getDragInnovRatio(&test_ratios.drag[0]);
+#endif // CONFIG_EKF2_DRAG_FUSION
 	_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
 	_ekf.getBetaInnovRatio(test_ratios.beta);
 	_ekf.getHaglInnovRatio(test_ratios.hagl);
@@ -1196,7 +1202,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getFlowInnovVar(variances.flow);
 	_ekf.getHeadingInnovVar(variances.heading);
 	_ekf.getMagInnovVar(variances.mag_field);
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	_ekf.getDragInnovVar(variances.drag);
+#endif // CONFIG_EKF2_DRAG_FUSION
 	_ekf.getAirspeedInnovVar(variances.airspeed);
 	_ekf.getBetaInnovVar(variances.beta);
 	_ekf.getHaglInnovVar(variances.hagl);

src/modules/ekf2/EKF2.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -613,12 +613,14 @@ private:
 
 		(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim,	///< Gyro bias learning limit (rad/s)
 
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 		// Multi-rotor drag specific force fusion
 		(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
 		_param_ekf2_drag_noise,	///< observation noise variance for drag specific force measurements (m/sec**2)**2
 		(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x,		///< ballistic coefficient along the X-axis (kg/m**2)
 		(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y,		///< ballistic coefficient along the Y-axis (kg/m**2)
 		(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef,		///< propeller momentum drag coefficient (1/s)
+#endif // CONFIG_EKF2_DRAG_FUSION
 
 		// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
 		// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2

src/modules/ekf2/Kconfig

@@ -13,6 +13,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 support multiple instances and selector.
 
+menuconfig EKF2_DRAG_FUSION
+depends on MODULES_EKF2
+        bool "drag fusion support"
+        default y
+	---help---
+		EKF2 drag fusion support.
+
 menuconfig USER_EKF2
 	bool "ekf2 running as userspace module"
 	default n