Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Math: added a test for the rotate() method

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-19 17:23:22 +11:00
parent 7d155c77b1
commit 9a40b4b1de
1 changed files with 47 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -212,6 +212,52 @@ void test_frame_transforms(void)
printf("%f %f %f\n", v2.x, v2.y, v2.z);
}
// generate a random float between -1 and 1
static float rand_num(void)
{
float ret = ((unsigned)random()) % 2000000;
return (ret - 1.0e6) / 1.0e6;
}
void test_matrix_rotate(void)
{
Matrix3f m1, m2, diff;
Vector3f r;
m1.identity();
m2.identity();
r.x = rand_num();
r.y = rand_num();
r.z = rand_num();
for (uint16_t i = 0; i<1000; i++) {
// old method
Matrix3f temp_matrix;
temp_matrix.a.x = 0;
temp_matrix.a.y = -r.z;
temp_matrix.a.z = r.y;
temp_matrix.b.x = r.z;
temp_matrix.b.y = 0;
temp_matrix.b.z = -r.x;
temp_matrix.c.x = -r.y;
temp_matrix.c.y = r.x;
temp_matrix.c.z = 0;
temp_matrix = m1 * temp_matrix;
m1 += temp_matrix;
// new method
m2.rotate(r);
// check they behave in the same way
diff = m1 - m2;
float err = diff.a.length() + diff.b.length() + diff.c.length();
if (err > 0) {
Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err);
}
}
}
/*
euler angle tests
*/
@ -226,6 +272,7 @@ void setup(void)
test_conversions();
test_quaternion_eulers();
test_matrix_eulers();
test_matrix_rotate();
}
void