add odometry reset counter (ekf2, mavlink, etc)

msg/vehicle_odometry.msg

@@ -62,5 +62,7 @@ float32 yawspeed		# Angular velocity about Z body axis
 # If angular velocity covariance invalid/unknown, 16th cell is NaN
 float32[21] velocity_covariance
 
+uint8 reset_counter
+
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
 # TOPICS estimator_odometry estimator_visual_odometry_aligned

src/modules/ekf2/EKF/common.h

@@ -152,6 +152,7 @@ struct extVisionSample {
 	Matrix3f velCov;	///< XYZ velocity covariances ((m/sec)**2)
 	float angVar;		///< angular heading variance (rad**2)
 	velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+	uint8_t reset_counter{0};
 };
 
 struct dragSample {

src/modules/ekf2/EKF/control.cpp

@@ -212,6 +212,12 @@ void Ekf::controlExternalVisionFusion()
 	// Check for new external vision data
 	if (_ev_data_ready) {
 
+		bool reset = false;
+
+		if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) {
+			reset = true;
+		}
+
 		if (_inhibit_ev_yaw_use) {
 			stopEvYawFusion();
 		}
@@ -257,6 +263,9 @@ void Ekf::controlExternalVisionFusion()
 
 		// determine if we should use the horizontal position observations
 		if (_control_status.flags.ev_pos) {
+			if (reset && _control_status_prev.flags.ev_pos) {
+				resetHorizontalPosition();
+			}
 
 			Vector3f ev_pos_obs_var;
 			Vector2f ev_pos_innov_gates;
@@ -281,10 +290,8 @@ void Ekf::controlExternalVisionFusion()
 
 				} else {
 					// calculate the change in position since the last measurement
-					Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev;
-
 					// rotate measurement into body frame is required when fusing with GPS
-					ev_delta_pos = _R_ev_to_ekf * ev_delta_pos;
+					Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos);
 
 					// use the change in position since the last measurement
 					_ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
@@ -296,11 +303,6 @@ void Ekf::controlExternalVisionFusion()
 					ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
 					ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
 				}
-
-				// record observation and estimate for use next time
-				_pos_meas_prev = _ev_sample_delayed.pos;
-				_hpos_pred_prev = _state.pos.xy();
-
 			} else {
 				// use the absolute position
 				Vector3f ev_pos_meas = _ev_sample_delayed.pos;
@@ -337,6 +339,9 @@ void Ekf::controlExternalVisionFusion()
 
 		// determine if we should use the velocity observations
 		if (_control_status.flags.ev_vel) {
+			if (reset && _control_status_prev.flags.ev_vel) {
+				resetVelocity();
+			}
 
 			Vector2f ev_vel_innov_gates;
 
@@ -362,9 +367,17 @@ void Ekf::controlExternalVisionFusion()
 
 		// determine if we should use the yaw observation
 		if (_control_status.flags.ev_yaw) {
+			if (reset && _control_status_prev.flags.ev_yaw) {
+				resetYawToEv();
+			}
+
 			fuseHeading();
 		}
 
+		// record observation and estimate for use next time
+		_ev_sample_delayed_prev = _ev_sample_delayed;
+		_hpos_pred_prev = _state.pos.xy();
+
 	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel ||  _control_status.flags.ev_yaw)
 		   && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
 

src/modules/ekf2/EKF/ekf.h

@@ -252,6 +252,7 @@ public:
 	const auto &state_reset_status() const { return _state_reset_status; }
 
 	// return the amount the local vertical position changed in the last reset and the number of reset events
+	uint8_t get_posD_reset_count() const { return _state_reset_status.posD_counter; }
 	void get_posD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.posD_change;
@@ -259,6 +260,7 @@ public:
 	}
 
 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
+	uint8_t get_velD_reset_count() const { return _state_reset_status.velD_counter; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
 	{
 		*delta = _state_reset_status.velD_change;
@@ -266,6 +268,7 @@ public:
 	}
 
 	// return the amount the local horizontal position changed in the last reset and the number of reset events
+	uint8_t get_posNE_reset_count() const { return _state_reset_status.posNE_counter; }
 	void get_posNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.posNE_change.copyTo(delta);
@@ -273,6 +276,7 @@ public:
 	}
 
 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
+	uint8_t get_velNE_reset_count() const { return _state_reset_status.velNE_counter; }
 	void get_velNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.velNE_change.copyTo(delta);
@@ -280,6 +284,7 @@ public:
 	}
 
 	// return the amount the quaternion has changed in the last reset and the number of reset events
+	uint8_t get_quat_reset_count() const { return _state_reset_status.quat_counter; }
 	void get_quat_reset(float delta_quat[4], uint8_t *counter) const
 	{
 		_state_reset_status.quat_change.copyTo(delta_quat);
@@ -352,7 +357,6 @@ private:
 
 	// variables used when position data is being fused using a relative position odometry model
 	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
-	Vector3f _pos_meas_prev{};		///< previous value of NED position measurement fused using odometry assumption (m)
 	Vector2f _hpos_pred_prev{};		///< previous value of NE position state used by odometry fusion (m)
 	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
 	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1566,7 +1566,7 @@ void Ekf::stopGpsFusion()
 
 	// We do not need to know the true North anymore
 	// EV yaw can start again
-	_inhibit_ev_yaw_use = false;;
+	_inhibit_ev_yaw_use = false;
 }
 
 void Ekf::stopGpsPosFusion()

src/modules/ekf2/EKF/estimator_interface.h

@@ -306,6 +306,7 @@ protected:
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
 	extVisionSample _ev_sample_delayed{};
+	extVisionSample _ev_sample_delayed_prev{};
 	dragSample _drag_sample_delayed{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 	auxVelSample _auxvel_sample_delayed{};

src/modules/ekf2/EKF2.cpp

@@ -1048,6 +1048,10 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
 	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
 
+	odom.reset_counter = _ekf.get_quat_reset_count()
+			     + _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
+			     + _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();
+
 	// publish vehicle odometry data
 	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_odometry_pub.publish(odom);

src/modules/ekf2/EKF2Selector.cpp

@@ -531,6 +531,24 @@ void EKF2Selector::PublishVehicleOdometry()
 	vehicle_odometry_s odometry;
 
 	if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) {
+
+		bool instance_change = false;
+
+		if (_instance[_selected_instance].estimator_odometry_sub.get_instance() != _odometry_instance_prev) {
+			_odometry_instance_prev = _instance[_selected_instance].estimator_odometry_sub.get_instance();
+			instance_change = true;
+		}
+
+		if (_odometry_last.timestamp != 0) {
+			// reset
+			if (instance_change || (odometry.reset_counter != _odometry_last.reset_counter)) {
+				++_odometry_reset_counter;
+			}
+
+		} else {
+			_odometry_reset_counter = odometry.reset_counter;
+		}
+
 		bool publish = true;
 
 		// ensure monotonically increasing timestamp_sample through reset, don't publish
@@ -541,10 +559,13 @@ void EKF2Selector::PublishVehicleOdometry()
 			publish = false;
 		}
 
-		// save last primary estimator_odometry
+		// save last primary estimator_odometry as published with original resets
 		_odometry_last = odometry;
 
 		if (publish) {
+			// republish with total reset count and current timestamp
+			odometry.reset_counter = _odometry_reset_counter;
+
 			odometry.timestamp = hrt_absolute_time();
 			_vehicle_odometry_pub.publish(odometry);
 		}

src/modules/ekf2/EKF2Selector.hpp

@@ -211,6 +211,7 @@ private:
 
 	// vehicle_odometry
 	vehicle_odometry_s _odometry_last{};
+	uint8_t _odometry_reset_counter{0};
 
 	// vehicle_global_position: reset counters
 	vehicle_global_position_s _global_position_last{};
@@ -226,6 +227,7 @@ private:
 	uint8_t _attitude_instance_prev{INVALID_INSTANCE};
 	uint8_t _local_position_instance_prev{INVALID_INSTANCE};
 	uint8_t _global_position_instance_prev{INVALID_INSTANCE};
+	uint8_t _odometry_instance_prev{INVALID_INSTANCE};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};

src/modules/mavlink/mavlink_receiver.cpp

@@ -1377,6 +1377,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	visual_odom.yawspeed = NAN;
 	visual_odom.velocity_covariance[0] = NAN;
 
+	visual_odom.reset_counter = ev.reset_counter;
+
 	_visual_odometry_pub.publish(visual_odom);
 }
 
@@ -1444,6 +1446,8 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 		PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id);
 	}
 
+	odometry.reset_counter = odom.reset_counter;
+
 	/**
 	 * Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD
 	 * The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION,

src/modules/mavlink/streams/ODOMETRY.hpp

@@ -159,6 +159,8 @@ private:
 				msg.velocity_covariance[i] = odom.velocity_covariance[i];
 			}
 
+			msg.reset_counter = odom.reset_counter;
+
 			mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
 
 			return true;

src/modules/simulator/simulator_mavlink.cpp

@@ -1358,6 +1358,8 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
 			odom.velocity_covariance[i] = odom_msg.velocity_covariance[i];
 		}
 
+		odom.reset_counter = odom_msg.reset_counter;
+
 		/* Publish the odometry based on the source */
 		if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VIO) {
 			_visual_odometry_pub.publish(odom);
@@ -1403,6 +1405,8 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
 		/* The velocity covariance URT - unknown */
 		odom.velocity_covariance[0] = NAN;
 
+		odom.reset_counter = ev.reset_counter;
+
 		/* Publish the odometry */
 		_visual_odometry_pub.publish(odom);
 	}