From 4bffb1478fa65778f45f5a19602034a06b7e09d7 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 22 Feb 2017 15:39:36 +0100 Subject: [PATCH] ekf2: added parameter for range sensor pitch offset Signed-off-by: Roman --- src/modules/ekf2/ekf2_main.cpp | 2 ++ src/modules/ekf2/ekf2_params.c | 11 +++++++++++ 2 files changed, 13 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 66e2cd5b7a..39bb0d4992 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -233,6 +233,7 @@ private: control::BlockParamExtFloat _range_noise; // observation noise for range finder measurements (m) control::BlockParamExtFloat _range_innov_gate; // range finder fusion innovation consistency gate size (STD) control::BlockParamExtFloat _rng_gnd_clearance; // minimum valid value for range when on ground (m) + control::BlockParamExtFloat _rng_pitch_offset; // range sensor pitch offset (rad) // vision estimate fusion control::BlockParamExtFloat _ev_pos_noise; // default position observation noise for exernal vision measurements (m) @@ -351,6 +352,7 @@ Ekf2::Ekf2(): _range_noise(this, "EKF2_RNG_NOISE", false, _params->range_noise), _range_innov_gate(this, "EKF2_RNG_GATE", false, _params->range_innov_gate), _rng_gnd_clearance(this, "EKF2_MIN_RNG", false, _params->rng_gnd_clearance), + _rng_pitch_offset(this, "EKF2_RNG_PITCH", false, _params->rng_sens_pitch), _ev_pos_noise(this, "EKF2_EVP_NOISE", false, _default_ev_pos_noise), _ev_ang_noise(this, "EKF2_EVA_NOISE", false, _default_ev_ang_noise), _ev_innov_gate(this, "EKF2_EV_GATE", false, _params->ev_innov_gate), diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index e214955aeb..14c7edc603 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -895,3 +895,14 @@ PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f); * @decimal 3 */ PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f); + +/** + * Range sensor pitch offset. + * + * @group EKF2 + * @min -0.75 + * @max 0.75 + * @unit rad + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);