From e52ef945df4330d756657f815658c0c1bd28b402 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Oct 2022 10:13:28 -0400 Subject: [PATCH] ekf2: add new EKF2_EV_QMIN parameter --- src/modules/ekf2/EKF/common.h | 2 ++ src/modules/ekf2/EKF2.cpp | 3 ++- src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/ekf2_params.c | 13 +++++++++++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index dbc5b55680..478e87a4fb 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -231,6 +231,7 @@ struct extVisionSample { float angVar{}; ///< angular heading variance (rad**2) VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; uint8_t reset_counter{}; + int8_t quality{}; ///< quality indicator between 0 and 100 }; struct dragSample { @@ -356,6 +357,7 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check // vision position fusion + int32_t ev_quality_minimum{0}; ///< vision minimum acceptable quality integer float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz)) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b5ef696ad1..96f83f5338 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -123,6 +123,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), + _param_ekf2_ev_qmin(_params->ev_quality_minimum), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), @@ -1848,7 +1849,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo // use timestamp from external computer, clocks are synchronized when using MAVROS ev_data.time_us = ev_odom.timestamp_sample; ev_data.reset_counter = ev_odom.reset_counter; - //ev_data.quality = ev_odom.quality; + ev_data.quality = ev_odom.quality; if (new_ev_odom) { _ekf.setExtVisionData(ev_data); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 652e3f6b6f..e00616fc42 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -522,6 +522,7 @@ private: // vision estimate fusion (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise + (ParamExtInt) _param_ekf2_ev_qmin, (ParamFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) (ParamFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 0a69cd8f8c..36242b190a 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -779,6 +779,19 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); */ PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); +/** + * External vision (EV) minimum quality (optional) + * + * External vision will only be started and fused if the quality metric is above this threshold. + * The quality metric is a completely optional field provided by some VIO systems. + * + * @group EKF2 + * @min 0 + * @max 100 + * @decimal 1 + */ +PARAM_DEFINE_INT32(EKF2_EV_QMIN, 0); + /** * Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message *