From b203bc15df8002fa628e0dbe0c23673868b50c51 Mon Sep 17 00:00:00 2001 From: Nick Steele Date: Wed, 10 Jul 2019 14:33:00 -0400 Subject: [PATCH] Fix rotation of odometry velocities from body frame to local frame in the mavlink receiver odometry handler Summary: PX4 assumed that odometry orientation was a way to rotate data from parent frame to child frame, so to rotate child to parent, it was taking the inverse rotation of the odometry orientation, but orientation rotates the parent frame to the child frame, whereas for data, it represents a rotation from child to parent; hence, the rotation inverse operations were removed. Test Plan: Checked that vehicle_mocap_odometry and vehicle_local_position gave correct linear velocities. Also visualized px4 output twist in rviz. --- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 59023cd659..ecd9fbf38e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1160,8 +1160,10 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } 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 */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I(); + /* 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 linear velocities needs to be transformed to the local NED frame */ matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); @@ -1179,8 +1181,10 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } } 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 */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I(); + /* 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(odom.x, -odom.y, -odom.z)); @@ -1206,8 +1210,10 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */ if (_vehicle_attitude_sub.copy(&_att)) { - /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(_att.q)).I(); + /* 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::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz));