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

Fixed euler angles.

Browse Source
This commit is contained in:
James Goppert
2016-01-24 01:50:34 -05:00
parent 6e6033a1af
commit 5aef810e7c
2 changed files with 48 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+16 -12
View File
@@ -48,20 +48,24 @@ public:
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{
theta() = (Type)asin(-dcm(2,0));
Type phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
Type theta_val = Type(asin(-dcm(2,0)));
Type psi_val = Type(atan2(dcm(1,0), dcm(0,0)));
Type pi = Type(M_PI);
if (fabs(theta() - (Type)M_PI_2) < 1.0e-3) {
phi() = (Type)0.0;
psi() = (Type)atan2(dcm(0,1), dcm(1,1));
psi() = (Type)atan2(dcm(1,2), dcm(0,2));
} else if ((Type)fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) {
phi() = (Type)0.0;
psi() = (Type)atan2(-dcm(1,2), -dcm(0,2));
} else {
phi() = (Type)atan2(dcm(2,1), dcm(2,2));
psi() = (Type)atan2(dcm(1,0), dcm(0,0));
if (fabs(theta_val - pi/2) < 1.0e-3) {
phi_val = Type(0.0);
psi_val = Type(atan2(dcm(1,2), dcm(0,2)));
} else if (Type(fabs(theta_val + pi/2) < Type(1.0e-3))) {
phi_val = Type(0.0);
psi_val = Type(atan2(-dcm(1,2), -dcm(0,2)));
}
if (psi_val < 0) psi_val += 2*pi;
phi() = phi_val;
theta() = theta_val;
psi() = psi_val;
}
Euler(const Quaternion<Type> & q) :
test/attitude.cpp
+32 -13
View File
@@ -91,27 +91,46 @@ int main()
double rad2deg = 180.0/M_PI;
// euler dcm round trip check
for (int roll=-90; roll<90; roll+=1) {
for (int pitch=-90; pitch<90; pitch+=1) {
for (int yaw=0; yaw<360; yaw+=1) {
for (int roll=-90; roll<=90; roll+=90) {
for (int pitch=-90; pitch<=90; pitch+=90) {
for (int yaw=1; yaw<=360; yaw+=90) {
// note if theta = pi/2, then roll is set to zero
if (pitch == 90 || pitch == -90) {
roll = 0;
int roll_expected = roll;
int yaw_expected = yaw;
if (pitch == 90) {
roll_expected = 0;
yaw_expected = yaw - roll;
} else if (pitch == -90) {
roll_expected = 0;
yaw_expected = yaw + roll;
}
printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler(deg2rad*double(roll),
deg2rad*double(pitch),
deg2rad*double(yaw));
Dcm<double> dcm_from_euler(euler);
Euler<double> euler_out(dcm_from_euler);
TEST(isEqual(rad2deg*euler, rad2deg*euler_out));
if (yaw_expected < 0) yaw_expected += 360;
if (yaw_expected > 360) yaw_expected -= 360;
printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler_expected(
deg2rad*double(roll_expected),
deg2rad*double(pitch),
deg2rad*double(yaw_expected));
Euler<double> euler(
deg2rad*double(roll),
deg2rad*double(pitch),
deg2rad*double(yaw));
Dcm<double> dcm_from_euler(euler);
dcm_from_euler.print();
Euler<double> euler_out(dcm_from_euler);
TEST(isEqual(rad2deg*euler_expected, rad2deg*euler_out));
Eulerf eulerf_expected(
float(deg2rad)*float(roll_expected),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw_expected));
Eulerf eulerf(float(deg2rad)*float(roll),
float(deg2rad)*float(pitch),
float(deg2rad)*float(yaw));
Dcm<float> dcm_from_eulerf(eulerf);
Euler<float> euler_outf(dcm_from_eulerf);
TEST(isEqual(float(rad2deg)*eulerf,
TEST(isEqual(float(rad2deg)*eulerf_expected,
float(rad2deg)*euler_outf));
}
}