diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index bae1fcb6bb..4b37e9a61d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -387,6 +387,8 @@ private: (ParamExtInt) _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used (ParamExtInt) _param_ekf2_hgt_mode, ///< selects the primary source for height data + (ParamExtInt) + _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation (ParamExtInt) _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) @@ -594,6 +596,7 @@ Ekf2::Ekf2(bool replay_mode): _param_ekf2_req_vdrift(_params->req_vdrift), _param_ekf2_aid_mask(_params->fusion_mode), _param_ekf2_hgt_mode(_params->vdist_sensor_type), + _param_ekf2_terr_mask(_params->terrain_fusion_mode), _param_ekf2_noaid_tout(_params->valid_timeout_max), _param_ekf2_rng_noise(_params->range_noise), _param_ekf2_rng_sfe(_params->range_noise_scaler), diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index c4f256b826..9568ddb6ed 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -644,6 +644,21 @@ PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); */ PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0); +/** + * Integer bitmask controlling fusion sources of the terrain estimator + * + * Set bits in the following positions to enable: + * 0 : Set to true to use range finder data if available + * 1 : Set to true to use optical flow data if available + * + * @group EKF2 + * @min 0 + * @max 3 + * @bit 0 use range finder + * @bit 1 use optical flow + */ +PARAM_DEFINE_INT32(EKF2_TERR_MASK, 3); + /** * Maximum lapsed time from last fusion of measurements that constrain velocity drift before the EKF will report the horizontal nav solution as invalid. *