16,7 → 16,7 |
// Fast arctan2 with max error of .07 rads |
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm |
//----------------------------------------------- |
signed char arctan_f(signed int x, signed int y) |
signed int arctan_f(signed int x, signed int y) |
{ |
float rad,r; |
short int m; |
96,18 → 96,35 |
|
|
|
const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000}; |
|
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000 |
signed int cos_i(signed int winkel) |
{ |
return (sin_i(90-winkel)); |
|
const float pgm_sinus_f [91] PROGMEM = {0.000,0.017,0.035,0.052,0.070,0.087,0.105,0.122,0.139,0.156,0.174,0.191,0.208,0.225,0.242,0.259,0.276,0.292,0.309,0.326,0.342,0.358,0.375,0.391,0.407,0.423,0.438,0.454,0.469,0.485,0.500,0.515,0.530,0.545,0.559,0.574,0.588,0.602,0.616,0.629,0.643,0.656,0.669,0.682,0.695,0.707,0.719,0.731,0.743,0.755,0.766,0.777,0.788,0.799,0.809,0.819,0.829,0.839,0.848,0.857,0.866,0.875,0.883,0.891,0.899,0.906,0.914,0.921,0.927,0.934,0.940,0.946,0.951,0.956,0.961,0.966,0.970,0.974,0.978,0.982,0.985,0.988,0.990,0.993,0.995,0.996,0.998,0.999,0.999,1.000,1.000}; |
|
inline float pgm_read_float(const float *addr) |
{ |
union |
{ |
uint16_t i[2]; // 2 16-bit-Worte |
float f; |
} u; |
|
u.i[0]=pgm_read_word((PGM_P)addr); |
u.i[1]=pgm_read_word((PGM_P)addr+2); |
|
return u.f; |
} |
|
// cosinus Funktion: Eingabewert Winkel in Grad |
float cos_f(signed int winkel) |
{ |
return (sin_f(90-winkel)); |
} |
|
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000 |
signed int sin_i(signed int winkel) |
// sinus Funktion: Eingabewert Winkel in Grad |
float sin_f(signed int winkel) |
{ |
short int m,n; |
float sinus; |
|
//winkel = winkel % 360; |
|
124,40 → 141,28 |
else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;} |
else n = 1; //0 - 90 Grad |
|
winkel = pgm_read_word(&pgm_sinus[winkel]); |
sinus = pgm_read_float(&pgm_sinus_f[winkel]); |
|
return (winkel*m*n); |
return (sinus*m*n); |
} |
|
|
|
const float pgm_sinus_f [91] PROGMEM = {0.00,0.02,0.03,0.05,0.07,0.09,0.10,0.12,0.14,0.16,0.17,0.19,0.21,0.23,0.24,0.26,0.28,0.29,0.31,0.33,0.34,0.36,0.38,0.39,0.41,0.42,0.44,0.45,0.47,0.49,0.50,0.52,0.53,0.55,0.56,0.57,0.59,0.60,0.62,0.63,0.64,0.66,0.67,0.68,0.70,0.71,0.72,0.73,0.74,0.76,0.77,0.78,0.79,0.80,0.81,0.82,0.83,0.84,0.85,0.86,0.87,0.88,0.88,0.89,0.90,0.91,0.91,0.92,0.93,0.93,0.94,0.95,0.95,0.96,0.96,0.97,0.97,0.97,0.98,0.98,0.98,0.99,0.99,0.99,0.99,1.00,1.00,1.00,1.00,1.00,1.00}; |
|
inline float pgm_read_float(const float *addr) |
{ |
union |
{ |
uint16_t i[2]; // 2 16-bit-Worte |
float f; |
} u; |
|
u.i[0]=pgm_read_word((PGM_P)addr); |
u.i[1]=pgm_read_word((PGM_P)addr+2); |
|
return u.f; |
} |
|
// cosinus Funktion: Eingabewert Winkel in Grad |
float cos_f(signed int winkel) |
{ |
return (sin_f(90-winkel)); |
|
const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000}; |
|
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000 |
signed int cos_i(signed int winkel) |
{ |
return (sin_i(90-winkel)); |
} |
|
// sinus Funktion: Eingabewert Winkel in Grad |
float sin_f(signed int winkel) |
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000 |
signed int sin_i(signed int winkel) |
{ |
short int m,n; |
float sinus; |
|
//winkel = winkel % 360; |
|
174,7 → 179,7 |
else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;} |
else n = 1; //0 - 90 Grad |
|
sinus = pgm_read_float(&pgm_sinus_f[winkel]); |
winkel = pgm_read_word(&pgm_sinus[winkel]); |
|
return (sinus*m*n); |
return (winkel*m*n); |
} |