diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg
index 86a3c9094f..874e73804f 100644
--- a/msg/vehicle_attitude.msg
+++ b/msg/vehicle_attitude.msg
@@ -2,7 +2,7 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-float32[4] q			# Quaternion rotation from NED earth frame to XYZ body frame
+float32[4] q			# Quaternion rotation from XYZ body frame to NED earth frame.
 float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
 uint8 quat_reset_counter	# Quaternion reset counter
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index dc8f0e44ad..6e97675f39 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2451,7 +2451,7 @@ protected:
 
 		if (odom_updated) {
 			msg.time_usec = odom.timestamp;
-			msg.child_frame_id = MAV_FRAME_BODY_NED;
+			msg.child_frame_id = MAV_FRAME_BODY_FRD;
 
 			// Current position
 			msg.x = odom.x;
@@ -2464,11 +2464,11 @@ protected:
 			msg.q[2] = odom.q[2];
 			msg.q[3] = odom.q[3];
 
-			// Local NED to body-NED Dcm matrix
-			matrix::Dcmf Rlb(matrix::Quatf(odom.q));
+			// Body-FRD frame to local NED frame Dcm matrix
+			matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q));
 
 			// Rotate linear and angular velocity from local NED to body-NED frame
-			matrix::Vector3f linvel_body(Rlb * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
+			matrix::Vector3f linvel_body(R_body_to_local.transpose() * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
 
 			// Current linear velocity
 			msg.vx = linvel_body(0);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4bdab3e282..ebad5a8370 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1029,15 +1029,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 
 	odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec);
 
-	/* The position is in the local NED frame */
+	/* The position is in a local FRD frame */
 	odometry.x = odom.x;
 	odometry.y = odom.y;
 	odometry.z = odom.z;
 
-	/* The quaternion of the ODOMETRY msg represents a rotation from NED
-	 * earth/local frame to XYZ body frame */
-	const matrix::Quatf q(odom.q);
-	q.copyTo(odometry.q);
+	/* 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);
 
 	// TODO:
 	// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
@@ -1060,14 +1061,20 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 		odometry.pose_covariance[i] = odom.pose_covariance[i];
 	}
 
-	if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { /* WRT to estimated vehicle body-fixed frame */
-		/* Get quaternion from the msg quaternion itself and build DCM matrix from it
-		 * Note that the msg quaternion represents the rotation of the msg frame to the msg child frame
-		 * but rotates msg child frame *data* into the msg frame */
-		const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q));
+
+	/*
+	 * 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) {
+		/* Linear velocity has to be rotated to the local NED frame
+		 * Angular velocities are already in the right body frame  */
+		const matrix::Dcmf R_body_to_local = matrix::Dcmf(q_body_to_local);
 
 		/* the linear velocities needs to be transformed to the local NED frame */
-		matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
+		matrix::Vector3<float> linvel_local(R_body_to_local * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
 		odometry.vx = linvel_local(0);
 		odometry.vy = linvel_local(1);
 		odometry.vz = linvel_local(2);
@@ -1081,86 +1088,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 			odometry.velocity_covariance[i] = odom.velocity_covariance[i];
 		}
 
-	} else if (odom.child_frame_id == MAV_FRAME_BODY_FLU) { /* WRT to estimated vehicle body-fixed frame */
-		/* Get quaternion from the msg quaternion itself and build DCM matrix from it
-		 * Note that the msg quaternion represents the rotation of the msg frame to the msg child frame
-		 * but rotates msg child frame *data* into the msg frame */
-		const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q));
-
-		/* the position needs to be transformed to the local NED frame */
-		matrix::Vector3f pos(Rbl * matrix::Vector3<float>(odom.x, -odom.y, -odom.z));
-		odometry.x = pos(0);
-		odometry.y = pos(1);
-		odometry.z = pos(2);
-
-		/* the linear velocities needs to be transformed to the local NED frame */
-		matrix::Vector3f linvel_local(Rbl * matrix::Vector3<float>(odom.vx, -odom.vy, -odom.vz));
-		odometry.vx = linvel_local(0);
-		odometry.vy = linvel_local(1);
-		odometry.vz = linvel_local(2);
-
-		odometry.rollspeed = odom.rollspeed;
-		odometry.pitchspeed = odom.pitchspeed;
-		odometry.yawspeed = odom.yawspeed;
-
-		//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
-		for (size_t i = 0; i < VEL_URT_SIZE; i++) {
-			odometry.velocity_covariance[i] = odom.velocity_covariance[i];
-		}
-
-	} else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */
-		vehicle_attitude_s att{};
-
-		if (_vehicle_attitude_sub.copy(&att)) {
-
-			/* Get quaternion from vehicle_attitude quaternion and build DCM matrix from it
-			 * Note that the msg quaternion represents the rotation of the msg frame to the msg child frame
-			 * but rotates msg child frame *data* into the msg frame */
-			const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(att.q));
-
-			/* the linear velocities needs to be transformed to the local NED frame */
-			matrix::Vector3f linvel_local(Rbl * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
-			odometry.vx = linvel_local(0);
-			odometry.vy = linvel_local(1);
-			odometry.vz = linvel_local(2);
-
-			odometry.rollspeed = odom.rollspeed;
-			odometry.pitchspeed = odom.pitchspeed;
-			odometry.yawspeed = odom.yawspeed;
-
-			//TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame
-			for (size_t i = 0; i < VEL_URT_SIZE; i++) {
-				odometry.velocity_covariance[i] = odom.velocity_covariance[i];
-			}
-
-		}
-
-	} else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */
-		   odom.child_frame_id == MAV_FRAME_MOCAP_NED) {
-
-		vehicle_attitude_s att{};
-
-		if (_vehicle_attitude_sub.copy(&att)) {
-
-			/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
-			matrix::Dcmf Rlb = matrix::Quatf(att.q);
-
-			odometry.vx = odom.vx;
-			odometry.vy = odom.vy;
-			odometry.vz = odom.vz;
-
-			/* the angular rates need to be transformed to the body frame */
-			matrix::Vector3f angvel_local(Rlb * matrix::Vector3f(odom.rollspeed, odom.pitchspeed, odom.yawspeed));
-			odometry.rollspeed = angvel_local(0);
-			odometry.pitchspeed = angvel_local(1);
-			odometry.yawspeed = angvel_local(2);
-
-			//TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame
-			for (size_t i = 0; i < VEL_URT_SIZE; i++) {
-				odometry.velocity_covariance[i] = odom.velocity_covariance[i];
-			}
-		}
-
 	} else {
 		PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id);
 	}