fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

- better description for quaternion class

Browse Source 
- revert conversion functions to constructor
This commit is contained in:
Roman 2016-05-16 09:21:44 +02:00
parent 81eacb0cfa
commit f4e2b21608
3 changed files with 34 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
matrix/Dcm.hpp
View File

@ -78,32 +78,6 @@ public:
* @param q quaternion to set dcm to
*/
Dcm(const Quaternion<Type> & q) {
set_from_quaternion(q);
}
/**
* Constructor from euler angles
*
* This sets the transformation matrix from frame 2 to frame 1 where the rotation
* from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
*
*
* @param euler euler angle instance
*/
Dcm(const Euler<Type> & euler) {
set_from_euler(euler);
}
/**
* Set from quaternion
*
* Instance is set from quaternion representing
* transformation from frame 2 to frame 1.
*
* @param q quaternion to set dcm to
*/
void set_from_quaternion(const Quaternion<Type> & q) {
Dcm &dcm = *this;
Type a = q(0);
Type b = q(1);
@ -125,14 +99,15 @@ public:
}
/**
* Set from euler angles
* Constructor from euler angles
*
* This provides the transformation matrix from frame 2 to frame 1 where the rotation
* from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence
* This sets the transformation matrix from frame 2 to frame 1 where the rotation
* from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
*
* @param euler euler angle instannce
*
* @param euler euler angle instance
*/
void set_from_euler(const Euler<Type> & euler) {
Dcm(const Euler<Type> & euler) {
Dcm &dcm = *this;
Type cosPhi = Type(cos(euler.phi()));
Type sinPhi = Type(sin(euler.phi()));

59
matrix/Euler.hpp
View File

@ -82,7 +82,9 @@ public:
*/
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
{
set_from_euler(phi_, theta_, psi_);
phi() = phi_;
theta() = theta_;
psi() = psi_;
}
/**
@ -96,54 +98,6 @@ public:
* @param dcm Direction cosine matrix
*/
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{
set_from_dcm(dcm);
}
/**
* Constructor from quaternion instance.
*
* Instance is set from a quaternion representing transformation
* from frame 2 to frame 1.
* This instance will hold the angles defining the 3-2-1 intrinsic
* Tait-Bryan rotation sequence from frame 1 to frame 2.
*
* @param q quaternion
*/
Euler(const Quaternion<Type> & q) :
Vector<Type, 3>()
{
set_from_quaternion(q);
}
/**
* Set from euler angles
*
* Instance is set from an 3-2-1 intrinsic Tait-Bryan rotation
* sequence representing transformation from frame 1 to frame 2.
*
* @param phi_ rotation angle about X axis
* @param theta_ rotation angle about Y axis
* @param psi_ rotation angle about Z axis
*/
void set_from_euler(Type phi_, Type theta_, Type psi_)
{
phi() = phi_;
theta() = theta_;
psi() = psi_;
}
/**
* Set from DCM matrix
*
* Instance is set from Dcm representing transformation from
* frame 2 to frame 1.
* This instance will hold the angles defining the 3-2-1 intrinsic
* Tait-Bryan rotation sequence from frame 1 to frame 2.
*
* @param dcm Direction cosine matrix
*/
void set_from_dcm(const Dcm<Type> & dcm)
{
Type phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
Type theta_val = Type(asin(-dcm(2,0)));
@ -164,7 +118,7 @@ public:
}
/**
* Set from quaternion instance.
* Constructor from quaternion instance.
*
* Instance is set from a quaternion representing transformation
* from frame 2 to frame 1.
@ -172,8 +126,9 @@ public:
* Tait-Bryan rotation sequence from frame 1 to frame 2.
*
* @param q quaternion
*/
void set_from_quaternion(const Quaternion<Type> & q)
*/
Euler(const Quaternion<Type> & q) :
Vector<Type, 3>()
{
*this = Euler(Dcm<Type>(q));
}

80
matrix/Quaternion.hpp
View File

@ -3,8 +3,13 @@
*
* All rotations and axis systems follow the right-hand rule.
*
* A quaternion instance of this class describes a rotation from
* coordinate frame 1 to coordinate frame 2. The first element of the quaternion
* In order to rotate a vector v by a righthand rotation defined by the quaternion q
* one can use the following operation:
* v_rotated = q^(-1) * [0;v] * q
* where q^(-1) represents the inverse of the quaternion q.
* The product z of two quaternions z = q1 * q2 represents an intrinsic rotation
* in the order of first q1 followed by q2.
* The first element of the quaternion
* represents the real part, thus, a quaternion representing a zero-rotation
* is defined as (1,0,0,0).
*
@ -84,7 +89,15 @@ public:
Quaternion(const Dcm<Type> & dcm) :
Vector<Type, 4>()
{
set_from_dcm(dcm);
Quaternion &q = *this;
q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(4 * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(4 * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(4 * q(0)));
}
/**
@ -98,59 +111,6 @@ public:
*/
Quaternion(const Euler<Type> & euler) :
Vector<Type, 4>()
{
set_from_euler(euler);
}
/**
* Constructor from quaternion values
*
* Instance is initialized from quaternion values representing coordinate
* transformation from frame 2 to frame 1.
* A zero-rotation quaternion is represented by (1,0,0,0).
*
* @param a set quaternion value 0
* @param b set quaternion value 1
* @param c set quaternion value 2
* @param d set quaternion value 3
*/
Quaternion(Type a, Type b, Type c, Type d) :
Vector<Type, 4>()
{
set_from_quaternion(a, b, c, d);
}
/**
* Set from dcm
*
* Instance is set from a dcm representing coordinate transformation
* from frame 2 to frame 1.
*
* @param dcm dcm to set quaternion to
*/
void set_from_dcm(const Dcm<Type> & dcm)
{
Quaternion &q = *this;
q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(4 * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(4 * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(4 * q(0)));
}
/**
* Set from euler angles
*
* This sets the instance to a quaternion representing coordinate transformation from
* frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
* by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
*
* @param euler euler angles to set quaternion to
*/
void set_from_euler(const Euler<Type> & euler)
{
Quaternion &q = *this;
Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0));
@ -170,17 +130,19 @@ public:
}
/**
* Set from quaternion values
* Constructor from quaternion values
*
* Instance is set from quaternion values representing coordinate
* Instance is initialized from quaternion values representing coordinate
* transformation from frame 2 to frame 1.
* A zero-rotation quaternion is represented by (1,0,0,0).
*
* @param a set quaternion value 0
* @param b set quaternion value 1
* @param c set quaternion value 2
* @param d set quaternion value 3
*/
void set_from_quaternion(Type a, Type b, Type c, Type d)
Quaternion(Type a, Type b, Type c, Type d) :
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = a;