mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Math: added comments and a test for euler ordering
our main euler functions did not have a comment on the ordering convention
This commit is contained in:
parent
f1a1b11830
commit
98733882f5
@ -235,13 +235,17 @@ public:
|
|||||||
return a.is_nan() || b.is_nan() || c.is_nan();
|
return a.is_nan() || b.is_nan() || c.is_nan();
|
||||||
}
|
}
|
||||||
|
|
||||||
// create a rotation matrix from Euler angles
|
/*
|
||||||
|
create a rotation matrix from Euler angles in 321 euler orderin
|
||||||
|
*/
|
||||||
void from_euler(T roll, T pitch, T yaw);
|
void from_euler(T roll, T pitch, T yaw);
|
||||||
|
|
||||||
// create eulers from a rotation matrix.
|
/* create eulers from a rotation matrix.
|
||||||
// roll is from -Pi to Pi
|
roll is from -Pi to Pi
|
||||||
// pitch is from -Pi/2 to Pi/2
|
pitch is from -Pi/2 to Pi/2
|
||||||
// yaw is from -Pi to Pi
|
yaw is from -Pi to Pi
|
||||||
|
euler order is 321
|
||||||
|
*/
|
||||||
void to_euler(T *roll, T *pitch, T *yaw) const;
|
void to_euler(T *roll, T *pitch, T *yaw) const;
|
||||||
|
|
||||||
// create matrix from rotation enum
|
// create matrix from rotation enum
|
||||||
|
@ -81,11 +81,11 @@ public:
|
|||||||
// convert a vector from earth to body frame
|
// convert a vector from earth to body frame
|
||||||
void earth_to_body(Vector3<T> &v) const;
|
void earth_to_body(Vector3<T> &v) const;
|
||||||
|
|
||||||
// create a quaternion from Euler angles
|
// create a quaternion from Euler angles using 321 euler ordering
|
||||||
void from_euler(T roll, T pitch, T yaw);
|
void from_euler(T roll, T pitch, T yaw);
|
||||||
void from_euler(const Vector3<T> &v);
|
void from_euler(const Vector3<T> &v);
|
||||||
|
|
||||||
// create a quaternion from Euler angles applied in yaw, roll, pitch order
|
// create a quaternion from Euler angles applied in yaw, roll, pitch order (312)
|
||||||
// instead of the normal yaw, pitch, roll order
|
// instead of the normal yaw, pitch, roll order
|
||||||
void from_vector312(T roll, T pitch, T yaw);
|
void from_vector312(T roll, T pitch, T yaw);
|
||||||
|
|
||||||
@ -129,7 +129,7 @@ public:
|
|||||||
// get euler yaw angle in radians
|
// get euler yaw angle in radians
|
||||||
T get_euler_yaw() const;
|
T get_euler_yaw() const;
|
||||||
|
|
||||||
// create eulers (in radians) from a quaternion
|
// create eulers (in radians) from a quaternion, using 321 ordering
|
||||||
void to_euler(float &roll, float &pitch, float &yaw) const;
|
void to_euler(float &roll, float &pitch, float &yaw) const;
|
||||||
void to_euler(Vector3f &rpy) const {
|
void to_euler(Vector3f &rpy) const {
|
||||||
to_euler(rpy.x, rpy.y, rpy.z);
|
to_euler(rpy.x, rpy.y, rpy.z);
|
||||||
@ -139,7 +139,7 @@ public:
|
|||||||
to_euler(rpy.x, rpy.y, rpy.z);
|
to_euler(rpy.x, rpy.y, rpy.z);
|
||||||
}
|
}
|
||||||
|
|
||||||
// create eulers from a quaternion
|
// create eulers from a quaternion with 312 ordering
|
||||||
Vector3<T> to_vector312(void) const;
|
Vector3<T> to_vector312(void) const;
|
||||||
|
|
||||||
T length_squared(void) const;
|
T length_squared(void) const;
|
||||||
|
@ -189,5 +189,83 @@ TEST(RotationsTest, TestFailedGetLinux)
|
|||||||
}
|
}
|
||||||
}*/
|
}*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
rotate a matrix using a give order, specified as a string
|
||||||
|
for example "321"
|
||||||
|
*/
|
||||||
|
static void rotate_ordered(Matrix3f &m, const char *order,
|
||||||
|
const float roll_deg,
|
||||||
|
const float pitch_deg,
|
||||||
|
const float yaw_deg)
|
||||||
|
{
|
||||||
|
while (*order) {
|
||||||
|
Matrix3f m2;
|
||||||
|
switch (*order) {
|
||||||
|
case '1':
|
||||||
|
m2.from_euler(radians(roll_deg), 0, 0);
|
||||||
|
break;
|
||||||
|
case '2':
|
||||||
|
m2.from_euler(0, radians(pitch_deg), 0);
|
||||||
|
break;
|
||||||
|
case '3':
|
||||||
|
m2.from_euler(0, 0, radians(yaw_deg));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
m *= m2;
|
||||||
|
order++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
test the two euler orders we use in ArduPilot
|
||||||
|
*/
|
||||||
|
TEST(RotationsTest, TestEulerOrder)
|
||||||
|
{
|
||||||
|
const float roll_deg = 20;
|
||||||
|
const float pitch_deg = 31;
|
||||||
|
const float yaw_deg = 72;
|
||||||
|
float r, p, y;
|
||||||
|
Vector3f v;
|
||||||
|
|
||||||
|
Matrix3f m;
|
||||||
|
|
||||||
|
// apply in 321 ordering
|
||||||
|
m.identity();
|
||||||
|
rotate_ordered(m, "321", roll_deg, pitch_deg, yaw_deg);
|
||||||
|
|
||||||
|
// get using to_euler
|
||||||
|
m.to_euler(&r, &p, &y);
|
||||||
|
|
||||||
|
EXPECT_FLOAT_EQ(degrees(r), roll_deg);
|
||||||
|
EXPECT_FLOAT_EQ(degrees(p), pitch_deg);
|
||||||
|
EXPECT_FLOAT_EQ(degrees(y), yaw_deg);
|
||||||
|
|
||||||
|
// get using to_euler312, should not match
|
||||||
|
v = m.to_euler312();
|
||||||
|
|
||||||
|
EXPECT_GE(fabsf(degrees(v.x)-roll_deg), 1);
|
||||||
|
EXPECT_GE(fabsf(degrees(v.y)-pitch_deg), 1);
|
||||||
|
EXPECT_GE(fabsf(degrees(v.z)-yaw_deg), 1);
|
||||||
|
|
||||||
|
// apply in 312 ordering
|
||||||
|
m.identity();
|
||||||
|
rotate_ordered(m, "312", roll_deg, pitch_deg, yaw_deg);
|
||||||
|
|
||||||
|
// get using to_euler312
|
||||||
|
v = m.to_euler312();
|
||||||
|
|
||||||
|
EXPECT_FLOAT_EQ(degrees(v.x), roll_deg);
|
||||||
|
EXPECT_FLOAT_EQ(degrees(v.y), pitch_deg);
|
||||||
|
EXPECT_FLOAT_EQ(degrees(v.z), yaw_deg);
|
||||||
|
|
||||||
|
// get using to_euler, should not match
|
||||||
|
m.to_euler(&r, &p, &y);
|
||||||
|
|
||||||
|
EXPECT_GE(fabsf(degrees(r)-roll_deg), 1);
|
||||||
|
EXPECT_GE(fabsf(degrees(p)-pitch_deg), 1);
|
||||||
|
EXPECT_GE(fabsf(degrees(y)-yaw_deg), 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
AP_GTEST_PANIC()
|
AP_GTEST_PANIC()
|
||||||
AP_GTEST_MAIN()
|
AP_GTEST_MAIN()
|
||||||
|
Loading…
Reference in New Issue
Block a user