Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

Math: any euler angle with pitch > 90 is invalid

Browse Source 
both DCM and quaternion always return pitch in the range -90 to 90
This commit is contained in:
Andrew Tridgell 2012-03-10 17:29:32 +11:00
parent 36e92bf4ba
commit cc1961b1e8
1 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -57,7 +57,9 @@ static void check_result(float roll, float pitch, float yaw,
if (rad_diff(roll2,roll) > 0.01 || if (rad_diff(roll2,roll) > 0.01 ||
rad_diff(pitch2, pitch) > 0.01 || rad_diff(pitch2, pitch) > 0.01 ||
rad_diff(yaw2, yaw) > 0.01) { rad_diff(yaw2, yaw) > 0.01) {
if (ToDeg(rad_diff(pitch, PI/2)) < 1 || if (pitch >= PI/2 ||
pitch <= -PI/2 ||
ToDeg(rad_diff(pitch, PI/2)) < 1 ||
ToDeg(rad_diff(pitch, -PI/2)) < 1) { ToDeg(rad_diff(pitch, -PI/2)) < 1) {
// we expect breakdown at these poles // we expect breakdown at these poles
Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
@ -152,14 +154,19 @@ static void test_conversion(float roll, float pitch, float yaw)
float roll3, pitch3, yaw3; float roll3, pitch3, yaw3;
q.from_euler(roll, pitch, yaw); q.from_euler(roll, pitch, yaw);
q.to_euler(&roll2, &pitch2, &yaw2);
check_result(roll, pitch, yaw, roll2, pitch2, yaw2);
q.rotation_matrix(m); q.rotation_matrix(m);
m.to_euler(&roll2, &pitch2, &yaw2); m.to_euler(&roll2, &pitch2, &yaw2);
m2.from_euler(roll, pitch, yaw); m2.from_euler(roll, pitch, yaw);
m2.to_euler(&roll3, &pitch3, &yaw3); 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); check_result(roll, pitch, yaw, roll3, pitch3, yaw3);
} }
@ -206,6 +213,9 @@ void setup(void)
{ {
Serial.begin(115200); Serial.begin(115200);
Serial.println("euler unit tests\n"); Serial.println("euler unit tests\n");
test_conversion(0, PI, 0);
test_frame_transforms(); test_frame_transforms();
test_conversions(); test_conversions();
test_quaternion_eulers(); test_quaternion_eulers();