You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
678 lines
25 KiB
678 lines
25 KiB
|
|
// given we are in the Math library, you're epected to know what |
|
// you're doing when directly comparing floats: |
|
#pragma GCC diagnostic push |
|
#pragma GCC diagnostic ignored "-Wfloat-equal" |
|
|
|
#include <AP_gtest.h> |
|
|
|
#include <AP_Math/AP_Math.h> |
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); |
|
|
|
#define SQRT_2 1.4142135623730950488016887242097 |
|
|
|
TEST(VectorTest, Rotations) |
|
{ |
|
unsigned rotation_count = 0; |
|
|
|
#define TEST_ROTATION(rotation, _x, _y, _z) { \ |
|
const float accuracy = 1.0e-6; \ |
|
Vector3f v(1, 1, 1); \ |
|
Vector3f v2 = v; \ |
|
v.rotate(rotation); \ |
|
Vector3f expected(_x, _y, _z); \ |
|
EXPECT_NEAR(expected.length(), v.length(), accuracy); \ |
|
EXPECT_FLOAT_EQ(expected.x, v.x); \ |
|
EXPECT_FLOAT_EQ(expected.y, v.y); \ |
|
EXPECT_FLOAT_EQ(expected.z, v.z); \ |
|
Quaternion quat; \ |
|
quat.from_rotation(rotation); \ |
|
quat.earth_to_body(v2); \ |
|
EXPECT_NEAR(expected.length(), v.length(), accuracy); \ |
|
EXPECT_NEAR(expected.x, v2.x, accuracy); \ |
|
EXPECT_NEAR(expected.y, v2.y, accuracy); \ |
|
EXPECT_NEAR(expected.z, v2.z, accuracy); \ |
|
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_ROLL_90_PITCH_68_YAW_293, -0.40663092252764576617352076937095, -1.5839677018260314156350432313047, -0.57069923113341980425161636958364); |
|
TEST_ROTATION(ROTATION_PITCH_315, 0, 1, SQRT_2); |
|
TEST_ROTATION(ROTATION_ROLL_90_PITCH_315, 0, -1, SQRT_2); |
|
TEST_ROTATION(ROTATION_PITCH_7, 1.1144154950464695286171945554088, 1, 0.87067680823617454866081288855639); |
|
TEST_ROTATION(ROTATION_ROLL_45, 1, 0, SQRT_2); |
|
TEST_ROTATION(ROTATION_ROLL_315, 1, SQRT_2, 0); |
|
EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested"; |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
|
TEST_ROTATION(ROTATION_CUSTOM, 1, 1, 1); |
|
TEST_ROTATION(ROTATION_MAX, 1, 1, 1); |
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
Vector3F v {1, 1, 1}; |
|
EXPECT_EXIT(v.rotate(ROTATION_CUSTOM), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::flow_of_ctrl"); |
|
EXPECT_EXIT(v.rotate(ROTATION_MAX), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation"); |
|
#endif |
|
} |
|
|
|
TEST(MathTest, IsZero) |
|
{ |
|
EXPECT_FALSE(is_zero(0.1f)); |
|
EXPECT_FALSE(is_zero(0.0001f)); |
|
EXPECT_TRUE(is_zero(0.0f)); |
|
EXPECT_TRUE(is_zero(FLT_MIN)); |
|
EXPECT_TRUE(is_zero(-FLT_MIN)); |
|
AP_Float t_float; |
|
t_float = 0.1f; |
|
EXPECT_FALSE(is_zero(t_float)); |
|
t_float = 0.0f; |
|
EXPECT_TRUE(is_zero(t_float)); |
|
} |
|
|
|
TEST(MathTest, IsPositive) |
|
{ |
|
EXPECT_TRUE(is_positive(1.0f)); |
|
EXPECT_TRUE(is_positive(FLT_EPSILON)); |
|
EXPECT_FALSE(is_positive(0.0f)); |
|
EXPECT_FALSE(is_positive(-1.0f)); |
|
AP_Float t_float; |
|
t_float = 0.1f; |
|
EXPECT_TRUE(is_positive(t_float)); |
|
t_float = -0.1f; |
|
EXPECT_FALSE(is_positive(t_float)); |
|
} |
|
|
|
TEST(MathTest, IsNegative) |
|
{ |
|
EXPECT_TRUE(is_negative(-1.0f * FLT_EPSILON)); |
|
EXPECT_TRUE(is_negative(-1.0f)); |
|
EXPECT_FALSE(is_negative(0.0f)); |
|
EXPECT_FALSE(is_negative(1.0f)); |
|
AP_Float t_float; |
|
t_float = 0.1f; |
|
EXPECT_FALSE(is_negative(t_float)); |
|
t_float = -0.1f; |
|
EXPECT_TRUE(is_negative(t_float)); |
|
} |
|
|
|
TEST(MathTest, MIN) |
|
{ |
|
const bool testb = true; |
|
EXPECT_EQ(1, MIN(testb, 2)); |
|
const uint8_t testui8 = 1; |
|
EXPECT_EQ(1u, MIN(testui8, 2u)); |
|
EXPECT_EQ(1u, MIN(testui8, testui8 + 2u)); |
|
const signed char testcc1 = 1; |
|
const signed char testcc2 = 2; |
|
EXPECT_EQ(1, MIN(testcc1, 2)); |
|
EXPECT_EQ(1, MIN(testcc1, testcc2)); |
|
const unsigned char testc = 1; |
|
const unsigned char testc2 = 2; |
|
EXPECT_EQ(1u, MIN(testc, 2u)); |
|
EXPECT_EQ(1u, MIN(testc, testc2)); |
|
const unsigned int testuint = 1; |
|
EXPECT_EQ(1u, MIN(testuint, 2u)); |
|
EXPECT_EQ(1u, MIN(testuint, testuint + 2u)); |
|
const unsigned long testulong = 1; |
|
EXPECT_EQ(1u, MIN(testulong, 2u)); |
|
EXPECT_EQ(1u, MIN(testulong, testulong + 2u)); |
|
const unsigned short testushort = 1; |
|
const unsigned short testushort2 = 2; |
|
EXPECT_EQ(1u, MIN(testushort, 2u)); |
|
EXPECT_EQ(1u, MIN(testushort, testushort2)); |
|
EXPECT_EQ(1u, MIN(testc, testulong)); |
|
EXPECT_EQ(1, MIN(2, testc)); |
|
EXPECT_EQ(1u, MIN(testuint, 2.0f)); |
|
EXPECT_EQ(1u, MIN(testuint, testulong)); |
|
EXPECT_EQ(1u, MIN(testuint, testc)); |
|
EXPECT_EQ(1u, MIN(testulong, testc)); |
|
EXPECT_EQ(1u, MIN(testulong, testuint)); |
|
EXPECT_EQ(1u, MIN(testulong, testushort)); |
|
EXPECT_EQ(1, MIN(testushort, 1)); |
|
EXPECT_EQ(1u, MIN(testushort, testulong)); |
|
const int8_t testi8 = 1; |
|
EXPECT_EQ(1, MIN(testi8, 2)); |
|
EXPECT_EQ(1, MIN(testui8, testi8 + 2)); |
|
const uint16_t testui16 = 1; |
|
EXPECT_EQ(1u, MIN(testui16, 2u)); |
|
EXPECT_EQ(1u, MIN(testui16, testui16 + 2u)); |
|
const int16_t testi16 = 1; |
|
EXPECT_EQ(1, MIN(testi16, 2)); |
|
EXPECT_EQ(1, MIN(testi16, testi16 + 2)); |
|
const uint32_t testui32 = 1; |
|
EXPECT_EQ(1u, MIN(testui32, 2u)); |
|
EXPECT_EQ(1u, MIN(testui32, testui32 + 2u)); |
|
const int32_t testi32 = 1; |
|
EXPECT_EQ(1, MIN(testi32, 2)); |
|
EXPECT_EQ(1, MIN(testi32, 2.0f)); |
|
EXPECT_EQ(1.0f, MIN(1.0f, 2.0f)); |
|
EXPECT_EQ(1.0f, MIN(1.0f, 2)); |
|
EXPECT_EQ(1.0f, MIN(2.0f, 1.0f)); |
|
} |
|
|
|
TEST(MathTest, MAX) |
|
{ |
|
const bool testb = true; |
|
EXPECT_EQ(2, MAX(testb, 2)); |
|
const unsigned char testc = 1; |
|
const unsigned char testc2 = 2; |
|
EXPECT_EQ(2u, MAX(testc, 2u)); |
|
EXPECT_EQ(2u, MAX(testc, testc2)); |
|
const uint8_t testui8 = 1; |
|
EXPECT_EQ(2u, MAX(testui8, 2u)); |
|
const int8_t testi8 = 1; |
|
EXPECT_EQ(2, MAX(testi8, 2)); |
|
const uint16_t testui16 = 1; |
|
EXPECT_EQ(2u, MAX(testui16, 2u)); |
|
const int16_t testi16 = 1; |
|
EXPECT_EQ(2, MAX(testi16, 2)); |
|
const uint32_t testui32 = 1; |
|
EXPECT_EQ(2u, MAX(testui32, 2u)); |
|
const int32_t testi32 = 1; |
|
EXPECT_EQ(2, MAX(testi32, 2)); |
|
const unsigned long testulong = 1; |
|
EXPECT_EQ(2u, MAX(testulong, 2u)); |
|
EXPECT_EQ(3u, MAX(testulong, testulong + 2u)); |
|
const unsigned short testushort = 1; |
|
const unsigned short testushort2 = 2; |
|
EXPECT_EQ(2u, MAX(testushort, 2u)); |
|
EXPECT_EQ(2u, MAX(testushort, testushort2)); |
|
EXPECT_EQ(2.0f, MAX(testushort, 2.0f)); |
|
EXPECT_EQ(2.0f, MAX(testi32, 2.0f)); |
|
AP_Float t_float; |
|
t_float = 0.1f; |
|
EXPECT_EQ(2.0f, MAX(t_float, 2.0f)); |
|
EXPECT_EQ(2.0f, MAX(2.0f, t_float)); |
|
AP_Int8 t_int8; |
|
t_int8 = 1; |
|
EXPECT_EQ(2, MAX(t_int8, 2)); |
|
EXPECT_EQ(2, MAX(2, t_int8)); |
|
AP_Int16 t_int16; |
|
t_int16 = 1; |
|
EXPECT_EQ(2, MAX(t_int16, 2)); |
|
AP_Int32 t_int32; |
|
t_int32 = 1; |
|
EXPECT_EQ(2, MAX(t_int32, 2)); |
|
EXPECT_EQ(2.0f, MAX(1.0f, 2.0f)); |
|
EXPECT_EQ(2.0f, MAX(1.0f, 2)); |
|
EXPECT_EQ(2.0f, MAX(2.0f, 1.0f)); |
|
} |
|
|
|
TEST(MathTest, Convert) |
|
{ |
|
EXPECT_TRUE(1000000 == hz_to_nsec(1000u)); |
|
EXPECT_TRUE(1000000 == nsec_to_hz(1000u)); |
|
EXPECT_TRUE(1 == nsec_to_usec(1000u)); |
|
EXPECT_TRUE(1000 == hz_to_usec(1000u)); |
|
EXPECT_TRUE(1000 == usec_to_hz(1000u)); |
|
} |
|
|
|
TEST(MathTest, IsEqual) |
|
{ |
|
EXPECT_FALSE(is_equal(1, 0)); |
|
EXPECT_TRUE(is_equal(1, 1)); |
|
EXPECT_FALSE(is_equal(0.1, 0.10001)); |
|
EXPECT_FALSE(is_equal(0.1, -0.1001)); |
|
EXPECT_TRUE(is_equal(0.f, 0.0f)); |
|
EXPECT_FALSE(is_equal(1.f, 1.f + FLT_EPSILON)); |
|
EXPECT_TRUE(is_equal(1.f, 1.f + FLT_EPSILON / 2.f)); |
|
EXPECT_TRUE(is_equal(1.f, (float)(1.f - DBL_EPSILON))); |
|
|
|
// false because the common type is double |
|
EXPECT_FALSE(is_equal(double(1.), 1 + 2 * std::numeric_limits<double>::epsilon())); |
|
|
|
// true because the common type is float |
|
EXPECT_TRUE(is_equal(1.f, (float)(1. + std::numeric_limits<double>::epsilon()))); |
|
|
|
EXPECT_TRUE(is_equal(short(1), short(1))); |
|
EXPECT_TRUE(is_equal(long(1), long(1))); |
|
} |
|
|
|
TEST(MathTest, Square) |
|
{ |
|
float sq_0 = sq(0); |
|
float sq_1 = sq(1); |
|
float sq_2 = sq(2); |
|
|
|
EXPECT_EQ(0.f, sq_0); |
|
EXPECT_EQ(1.f, sq_1); |
|
EXPECT_EQ(4.f, sq_2); |
|
AP_Float t_sqfloat; |
|
t_sqfloat = sq(2); |
|
EXPECT_EQ(4.f, t_sqfloat); |
|
} |
|
|
|
TEST(MathTest, Norm) |
|
{ |
|
float norm_1 = norm(1, 4.2); |
|
float norm_2 = norm(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1); |
|
float norm_3 = norm(0, 5.3); |
|
float norm_4 = norm(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0); |
|
float norm_5 = norm(3,4); |
|
float norm_6 = norm(4,3,12); |
|
AP_Float t_float1, t_float2, t_float3; |
|
t_float1 = 4.0f; |
|
t_float2 = 3.0f; |
|
t_float3 = 12.f; |
|
float norm_7 = norm(t_float1, t_float2, t_float3); |
|
|
|
EXPECT_FLOAT_EQ(norm_1, 4.3174066f); |
|
EXPECT_EQ(norm_2, 4.f); |
|
EXPECT_EQ(norm_3, 5.3f); |
|
EXPECT_EQ(norm_4, 0.f); |
|
EXPECT_EQ(norm_5, 5.f); |
|
EXPECT_EQ(norm_6, 13.f); |
|
EXPECT_EQ(norm_7, 13.f); |
|
} |
|
|
|
|
|
TEST(MathTest, Constrain) |
|
{ |
|
for (int i = 0; i < 1000; i++) { |
|
if (i < 250) { |
|
EXPECT_EQ(250, constrain_float(i, 250, 500)); |
|
EXPECT_EQ(250, constrain_int16(i, 250, 500)); |
|
EXPECT_EQ(250, constrain_int32(i, 250, 500)); |
|
EXPECT_EQ(250, constrain_int64(i, 250, 500)); |
|
} else if (i > 500) { |
|
EXPECT_EQ(500, constrain_float(i, 250, 500)); |
|
EXPECT_EQ(500, constrain_int16(i, 250, 500)); |
|
EXPECT_EQ(500, constrain_int32(i, 250, 500)); |
|
EXPECT_EQ(500, constrain_int64(i, 250, 500)); |
|
} else { |
|
EXPECT_EQ(i, constrain_float(i, 250, 500)); |
|
EXPECT_EQ(i, constrain_int16(i, 250, 500)); |
|
EXPECT_EQ(i, constrain_int32(i, 250, 500)); |
|
EXPECT_EQ(i, constrain_int64(i, 250, 500)); |
|
} |
|
} |
|
|
|
for (int i = 0; i <= 1000; i++) { |
|
int c = i - 1000; |
|
if (c < -250) { |
|
EXPECT_EQ(-250, constrain_float(c, -250, -50)); |
|
EXPECT_EQ(-250, constrain_int16(c, -250, -50)); |
|
EXPECT_EQ(-250, constrain_int32(c, -250, -50)); |
|
EXPECT_EQ(-250, constrain_int64(c, -250, -50)); |
|
} else if(c > -50) { |
|
EXPECT_EQ(-50, constrain_float(c, -250, -50)); |
|
EXPECT_EQ(-50, constrain_int16(c, -250, -50)); |
|
EXPECT_EQ(-50, constrain_int32(c, -250, -50)); |
|
EXPECT_EQ(-50, constrain_int64(c, -250, -50)); |
|
} else { |
|
EXPECT_EQ(c, constrain_float(c, -250, -50)); |
|
EXPECT_EQ(c, constrain_int16(c, -250, -50)); |
|
EXPECT_EQ(c, constrain_int32(c, -250, -50)); |
|
EXPECT_EQ(c, constrain_int64(c, -250, -50)); |
|
} |
|
} |
|
|
|
for (int i = 0; i <= 2000; i++) { |
|
int c = i - 1000; |
|
if (c < -250) { |
|
EXPECT_EQ(-250, constrain_float(c, -250, 50)); |
|
EXPECT_EQ(-250, constrain_int16(c, -250, 50)); |
|
EXPECT_EQ(-250, constrain_int32(c, -250, 50)); |
|
EXPECT_EQ(-250, constrain_int64(c, -250, 50)); |
|
} else if(c > 50) { |
|
EXPECT_EQ(50, constrain_float(c, -250, 50)); |
|
EXPECT_EQ(50, constrain_int16(c, -250, 50)); |
|
EXPECT_EQ(50, constrain_int32(c, -250, 50)); |
|
EXPECT_EQ(50, constrain_int64(c, -250, 50)); |
|
} else { |
|
EXPECT_EQ(c, constrain_float(c, -250, 50)); |
|
EXPECT_EQ(c, constrain_int16(c, -250, 50)); |
|
EXPECT_EQ(c, constrain_int32(c, -250, 50)); |
|
EXPECT_EQ(c, constrain_int64(c, -250, 50)); |
|
} |
|
} |
|
|
|
EXPECT_EQ(20.0, constrain_value(20.0, 19.9, 20.1)); |
|
EXPECT_EQ(20.0f, constrain_value(20.0f, 19.9f, 20.1f)); |
|
EXPECT_EQ((long long int)20, constrain_value((long long int)20, (long long int)19, (long long int)20)); |
|
|
|
EXPECT_EQ(20.1, constrain_value(21.0, 19.9, 20.1)); |
|
EXPECT_EQ(20.1f, constrain_value(21.0f, 19.9f, 20.1f)); |
|
|
|
EXPECT_EQ(19.9, constrain_value(19.9, 19.9, 20.1)); |
|
EXPECT_EQ(19.9f, constrain_value(19.9f, 19.9f, 20.1f)); |
|
|
|
EXPECT_EQ(19.9, constrain_value(19.8, 19.9, 20.1)); |
|
EXPECT_EQ(19.9f, constrain_value(19.8f, 19.9f, 20.1f)); |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
|
EXPECT_EQ(1.0f, constrain_float(nanf("0x4152"), 1.0f, 1.0f)); |
|
EXPECT_EQ(1.0f, constrain_value(nanf("0x4152"), 1.0f, 1.0f)); |
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
EXPECT_EXIT(constrain_float(nanf("0x4152"), 1.0f, 1.0f), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::cnstring_nan"); |
|
EXPECT_EXIT(constrain_value(nanf("0x4152"), 1.0f, 1.0f), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::cnstring_nan"); |
|
#endif |
|
} |
|
|
|
TEST(MathWrapTest, Angle180) |
|
{ |
|
// Full circle test |
|
for (int32_t i = 0; i < 36000; i += 100) { |
|
if (i < 18000) { |
|
// smaller pole position |
|
EXPECT_EQ(i, wrap_180_cd(i)); |
|
EXPECT_EQ(-i, wrap_180_cd(-i)); |
|
EXPECT_EQ(i, wrap_180_cd((long)i)); |
|
EXPECT_EQ(-i, wrap_180_cd((long)-i)); |
|
} else if (i == 18000) { |
|
// hit pole position -180/+180 degree |
|
EXPECT_EQ(i, wrap_180_cd(i)); |
|
EXPECT_EQ(i, wrap_180_cd(-i)); |
|
EXPECT_EQ(i, wrap_180_cd((long)i)); |
|
EXPECT_EQ(i, wrap_180_cd((long)-i)); |
|
} else { |
|
// bigger pole position |
|
EXPECT_EQ(-(36000 - i), wrap_180_cd(i)); |
|
EXPECT_EQ(36000 - i, wrap_180_cd(-i)); |
|
EXPECT_EQ(-(36000 - i), wrap_180_cd((long)i)); |
|
EXPECT_EQ(36000 - i, wrap_180_cd((long)-i)); |
|
} |
|
} |
|
|
|
EXPECT_EQ(4500.f, wrap_180_cd(4500.f)); |
|
EXPECT_EQ(9000.f, wrap_180_cd(9000.f)); |
|
EXPECT_EQ(18000.f, wrap_180_cd(18000.f)); |
|
EXPECT_EQ(-17990.f, wrap_180_cd(18010.f)); |
|
EXPECT_EQ(-9000.f, wrap_180_cd(27000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(36000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(72000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(360000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(720000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(-3600000000.f)); |
|
|
|
EXPECT_EQ(-4500.f, wrap_180_cd(-4500.f)); |
|
EXPECT_EQ(-9000.f, wrap_180_cd(-9000.f)); |
|
EXPECT_EQ(18000.f, wrap_180_cd(-18000.f)); |
|
EXPECT_EQ(17990.f, wrap_180_cd(-18010.f)); |
|
EXPECT_EQ(9000.f, wrap_180_cd(-27000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(-36000.f)); |
|
EXPECT_EQ(0.f, wrap_180_cd(-72000.f)); |
|
|
|
EXPECT_EQ(4500, wrap_180_cd((short)4500)); |
|
EXPECT_EQ(-4500, wrap_180_cd((short)-4500)); |
|
|
|
EXPECT_EQ(45, wrap_180(int16_t(45))); |
|
EXPECT_EQ(90, wrap_180(int16_t(90))); |
|
EXPECT_EQ(180, wrap_180(int16_t(180))); |
|
EXPECT_EQ(-179, wrap_180(int16_t(181))); |
|
EXPECT_EQ(-90, wrap_180(int16_t(270))); |
|
EXPECT_EQ(0, wrap_180(int16_t(360))); |
|
EXPECT_EQ(0, wrap_180(int16_t(720))); |
|
EXPECT_EQ(0, wrap_180(int16_t(3600))); |
|
EXPECT_EQ(0, wrap_180((int16_t(7200)))); |
|
EXPECT_EQ(0, wrap_180((int16_t)-3600)); |
|
} |
|
|
|
TEST(MathWrapTest, Angle360) |
|
{ |
|
// Full circle test |
|
for (int32_t i = 0; i <= 36000; i += 100) { |
|
if (i == 0) { |
|
// hit pole position |
|
EXPECT_EQ(i, wrap_360_cd(i)); |
|
EXPECT_EQ(i, wrap_360_cd(-i)); |
|
EXPECT_EQ(i, wrap_360_cd(long(i))); |
|
EXPECT_EQ(i, wrap_360_cd(long(-i))); |
|
} else if (i < 36000) { |
|
// between pole position |
|
EXPECT_EQ(i, wrap_360_cd(i)); |
|
EXPECT_EQ(36000 - i, wrap_360_cd(-i)); |
|
EXPECT_EQ(i, wrap_360_cd(long(i))); |
|
EXPECT_EQ(36000 - i, wrap_360_cd(long(-i))); |
|
} else if (i == 36000) { |
|
// hit pole position |
|
EXPECT_EQ(0, wrap_360_cd(i)); |
|
EXPECT_EQ(0, wrap_360_cd(-i)); |
|
EXPECT_EQ(0, wrap_360_cd(long(i))); |
|
EXPECT_EQ(0, wrap_360_cd(long(-i))); |
|
} |
|
} |
|
|
|
EXPECT_EQ(4500.f, wrap_360_cd(4500.f)); |
|
EXPECT_EQ(9000.f, wrap_360_cd(9000.f)); |
|
EXPECT_EQ(18000.f, wrap_360_cd(18000.f)); |
|
EXPECT_EQ(27000.f, wrap_360_cd(27000.f)); |
|
EXPECT_EQ(0.f, wrap_360_cd(36000.f)); |
|
EXPECT_EQ(5.f, wrap_360_cd(36005.f)); |
|
EXPECT_EQ(0.f, wrap_360_cd(72000.f)); |
|
EXPECT_EQ(0.f, wrap_360_cd(360000.f)); |
|
EXPECT_EQ(0.f, wrap_360_cd(720000.f)); |
|
EXPECT_EQ( 0.f, wrap_360_cd(-3600000000.f)); |
|
|
|
EXPECT_EQ(31500.f, wrap_360_cd(-4500.f)); |
|
EXPECT_EQ(27000.f, wrap_360_cd(-9000.f)); |
|
EXPECT_EQ(18000.f, wrap_360_cd(-18000.f)); |
|
EXPECT_EQ(9000.f, wrap_360_cd(-27000.f)); |
|
EXPECT_EQ(0.f, wrap_360_cd(-36000.f)); |
|
EXPECT_EQ(35995.0f,wrap_360_cd(-36005.f)); |
|
EXPECT_EQ(0.f, wrap_360_cd(-72000.f)); |
|
|
|
|
|
EXPECT_EQ(45.0f, wrap_360(45.0f)); |
|
EXPECT_EQ(90.0f, wrap_360(90.0f)); |
|
EXPECT_EQ(180.0f, wrap_360(180.0f)); |
|
EXPECT_EQ(270.0f, wrap_360(270.0f)); |
|
EXPECT_EQ(0.0f, wrap_360(360.0f)); |
|
EXPECT_EQ(1.0f, wrap_360(361.0f)); |
|
EXPECT_EQ(0.0f, wrap_360(720.0f)); |
|
EXPECT_EQ(0.0f, wrap_360(3600.0f)); |
|
EXPECT_EQ(0.0f, wrap_360(7200.0f)); |
|
EXPECT_EQ(260.0f, wrap_360(-100.0f)); |
|
|
|
EXPECT_EQ(45, wrap_360((int16_t)45)); |
|
EXPECT_EQ(90, wrap_360((int16_t)90)); |
|
EXPECT_EQ(180, wrap_360((int16_t)180)); |
|
EXPECT_EQ(270, wrap_360((int16_t)270)); |
|
EXPECT_EQ(0, wrap_360((int16_t)360)); |
|
EXPECT_EQ(1, wrap_360((int16_t)361)); |
|
EXPECT_EQ(0, wrap_360((int16_t)720)); |
|
EXPECT_EQ(0, wrap_360((int16_t)3600)); |
|
EXPECT_EQ(0, wrap_360((int16_t)7200)); |
|
EXPECT_EQ(260, wrap_360((int16_t)-100)); |
|
|
|
} |
|
|
|
TEST(MathWrapTest, AnglePI) |
|
{ |
|
const float accuracy = 1.0e-5; |
|
|
|
EXPECT_NEAR(M_PI, wrap_PI(M_PI), accuracy); |
|
EXPECT_NEAR(0.f, wrap_PI(M_2PI), accuracy); |
|
EXPECT_NEAR(0, wrap_PI(M_PI * 10), accuracy); |
|
EXPECT_NEAR(-2.1415925025939941f, wrap_PI(M_PI+1), accuracy); |
|
EXPECT_NEAR(1.f, wrap_PI(1), accuracy); |
|
EXPECT_NEAR(1.f, wrap_PI((short)1), accuracy); |
|
} |
|
|
|
TEST(MathWrapTest, Angle2PI) |
|
{ |
|
const float accuracy = 1.0e-5; |
|
|
|
EXPECT_NEAR(M_PI, wrap_2PI(M_PI), accuracy); |
|
EXPECT_NEAR(0.f, wrap_2PI(M_2PI), accuracy); |
|
EXPECT_NEAR(0.f, wrap_2PI(M_PI * 10), accuracy); |
|
EXPECT_NEAR(0.f, wrap_2PI(0.f), accuracy); |
|
EXPECT_NEAR(M_PI, wrap_2PI(-M_PI), accuracy); |
|
EXPECT_NEAR(0, wrap_2PI(-M_2PI), accuracy); |
|
EXPECT_NEAR(1, wrap_2PI(1), accuracy); |
|
EXPECT_NEAR(1, wrap_2PI((short)1), accuracy); |
|
} |
|
|
|
TEST(MathTest, ASin) |
|
{ |
|
const float accuracy = 1.0e-5; |
|
EXPECT_NEAR(0.0f, safe_asin(0.0f), accuracy); |
|
EXPECT_NEAR(0.9033391107665127f, safe_asin(M_PI_2 * 0.5f), accuracy); |
|
EXPECT_NEAR(M_PI_2, safe_asin(M_PI_2 ), accuracy); |
|
EXPECT_NEAR(M_PI_2, safe_asin(M_PI), accuracy); |
|
EXPECT_NEAR(M_PI_2, safe_asin(M_2PI), accuracy); |
|
EXPECT_NEAR(-M_PI_2, safe_asin(-M_PI_2), accuracy); |
|
EXPECT_NEAR(-M_PI_2, safe_asin(-M_PI), accuracy); |
|
EXPECT_NEAR(-0.9033391107665127f, safe_asin(-M_PI_2 * 0.5f), accuracy); |
|
EXPECT_NEAR(0.0f, safe_asin(nanf("0x4152")), accuracy); |
|
EXPECT_NEAR(0.0f, safe_asin(int(0)), accuracy); |
|
EXPECT_NEAR(0.0f, safe_asin(short(0)), accuracy); |
|
EXPECT_NEAR(0.0f, safe_asin(double(0.0)), accuracy); |
|
|
|
} |
|
|
|
TEST(MathTest, Sqrt) |
|
{ |
|
const float accuracy = 1.0e-5; |
|
EXPECT_NEAR(0.0f, safe_sqrt(0.0f), accuracy); |
|
EXPECT_NEAR(32.0f, safe_sqrt(1024.0f), accuracy); |
|
EXPECT_NEAR(0.0f, safe_sqrt(-1.0f), accuracy); |
|
|
|
EXPECT_NEAR(0.0f, safe_sqrt(nanf("0x4152")), accuracy); |
|
EXPECT_NEAR(0.0f, safe_sqrt(int(0)), accuracy); |
|
EXPECT_NEAR(0.0f, safe_sqrt(short(0)), accuracy); |
|
EXPECT_NEAR(0.0f, safe_sqrt(double(0.0)), accuracy); |
|
|
|
} |
|
|
|
TEST(MathTest, Interpolation) |
|
{ |
|
const float accuracy = 1.0e-5; |
|
EXPECT_NEAR(1500.0f, linear_interpolate(1200.0f, 1800.0f, 1500.0f, 1000.0f, 2000.0f), accuracy); |
|
EXPECT_NEAR(1740.0f, linear_interpolate(1200.0f, 1800.0f, 1900.0f, 1000.0f, 2000.0f), accuracy); |
|
EXPECT_NEAR(1200.0f, linear_interpolate(1200.0f, 1800.0f, 1000.0f, 1000.0f, 2000.0f), accuracy); |
|
EXPECT_NEAR(1000.0f, linear_interpolate(1000.0f, 2000.0f, 1100.0f, 1200.0f, 1800.0f), accuracy); |
|
EXPECT_NEAR(2000.0f, linear_interpolate(1000.0f, 2000.0f, 1900.0f, 1200.0f, 1800.0f), accuracy); |
|
|
|
} |
|
|
|
TEST(MathTest, ThrottleCurve) |
|
{ |
|
// get hover throttle level [0,1] |
|
static constexpr float THR_MID = 0.5f; |
|
|
|
const float accuracy = 1.0e-5; |
|
EXPECT_NEAR(0.0f, throttle_curve(THR_MID, 0.0f, 0.0f), accuracy); |
|
EXPECT_NEAR(0.25f, throttle_curve(THR_MID, 0.0f, 0.25f), accuracy); |
|
EXPECT_NEAR(0.5f, throttle_curve(THR_MID, 0.0f, 0.5f), accuracy); |
|
EXPECT_NEAR(0.75f, throttle_curve(THR_MID, 0.0f, 0.75f), accuracy); |
|
EXPECT_NEAR(1.0f, throttle_curve(THR_MID, 0.0f, 1.0f), accuracy); |
|
EXPECT_NEAR(0.0f, throttle_curve(THR_MID, 0.25f, 0.0f), accuracy); |
|
EXPECT_NEAR(0.296875f, throttle_curve(THR_MID, 0.25f, 0.25f), accuracy); |
|
EXPECT_NEAR(0.5f, throttle_curve(THR_MID, 0.25f, 0.5f), accuracy); |
|
EXPECT_NEAR(0.703125f, throttle_curve(THR_MID, 0.25f, 0.75f), accuracy); |
|
EXPECT_NEAR(1.0f, throttle_curve(THR_MID, 0.25f, 1.0f), accuracy); |
|
EXPECT_NEAR(0.0f, throttle_curve(THR_MID, 0.5f, 0.0f), accuracy); |
|
EXPECT_NEAR(0.34375f, throttle_curve(THR_MID, 0.5f, 0.25f), accuracy); |
|
EXPECT_NEAR(0.5f, throttle_curve(THR_MID, 0.5f, 0.5f), accuracy); |
|
EXPECT_NEAR(0.65625f, throttle_curve(THR_MID, 0.5f, 0.75f), accuracy); |
|
EXPECT_NEAR(1.0f, throttle_curve(THR_MID, 0.5f, 1.0f), accuracy); |
|
EXPECT_NEAR(0.0f, throttle_curve(THR_MID, 0.75f, 0.0f), accuracy); |
|
EXPECT_NEAR(0.390625f, throttle_curve(THR_MID, 0.75f, 0.25f), accuracy); |
|
EXPECT_NEAR(0.5f, throttle_curve(THR_MID, 0.75f, 0.5f), accuracy); |
|
EXPECT_NEAR(0.609375f, throttle_curve(THR_MID, 0.75f, 0.75f), accuracy); |
|
EXPECT_NEAR(1.0f, throttle_curve(THR_MID, 0.75f, 1.0f), accuracy); |
|
EXPECT_NEAR(0.0f, throttle_curve(THR_MID, 1.0f, 0.0f), accuracy); |
|
EXPECT_NEAR(0.4375f, throttle_curve(THR_MID, 1.0f, 0.25f), accuracy); |
|
EXPECT_NEAR(0.5f, throttle_curve(THR_MID, 1.0f, 0.5f), accuracy); |
|
EXPECT_NEAR(0.5625f, throttle_curve(THR_MID, 1.0f, 0.75f), accuracy); |
|
EXPECT_NEAR(1.0f, throttle_curve(THR_MID, 1.0f, 1.0f), accuracy); |
|
} |
|
|
|
TEST(MathTest, RotationEqual) |
|
{ |
|
EXPECT_TRUE(rotation_equal(ROTATION_ROLL_180_YAW_90, ROTATION_ROLL_180_YAW_90)); |
|
EXPECT_TRUE(rotation_equal(ROTATION_ROLL_180_YAW_90, ROTATION_PITCH_180_YAW_270)); |
|
EXPECT_FALSE(rotation_equal(ROTATION_NONE, ROTATION_PITCH_180_YAW_270)); |
|
} |
|
|
|
TEST(MathTest, FIXED2FLOAT) |
|
{ |
|
static constexpr uint16_t test_value = 1024; |
|
EXPECT_EQ(512.0f, fixed2float(test_value, 1)); |
|
EXPECT_EQ(256.0f, fixed2float(test_value, 2)); |
|
} |
|
|
|
TEST(MathTest, FLOAT2FIXED) |
|
{ |
|
static constexpr float test_value = 1024.0f; |
|
EXPECT_EQ(2048, float2fixed(test_value, 1)); |
|
EXPECT_EQ(4096, float2fixed(test_value, 2)); |
|
} |
|
|
|
TEST(MathTest, RANDOM16) |
|
{ |
|
static const uint16_t random_value = get_random16(); |
|
EXPECT_NE(random_value, get_random16()); |
|
} |
|
|
|
TEST(MathTest, VELCORRECTION) |
|
{ |
|
static constexpr Vector3F pos{1.0f, 1.0f, 0.0f}; |
|
static constexpr Matrix3F rot(0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); |
|
static constexpr Vector3F rate{-1.0f, -1.0f, -1.0f}; |
|
EXPECT_TRUE(Vector3F(1.0f, 1.0f, 0.0f) == get_vel_correction_for_sensor_offset(pos, rot, rate)); |
|
EXPECT_TRUE(Vector3F() == get_vel_correction_for_sensor_offset(Vector3F(), rot, rate)); |
|
} |
|
|
|
TEST(MathTest, LOWPASSALPHA) |
|
{ |
|
const float accuracy = 1.0e-5f; |
|
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(0.0f, 2.0f)); |
|
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(-1.0f, 2.0f)); |
|
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, 0.0f)); |
|
EXPECT_EQ(1.0f, calc_lowpass_alpha_dt(1.0f, -2.0f)); |
|
|
|
EXPECT_NEAR(0.926288f, calc_lowpass_alpha_dt(1.0f, 2.0f), accuracy); |
|
} |
|
|
|
TEST(MathTest, FIXEDWINGTURNRATE) |
|
{ |
|
const float accuracy = 1.0e-5f; |
|
|
|
EXPECT_NEAR(-318.65771484375f, fixedwing_turn_rate(-90, 10.0f), accuracy); |
|
EXPECT_NEAR(318.65771484375f, fixedwing_turn_rate(90, 10.0f), accuracy); |
|
EXPECT_NEAR(56.187965393066406f, fixedwing_turn_rate(45, 10.0f), accuracy); |
|
} |
|
|
|
AP_GTEST_PANIC() |
|
AP_GTEST_MAIN() |
|
|
|
#pragma GCC diagnostic pop
|
|
|