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

Math: any euler angle with pitch > 90 is invalid 

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 4bf28d499a
commit 0ec9857689
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 ||
rad_diff(pitch2, pitch) > 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) {
// we expect breakdown at these poles
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;
q.from_euler(roll, pitch, yaw);
q.to_euler(&roll2, &pitch2, &yaw2);
check_result(roll, pitch, yaw, roll2, pitch2, yaw2);
q.rotation_matrix(m);
m.to_euler(&roll2, &pitch2, &yaw2);
m2.from_euler(roll, pitch, yaw);
m2.to_euler(&roll3, &pitch3, &yaw3);
if (m.is_nan()) {
Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
check_result(roll, pitch, yaw, roll2, pitch2, yaw2);
check_result(roll, pitch, yaw, roll3, pitch3, yaw3);
}
@ -206,6 +213,9 @@ void setup(void)
{
Serial.begin(115200);
Serial.println("euler unit tests\n");
test_conversion(0, PI, 0);
test_frame_transforms();
test_conversions();
test_quaternion_eulers();