diff --git a/libraries/AP_Math/examples/eulers/eulers.pde b/libraries/AP_Math/examples/eulers/eulers.pde index 6d986681f2..b95ada4e3d 100644 --- a/libraries/AP_Math/examples/eulers/eulers.pde +++ b/libraries/AP_Math/examples/eulers/eulers.pde @@ -239,10 +239,27 @@ void test_frame_transforms(void) 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); 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(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