diff options
author | tumbili <bapstr@ethz.ch> | 2015-02-23 22:15:47 +0100 |
---|---|---|
committer | tumbili <bapstr@ethz.ch> | 2015-02-23 22:18:51 +0100 |
commit | 41377709e630943593bca810295df9f488602831 (patch) | |
tree | 5f765aa9f22de09fd4aa1ffc950836d0694dc095 | |
parent | 9a875c53af750e115529b1bb64269d2ded712c46 (diff) | |
download | px4-firmware-41377709e630943593bca810295df9f488602831.tar.gz px4-firmware-41377709e630943593bca810295df9f488602831.tar.bz2 px4-firmware-41377709e630943593bca810295df9f488602831.zip |
added test for quaternion methods
-rw-r--r-- | src/systemcmds/tests/test_mathlib.cpp | 67 |
1 files changed, 67 insertions, 0 deletions
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 9fa52aaaa..f56667d67 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -205,5 +205,72 @@ int test_mathlib(int argc, char *argv[]) } + { + // test conversion rotation matrix to quaternion and back + math::Matrix<3,3> R_orig; + math::Matrix<3,3> R; + math::Quaternion q; + float diff = 0.1f; + float tol = 0.00001f; + + for(float roll = -M_PI_F;roll <= M_PI_F;roll+=diff) { + for(float pitch = -M_PI_2_F;pitch <= M_PI_2_F;pitch+=diff) { + for(float yaw = -M_PI_F;yaw <= M_PI_F;yaw+=diff) { + R_orig.from_euler(roll,pitch,yaw); + q.from_dcm(R_orig); + R = q.to_dcm(); + for(int i = 0;i< 3;i++) { + for(int j=0;j<3;j++) { + if(fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + math::Quaternion q_true = {1.0f,0.0f,0.0f,0.0f}; + R_orig.identity(); + q.from_dcm(R_orig); + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(0.3f,0.2f,0.1f); + q = {0.9833f,0.1436f,0.1060f,0.0343f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(-1.5f,-0.2f,0.5f); + q = {0.7222f,-0.6391f,-0.2386f,0.1142f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3); + q = {0.6830f,0.1830f,-0.6830f,0.1830f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + } + return rc; } |