From 36ec1fa811da9d85470baf32fe751e430311e56e Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 27 Jun 2023 17:07:12 -0600 Subject: [PATCH] Add EKF2_OF_QMIN_GND to handle 0 optical flow quality when on ground --- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/optical_flow_control.cpp | 7 ++++++- src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 4 +++- src/modules/ekf2/ekf2_params.c | 11 ++++++++++- 5 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b9fef6e6fa..5610885d14 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -416,6 +416,7 @@ struct parameters { float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor + int32_t flow_qual_min_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 0df8b140bd..8b99d2e5c7 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -70,7 +70,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } if (_flow_data_ready) { - const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min); + int32_t min_quality = _params.flow_qual_min; + if (!_control_status.flags.in_air) { + min_quality = _params.flow_qual_min_gnd; + } + + const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality); const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate); const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a9fad97550..c3beefafd1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -154,6 +154,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), _param_ekf2_of_qmin(_params->flow_qual_min), + _param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd), _param_ekf2_of_gate(_params->flow_innov_gate), _param_ekf2_of_pos_x(_params->flow_pos_body(0)), _param_ekf2_of_pos_y(_params->flow_pos_body(1)), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index a40971e79f..c3da913716 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -662,7 +662,9 @@ private: (ParamExtFloat) _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtInt) - _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air + (ParamExtInt) + _param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground (ParamExtFloat) _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index cf4a49ff75..34ffdecb5b 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -901,7 +901,7 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f); PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); /** - * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN. + * Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN. * * @group EKF2 * @min 0 @@ -909,6 +909,15 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); */ PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1); +/** + * Optical Flow data will only be used on the ground if the sensor reports a quality metric >= EKF2_OF_QMIN_GND. + * + * @group EKF2 + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(EKF2_OF_QMIN_GND, 0); + /** * Gate size for optical flow fusion *