uas_quaternion_utils.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2015 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 #include <mavros/mavros_uas.h>
00017 
00018 using namespace mavros;
00019 
00020 /*
00021  * Note: order of axis are match tf2::LinearMath (bullet).
00022  * Compatibility checked by unittests.
00023  */
00024 
00025 Eigen::Quaterniond UAS::quaternion_from_rpy(const Eigen::Vector3d &rpy)
00026 {
00027         // YPR - ZYX
00028         return Eigen::Quaterniond(
00029                         Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) *
00030                         Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) *
00031                         Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX())
00032                         );
00033 }
00034 
00035 Eigen::Vector3d UAS::quaternion_to_rpy(const Eigen::Quaterniond &q)
00036 {
00037         // YPR - ZYX
00038         return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
00039 }
00040 
00041 double UAS::quaternion_get_yaw(const Eigen::Quaterniond &q)
00042 {
00043         // to match equation from:
00044         // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
00045         const double &q0 = q.w();
00046         const double &q1 = q.x();
00047         const double &q2 = q.y();
00048         const double &q3 = q.z();
00049 
00050         return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
00051 }


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19