Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: update the test suite

This commit is contained in:
Andrew Tridgell 2012-03-10 17:06:46 +11:00
parent 4422486d44
commit 4d3789d11c
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Math/examples/eulers/eulers.pde
View File

@ -66,9 +66,6 @@ static void check_result(float roll, float pitch, float yaw,
Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2)); ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
} }
} else {
Serial.printf("correct eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
} }
} }
@ -77,8 +74,8 @@ static void test_euler(float roll, float pitch, float yaw)
Matrix3f m; Matrix3f m;
float roll2, pitch2, yaw2; float roll2, pitch2, yaw2;
rotation_matrix_from_euler(m, roll, pitch, yaw); m.from_euler(roll, pitch, yaw);
calculate_euler_angles(m, &roll2, &pitch2, &yaw2); m.to_euler(&roll2, &pitch2, &yaw2);
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); check_result(roll, pitch, yaw, roll2, pitch2, yaw2);
} }
@ -107,8 +104,8 @@ static void test_quaternion(float roll, float pitch, float yaw)
Quaternion q; Quaternion q;
float roll2, pitch2, yaw2; float roll2, pitch2, yaw2;
quaternion_from_euler(q, roll, pitch, yaw); q.from_euler(roll, pitch, yaw);
euler_from_quaternion(q, &roll2, &pitch2, &yaw2); q.to_euler(&roll2, &pitch2, &yaw2);
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); check_result(roll, pitch, yaw, roll2, pitch2, yaw2);
} }
@ -152,17 +149,19 @@ static void test_conversion(float roll, float pitch, float yaw)
Matrix3f m, m2; Matrix3f m, m2;
float roll2, pitch2, yaw2; float roll2, pitch2, yaw2;
float roll3, pitch3, yaw3;
quaternion_from_euler(q, roll, pitch, yaw); q.from_euler(roll, pitch, yaw);
quaternion_to_rotation_matrix(q, m); q.rotation_matrix(m);
rotation_matrix_from_euler(m2, roll, pitch, yaw); m.to_euler(&roll2, &pitch2, &yaw2);
m2.from_euler(roll, pitch, yaw);
calculate_euler_angles(m, &roll2, &pitch2, &yaw2); m2.to_euler(&roll3, &pitch3, &yaw3);
if (m.is_nan()) { if (m.is_nan()) {
Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n", Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw); roll, pitch, yaw);
} }
check_result(roll, pitch, yaw, roll2, pitch2, yaw2); check_result(roll, pitch, yaw, roll2, pitch2, yaw2);
check_result(roll, pitch, yaw, roll3, pitch3, yaw3);
} }
void test_conversions(void) void test_conversions(void)
@ -194,9 +193,9 @@ void test_frame_transforms(void)
Serial.println("frame transform tests\n"); Serial.println("frame transform tests\n");
quaternion_from_euler(q, ToRad(90), 0, 0); q.from_euler(ToRad(90), 0, 0);
v2 = v = Vector3f(0, 0, 1); v2 = v = Vector3f(0, 0, 1);
quaternion_earth_to_body(q, v2); q.earth_to_body(v2);
printf("%f %f %f\n", v2.x, v2.y, v2.z); printf("%f %f %f\n", v2.x, v2.y, v2.z);
} }