lib #359: move quaternion utils.

mavlink · Aug 4, 2015 · 22ce10e · 22ce10e
1 parent de1626e
commit 22ce10e
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 31 deletions.
diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
@@ -173,6 +173,7 @@ add_library(mavros
   src/lib/uas_timesync.cpp
   src/lib/uas_sensor_orientation.cpp
   src/lib/uas_frame_conversions.cpp
+  src/lib/uas_quaternion_utils.cpp
   src/lib/mavros.cpp
   src/lib/mavlink_diag.cpp
   src/lib/rosconsole_bridge.cpp

diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp
@@ -15,7 +15,6 @@
  */
 #include <array>
 #include <mavros/mavros_uas.h>
-#include <boost/math/constants/constants.hpp>
 
 using namespace mavros;
 
@@ -28,36 +27,6 @@ static const Eigen::Quaterniond FRAME_ROTATE_Q = UAS::quaternion_from_rpy(M_PI,
 static const Eigen::Transform<double, 3, Eigen::Affine> FRAME_TRANSFORM_VECTOR3(FRAME_ROTATE_Q);
 
 
-Eigen::Quaterniond UAS::quaternion_from_rpy(const Eigen::Vector3d &rpy)
-{
-#if 0
-	// RPY - XYZ
-	return Eigen::Quaterniond(
-			Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) *
-			Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
-			Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ())
-			);
-#else
-	// YPR - ZYX
-	return Eigen::Quaterniond(
-			Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) *
-			Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
-			Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX())
-			);
-#endif
-}
-
-Eigen::Vector3d UAS::quaternion_to_rpy(const Eigen::Quaterniond &q)
-{
-#if 0
-	// RPY - XYZ
-	return q.toRotationMatrix().eulerAngles(0, 1, 2);
-#else
-	// YPR - ZYX
-	return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
-#endif
-}
-
 Eigen::Quaterniond UAS::transform_frame(const Eigen::Quaterniond &q)
 {
 	return FRAME_ROTATE_Q * q * FRAME_ROTATE_Q.inverse();

diff --git a/mavros/src/lib/uas_quaternion_utils.cpp b/mavros/src/lib/uas_quaternion_utils.cpp
@@ -0,0 +1,54 @@
+/**
+ * @brief Eigen::Quaternion helter functions
+ * @file uas_quaternion_utils.cpp
+ * @author Vladimir Ermakov <vooon341@gmail.com>
+ *
+ * @addtogroup nodelib
+ * @{
+ */
+/*
+ * Copyright 2015 Vladimir Ermakov.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+#include <mavros/mavros_uas.h>
+
+using namespace mavros;
+
+/*
+ * Note: order of axis are match tf2::LinearMath (bullet).
+ * Compatibility checked by unittests.
+ */
+
+Eigen::Quaterniond UAS::quaternion_from_rpy(const Eigen::Vector3d &rpy)
+{
+#if 0
+	// RPY - XYZ
+	return Eigen::Quaterniond(
+			Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) *
+			Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
+			Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ())
+			);
+#else
+	// YPR - ZYX
+	return Eigen::Quaterniond(
+			Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) *
+			Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
+			Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX())
+			);
+#endif
+}
+
+Eigen::Vector3d UAS::quaternion_to_rpy(const Eigen::Quaterniond &q)
+{
+#if 0
+	// RPY - XYZ
+	return q.toRotationMatrix().eulerAngles(0, 1, 2);
+#else
+	// YPR - ZYX
+	return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
+#endif
+}
+