lba.gitea/WLED
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Wled math bugfix.

Browse Source
This commit is contained in:
Blaz Kristan 2021-09-23 20:45:53 +02:00
parent 6db2240f8a
commit 93ee4716cc
1 changed files with 12 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
wled00/wled_math.h
View File

@ -15,10 +15,10 @@
#define modd(x, y) ((x) - (int)((x) / (y)) * (y)) #define modd(x, y) ((x) - (int)((x) / (y)) * (y))
float cos_t(float x) float cos_t(float phi)
{ {
x = modd(x, TWO_PI); float x = modd(phi, TWO_PI);
char sign = 1; int8_t sign = 1;
if (x > PI) if (x > PI)
{ {
x -= PI; x -= PI;
@ -28,7 +28,7 @@ float cos_t(float x)
float res = sign * (1 - ((xx) / (2)) + ((xx * xx) / (24)) - ((xx * xx * xx) / (720)) + ((xx * xx * xx * xx) / (40320)) - ((xx * xx * xx * xx * xx) / (3628800)) + ((xx * xx * xx * xx * xx * xx) / (479001600))); float res = sign * (1 - ((xx) / (2)) + ((xx * xx) / (24)) - ((xx * xx * xx) / (720)) + ((xx * xx * xx * xx) / (40320)) - ((xx * xx * xx * xx * xx) / (3628800)) + ((xx * xx * xx * xx * xx * xx) / (479001600)));
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("cos: %f,%f\n",res,cos(x)); Serial.printf("cos: %f,%f,%f,(%f)\n",phi,res,cos(x),res-cos(x));
#endif #endif
return res; return res;
} }
@ -36,7 +36,7 @@ float cos_t(float x)
float sin_t(float x) { float sin_t(float x) {
float res = cos_t(HALF_PI - x); float res = cos_t(HALF_PI - x);
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("sin: %f,%f\n",res,sin(x)); Serial.printf("sin: %f,%f,%f,(%f)\n",x,res,sin(x),res-sin(x));
#endif #endif
return res; return res;
} }
@ -46,7 +46,7 @@ float tan_t(float x) {
if (c==0.0) return 0; if (c==0.0) return 0;
float res = sin_t(x) / c; float res = sin_t(x) / c;
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("tan: %f,%f\n",res,tan(x)); Serial.printf("tan: %f,%f,%f,(%f)\n",x,res,tan(x),res-tan(x));
#endif #endif
return res; return res;
} }
@ -67,7 +67,7 @@ float acos_t(float x) {
ret = ret - 2 * negate * ret; ret = ret - 2 * negate * ret;
float res = negate * PI + ret; float res = negate * PI + ret;
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("acos,%f,%f,%f\n",x,res,acos(x)); Serial.printf("acos: %f,%f,%f,(%f)\n",x,res,acos(x),res-acos(x));
#endif #endif
return res; return res;
} }
@ -75,12 +75,12 @@ float acos_t(float x) {
float asin_t(float x) { float asin_t(float x) {
float res = HALF_PI - acos_t(x); float res = HALF_PI - acos_t(x);
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("asin,%f,%f,%f\n",x,res,asin(x)); Serial.printf("asin: %f,%f,%f,(%f)\n",x,res,asin(x),res-asin(x));
#endif #endif
return res; return res;
} }
/*
//https://stackoverflow.com/a/42542593 //https://stackoverflow.com/a/42542593
#define A 0.0776509570923569 #define A 0.0776509570923569
#define B -0.287434475393028 #define B -0.287434475393028
@ -112,43 +112,7 @@ float atan_t(float x) {
} }
if (neg) res = -res; if (neg) res = -res;
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("atan,%f,%f,%f\n",xinput,res,atan(xinput)); Serial.printf("atan: %f,%f,%f,(%f)\n",xinput,res,atan(xinput),res-atan(xinput));
#endif
return res;
}
*/
//https://stackoverflow.com/a/42542593
#define A 0.0776509570923569
#define B -0.287434475393028
#define C ((HALF_PI/2) - A - B)
//polynominal factors for approximation between 1 and 5
#define C0 0.089494f
#define C1 0.974207f
#define C2 -0.326175f
#define C3 0.05375f
#define C4 -0.003445f
float atan_t(float x) {
bool neg = (x < 0);
#ifdef WLED_DEBUG_MATH
float xinput = x;
#endif
x = std::abs(x);
float res;
if (x > 5.0f) { //atan(x) converges to pi/2 - (1/x) for large values
res = HALF_PI - (1.0f/x);
}
else if (x > 1.0f) { //1 < x < 5
float xx = x * x;
res = (C4*xx*xx)+(C3*xx*x)+(C2*xx)+(C1*x)+C0;
} else { //this approximation is only for x <= 1
float xx = x * x;
res = ((A*xx + B)*xx + C)*x;
}
if (neg) res = -res;
#ifdef WLED_DEBUG_MATH
Serial.printf("atan,%f,%f,%f\n",xinput,res,atan(xinput));
#endif #endif
return res; return res;
} }
@ -158,7 +122,7 @@ float floor_t(float x) {
int val = x; int val = x;
if (neg) val--; if (neg) val--;
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("floor: %f,%f\n",val,floor(x)); Serial.printf("floor: %f,%f,%f\n",x,(float)val,floor(x));
#endif #endif
return val; return val;
} }
@ -167,7 +131,7 @@ float fmod_t(float num, float denom) {
int tquot = num / denom; int tquot = num / denom;
float res = num - tquot * denom; float res = num - tquot * denom;
#ifdef WLED_DEBUG_MATH #ifdef WLED_DEBUG_MATH
Serial.printf("fmod: %f,%f\n",res,fmod(num,denom)); Serial.printf("fmod: %f,%f,(%f)\n",res,fmod(num,denom),res-fmod(num,denom));
#endif #endif
return res; return res;
} }