msg/vehicle_odometry.msg: simplify covariance handling and update all usage (#19966)

- replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly)
 - these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit
 - ekf2: add new helper to get roll/pitch/yaw covariances
 - mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD)
 - mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough)

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

src/modules/mavlink/mavlink_receiver.cpp

@@ -80,6 +80,22 @@ MavlinkReceiver::~MavlinkReceiver()
 #endif // !CONSTRAINED_FLASH
 }
 
+static constexpr vehicle_odometry_s vehicle_odometry_empty {
+	.timestamp = 0,
+	.timestamp_sample = 0,
+	.position = {NAN, NAN, NAN},
+	.q = {NAN, NAN, NAN, NAN},
+	.velocity = {NAN, NAN, NAN},
+	.angular_velocity = {NAN, NAN, NAN},
+	.position_variance = {NAN, NAN, NAN},
+	.orientation_variance = {NAN, NAN, NAN},
+	.velocity_variance = {NAN, NAN, NAN},
+	.pose_frame = vehicle_odometry_s::POSE_FRAME_UNKNOWN,
+	.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_UNKNOWN,
+	.reset_counter = 0,
+	.quality = 0
+};
+
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	ModuleParams(nullptr),
 	_mavlink(parent),
@@ -948,40 +964,39 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 {
-	mavlink_att_pos_mocap_t mocap;
-	mavlink_msg_att_pos_mocap_decode(msg, &mocap);
+	mavlink_att_pos_mocap_t att_pos_mocap;
+	mavlink_msg_att_pos_mocap_decode(msg, &att_pos_mocap);
 
-	vehicle_odometry_s mocap_odom{};
+	// fill vehicle_odometry from Mavlink ATT_POS_MOCAP
+	vehicle_odometry_s odom{vehicle_odometry_empty};
 
-	mocap_odom.timestamp = hrt_absolute_time();
-	mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);
+	odom.timestamp_sample = _mavlink_timesync.sync_stamp(att_pos_mocap.time_usec);
 
-	mocap_odom.x = mocap.x;
-	mocap_odom.y = mocap.y;
-	mocap_odom.z = mocap.z;
-	mocap_odom.q[0] = mocap.q[0];
-	mocap_odom.q[1] = mocap.q[1];
-	mocap_odom.q[2] = mocap.q[2];
-	mocap_odom.q[3] = mocap.q[3];
+	odom.q[0] = att_pos_mocap.q[0];
+	odom.q[1] = att_pos_mocap.q[1];
+	odom.q[2] = att_pos_mocap.q[2];
+	odom.q[3] = att_pos_mocap.q[3];
 
-	const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]);
-	static_assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0])),
-		      "Odometry Pose Covariance matrix URT array size mismatch");
+	odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
+	odom.position[0] = att_pos_mocap.x;
+	odom.position[1] = att_pos_mocap.y;
+	odom.position[2] = att_pos_mocap.z;
 
-	for (size_t i = 0; i < URT_SIZE; i++) {
-		mocap_odom.pose_covariance[i] = mocap.covariance[i];
-	}
+	// ATT_POS_MOCAP covariance
+	//  Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle
+	//  (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
+	//  If unknown, assign NaN value to first element in the array.
+	odom.position_variance[0] = att_pos_mocap.covariance[0];  // X  row 0, col 0
+	odom.position_variance[1] = att_pos_mocap.covariance[6];  // Y  row 1, col 1
+	odom.position_variance[2] = att_pos_mocap.covariance[11]; // Z  row 2, col 2
 
-	mocap_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
-	mocap_odom.vx = NAN;
-	mocap_odom.vy = NAN;
-	mocap_odom.vz = NAN;
-	mocap_odom.rollspeed = NAN;
-	mocap_odom.pitchspeed = NAN;
-	mocap_odom.yawspeed = NAN;
-	mocap_odom.velocity_covariance[0] = NAN;
+	odom.orientation_variance[0] = att_pos_mocap.covariance[15]; // R  row 3, col 3
+	odom.orientation_variance[1] = att_pos_mocap.covariance[18]; // P  row 4, col 4
+	odom.orientation_variance[2] = att_pos_mocap.covariance[20]; // Y  row 5, col 5
 
-	_mocap_odometry_pub.publish(mocap_odom);
+	odom.timestamp = hrt_absolute_time();
+
+	_mocap_odometry_pub.publish(odom);
 }
 
 void
@@ -1313,143 +1328,259 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
-	mavlink_vision_position_estimate_t ev;
-	mavlink_msg_vision_position_estimate_decode(msg, &ev);
+	mavlink_vision_position_estimate_t vpe;
+	mavlink_msg_vision_position_estimate_decode(msg, &vpe);
 
-	vehicle_odometry_s visual_odom{};
+	// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
+	vehicle_odometry_s odom{vehicle_odometry_empty};
 
-	visual_odom.timestamp = hrt_absolute_time();
-	visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);
+	odom.timestamp_sample = _mavlink_timesync.sync_stamp(vpe.usec);
 
-	visual_odom.x = ev.x;
-	visual_odom.y = ev.y;
-	visual_odom.z = ev.z;
-	matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
-	q.copyTo(visual_odom.q);
+	odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
+	odom.position[0] = vpe.x;
+	odom.position[1] = vpe.y;
+	odom.position[2] = vpe.z;
 
-	visual_odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
+	const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
+	q.copyTo(odom.q);
 
-	const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]);
-	static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
-		      "Odometry Pose Covariance matrix URT array size mismatch");
+	// VISION_POSITION_ESTIMATE covariance
+	//  Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
+	//  (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
+	//  If unknown, assign NaN value to first element in the array.
+	odom.position_variance[0] = vpe.covariance[0];  // X  row 0, col 0
+	odom.position_variance[1] = vpe.covariance[6];  // Y  row 1, col 1
+	odom.position_variance[2] = vpe.covariance[11]; // Z  row 2, col 2
 
-	for (size_t i = 0; i < URT_SIZE; i++) {
-		visual_odom.pose_covariance[i] = ev.covariance[i];
-	}
+	odom.orientation_variance[0] = vpe.covariance[15]; // R  row 3, col 3
+	odom.orientation_variance[1] = vpe.covariance[18]; // P  row 4, col 4
+	odom.orientation_variance[2] = vpe.covariance[20]; // Y  row 5, col 5
 
-	visual_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
-	visual_odom.vx = NAN;
-	visual_odom.vy = NAN;
-	visual_odom.vz = NAN;
-	visual_odom.rollspeed = NAN;
-	visual_odom.pitchspeed = NAN;
-	visual_odom.yawspeed = NAN;
-	visual_odom.velocity_covariance[0] = NAN;
+	odom.reset_counter = vpe.reset_counter;
 
-	visual_odom.reset_counter = ev.reset_counter;
+	odom.timestamp = hrt_absolute_time();
 
-	_visual_odometry_pub.publish(visual_odom);
+	_visual_odometry_pub.publish(odom);
 }
 
 void
 MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 {
-	mavlink_odometry_t odom;
-	mavlink_msg_odometry_decode(msg, &odom);
+	mavlink_odometry_t odom_in;
+	mavlink_msg_odometry_decode(msg, &odom_in);
 
-	vehicle_odometry_s odometry{};
+	// fill vehicle_odometry from Mavlink ODOMETRY
+	vehicle_odometry_s odom{vehicle_odometry_empty};
 
-	odometry.timestamp = hrt_absolute_time();
-	odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);
+	odom.timestamp_sample = _mavlink_timesync.sync_stamp(odom_in.time_usec);
 
-	/* The position is in a local FRD frame */
-	odometry.x = odom.x;
-	odometry.y = odom.y;
-	odometry.z = odom.z;
+	// position x/y/z (m)
+	if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) {
+		// frame_id: Coordinate frame of reference for the pose data.
+		switch (odom_in.frame_id) {
+		case MAV_FRAME_LOCAL_NED:
+			// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
+			odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
+			odom.position[0] = odom_in.x;
+			odom.position[1] = odom_in.y;
+			odom.position[2] = odom_in.z;
+			break;
 
-	/**
-	 * The quaternion of the ODOMETRY msg represents a rotation from body frame
-	 * to a local frame
-	 */
-	matrix::Quatf q_body_to_local(odom.q);
-	q_body_to_local.normalize();
-	q_body_to_local.copyTo(odometry.q);
+		case MAV_FRAME_LOCAL_ENU:
+			// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
+			odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
+			odom.position[0] =  odom_in.y; // y: North
+			odom.position[1] =  odom_in.x; // x: East
+			odom.position[2] = -odom_in.z; // z: Up
+			break;
 
-	// pose_covariance
-	static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
-	static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
-		      "Odometry Pose Covariance matrix URT array size mismatch");
+		case MAV_FRAME_LOCAL_FRD:
+			// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
+			odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
+			odom.position[0] = odom_in.x;
+			odom.position[1] = odom_in.y;
+			odom.position[2] = odom_in.z;
+			break;
 
-	// velocity_covariance
-	static constexpr size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
-	static_assert(VEL_URT_SIZE == (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])),
-		      "Odometry Velocity Covariance matrix URT array size mismatch");
+		case MAV_FRAME_LOCAL_FLU:
+			// FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth.
+			odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
+			odom.position[0] =  odom_in.x; // x: Forward
+			odom.position[1] = -odom_in.y; // y: Left
+			odom.position[2] = -odom_in.z; // z: Up
+			break;
 
-	// TODO: create a method to simplify covariance copy
-	for (size_t i = 0; i < POS_URT_SIZE; i++) {
-		odometry.pose_covariance[i] = odom.pose_covariance[i];
+		default:
+			break;
+		}
+
+		// pose_covariance
+		//  Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw)
+		//  first six entries are the first ROW, next five entries are the second ROW, etc.
+		if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
+			switch (odom_in.frame_id) {
+			case MAV_FRAME_LOCAL_NED:
+			case MAV_FRAME_LOCAL_FRD:
+			case MAV_FRAME_LOCAL_FLU:
+				// position variances copied directly
+				odom.position_variance[0] = odom_in.pose_covariance[0];  // X  row 0, col 0
+				odom.position_variance[1] = odom_in.pose_covariance[6];  // Y  row 1, col 1
+				odom.position_variance[2] = odom_in.pose_covariance[11]; // Z  row 2, col 2
+				break;
+
+			case MAV_FRAME_LOCAL_ENU:
+				// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
+				odom.position_variance[0] = odom_in.pose_covariance[6];  // Y  row 1, col 1
+				odom.position_variance[1] = odom_in.pose_covariance[0];  // X  row 0, col 0
+				odom.position_variance[2] = odom_in.pose_covariance[11]; // Z  row 2, col 2
+				break;
+
+			default:
+				break;
+			}
+		}
 	}
 
-	/**
-	 * PX4 expects the body's linear velocity in the local frame,
-	 * the linear velocity is rotated from the odom child_frame to the
-	 * local NED frame. The angular velocity needs to be expressed in the
-	 * body (fcu_frd) frame.
-	 */
-	if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
+	// q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame
+	if (PX4_ISFINITE(odom_in.q[0])
+	    && PX4_ISFINITE(odom_in.q[1])
+	    && PX4_ISFINITE(odom_in.q[2])
+	    && PX4_ISFINITE(odom_in.q[3])) {
 
-		odometry.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD;
-		odometry.vx = odom.vx;
-		odometry.vy = odom.vy;
-		odometry.vz = odom.vz;
+		odom.q[0] = odom_in.q[0];
+		odom.q[1] = odom_in.q[1];
+		odom.q[2] = odom_in.q[2];
+		odom.q[3] = odom_in.q[3];
 
-		odometry.rollspeed = odom.rollspeed;
-		odometry.pitchspeed = odom.pitchspeed;
-		odometry.yawspeed = odom.yawspeed;
-
-		for (size_t i = 0; i < VEL_URT_SIZE; i++) {
-			odometry.velocity_covariance[i] = odom.velocity_covariance[i];
+		// pose_covariance (roll, pitch, yaw)
+		//  states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.
+		//  TODO: fix pose_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU
+		if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
+			odom.orientation_variance[0] = odom_in.pose_covariance[15]; // R  row 3, col 3
+			odom.orientation_variance[1] = odom_in.pose_covariance[18]; // P  row 4, col 4
+			odom.orientation_variance[2] = odom_in.pose_covariance[20]; // Y  row 5, col 5
 		}
-
-	} else {
-		PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id);
 	}
 
-	odometry.reset_counter = odom.reset_counter;
+	// velocity vx/vy/vz (m/s)
+	if (PX4_ISFINITE(odom_in.vx) && PX4_ISFINITE(odom_in.vy) && PX4_ISFINITE(odom_in.vz)) {
+		// child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data.
+		switch (odom_in.child_frame_id) {
+		case MAV_FRAME_LOCAL_NED:
+			// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
+			odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
+			odom.velocity[0] = odom_in.vx;
+			odom.velocity[1] = odom_in.vy;
+			odom.velocity[2] = odom_in.vz;
+			break;
 
-	/**
-	 * 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,
-	 * MAV_ESTIMATOR_TYPE_VIO and MAV_ESTIMATOR_TYPE_MOCAP
-	 *
-	 * @note Regarding the local frames of reference, the appropriate EKF_AID_MASK
-	 * should be set in order to match a frame aligned (NED) or not aligned (FRD)
-	 * with true North
-	 */
-	if (odom.frame_id == MAV_FRAME_LOCAL_NED || odom.frame_id == MAV_FRAME_LOCAL_FRD) {
+		case MAV_FRAME_LOCAL_ENU:
+			// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
+			odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
+			odom.velocity[0] =  odom_in.vy; // y: East
+			odom.velocity[1] =  odom_in.vx; // x: North
+			odom.velocity[2] = -odom_in.vz; // z: Up
+			break;
 
-		if (odom.frame_id == MAV_FRAME_LOCAL_NED) {
-			odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
+		case MAV_FRAME_LOCAL_FRD:
+			// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
+			odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
+			odom.velocity[0] = odom_in.vx;
+			odom.velocity[1] = odom_in.vy;
+			odom.velocity[2] = odom_in.vz;
+			break;
 
-		} else {
-			odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
+		case MAV_FRAME_LOCAL_FLU:
+			// FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth.
+			odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
+			odom.velocity[0] =  odom_in.vx; // x: Forward
+			odom.velocity[1] = -odom_in.vy; // y: Left
+			odom.velocity[2] = -odom_in.vz; // z: Up
+			break;
+
+		case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
+		case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
+		case MAV_FRAME_BODY_FRD:
+			// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle.
+			odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD;
+			odom.velocity[0] = odom_in.vx;
+			odom.velocity[1] = odom_in.vy;
+			odom.velocity[2] = odom_in.vz;
+			break;
+
+		default:
+			// unsupported child_frame_id
+			break;
 		}
 
-		if ((odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION)
-		    || (odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO)
-		    || (odom.estimator_type == MAV_ESTIMATOR_TYPE_UNKNOWN)) {
-			// accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
-			_visual_odometry_pub.publish(odometry);
+		// velocity_covariance (vx, vy, vz)
+		//  states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.
+		//  TODO: fix velocity_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU, MAV_FRAME_LOCAL_FLU
+		if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
+			switch (odom_in.child_frame_id) {
+			case MAV_FRAME_LOCAL_NED:
+			case MAV_FRAME_LOCAL_FRD:
+			case MAV_FRAME_LOCAL_FLU:
+			case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
+			case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
+			case MAV_FRAME_BODY_FRD:
+				// velocity covariances copied directly
+				odom.velocity_variance[0] = odom_in.velocity_covariance[0];  // X  row 0, col 0
+				odom.velocity_variance[1] = odom_in.velocity_covariance[6];  // Y  row 1, col 1
+				odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z  row 2, col 2
+				break;
 
-		} else if (odom.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) {
-			_mocap_odometry_pub.publish(odometry);
+			case MAV_FRAME_LOCAL_ENU:
+				// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
+				odom.velocity_variance[0] = odom_in.velocity_covariance[6];  // Y  row 1, col 1
+				odom.velocity_variance[1] = odom_in.velocity_covariance[0];  // X  row 0, col 0
+				odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z  row 2, col 2
+				break;
 
-		} else {
-			PX4_ERR("Estimator source %" PRIu8 " not supported. Unable to publish pose and velocity", odom.estimator_type);
+			default:
+				// unsupported child_frame_id
+				break;
+			}
 		}
+	}
 
-	} else {
-		PX4_ERR("Local frame %" PRIu8 " not supported. Unable to publish pose and velocity", odom.frame_id);
+	// Roll/Pitch/Yaw angular speed (rad/s)
+	if (PX4_ISFINITE(odom_in.rollspeed)
+	    && PX4_ISFINITE(odom_in.pitchspeed)
+	    && PX4_ISFINITE(odom_in.yawspeed)) {
+
+		odom.angular_velocity[0] = odom_in.rollspeed;
+		odom.angular_velocity[1] = odom_in.pitchspeed;
+		odom.angular_velocity[2] = odom_in.yawspeed;
+	}
+
+	odom.reset_counter = odom_in.reset_counter;
+	odom.quality = odom_in.quality;
+
+	switch (odom_in.estimator_type) {
+	case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
+	case MAV_ESTIMATOR_TYPE_NAIVE:
+	case MAV_ESTIMATOR_TYPE_VISION:
+	case MAV_ESTIMATOR_TYPE_VIO:
+		odom.timestamp = hrt_absolute_time();
+		_visual_odometry_pub.publish(odom);
+		break;
+
+	case MAV_ESTIMATOR_TYPE_MOCAP:
+		odom.timestamp = hrt_absolute_time();
+		_mocap_odometry_pub.publish(odom);
+		break;
+
+	case MAV_ESTIMATOR_TYPE_GPS:
+	case MAV_ESTIMATOR_TYPE_GPS_INS:
+	case MAV_ESTIMATOR_TYPE_LIDAR:
+	case MAV_ESTIMATOR_TYPE_AUTOPILOT:
+	default:
+		mavlink_log_critical(&_mavlink_log_pub, "ODOMETRY: estimator_type %" PRIu8 " unsupported\t",
+				     odom_in.estimator_type);
+		events::send<uint8_t>(events::ID("mavlink_rcv_odom_unsup_estimator_type"), events::Log::Error,
+				      "ODOMETRY: unsupported estimator_type {1}", odom_in.estimator_type);
+		return;
 	}
 }