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

AP_Math: expand frame transformation test case for quaternions

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-19 12:14:31 -07:00 committed by Andrew Tridgell
parent 3befe74afa
commit cdd2199138
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -239,10 +239,27 @@ void test_frame_transforms(void)
hal.console->println("frame transform tests\n"); hal.console->println("frame transform tests\n");
q.from_euler(ToRad(90), 0, 0); q.from_euler(ToRad(45), ToRad(45), ToRad(45));
q.normalize();
m.from_euler(ToRad(45), ToRad(45), ToRad(45));
v2 = v = Vector3f(0, 0, 1); v2 = v = Vector3f(0, 0, 1);
q.earth_to_body(v2); q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z); hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
v2 = m * v;
hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z);
v2 = v = Vector3f(0, 1, 0);
q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
v2 = m * v;
hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z);
v2 = v = Vector3f(1, 0, 0);
q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
v2 = m * v;
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
} }
// generate a random float between -1 and 1 // generate a random float between -1 and 1