mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-05 00:41:36 +08:00
- better description for quaternion class
- revert conversion functions to constructor
This commit is contained in:
+6
-31
@@ -78,32 +78,6 @@ public:
|
|||||||
* @param q quaternion to set dcm to
|
* @param q quaternion to set dcm to
|
||||||
*/
|
*/
|
||||||
Dcm(const Quaternion<Type> & q) {
|
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;
|
Dcm &dcm = *this;
|
||||||
Type a = q(0);
|
Type a = q(0);
|
||||||
Type b = q(1);
|
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
|
* 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
|
* 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;
|
Dcm &dcm = *this;
|
||||||
Type cosPhi = Type(cos(euler.phi()));
|
Type cosPhi = Type(cos(euler.phi()));
|
||||||
Type sinPhi = Type(sin(euler.phi()));
|
Type sinPhi = Type(sin(euler.phi()));
|
||||||
|
|||||||
+7
-52
@@ -82,7 +82,9 @@ public:
|
|||||||
*/
|
*/
|
||||||
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
|
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
|
* @param dcm Direction cosine matrix
|
||||||
*/
|
*/
|
||||||
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
|
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 phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
|
||||||
Type theta_val = Type(asin(-dcm(2,0)));
|
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
|
* Instance is set from a quaternion representing transformation
|
||||||
* from frame 2 to frame 1.
|
* from frame 2 to frame 1.
|
||||||
@@ -172,8 +126,9 @@ public:
|
|||||||
* Tait-Bryan rotation sequence from frame 1 to frame 2.
|
* Tait-Bryan rotation sequence from frame 1 to frame 2.
|
||||||
*
|
*
|
||||||
* @param q quaternion
|
* @param q quaternion
|
||||||
*/
|
*/
|
||||||
void set_from_quaternion(const Quaternion<Type> & q)
|
Euler(const Quaternion<Type> & q) :
|
||||||
|
Vector<Type, 3>()
|
||||||
{
|
{
|
||||||
*this = Euler(Dcm<Type>(q));
|
*this = Euler(Dcm<Type>(q));
|
||||||
}
|
}
|
||||||
|
|||||||
+21
-59
@@ -3,8 +3,13 @@
|
|||||||
*
|
*
|
||||||
* All rotations and axis systems follow the right-hand rule.
|
* All rotations and axis systems follow the right-hand rule.
|
||||||
*
|
*
|
||||||
* A quaternion instance of this class describes a rotation from
|
* In order to rotate a vector v by a righthand rotation defined by the quaternion q
|
||||||
* coordinate frame 1 to coordinate frame 2. The first element of the quaternion
|
* 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
|
* represents the real part, thus, a quaternion representing a zero-rotation
|
||||||
* is defined as (1,0,0,0).
|
* is defined as (1,0,0,0).
|
||||||
*
|
*
|
||||||
@@ -84,7 +89,15 @@ public:
|
|||||||
Quaternion(const Dcm<Type> & dcm) :
|
Quaternion(const Dcm<Type> & dcm) :
|
||||||
Vector<Type, 4>()
|
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) :
|
Quaternion(const Euler<Type> & euler) :
|
||||||
Vector<Type, 4>()
|
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;
|
Quaternion &q = *this;
|
||||||
Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0));
|
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.
|
* 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 a set quaternion value 0
|
||||||
* @param b set quaternion value 1
|
* @param b set quaternion value 1
|
||||||
* @param c set quaternion value 2
|
* @param c set quaternion value 2
|
||||||
* @param d set quaternion value 3
|
* @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;
|
Quaternion &q = *this;
|
||||||
q(0) = a;
|
q(0) = a;
|
||||||
|
|||||||
Reference in New Issue
Block a user