PX4 Firmware
PX4 Autopilot Software http://px4.io
Euler.hpp
Go to the documentation of this file.
1 /**
2  * @file Euler.hpp
3  *
4  * All rotations and axis systems follow the right-hand rule
5  *
6  * An instance of this class defines a rotation from coordinate frame 1 to coordinate frame 2.
7  * It follows the convention of a 3-2-1 intrinsic Tait-Bryan rotation sequence.
8  * In order to go from frame 1 to frame 2 we apply the following rotations consecutively.
9  * 1) We rotate about our initial Z axis by an angle of _psi.
10  * 2) We rotate about the newly created Y' axis by an angle of _theta.
11  * 3) We rotate about the newly created X'' axis by an angle of _phi.
12  *
13  * @author James Goppert <james.goppert@gmail.com>
14  */
15 
16 #pragma once
17 
18 #include "math.hpp"
19 
20 namespace matrix
21 {
22 
23 template <typename Type>
24 class Dcm;
25 
26 template <typename Type>
27 class Quaternion;
28 
29 /**
30  * Euler angles class
31  *
32  * This class describes the rotation from frame 1
33  * to frame 2 via 3-2-1 intrinsic Tait-Bryan rotation sequence.
34  */
35 template<typename Type>
36 class Euler : public Vector<Type, 3>
37 {
38 public:
39  /**
40  * Standard constructor
41  */
42  Euler() = default;
43 
44  /**
45  * Copy constructor
46  *
47  * @param other vector to copy
48  */
49  Euler(const Vector<Type, 3> &other) :
50  Vector<Type, 3>(other)
51  {
52  }
53 
54  /**
55  * Constructor from Matrix31
56  *
57  * @param other Matrix31 to copy
58  */
59  Euler(const Matrix<Type, 3, 1> &other) :
60  Vector<Type, 3>(other)
61  {
62  }
63 
64  /**
65  * Constructor from euler angles
66  *
67  * Instance is initialized from an 3-2-1 intrinsic Tait-Bryan
68  * rotation sequence representing transformation from frame 1
69  * to frame 2.
70  *
71  * @param phi_ rotation angle about X axis
72  * @param theta_ rotation angle about Y axis
73  * @param psi_ rotation angle about Z axis
74  */
75  Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
76  {
77  phi() = phi_;
78  theta() = theta_;
79  psi() = psi_;
80  }
81 
82  /**
83  * Constructor from DCM matrix
84  *
85  * Instance is set from Dcm representing transformation from
86  * frame 2 to frame 1.
87  * This instance will hold the angles defining the 3-2-1 intrinsic
88  * Tait-Bryan rotation sequence from frame 1 to frame 2.
89  *
90  * @param dcm Direction cosine matrix
91  */
92  Euler(const Dcm<Type> &dcm)
93  {
94  Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2)));
95  Type theta_val = Type(asin(-dcm(2, 0)));
96  Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0)));
97  Type pi = Type(M_PI);
98 
99  if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
100  phi_val = Type(0);
101  psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
102 
103  } else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
104  phi_val = Type(0);
105  psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
106  }
107 
108  phi() = phi_val;
109  theta() = theta_val;
110  psi() = psi_val;
111  }
112 
113  /**
114  * Constructor from quaternion instance.
115  *
116  * Instance is set from a quaternion representing transformation
117  * from frame 2 to frame 1.
118  * This instance will hold the angles defining the 3-2-1 intrinsic
119  * Tait-Bryan rotation sequence from frame 1 to frame 2.
120  *
121  * @param q quaternion
122  */
124  {
125  *this = Euler(Dcm<Type>(q));
126  }
127 
128  inline Type phi() const
129  {
130  return (*this)(0);
131  }
132  inline Type theta() const
133  {
134  return (*this)(1);
135  }
136  inline Type psi() const
137  {
138  return (*this)(2);
139  }
140 
141  inline Type &phi()
142  {
143  return (*this)(0);
144  }
145  inline Type &theta()
146  {
147  return (*this)(1);
148  }
149  inline Type &psi()
150  {
151  return (*this)(2);
152  }
153 
154 };
155 
157 
158 } // namespace matrix
159 
160 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
Type theta() const
Definition: Euler.hpp:132
Euler(Type phi_, Type theta_, Type psi_)
Constructor from euler angles.
Definition: Euler.hpp:75
Direction cosine matrix class.
Definition: AxisAngle.hpp:15
Type & phi()
Definition: Euler.hpp:141
Dual< Scalar, N > asin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:301
Type phi() const
Definition: Euler.hpp:128
Quaternion class.
Definition: Dcm.hpp:24
Type psi() const
Definition: Euler.hpp:136
Euler(const Dcm< Type > &dcm)
Constructor from DCM matrix.
Definition: Euler.hpp:92
Euler(const Vector< Type, 3 > &other)
Copy constructor.
Definition: Euler.hpp:49
Euler()=default
Standard constructor.
Euler< float > Eulerf
Definition: Euler.hpp:156
Euler angles class.
Definition: AxisAngle.hpp:18
Type & psi()
Definition: Euler.hpp:149
Euler(const Quaternion< Type > &q)
Constructor from quaternion instance.
Definition: Euler.hpp:123
Type & theta()
Definition: Euler.hpp:145
#define M_PI
Definition: gps_helper.cpp:38
Euler(const Matrix< Type, 3, 1 > &other)
Constructor from Matrix31.
Definition: Euler.hpp:59
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:325