replaced look-up tables with <math.h> lib functions and updated press…

…ure altitude formula
rosflight · Sep 25, 2024 · fab3821 · fab3821
1 parent a8cccf7
commit fab3821
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 244 deletions.
diff --git 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,24 @@ 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<float>(sin_num_entries);
-  int16_t index = static_cast<int16_t>(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<float>(atan_num_entries);
-  int16_t index = static_cast<int16_t>(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<float>(asin_num_entries);
-  int16_t index = static_cast<int16_t>(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<float>(pressure_num_entries);
-    int16_t index = static_cast<int16_t>(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;
-  }
+#define ISA_PRESSURE (101325.0) // Pa
+#define ISA_EXPONENT (0.190326730028458)
+#define ISA_SCALE_FACTOR (44318.1386038261) //m
+  return (1.0-pow(press/ISA_PRESSURE,ISA_EXPONENT))*ISA_SCALE_FACTOR;
 }
 
-float fabs(float x)
-{
-  if (x < 0) {
-    return -x;
-  } else {
-    return x;
-  }
-}
+float inv_sqrt(float x) { return 1./sqrt(x);}
 
-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);
-}
 
 } // namespace turbomath
diff --git a/lib/turbomath/turbomath.h b/lib/turbomath/turbomath.h
@@ -35,23 +35,17 @@
 #define TURBOMATH_TURBOMATH_H
 
 #include <cstdint>
+#include <math.h>
 
 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);
+
 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
 {