ekf2: add kconfig option to enable/disable baro compensation

boards/px4/fmu-v2/default.px4board

@@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y

src/modules/ekf2/EKF/common.h

@@ -438,6 +438,7 @@ struct parameters {
 
 	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)
 	float static_pressure_coef_xn{0.0f};    // (-)
@@ -447,6 +448,7 @@ struct parameters {
 
 	// upper limit on airspeed used for correction  (m/s**2)
 	float max_correction_airspeed {20.0f};
+#endif // CONFIG_EKF2_BARO_COMPENSATION
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	// multi-rotor drag specific force fusion

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -231,6 +231,7 @@ void Ekf::constrainStates()
 
 float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
 {
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	if (_control_status.flags.wind && local_position_is_valid()) {
 		// calculate static pressure error = Pmeas - Ptruth
 		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
@@ -258,6 +259,7 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
 		// correct baro measurement using pressure error estimate and assuming sea level gravity
 		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
 	}
+#endif // CONFIG_EKF2_BARO_COMPENSATION
 
 	// otherwise return the uncorrected baro measurement
 	return baro_alt_uncompensated;

src/modules/ekf2/EKF2.cpp

@@ -167,12 +167,14 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_bcoef_y(_params->bcoef_y),
 	_param_ekf2_mcoef(_params->mcoef),
 #endif // CONFIG_EKF2_DRAG_FUSION
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	_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),
 	_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
 	_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
 	_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
+#endif // CONFIG_EKF2_BARO_COMPENSATION
 	_param_ekf2_mag_check(_params->check_mag_strength),
 	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)

src/modules/ekf2/EKF2.hpp

@@ -622,6 +622,7 @@ private:
 		(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef,		///< propeller momentum drag coefficient (1/s)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 		// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
 		// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
 		(ParamExtFloat<px4::params::EKF2_ASPD_MAX>)
@@ -636,6 +637,7 @@ private:
 		_param_ekf2_pcoef_yn,	///< static pressure position error coefficient along the negative Y body axis
 		(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
 		_param_ekf2_pcoef_z,	///< static pressure position error coefficient along the Z body axis
+#endif // CONFIG_EKF2_BARO_COMPENSATION
 
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
 		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check

src/modules/ekf2/Kconfig

@@ -13,6 +13,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 support multiple instances and selector.
 
+menuconfig EKF2_BARO_COMPENSATION
+depends on MODULES_EKF2
+        bool "barometer compensation support"
+        default y
+	---help---
+		EKF2 pressure compensation support.
+
 menuconfig EKF2_DRAG_FUSION
 depends on MODULES_EKF2
         bool "drag fusion support"