Rev 214 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 214 | Rev 226 | ||
---|---|---|---|
Line 11... | Line 11... | ||
11 | */ |
11 | */ |
Line 12... | Line 12... | ||
12 | 12 | ||
Line 13... | Line 13... | ||
13 | #include "main.h" |
13 | #include "main.h" |
14 | 14 | ||
15 | //----------------------------------------------- |
15 | //----------------------------------------------- |
16 | // Fast arctan2 with max error of .01 rads |
16 | // Fast arctan2 with max error of .07 rads |
17 | // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm |
17 | // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm |
18 | //----------------------------------------------- |
18 | //----------------------------------------------- |
19 | float arctan_f(signed int x, signed int y) |
19 | signed char arctan_f(signed int x, signed int y) |
20 | { |
20 | { |
- | 21 | float rad,r; |
|
- | 22 | short int m; |
|
21 | float rad,r; |
23 | #define coeff_1 0.7854 |
Line 22... | Line 24... | ||
22 | signed int angle; |
24 | #define coeff_2 2.3562 |
Line 23... | Line 25... | ||
23 | short int m; |
25 | #define rad2grad 57.2958 |
24 | 26 | ||
Line 25... | Line 27... | ||
25 | if (!x && !y) return 0; |
27 | if (!x && !y) return 0; |
26 | 28 | ||
27 | if (y < 0) {y = abs(y); m = -1;} |
29 | if (y < 0) {y = abs(y); m = -1;} |
28 | else m = 1; |
30 | else m = 1; |
29 | 31 | ||
30 | if (x>=0) |
32 | if (x>=0) |
31 | { |
33 | { |
32 | r = (x - y) / (x + y); |
34 | r = (float)(x - y) / (float)(x + y); |
33 | rad = 0.1963*r*r*r - 0.9817*r + 0.7854; |
35 | rad = coeff_1 - coeff_1*r; |
34 | } |
36 | } |
35 | else |
37 | else |
- | 38 | { |
|
- | 39 | r = (float)(x + y) / (float)(y - x); |
|
36 | { |
40 | rad = coeff_2 - coeff_1*r; |
37 | r = (x + y) / (y - x); |
41 | } |
Line 38... | Line 42... | ||
38 | rad = 0.1963*r*r*r - 0.9817*r + 2.3562; |
42 | |
39 | } |
43 | rad *= rad2grad; |
40 | 44 | ||
41 | return(rad*m); |
45 | return(rad*m); |
42 | } |
46 | } |
43 | 47 | ||
44 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
45 | Peter Muehlenbrock |
48 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
46 | arctan in brute-force Art |
49 | Peter Muehlenbrock |
47 | Stand 1.10.2007 |
50 | arctan in brute-force Art |
48 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
51 | Stand 1.10.2007 |
49 | */ |
52 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
50 | 53 | */ |
|
51 | // arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad |
54 | // arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad |
Line 65... | Line 68... | ||
65 | } |
68 | } |
Line 66... | Line 69... | ||
66 | 69 | ||
Line 67... | Line 70... | ||
67 | // Quadranten ermitteln |
70 | // Quadranten ermitteln |
68 | 71 | ||
- | 72 | // Wert durch lineare Interpolation ermitteln |
|
69 | // Wert durch lineare Interpolation ermitteln |
73 | if (!y && !x) return 0; // Division durch 0 nicht erlaubt |
Line 70... | Line 74... | ||
70 | if ((y == 0) && (x == 0)) wert =1; // Division durch 0 nicht erlaubt |
74 | |
71 | else wert= abs((x*1000)/y); |
75 | else wert= abs((x*1000)/y); |
72 | 76 | ||
Line 88... | Line 92... | ||
88 | else if ((x < 0) && (y >= 0)) return_value = - return_value; |
92 | else if ((x < 0) && (y >= 0)) return_value = - return_value; |
Line 89... | Line 93... | ||
89 | 93 | ||
90 | return return_value; |
94 | return return_value; |
Line 91... | Line -... | ||
91 | } |
- | |
92 | - | ||
93 | // cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000 |
- | |
94 | signed int cos_i(signed int winkel) |
- | |
95 | { |
- | |
96 | winkel = sin_i(90-winkel); |
- | |
Line 97... | Line 95... | ||
97 | return winkel; |
95 | } |
Line -... | Line 96... | ||
- | 96 | ||
- | 97 | ||
- | 98 | ||
- | 99 | 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}; |
|
- | 100 | ||
- | 101 | // cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000 |
|
98 | } |
102 | signed int cos_i(signed int winkel) |
99 | 103 | { |
|
100 | 104 | return (sin_i(90-winkel)); |
|
101 | 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}; |
105 | } |
Line 124... | Line 128... | ||
124 | 128 | ||
125 | return (winkel*m*n); |
129 | return (winkel*m*n); |
Line -... | Line 130... | ||
- | 130 | } |
|
- | 131 | ||
- | 132 | ||
- | 133 | ||
- | 134 | 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}; |
|
- | 135 | ||
- | 136 | inline float pgm_read_float(const float *addr) |
|
- | 137 | { |
|
- | 138 | union |
|
- | 139 | { |
|
- | 140 | uint16_t i[2]; // 2 16-bit-Worte |
|
- | 141 | float f; |
|
- | 142 | } u; |
|
- | 143 | ||
- | 144 | u.i[0]=pgm_read_word((PGM_P)addr); |
|
- | 145 | u.i[1]=pgm_read_word((PGM_P)addr+2); |
|
- | 146 | ||
- | 147 | return u.f; |
|
- | 148 | } |
|
- | 149 | ||
- | 150 | // cosinus Funktion: Eingabewert Winkel in Grad |
|
- | 151 | float cos_f(signed int winkel) |
|
- | 152 | { |
|
- | 153 | return (sin_f(90-winkel)); |
|
- | 154 | } |
|
- | 155 | ||
- | 156 | // sinus Funktion: Eingabewert Winkel in Grad |
|
- | 157 | float sin_f(signed int winkel) |
|
- | 158 | { |
|
- | 159 | short int m,n; |
|
- | 160 | float sinus; |
|
- | 161 | ||
- | 162 | //winkel = winkel % 360; |
|
- | 163 | ||
- | 164 | if (winkel < 0) |
|
- | 165 | { |
|
- | 166 | m = -1; |
|
- | 167 | winkel = abs(winkel); |
|
- | 168 | } |
|
- | 169 | else m = +1; |
|
- | 170 | ||
- | 171 | // Quadranten auswerten |
|
- | 172 | if ((winkel > 90 ) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;} |
|
- | 173 | else if ((winkel > 180 ) && (winkel <= 270)) {winkel = winkel - 180; n = -1;} |
|
- | 174 | else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;} |
|
- | 175 | else n = 1; //0 - 90 Grad |
|
- | 176 | ||
- | 177 | sinus = pgm_read_float(&pgm_sinus_f[winkel]); |