diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index d48937ec92..de351376b0 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -244,6 +244,10 @@ private: control::BlockParamFloat _flow_pos_y; // Y position of optical flow sensor focal point in body frame (m) control::BlockParamFloat _flow_pos_z; // Z position of optical flow sensor focal point in body frame (m) + // output predictor filter time constants + control::BlockParamFloat _tau_vel; // time constant used by the output velocity complementary filter (s) + control::BlockParamFloat _tau_pos; // time constant used by the output position complementary filter (s) + int update_subscriptions(); }; @@ -326,7 +330,9 @@ Ekf2::Ekf2(): _rng_pos_z(this, "EKF2_RNG_POS_Z", false, &_params->rng_pos_body(2)), _flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)), _flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)), - _flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)) + _flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)), + _tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau), + _tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau) { } diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index d786a98724..84ceebdf0d 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -699,3 +699,25 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f); * @decimal 3 */ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f); + +/** + * Time constant of the velocity output prediction and smoothing filter + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.5f); + +/** + * Time constant of the position output prediction and smoothing filter + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f);