WIP: ODOMETRY quality metric

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

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

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;
 

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);
 		}

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<px4::params::EKF2_EV_NOISE_MD>)
-		_param_ekf2_ev_noise_md,	///< determine source of vision observation noise
+		(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
+		(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
 		(ParamFloat<px4::params::EKF2_EVP_NOISE>)
 		_param_ekf2_evp_noise,	///< default position observation noise for exernal vision measurements (m)
 		(ParamFloat<px4::params::EKF2_EVV_NOISE>)

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
  *

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

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);