Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fab3821
replaced look-up tables with <math.h> lib functions and updated press…
avtoku Sep 25, 2024
e0a8c71
Merge branch 'main' into ptt/turbomath
iandareid Oct 7, 2024
9e330d9
Fixed the test for this version of the firmware.
iandareid Oct 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
257 changes: 21 additions & 236 deletions lib/turbomath/turbomath.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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
10 changes: 2 additions & 8 deletions lib/turbomath/turbomath.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Loading