From 8b2205810bf5fa17d5b25e6d7a941f370acd643e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Mar 2023 10:03:41 -0400 Subject: [PATCH] ekf2: add kconfig option to enable/disable baro compensation --- boards/px4/fmu-v2/default.px4board | 1 + src/modules/ekf2/EKF/common.h | 2 ++ src/modules/ekf2/EKF/ekf_helper.cpp | 2 ++ src/modules/ekf2/EKF2.cpp | 2 ++ src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/ekf2/Kconfig | 7 +++++++ 6 files changed, 16 insertions(+) diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 5f593c6267..6fea198d9c 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/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 diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index fdd74b2803..d447a6d21c 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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 diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ffca0b2a93..aedcb0f249 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 469cff3f39..1b411d6bda 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 94588cccf4..e8a1bffad7 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -622,6 +622,7 @@ private: (ParamExtFloat) _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) @@ -636,6 +637,7 @@ private: _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis (ParamExtFloat) _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis +#endif // CONFIG_EKF2_BARO_COMPENSATION (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index b869050945..599a669b7c 100644 --- a/src/modules/ekf2/Kconfig +++ b/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"