Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
149 salvo 1
/*
2
This program (files math.c and math.h) is free software; you can redistribute it and/or modify
3
it under the terms of the GNU General Public License as published by the Free Software Foundation;
4
either version 3 of the License, or (at your option) any later version.  
5
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
6
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
7
GNU General Public License for more details. You should have received a copy of the GNU General Public License
8
along with this program. If not, see <http://www.gnu.org/licenses/>.
9
 
10
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
11
*/
12
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
Peter Muehlenbrock
14
Winkelfunktionen sin, cos und arctan in
15
brute-force Art: Sehr Schnell, nicht sonderlich genau, aber ausreichend
16
Stand 11.9.2007
17
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18
*/
19
 
20
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
21
signed int arctan_i(signed int x, signed int y)
22
{
23
        short int sign = 0;
24
        signed int i;
25
 
26
        if (y > x)  // x,y Werte vertauschen
27
        {      
28
                sign = 1;
29
                i = x;
30
                x = y;
31
                y = i;
32
        }
33
 
34
}
35
 
36
 
37
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
38
signed int cos_i(signed int winkel)
39
{
40
  winkel = sin_i(90-winkel);
41
  return winkel;
42
}
43
 
44
 
45
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
46
signed int sin_i(signed int winkel)
47
{
48
 short int m;
49
 winkel = winkel % 360;
50
 // Quadranten auswerten
51
 if ((winkel >= 90 ) && (winkel <180))
52
 {
53
        winkel = 180 - winkel;
54
        m = 1;
55
 }
56
 else if ((winkel >= 180 ) && (winkel <270))
57
 {
58
        winkel = winkel - 180;
59
        m = -1;
60
 }
61
 else if ((winkel >= 270 ) && (winkel < 360))
62
 {
63
        winkel = 360 -winkel ;
64
        m = -1;
65
 }
66
 else if ((winkel < 0) && (winkel > -90))
67
 {
68
        winkel = (abs(winkel));
69
        m = -1;
70
 }
71
else if ((winkel < -90) && (winkel > -180))
72
 {
73
        winkel = 180-(abs(winkel));
74
        m = -1;
75
 }
76
else if ((winkel < -180) && (winkel > -270))
77
 {
78
        winkel = (abs(winkel))-180;
79
        m = +1;
80
 }
81
else if ((winkel < -270) && (winkel > -360))
82
 {
83
        winkel = 360-abs(winkel);
84
        m = +1;
85
 }
86
 else  
87
 {
88
        m = +1;
89
 }
90
 
91
 
92
 //Aus Tabelle Werte holen
93
        switch (winkel)
94
        {
95
                case 1:
96
                        winkel = 17;
97
                        break;
98
                case 2:
99
                        winkel = 35;
100
                        break;
101
                case 3:
102
                        winkel = 52;
103
                        break;
104
                case 4:
105
                        winkel = 70;
106
                        break;
107
                case 5:
108
                        winkel = 87;
109
                        break;
110
                case 6:
111
                        winkel = 105;
112
                        break;
113
                case 7:
114
                        winkel = 122;
115
                        break;
116
                case 8:
117
                        winkel = 139;
118
                        break;
119
                case 9:
120
                        winkel = 156;
121
                        break;
122
                case 10:
123
                        winkel = 174;
124
                        break;
125
                case 11:
126
                        winkel = 191;
127
                        break;
128
                case 12:
129
                        winkel = 208;
130
                        break;
131
                case 13:
132
                        winkel = 225;
133
                        break;
134
                case 14:
135
                        winkel = 242;
136
                        break;
137
                case 15:
138
                        winkel = 259;
139
                        break;
140
                case 16:
141
                        winkel = 276;
142
                        break;
143
                case 17:
144
                        winkel = 292;
145
                        break;
146
                case 18:
147
                        winkel = 309;
148
                        break;
149
                case 19:
150
                        winkel = 326;
151
                        break;
152
                case 20:
153
                        winkel = 342;
154
                        break;
155
                case 21:
156
                        winkel = 359;
157
                        break;
158
                case 22:
159
                        winkel = 375;
160
                        break;
161
                case 23:
162
                        winkel = 391;
163
                        break;
164
                case 24:
165
                        winkel = 407;
166
                        break;
167
                case 25:
168
                        winkel = 423;
169
                        break;
170
                case 26:
171
                        winkel = 438;
172
                        break;
173
                case 27:
174
                        winkel = 454;
175
                        break;
176
                case 28:
177
                        winkel = 469;
178
                        break;
179
                case 29:
180
                        winkel = 485;
181
                        break;
182
                case 30:
183
                        winkel = 500;
184
                        break;
185
                case 31:
186
                        winkel = 515;
187
                        break;
188
                case 32:
189
                        winkel = 530;
190
                        break;
191
                case 33:
192
                        winkel = 545;
193
                        break;
194
                case 34:
195
                        winkel = 559;
196
                        break;
197
                case 35:
198
                        winkel = 574;
199
                        break;
200
                case 36:
201
                        winkel = 588;
202
                        break;
203
                case 37:
204
                        winkel = 602;
205
                        break;
206
                case 38:
207
                        winkel = 616;
208
                        break;
209
                case 39:
210
                        winkel = 630;
211
                        break;
212
                case 40:
213
                        winkel = 643;
214
                        break;
215
                case 41:
216
                        winkel = 656;
217
                        break;
218
                case 42:
219
                        winkel = 682;
220
                        break;
221
                case 43:
222
                        winkel = 682;
223
                        break;
224
                case 44:
225
                        winkel = 695;
226
                        break;
227
                case 45:
228
                        winkel = 707;
229
                        break;
230
                case 46:
231
                        winkel = 719;
232
                        break;
233
                case 47:
234
                        winkel = 731;
235
                case 48:
236
                        winkel = 743;
237
                        break;
238
                case 49:
239
                        winkel = 755;
240
                        break;
241
                case 50:
242
                        winkel = 766;
243
                        break;
244
                case 51:
245
                        winkel = 777;
246
                        break;
247
                case 52:
248
                        winkel = 788;
249
                        break;
250
                case 53:
251
                        winkel = 799;
252
                        break;
253
                case 54:
254
                        winkel = 809;
255
                        break;
256
                case 55:
257
                        winkel = 819;
258
                        break;
259
                case 56:
260
                        winkel = 829;
261
                        break;
262
                case 57:
263
                        winkel = 839;
264
                        break;
265
                case 58:
266
                        winkel = 848;
267
                        break;
268
                case 59:
269
                        winkel = 857;
270
                        break;
271
                case 60:
272
                        winkel = 866;
273
                        break;
274
                case 61:
275
                        winkel = 875;
276
                        break;
277
                case 62:
278
                        winkel = 883;
279
                        break;
280
                case 63:
281
                        winkel = 891;
282
                        break;
283
                case 64:
284
                        winkel = 899;
285
                        break;
286
                case 65:
287
                        winkel = 906;
288
                        break;
289
                case 66:
290
                        winkel = 914;
291
                        break;
292
                case 67:
293
                        winkel = 921;
294
                        break;
295
                case 68:
296
                        winkel = 927;
297
                        break;
298
                case 69:
299
                        winkel = 934;
300
                        break;
301
                case 70:
302
                        winkel = 940;
303
                        break;
304
                case 71:
305
                        winkel = 946;
306
                        break;
307
                case 72:
308
                        winkel = 951;
309
                        break;
310
                case 73:
311
                        winkel = 956;
312
                        break;
313
                case 74:
314
                        winkel = 961;
315
                        break;
316
                case 75:
317
                        winkel = 966;
318
                        break;
319
                case 76:
320
                        winkel = 970;
321
                        break;
322
                case 77:
323
                        winkel = 974;
324
                        break;
325
                case 78:
326
                        winkel = 978;
327
                        break;
328
                case 79:
329
                        winkel = 982;
330
                        break;
331
                case 80:
332
                        winkel = 985;
333
                        break;
334
                case 81:
335
                        winkel = 988;
336
                        break;
337
                case 82:
338
                        winkel = 990;
339
                        break;
340
                case 83:
341
                        winkel = 993;
342
                        break;
343
                case 84:
344
                        winkel = 995;
345
                        break;
346
                case 85:
347
                        winkel = 996;
348
                        break;
349
                case 86:
350
                        winkel = 998;
351
                        break;
352
                case 87:
353
                        winkel = 999;
354
                        break;
355
                case 88:
356
                        winkel = 999;
357
                        break;
358
                case 89:
359
                        winkel = 1000;
360
                        break;
361
                case 90:
362
                        winkel = 1000;
363
                        break;
364
                default:
365
                        winkel = 0;
366
                        break;
367
 
368
        }
369
return (winkel*m);
370
 
371
}
372
 
373