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

AP_Math: add quaternion rotation test

This commit is contained in:
Randy Mackay 2020-04-07 14:53:05 +09:00
parent f78d8b009a
commit c18fce6714
1 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Math/examples/rotations/rotations.cpp
View File

@ -156,6 +156,25 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya
print_vector(v2);
hal.console->printf("\n");
}
// quaternion rotation test
Quaternion q;
q.from_rotation(rotation);
float q_roll, q_pitch, q_yaw;
q.to_euler(q_roll, q_pitch, q_yaw);
q_roll = degrees(wrap_2PI(q_roll));
q_pitch = degrees(wrap_2PI(q_pitch));
q_yaw = degrees(wrap_2PI(q_yaw));
if ((fabsf(q_roll - roll) > accuracy) || (fabsf(q_pitch - pitch) > accuracy) || (fabsf(q_yaw - yaw) > accuracy)) {
hal.console->printf("quaternion test %u failed : yaw:%d roll:%d pitch:%d vs yaw:%f roll:%f pitch:%f\n",
(unsigned)rotation,
(int)yaw,
(int)roll,
(int)pitch,
(double)q_yaw,
(double)q_roll,
(double)q_pitch);
}
}
static void test_rotate_inverse(void)