From 9137813472f657535ec0d465e9536b748160029c Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sun, 3 May 2020 16:01:17 +0200 Subject: [PATCH] Received quaternion represents body to world --- src/modules/simulator/simulator_mavlink.cpp | 23 +++++++++++---------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a0756467ab..7f30ff29dd 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1019,27 +1019,24 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) mavlink_odometry_t odom_msg; mavlink_msg_odometry_decode(odom_mavlink, &odom_msg); + /* The quaternion of the ODOMETRY msg represents a rotation from + * FRD body frame to the NED local frame */ + matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]); + q.normalize(); + q.copyTo(odom.q); + /* Dcm rotation matrix from body frame to local NED frame */ - matrix::Dcm Rbl; + const matrix::Dcmf R_body_to_local(q); /* since 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 */ /* No need to transform the covariance matrices since the non-diagonal values are all zero */ - Rbl = matrix::Dcm(matrix::Quatf(odom_msg.q)).I(); - - /* the linear velocities needs to be transformed to the local NED frame */ - matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom_msg.vx, odom_msg.vy, odom_msg.vz)); /* The position in the local NED frame */ odom.x = odom_msg.x; odom.y = odom_msg.y; odom.z = odom_msg.z; - /* The quaternion of the ODOMETRY msg represents a rotation from - * NED earth/local frame to XYZ body frame */ - matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]); - q.copyTo(odom.q); - odom.local_frame = odom.LOCAL_FRAME_NED; + odom.local_frame = odom.LOCAL_FRAME_FRD; static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])), "Odometry Pose Covariance matrix URT array size mismatch"); @@ -1049,6 +1046,10 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) odom.pose_covariance[i] = odom_msg.pose_covariance[i]; } + /* the linear velocities needs to be transformed from the body frame to the local frame */ + const matrix::Vector3 linvel_local(R_body_to_local * + matrix::Vector3(odom_msg.vx, odom_msg.vy, odom_msg.vz)); + /* The velocity in the local NED frame */ odom.vx = linvel_local(0); odom.vy = linvel_local(1);