fix vision quaternion transformation

PX4 · May 18, 2018 · 651ca35 · 651ca35
1 parent cd91ef4
commit 651ca35
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
@@ -780,7 +780,7 @@ void GazeboMavlinkInterface::VisionCallback(OdomPtr& odom_message) {
   // transform orientation from body FLU to body FRD frame:
   // q_nb is the quaternion that represents a rotation from NED earth/local
   // frame to XYZ body FRD frame
-  ignition::math::Quaterniond q_nb = q_ng * q_gr * q_ng.Inverse();
+  ignition::math::Quaterniond q_nb = q_ng * q_gb;
 
   // transform linear velocity from local ENU to body FRD frame
   ignition::math::Vector3d linear_velocity = q_ng.RotateVector(