mirror of https://github.com/ArduPilot/ardupilot
194 lines
6.8 KiB
C++
194 lines
6.8 KiB
C++
|
#include <AP_gtest.h>
|
||
|
|
|
||
|
|
#include <AP_Math/AP_Math.h>
|
||
|
|
#include <AP_CustomRotations/AP_CustomRotations.h>
|
||
|
|
|
||
|
|
|
||
|
|
AP_CustomRotations cust_rot;
|
||
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||
|
|
|
||
|
|
|
||
|
|
static void test_euler(enum Rotation rotation, float roll, float pitch, float yaw)
|
||
|
|
{
|
||
|
|
Vector3f v, v1, v2, diff;
|
||
|
|
Matrix3f rotmat;
|
||
|
|
const float accuracy = 1.0e-6f;
|
||
|
|
|
||
|
|
v.x = 1;
|
||
|
|
v.y = 2;
|
||
|
|
v.z = 3;
|
||
|
|
v1 = v;
|
||
|
|
|
||
|
|
v1.rotate(rotation);
|
||
|
|
|
||
|
|
rotmat.from_euler(radians(roll), radians(pitch), radians(yaw));
|
||
|
|
v2 = v;
|
||
|
|
v2 = rotmat * v2;
|
||
|
|
|
||
|
|
diff = (v2 - v1);
|
||
|
|
EXPECT_LE(diff.length(), accuracy);
|
||
|
|
|
||
|
|
// quaternion rotation test
|
||
|
|
const float q_accuracy = 1.0e-3f;
|
||
|
|
Quaternion q, qe;
|
||
|
|
q.from_rotation(rotation);
|
||
|
|
qe.from_euler(radians(roll), radians(pitch), radians(yaw));
|
||
|
|
float q_roll, q_pitch, q_yaw, qe_roll, qe_pitch, qe_yaw;
|
||
|
|
q.to_euler(q_roll, q_pitch, q_yaw);
|
||
|
|
qe.to_euler(qe_roll, qe_pitch, qe_yaw);
|
||
|
|
float roll_diff = fabsf(wrap_PI(q_roll - qe_roll));
|
||
|
|
float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch));
|
||
|
|
float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw));
|
||
|
|
EXPECT_LE(roll_diff, q_accuracy);
|
||
|
|
EXPECT_LE(pitch_diff, q_accuracy);
|
||
|
|
EXPECT_LE(yaw_diff, q_accuracy);
|
||
|
|
|
||
|
|
// test custom rotations
|
||
|
|
AP::custom_rotations().set(ROTATION_CUSTOM_1, roll, pitch, yaw);
|
||
|
|
v1 = v;
|
||
|
|
v1.rotate(ROTATION_CUSTOM_1);
|
||
|
|
|
||
|
|
diff = (v2 - v1);
|
||
|
|
EXPECT_LE(diff.length(), accuracy);
|
||
|
|
|
||
|
|
Quaternion qc;
|
||
|
|
qc.from_rotation(ROTATION_CUSTOM_1);
|
||
|
|
float qc_roll, qc_pitch, qc_yaw;
|
||
|
|
qc.to_euler(qc_roll, qc_pitch, qc_yaw);
|
||
|
|
roll_diff = fabsf(wrap_PI(qc_roll - qe_roll));
|
||
|
|
pitch_diff = fabsf(wrap_PI(qc_pitch - qe_pitch));
|
||
|
|
yaw_diff = fabsf(wrap_PI(qc_yaw - qe_yaw));
|
||
|
|
EXPECT_LE(roll_diff, q_accuracy);
|
||
|
|
EXPECT_LE(pitch_diff, q_accuracy);
|
||
|
|
EXPECT_LE(yaw_diff, q_accuracy);
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(RotationsTest, TestEulers)
|
||
|
|
{
|
||
|
|
test_euler(ROTATION_NONE, 0, 0, 0);
|
||
|
|
test_euler(ROTATION_YAW_45, 0, 0, 45);
|
||
|
|
test_euler(ROTATION_YAW_90, 0, 0, 90);
|
||
|
|
test_euler(ROTATION_YAW_135, 0, 0, 135);
|
||
|
|
test_euler(ROTATION_YAW_180, 0, 0, 180);
|
||
|
|
test_euler(ROTATION_YAW_225, 0, 0, 225);
|
||
|
|
test_euler(ROTATION_YAW_270, 0, 0, 270);
|
||
|
|
test_euler(ROTATION_YAW_315, 0, 0, 315);
|
||
|
|
test_euler(ROTATION_ROLL_180, 180, 0, 0);
|
||
|
|
test_euler(ROTATION_ROLL_180_YAW_45, 180, 0, 45);
|
||
|
|
test_euler(ROTATION_ROLL_180_YAW_90, 180, 0, 90);
|
||
|
|
test_euler(ROTATION_ROLL_180_YAW_135, 180, 0, 135);
|
||
|
|
test_euler(ROTATION_PITCH_180, 0, 180, 0);
|
||
|
|
test_euler(ROTATION_ROLL_180_YAW_225, 180, 0, 225);
|
||
|
|
test_euler(ROTATION_ROLL_180_YAW_270, 180, 0, 270);
|
||
|
|
test_euler(ROTATION_ROLL_180_YAW_315, 180, 0, 315);
|
||
|
|
test_euler(ROTATION_ROLL_90, 90, 0, 0);
|
||
|
|
test_euler(ROTATION_ROLL_90_YAW_45, 90, 0, 45);
|
||
|
|
test_euler(ROTATION_ROLL_90_YAW_90, 90, 0, 90);
|
||
|
|
test_euler(ROTATION_ROLL_90_YAW_135, 90, 0, 135);
|
||
|
|
test_euler(ROTATION_ROLL_270, 270, 0, 0);
|
||
|
|
test_euler(ROTATION_ROLL_270_YAW_45, 270, 0, 45);
|
||
|
|
test_euler(ROTATION_ROLL_270_YAW_90, 270, 0, 90);
|
||
|
|
test_euler(ROTATION_ROLL_270_YAW_135, 270, 0, 135);
|
||
|
|
test_euler(ROTATION_PITCH_90, 0, 90, 0);
|
||
|
|
test_euler(ROTATION_PITCH_270, 0, 270, 0);
|
||
|
|
test_euler(ROTATION_PITCH_180_YAW_90, 0, 180, 90);
|
||
|
|
test_euler(ROTATION_PITCH_180_YAW_270, 0, 180, 270);
|
||
|
|
test_euler(ROTATION_ROLL_90_PITCH_90, 90, 90, 0);
|
||
|
|
test_euler(ROTATION_ROLL_180_PITCH_90,180, 90, 0);
|
||
|
|
test_euler(ROTATION_ROLL_270_PITCH_90,270, 90, 0);
|
||
|
|
test_euler(ROTATION_ROLL_90_PITCH_180, 90, 180, 0);
|
||
|
|
test_euler(ROTATION_ROLL_270_PITCH_180,270,180, 0);
|
||
|
|
test_euler(ROTATION_ROLL_90_PITCH_270, 90, 270, 0);
|
||
|
|
test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0);
|
||
|
|
test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0);
|
||
|
|
test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90);
|
||
|
|
test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270);
|
||
|
|
test_euler(ROTATION_ROLL_90_PITCH_68_YAW_293,90,68.8,293.3);
|
||
|
|
test_euler(ROTATION_ROLL_45,45,0,0);
|
||
|
|
test_euler(ROTATION_ROLL_315,315,0,0);
|
||
|
|
test_euler(ROTATION_PITCH_7, 0, 7, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
TEST(RotationsTest, TestRotationInverse)
|
||
|
|
{
|
||
|
|
// rotate inverse test(Vector (1,1,1))
|
||
|
|
Vector3f vec(1.0f,1.0f,1.0f), cmp_vec(1.0f, 1.0f, 1.0f);
|
||
|
|
for (enum Rotation r = ROTATION_NONE;
|
||
|
|
r < ROTATION_MAX;
|
||
|
|
r = (enum Rotation)((uint8_t)r+1)) {
|
||
|
|
vec.rotate(r);
|
||
|
|
vec.rotate_inverse(r);
|
||
|
|
EXPECT_LE((vec - cmp_vec).length(), 1e-5);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST(RotationsTest, TestRotateMatrix)
|
||
|
|
{
|
||
|
|
for (enum Rotation r = ROTATION_NONE;
|
||
|
|
r < ROTATION_MAX;
|
||
|
|
r = (enum Rotation)((uint8_t)r+1)) {
|
||
|
|
Vector3f vec(1,2,3);
|
||
|
|
Vector3f vec2 = vec;
|
||
|
|
vec.rotate(r);
|
||
|
|
Matrix3f m;
|
||
|
|
m.from_rotation(r);
|
||
|
|
vec2 = m * vec2;
|
||
|
|
EXPECT_LE((vec - vec2).length(), 1e-5);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||
|
|
TEST(RotationsTest, TestFailedGetLinux)
|
||
|
|
{
|
||
|
|
AP::custom_rotations().set(ROTATION_CUSTOM_OLD, 0, 0, 0);
|
||
|
|
Vector3f vec(1,2,3);
|
||
|
|
Vector3f vec2 = vec;
|
||
|
|
AP::custom_rotations().rotate(ROTATION_CUSTOM_OLD, vec2);
|
||
|
|
EXPECT_TRUE(vec == vec2);
|
||
|
|
Vector3d vecd(1,2,3);
|
||
|
|
Vector3d vecd2 = vecd;
|
||
|
|
AP::custom_rotations().rotate(ROTATION_CUSTOM_OLD, vecd2);
|
||
|
|
EXPECT_TRUE(vecd == vecd2);
|
||
|
|
Quaternion q(1.0f, 0.0f, 0.0f, 0.0f);
|
||
|
|
Quaternion q2(1.0f, 0.0f, 0.0f, 0.0f);
|
||
|
|
AP::custom_rotations().from_rotation(ROTATION_CUSTOM_OLD, q2);
|
||
|
|
for (int a = 0; a < 4; ++a) {
|
||
|
|
EXPECT_FLOAT_EQ(q[a], q2[a]);
|
||
|
|
}
|
||
|
|
QuaternionD qd(1.0, 0.0, 0.0, 0.0);
|
||
|
|
QuaternionD qd2(1.0, 0.0, 0.0, 0.0);
|
||
|
|
AP::custom_rotations().from_rotation(ROTATION_CUSTOM_OLD, qd2);
|
||
|
|
for (int a = 0; a < 4; ++a) {
|
||
|
|
EXPECT_FLOAT_EQ(qd[a], qd2[a]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
static void breakSingleton()
|
||
|
|
{
|
||
|
|
AP_CustomRotations cust_rot1;
|
||
|
|
}
|
||
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||
|
|
TEST(RotationsTest, TestFailedGetLinux)
|
||
|
|
{
|
||
|
|
EXPECT_EXIT(AP::custom_rotations().set(ROTATION_CUSTOM_OLD, 0, 0, 0), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation");
|
||
|
|
EXPECT_EXIT(AP::custom_rotations().set(ROTATION_CUSTOM_END, 0, 0, 0), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation");
|
||
|
|
EXPECT_EXIT(breakSingleton(), testing::KilledBySignal(SIGABRT), "AP_CustomRotations must be singleton");
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/*TEST(RotationsTest, TestRotateEqual)
|
||
|
|
{
|
||
|
|
for (enum Rotation r = (enum Rotation)((uint8_t)ROTATION_MAX-1); r > ROTATION_NONE; r = (enum Rotation)((uint8_t)r-1)) {
|
||
|
|
for (enum Rotation r2 = ROTATION_NONE; r2 < r; r2 = (enum Rotation)((uint8_t)r2+1)) {
|
||
|
|
if (rotation_equal(r,r2)) {
|
||
|
|
hal.console->printf("Rotation %i same as %i\n", r, r2);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}*/
|
||
|
|
|
||
|
|
AP_GTEST_PANIC()
|
||
|
|
AP_GTEST_MAIN()
|