From 97922f3e7ed2759d8b1e850cb26943aed3afedb3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 28 Jun 2022 21:35:29 -0400 Subject: [PATCH] WIP: ODOMETRY quality metric --- msg/vehicle_odometry.msg | 2 ++ src/modules/ekf2/EKF/common.h | 4 +++- src/modules/ekf2/EKF/control.cpp | 2 +- src/modules/ekf2/EKF2.cpp | 4 ++++ src/modules/ekf2/EKF2.hpp | 4 ++-- src/modules/ekf2/ekf2_params.c | 10 ++++++++++ src/modules/mavlink/mavlink | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/mavlink/streams/ODOMETRY.hpp | 1 + 9 files changed, 25 insertions(+), 5 deletions(-) diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 5f597f4d96..82a3b93dda 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -64,5 +64,7 @@ float32[21] velocity_covariance uint8 reset_counter +uint8 quality + # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry # TOPICS estimator_odometry estimator_visual_odometry_aligned diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 4c23a9c51d..f92fb3b3c5 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -208,6 +208,7 @@ struct extVisionSample { float angVar{}; ///< angular heading variance (rad**2) VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; uint8_t reset_counter{}; + uint8_t quality{}; ///< quality indicator between 0 and 100 }; struct dragSample { @@ -329,13 +330,14 @@ 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) // optical flow fusion 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{1}; ///< minimum acceptable quality integer from the flow sensor float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) // these parameters control the strictness of GPS quality checks used to determine if the GPS is diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index aa06c0d354..5172f4695d 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -201,7 +201,7 @@ void Ekf::controlFusionModes() void Ekf::controlExternalVisionFusion() { // Check for new external vision data - if (_ev_data_ready) { + if (_ev_data_ready && _ev_sample_delayed.quality >= _params.ev_quality_minimum) { bool reset = false; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 349b71c5aa..e695e0ad60 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -120,6 +120,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), @@ -1700,6 +1701,9 @@ 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; + if (new_ev_odom) { _ekf.setExtVisionData(ev_data); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index e279ff0b56..a0d3d237c8 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -469,8 +469,8 @@ private: _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) // vision estimate fusion - (ParamInt) - _param_ekf2_ev_noise_md, ///< determine source of vision observation noise + (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 271882a6e9..1faefe0618 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -721,6 +721,16 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); */ PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); +/** + * Vision minimum quality + * + * @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 * diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index c6e254c134..5a5ccd26c7 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit c6e254c134d220426273e42edd7fb6ba35b4308f +Subproject commit 5a5ccd26c74bc25a7e35019cc064795b1e2bbf09 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 196d991a6c..e92221085a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1415,6 +1415,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } odometry.reset_counter = odom.reset_counter; + odometry.quality = odom.quality; /** * Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD diff --git a/src/modules/mavlink/streams/ODOMETRY.hpp b/src/modules/mavlink/streams/ODOMETRY.hpp index 2a0e148541..59a77c3038 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -160,6 +160,7 @@ private: } msg.reset_counter = odom.reset_counter; + msg.quality = odom.quality; mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);