From 9ad0914fd34e729d40ab0aca97955ec4df2aa59f Mon Sep 17 00:00:00 2001 From: Spwizzard Date: Fri, 30 Aug 2019 11:37:32 -0400 Subject: [PATCH] Fixed quaternion transformation if tf_ned_to_enu --- src/main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 4a63627..85f818e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -218,10 +218,10 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& i if (tf_ned_to_enu) { // world frame - imu_msg.orientation.w = r.quat.get_scaled(2); + imu_msg.orientation.w = r.quat.get_scaled(0); imu_msg.orientation.x = r.quat.get_scaled(1); - imu_msg.orientation.y = -r.quat.get_scaled(3); - imu_msg.orientation.z = r.quat.get_scaled(0); + imu_msg.orientation.y = -r.quat.get_scaled(2); + imu_msg.orientation.z = -r.quat.get_scaled(3); // body-fixed frame imu_msg.angular_velocity.x = r.gyro.get_scaled(0);