| 13,14 → 13,16 |
|---|
| #include "main.h" |
| |
| //----------------------------------------------- |
| // Fast arctan2 with max error of .01 rads |
| // Fast arctan2 with max error of .07 rads |
| // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm |
| //----------------------------------------------- |
| float arctan_f(signed int x, signed int y) |
| signed char arctan_f(signed int x, signed int y) |
| { |
| float rad,r; |
| signed int angle; |
| short int m; |
| float rad,r; |
| short int m; |
| #define coeff_1 0.7854 |
| #define coeff_2 2.3562 |
| #define rad2grad 57.2958 |
| |
| if (!x && !y) return 0; |
| |
| 29,15 → 31,17 |
|---|
| |
| if (x>=0) |
| { |
| r = (x - y) / (x + y); |
| rad = 0.1963*r*r*r - 0.9817*r + 0.7854; |
| r = (float)(x - y) / (float)(x + y); |
| rad = coeff_1 - coeff_1*r; |
| } |
| else |
| { |
| r = (x + y) / (y - x); |
| rad = 0.1963*r*r*r - 0.9817*r + 2.3562; |
| r = (float)(x + y) / (float)(y - x); |
| rad = coeff_2 - coeff_1*r; |
| } |
| |
| |
| rad *= rad2grad; |
| |
| return(rad*m); |
| } |
| |
| 47,9 → 51,8 |
|---|
| Stand 1.10.2007 |
| ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| */ |
| |
| // arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad |
| int8_t arctan_i( signed int x, signed int y) |
| signed int arctan_i(signed int x, signed int y) |
| { |
| short int change_xy = 0; |
| signed int i; |
| 67,7 → 70,8 |
|---|
| // Quadranten ermitteln |
| |
| // Wert durch lineare Interpolation ermitteln |
| if ((y == 0) && (x == 0)) wert =1; // Division durch 0 nicht erlaubt |
| if (!y && !x) return 0; // Division durch 0 nicht erlaubt |
| |
| else wert= abs((x*1000)/y); |
| |
| if (wert <=268) //0...0.0,268 entsprechend 0..15 Grad |
| 90,16 → 94,16 |
|---|
| return return_value; |
| } |
| |
| |
| |
| 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) |
| { |
| winkel = sin_i(90-winkel); |
| return winkel; |
| { |
| return (sin_i(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}; |
| |
| // sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000 |
| signed int sin_i(signed int winkel) |
| { |
| 126,3 → 130,51 |
|---|
| } |
| |
| |
| |
| 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)); |
| } |
| |
| // sinus Funktion: Eingabewert Winkel in Grad |
| float sin_f(signed int winkel) |
| { |
| short int m,n; |
| float sinus; |
| |
| //winkel = winkel % 360; |
| |
| if (winkel < 0) |
| { |
| m = -1; |
| winkel = abs(winkel); |
| } |
| else m = +1; |
| |
| // Quadranten auswerten |
| if ((winkel > 90 ) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;} |
| else if ((winkel > 180 ) && (winkel <= 270)) {winkel = winkel - 180; n = -1;} |
| 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]); |
| |
| return (sinus*m*n); |
| } |