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

AP_Math: add unit test for vector rotations 

This unit test already shows that rotation is wrong for
ROTATION_YAW_293_PITCH_68_ROLL_90.
This commit is contained in:
Gustavo Jose de Sousa 2015-11-03 10:17:28 -02:00 committed by Andrew Tridgell
parent 6b4a6f5389
commit 4266e924d0
2 changed files with 71 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
libraries/AP_Math/tests/test_vectors.cpp Normal file
View File

@ -0,0 +1,61 @@
#include <AP_gtest.h>
#include <AP_Math/AP_Math.h>
#define SQRT_2 1.4142135623730951f
TEST(VectorTest, Rotations)
{
unsigned rotation_count = 0;
#define TEST_ROTATION(rotation, x, y, z) { \
Vector3f v(1, 1, 1); \
v.rotate(rotation); \
EXPECT_EQ(Vector3f(x, y, z), v); \
rotation_count++; \
}
TEST_ROTATION(ROTATION_NONE, 1, 1, 1);
TEST_ROTATION(ROTATION_YAW_45, 0, SQRT_2, 1);
TEST_ROTATION(ROTATION_YAW_90, -1, 1, 1);
TEST_ROTATION(ROTATION_YAW_135, -SQRT_2, 0, 1);
TEST_ROTATION(ROTATION_YAW_180, -1, -1, 1);
TEST_ROTATION(ROTATION_YAW_225, 0, -SQRT_2, 1);
TEST_ROTATION(ROTATION_YAW_270, 1, -1, 1);
TEST_ROTATION(ROTATION_YAW_315, SQRT_2, 0, 1);
TEST_ROTATION(ROTATION_ROLL_180, 1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_180_YAW_45, SQRT_2, 0, -1);
TEST_ROTATION(ROTATION_ROLL_180_YAW_90, 1, 1, -1);
TEST_ROTATION(ROTATION_ROLL_180_YAW_135, 0, SQRT_2, -1);
TEST_ROTATION(ROTATION_PITCH_180, -1, 1, -1);
TEST_ROTATION(ROTATION_ROLL_180_YAW_225, -SQRT_2, 0, -1);
TEST_ROTATION(ROTATION_ROLL_180_YAW_270, -1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_180_YAW_315, 0, -SQRT_2, -1);
TEST_ROTATION(ROTATION_ROLL_90, 1, -1, 1);
TEST_ROTATION(ROTATION_ROLL_90_YAW_45, SQRT_2, 0, 1);
TEST_ROTATION(ROTATION_ROLL_90_YAW_90, 1, 1, 1);
TEST_ROTATION(ROTATION_ROLL_90_YAW_135, 0, SQRT_2, 1);
TEST_ROTATION(ROTATION_ROLL_270, 1, 1, -1);
TEST_ROTATION(ROTATION_ROLL_270_YAW_45, 0, SQRT_2, -1);
TEST_ROTATION(ROTATION_ROLL_270_YAW_90, -1, 1, -1);
TEST_ROTATION(ROTATION_ROLL_270_YAW_135, -SQRT_2, 0, -1);
TEST_ROTATION(ROTATION_PITCH_90, 1, 1, -1);
TEST_ROTATION(ROTATION_PITCH_270, -1, 1, 1);
TEST_ROTATION(ROTATION_PITCH_180_YAW_90, -1, -1, -1);
TEST_ROTATION(ROTATION_PITCH_180_YAW_270, 1, 1, -1);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_90, 1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_180_PITCH_90, -1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_270_PITCH_90, -1, 1, -1);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_180, -1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_270_PITCH_180, -1, 1, 1);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_270, -1, -1, 1);
TEST_ROTATION(ROTATION_ROLL_180_PITCH_270, 1, -1, 1);
TEST_ROTATION(ROTATION_ROLL_270_PITCH_270, 1, 1, 1);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_180_YAW_90, 1, -1, -1);
TEST_ROTATION(ROTATION_ROLL_90_YAW_270, -1, -1, 1);
TEST_ROTATION(ROTATION_YAW_293_PITCH_68_ROLL_90, -0.4118548f, -1.58903555f, -0.55257726f);
EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
}
AP_GTEST_MAIN()

10
libraries/AP_Math/tests/wscript Normal file
View File

@ -0,0 +1,10 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.find_tests(
bld,
use='ap',
)