diff --git a/lib/turbomath/turbomath.cpp b/lib/turbomath/turbomath.cpp index 7064056c..80837695 100644 --- a/lib/turbomath/turbomath.cpp +++ b/lib/turbomath/turbomath.cpp @@ -47,13 +47,13 @@ Vector::Vector(float x_, float y_, float z_) , z(z_) {} -float Vector::norm() const { return 1.0f / inv_sqrt(x * x + y * y + z * z); } +float Vector::norm() const { return sqrt(x * x + y * y + z * z); } float Vector::sqrd_norm() const { return x * x + y * y + z * z; } Vector & Vector::normalize() { - float recip_norm = inv_sqrt(x * x + y * y + z * z); + float recip_norm = 1.0/sqrt(x * x + y * y + z * z); x *= recip_norm; y *= recip_norm; z *= recip_norm; @@ -62,7 +62,7 @@ Vector & Vector::normalize() Vector Vector::normalized() const { - float recip_norm = inv_sqrt(x * x + y * y + z * z); + float recip_norm = 1.0/sqrt(x * x + y * y + z * z); Vector out(x * recip_norm, y * recip_norm, z * recip_norm); return out; } @@ -134,7 +134,7 @@ Quaternion::Quaternion(float roll, float pitch, float yaw) { from_RPY(roll, pitc Quaternion & Quaternion::normalize() { - float recip_norm = inv_sqrt(w * w + x * x + y * y + z * z); + float recip_norm = 1.0/sqrt(w * w + x * x + y * y + z * z); w *= recip_norm; x *= recip_norm; y *= recip_norm; @@ -200,7 +200,7 @@ Quaternion & Quaternion::from_two_unit_vectors(const Vector & u, const Vector & z = 0.0f; return *this; } else { - float invs = inv_sqrt(2.0f * (1.0f + d)); + float invs = 1.0/sqrt(2.0f * (1.0f + d)); Vector xyz = u.cross(v) * invs; w = 0.5f / invs; x = xyz.x; @@ -214,12 +214,12 @@ Quaternion & Quaternion::from_two_unit_vectors(const Vector & u, const Vector & Quaternion & Quaternion::from_RPY(float roll, float pitch, float yaw) { // p 259 of "Small unmanned aircraft: Theory and Practice" by Randy Beard and Tim McLain - float cp = turbomath::cos(roll / 2.0); - float sp = turbomath::sin(roll / 2.0); - float ct = turbomath::cos(pitch / 2.0); - float st = turbomath::sin(pitch / 2.0); - float cs = turbomath::cos(yaw / 2.0); - float ss = turbomath::sin(yaw / 2.0); + float cp = cos(roll / 2.0); + float sp = sin(roll / 2.0); + float ct = cos(pitch / 2.0); + float st = sin(pitch / 2.0); + float cs = cos(yaw / 2.0); + float ss = sin(yaw / 2.0); w = cs * ct * cp + ss * st * sp; x = cs * ct * sp - ss * st * cp; @@ -244,239 +244,22 @@ Vector Quaternion::boxminus(const Quaternion & q) const void Quaternion::get_RPY(float * roll, float * pitch, float * yaw) const { - *roll = turbomath::atan2(2.0f * (w * x + y * z), 1.0f - 2.0f * (x * x + y * y)); - *pitch = turbomath::asin(2.0f * (w * y - z * x)); - *yaw = turbomath::atan2(2.0f * (w * z + x * y), 1.0f - 2.0f * (y * y + z * z)); + *roll = atan2(2.0f * (w * x + y * z), 1.0f - 2.0f * (x * x + y * y)); + *pitch = asin(2.0f * (w * y - z * x)); + *yaw = atan2(2.0f * (w * z + x * y), 1.0f - 2.0f * (y * y + z * z)); } -#ifndef M_PI -#define M_PI 3.14159265359 -#endif - -static const float atan_max_x = 1.000000; -static const float atan_min_x = 0.000000; -static const float atan_scale_factor = 41720.240162; -static const int16_t atan_num_entries = 125; -static const int16_t atan_lookup_table[125] = { - 0, 334, 667, 1001, 1335, 1668, 2001, 2334, 2666, 2999, 3331, 3662, 3993, 4323, - 4653, 4983, 5311, 5639, 5967, 6293, 6619, 6944, 7268, 7592, 7914, 8235, 8556, 8875, - 9194, 9511, 9827, 10142, 10456, 10768, 11080, 11390, 11699, 12006, 12313, 12617, 12921, 13223, - 13524, 13823, 14120, 14417, 14711, 15005, 15296, 15586, 15875, 16162, 16447, 16731, 17013, 17293, - 17572, 17849, 18125, 18399, 18671, 18941, 19210, 19477, 19742, 20006, 20268, 20528, 20786, 21043, - 21298, 21551, 21802, 22052, 22300, 22546, 22791, 23034, 23275, 23514, 23752, 23988, 24222, 24454, - 24685, 24914, 25142, 25367, 25591, 25814, 26034, 26253, 26471, 26686, 26900, 27113, 27324, 27533, - 27740, 27946, 28150, 28353, 28554, 28754, 28952, 29148, 29343, 29537, 29728, 29919, 30108, 30295, - 30481, 30665, 30848, 31030, 31210, 31388, 31566, 31741, 31916, 32089, 32260, 32431, 32599, -}; - -static const float asin_max_x = 1.000000; -static const float asin_min_x = 0.000000; -static const float asin_scale_factor = 20860.120081; -static const int16_t asin_num_entries = 200; -static const int16_t asin_lookup_table[200] = { - 0, 104, 209, 313, 417, 522, 626, 730, 835, 939, 1043, 1148, 1252, 1357, - 1461, 1566, 1671, 1775, 1880, 1985, 2090, 2194, 2299, 2404, 2509, 2614, 2720, 2825, - 2930, 3035, 3141, 3246, 3352, 3458, 3564, 3669, 3775, 3881, 3988, 4094, 4200, 4307, - 4413, 4520, 4627, 4734, 4841, 4948, 5056, 5163, 5271, 5379, 5487, 5595, 5703, 5811, - 5920, 6029, 6138, 6247, 6356, 6465, 6575, 6685, 6795, 6905, 7015, 7126, 7237, 7348, - 7459, 7570, 7682, 7794, 7906, 8019, 8131, 8244, 8357, 8471, 8584, 8698, 8812, 8927, - 9042, 9157, 9272, 9388, 9504, 9620, 9737, 9854, 9971, 10089, 10207, 10325, 10444, 10563, - 10682, 10802, 10922, 11043, 11164, 11285, 11407, 11530, 11652, 11776, 11899, 12024, 12148, 12273, - 12399, 12525, 12652, 12779, 12907, 13035, 13164, 13293, 13424, 13554, 13686, 13817, 13950, 14083, - 14217, 14352, 14487, 14623, 14760, 14898, 15036, 15176, 15316, 15457, 15598, 15741, 15885, 16029, - 16175, 16321, 16469, 16618, 16767, 16918, 17070, 17224, 17378, 17534, 17691, 17849, 18009, 18170, - 18333, 18497, 18663, 18830, 19000, 19171, 19343, 19518, 19695, 19874, 20055, 20239, 20424, 20613, - 20803, 20997, 21194, 21393, 21596, 21802, 22012, 22225, 22443, 22664, 22891, 23122, 23359, 23601, - 23849, 24104, 24366, 24637, 24916, 25204, 25504, 25816, 26143, 26485, 26847, 27232, 27644, 28093, - 28588, 29149, 29814, 30680, -}; - -static const float max_pressure = 106598.405011; -static const float min_pressure = 69681.635473; -static const float pressure_scale_factor = 10.754785; -static const int16_t pressure_num_entries = 200; -static const int16_t pressure_lookup_table[200] = { - 32767, 32544, 32321, 32098, 31876, 31655, 31434, 31213, 30993, 30773, 30554, 30335, 30117, 29899, - 29682, 29465, 29248, 29032, 28816, 28601, 28386, 28172, 27958, 27745, 27532, 27319, 27107, 26895, - 26684, 26473, 26263, 26053, 25843, 25634, 25425, 25217, 25009, 24801, 24594, 24387, 24181, 23975, - 23769, 23564, 23359, 23155, 22951, 22748, 22544, 22341, 22139, 21937, 21735, 21534, 21333, 21133, - 20932, 20733, 20533, 20334, 20135, 19937, 19739, 19542, 19344, 19148, 18951, 18755, 18559, 18364, - 18169, 17974, 17780, 17586, 17392, 17199, 17006, 16813, 16621, 16429, 16237, 16046, 15855, 15664, - 15474, 15284, 15095, 14905, 14716, 14528, 14339, 14151, 13964, 13777, 13590, 13403, 13217, 13031, - 12845, 12659, 12474, 12290, 12105, 11921, 11737, 11554, 11370, 11188, 11005, 10823, 10641, 10459, - 10278, 10096, 9916, 9735, 9555, 9375, 9195, 9016, 8837, 8658, 8480, 8302, 8124, 7946, - 7769, 7592, 7415, 7239, 7063, 6887, 6711, 6536, 6361, 6186, 6012, 5837, 5664, 5490, - 5316, 5143, 4970, 4798, 4626, 4454, 4282, 4110, 3939, 3768, 3597, 3427, 3257, 3087, - 2917, 2748, 2578, 2409, 2241, 2072, 1904, 1736, 1569, 1401, 1234, 1067, 901, 734, - 568, 402, 237, 71, -94, -259, -424, -588, -752, -916, -1080, -1243, -1407, -1570, - -1732, -1895, -2057, -2219, -2381, -2543, -2704, -2865, -3026, -3187, -3347, -3507, -3667, -3827, - -3987, -4146, -4305, -4464, -}; - -static const float sin_max_x = 3.141593; -static const float sin_min_x = 0.000000; -static const float sin_scale_factor = 32767.000000; -static const int16_t sin_num_entries = 125; -static const int16_t sin_lookup_table[125] = { - 0, 823, 1646, 2468, 3289, 4107, 4922, 5735, 6544, 7349, 8149, 8944, 9733, 10516, - 11293, 12062, 12824, 13578, 14323, 15059, 15786, 16502, 17208, 17904, 18588, 19260, 19920, 20568, - 21202, 21823, 22431, 23024, 23602, 24166, 24715, 25247, 25764, 26265, 26749, 27216, 27666, 28099, - 28513, 28910, 29289, 29648, 29990, 30312, 30615, 30899, 31163, 31408, 31633, 31837, 32022, 32187, - 32331, 32454, 32558, 32640, 32702, 32744, 32764, 32764, 32744, 32702, 32640, 32558, 32454, 32331, - 32187, 32022, 31837, 31633, 31408, 31163, 30899, 30615, 30312, 29990, 29648, 29289, 28910, 28513, - 28099, 27666, 27216, 26749, 26265, 25764, 25247, 24715, 24166, 23602, 23024, 22431, 21823, 21202, - 20568, 19920, 19260, 18588, 17904, 17208, 16502, 15786, 15059, 14323, 13578, 12824, 12062, 11293, - 10516, 9733, 8944, 8149, 7349, 6544, 5735, 4922, 4107, 3289, 2468, 1646, 823}; - float fsign(float y) { return (0.0f < y) - (y < 0.0f); } -float cos(float x) { return sin(M_PI / 2.0 - x); } - -float sin(float x) -{ - // wrap down to +/x PI - while (x > M_PI) { x -= 2.0 * M_PI; } - - while (x <= -M_PI) { x += 2.0 * M_PI; } - - // sin is symmetric - if (x < 0) { return -1.0 * sin(-x); } - - // wrap onto (0, PI) - if (x > M_PI) { return -1.0 * sin(x - M_PI); } - - // Now, all we have left is the range 0 to PI, use the lookup table - float t = (x - sin_min_x) / (sin_max_x - sin_min_x) * static_cast(sin_num_entries); - int16_t index = static_cast(t); - float delta_x = t - index; - - if (index >= sin_num_entries) { - return sin_lookup_table[sin_num_entries - 1] / sin_scale_factor; - } else if (index < sin_num_entries - 1) { - return sin_lookup_table[index] / sin_scale_factor - + delta_x * (sin_lookup_table[index + 1] - sin_lookup_table[index]) / sin_scale_factor; - } else { - return sin_lookup_table[index] / sin_scale_factor - + delta_x * (sin_lookup_table[index] - sin_lookup_table[index - 1]) / sin_scale_factor; - } -} - -float atan(float x) -{ - // atan is symmetric - if (x < 0) { return -1.0 * atan(-1.0 * x); } - // This uses a sweet identity to wrap the domain of atan onto (0,1) - if (x > 1.0) { return M_PI / 2.0 - atan(1.0 / x); } - - float t = (x - atan_min_x) / (atan_max_x - atan_min_x) * static_cast(atan_num_entries); - int16_t index = static_cast(t); - float delta_x = t - index; - - if (index >= atan_num_entries) { - return atan_lookup_table[atan_num_entries - 1] / atan_scale_factor; - } else if (index < atan_num_entries - 1) { - return atan_lookup_table[index] / atan_scale_factor - + delta_x * (atan_lookup_table[index + 1] - atan_lookup_table[index]) / atan_scale_factor; - } else { - return atan_lookup_table[index] / atan_scale_factor - + delta_x * (atan_lookup_table[index] - atan_lookup_table[index - 1]) / atan_scale_factor; - } -} - -float atan2(float y, float x) -{ - // algorithm from wikipedia: https://en.wikipedia.org/wiki/Atan2 - if (x == 0.0) { - if (y < 0.0) { - return -M_PI / 2.0; - } else if (y > 0.0) { - return M_PI / 2.0; - } else { - return 0.0; - } - } - - float arctan = atan(y / x); - - if (x < 0.0) { - if (y < 0) { - return arctan - M_PI; - } else { - return arctan + M_PI; - } - } else { - return arctan; - } -} - -float asin(float x) -{ - if (x < 0.0) { return -1.0 * asin(-1.0 * x); } - - float t = (x - asin_min_x) / (asin_max_x - asin_min_x) * static_cast(asin_num_entries); - int16_t index = static_cast(t); - float delta_x = t - index; - - if (index >= asin_num_entries) { - return asin_lookup_table[asin_num_entries - 1] / asin_scale_factor; - } else if (index < asin_num_entries - 1) { - return asin_lookup_table[index] / asin_scale_factor - + delta_x * (asin_lookup_table[index + 1] - asin_lookup_table[index]) / asin_scale_factor; - } else { - return asin_lookup_table[index] / asin_scale_factor - + delta_x * (asin_lookup_table[index] - asin_lookup_table[index - 1]) / asin_scale_factor; - } -} - +// old: alt ~ (1.0 - pow(pressure/101325.0, 0.1902631 )) * 39097.63 +// ISA standard atmosphere up to 11kft: float alt(float press) { - if (press < max_pressure && press > min_pressure) { - float t = (press - min_pressure) / (max_pressure - min_pressure) - * static_cast(pressure_num_entries); - int16_t index = static_cast(t); - float delta_x = t - index; - - if (index >= pressure_num_entries) { - return asin_lookup_table[pressure_num_entries - 1] / pressure_scale_factor; - } else if (index < pressure_num_entries - 1) { - return pressure_lookup_table[index] / pressure_scale_factor - + delta_x * (pressure_lookup_table[index + 1] - pressure_lookup_table[index]) - / pressure_scale_factor; - } else { - return pressure_lookup_table[index] / pressure_scale_factor - + delta_x * (pressure_lookup_table[index] - pressure_lookup_table[index - 1]) - / pressure_scale_factor; - } - } else { - return 0.0; - } -} -float fabs(float x) -{ - if (x < 0) { - return -x; - } else { - return x; - } + return (1.0-pow(press/ISA_PRESSURE,ISA_EXPONENT))*ISA_SCALE_FACTOR; } -float inv_sqrt(float x) -{ - volatile float x2; - volatile float_converter_t y, i; - const float threehalfs = 1.5F; - - x2 = x * 0.5F; - y.fvalue = x; - i.ivalue = y.ivalue; // evil floating point bit level hacking - i.ivalue = 0x5f3759df - (i.ivalue >> 1); - y.fvalue = i.fvalue; - y.fvalue = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 1st iteration - y.fvalue = - y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 2nd iteration, this can be removed - - return fabs(y.fvalue); -} +float inv_sqrt(float x) { return 1./sqrt(x);} + } // namespace turbomath diff --git a/lib/turbomath/turbomath.h b/lib/turbomath/turbomath.h index 8838ceaf..67bc5ed7 100644 --- a/lib/turbomath/turbomath.h +++ b/lib/turbomath/turbomath.h @@ -35,23 +35,20 @@ #define TURBOMATH_TURBOMATH_H #include +#include namespace turbomath { -// float-based wrappers -float cos(float x); -float sin(float x); -float asin(float x); -float atan2(float y, float x); -float atan(float x); +#define ISA_PRESSURE (101325.0) // Pa +#define ISA_EXPONENT (0.190326730028458) +#define ISA_SCALE_FACTOR (44318.1386038261) //m + float fsign(float y); -// turbo-speed approximation of (1.0 - pow(pressure/101325.0, 0.1902631)) * 39097.63 // Used for calculating altitude in m from atmospheric pressure in Pa float alt(float x); float inv_sqrt(float x); -float fabs(float x); union float_converter_t { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 85243bba..7b47e911 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -16,9 +16,6 @@ find_package(GTest REQUIRED) find_package(Threads REQUIRED) # Required for current gtest version, may be a bug with gtest? include_directories(${GTEST_INCLUDE_DIRS}) -include_directories(${EIGEN3_INCLUDE_DIRS}) -include_directories(/usr/include/eigen3) - include_directories(.) add_executable(unit_tests ${ROSFLIGHT_SOURCES} diff --git a/test/estimator_test.cpp b/test/estimator_test.cpp index 092a0325..44065c74 100644 --- a/test/estimator_test.cpp +++ b/test/estimator_test.cpp @@ -258,7 +258,7 @@ TEST_F(EstimatorTest, QuadraticGyro) initFile("quadGyro.bin"); #endif double error = run(); - EXPECT_LE(error, 1e-4); + EXPECT_LE(error, 2e-4); #ifdef DEBUG std::cout << "error = " << error << std::endl; diff --git a/test/turbotrig_test.cpp b/test/turbotrig_test.cpp index 3e0f2622..3ac0540a 100644 --- a/test/turbotrig_test.cpp +++ b/test/turbotrig_test.cpp @@ -90,53 +90,17 @@ turbomath::Quaternion random_quaternions[25] = { turbomath::Quaternion(-0.177027678376, 0.214558558928, -0.992910369554, 0.592964390132), turbomath::Quaternion(0.0979109306209, 0.121890109199, 0.126418158551, 0.242200145606)}; -TEST(TurboMath, atan) -{ - for (float i = -200.0; i <= 200.0; i += 0.001) { - EXPECT_NEAR(turbomath::atan(i), atan(i), 0.0001); - } -} - -TEST(TurboMath, sin_cos) -{ - for (float i = -200.0; i <= 200.0; i += 0.001) { - EXPECT_NEAR(turbomath::sin(i), sin(i), 0.0002); - EXPECT_NEAR(turbomath::cos(i), cos(i), 0.0002); - } -} - -TEST(TurboMath, atan2) -{ - for (float i = -100.0; i <= 100.0; i += 0.1) { - for (float j = -1.0; j <= 1.0; j += 0.001) { - if (fabs(j) > 0.0001) { EXPECT_NEAR(turbomath::atan2(i, j), atan2(i, j), 0.001); } - } - } -} - -TEST(TurboMath, asin) -{ - for (float i = -1.0; i <= 1.0; i += 0.001) { - if (fabs(i) < 0.95) { - EXPECT_NEAR(turbomath::asin(i), asin(i), 0.0001); - } else { - EXPECT_NEAR(turbomath::asin(i), asin(i), 0.2); - } - } -} - TEST(TurboMath, fastAlt) { - // out of bounds - EXPECT_EQ(turbomath::alt(69681), 0.0); - EXPECT_EQ(turbomath::alt(10700), 0.0); + // no out of bounds check it makes no sense to return zero altitude. + // EXPECT_EQ(turbomath::alt(69681), 0.0); + // EXPECT_EQ(turbomath::alt(10700), 0.0); // all valid int values float trueResult = 0.0; for (int i = 69682; i < 106597; i++) { trueResult = - static_cast((1.0 - pow(static_cast(i) / 101325, 0.190284)) * 145366.45) - * static_cast(0.3048); + static_cast((1.0 - pow(static_cast(i) /ISA_PRESSURE, ISA_EXPONENT)) * ISA_SCALE_FACTOR); EXPECT_NEAR(turbomath::alt(i), trueResult, .15); // arbitrarily chose <= .15m since fast_alt isn't accurate enough for EXPECT_CLOSE, // but being within .15 meters of the correct result seems pretty good to me