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
*/
155 salvo 19
#include "main.h"
149 salvo 20
 
155 salvo 21
 
149 salvo 22
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
155 salvo 23
int arctan_i(long signed int x, long signed int y)
149 salvo 24
{
155 salvo 25
        short int change_xy = 0;
149 salvo 26
        signed int i;
155 salvo 27
        long signed int wert;
28
        int return_value;
149 salvo 29
 
155 salvo 30
        if ((abs(x)) > (abs(y)))  // x,y Werte vertauschen damit arctan <45 grad bleibt
149 salvo 31
        {      
155 salvo 32
                change_xy       = 1;
33
                i                       = x;
34
                x                       = y;
35
                y                       = i;
149 salvo 36
        }
155 salvo 37
 
38
        // Quadranten ermitteln
149 salvo 39
 
155 salvo 40
        // Wert durch lineare Interpolation ermitteln 
41
        wert= abs((x*1000)/y);
42
 
43
        if (wert <=268) //0...0.0,268  entsprechend 0..15 Grad
44
        {
45
                return_value = (int)((wert*100)/(268-0)*(15-0)/100) +0;
46
        }
47
        else if (wert <=578) //0,268...0.0,568  entsprechend 15..30 Grad
48
        {      
49
                return_value = (int)((((wert-268)*100)/(578-268)*(30-15))/100) +15;            
50
        }
51
        else //0,568...1  entsprechend 30..45 Grad
52
        {      
53
                return_value = (int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30;             
54
        }
55
 
56
        if (change_xy > 0)  return_value = 90-return_value; //Quadrant 45..90 Grad
57
        if ((x > 0) && (y <0)) return_value = - return_value;
58
        else if ((x < 0) && (y > 0)) return_value = - return_value;
59
 
60
return return_value;
149 salvo 61
}
62
 
63
 
64
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
65
signed int cos_i(signed int winkel)
66
{
67
  winkel = sin_i(90-winkel);
68
  return winkel;
69
}
70
 
71
 
72
// sinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =sin(winkel)*1000
73
signed int sin_i(signed int winkel)
74
{
75
 short int m;
76
 winkel = winkel % 360;
77
 // Quadranten auswerten
78
 if ((winkel >= 90 ) && (winkel <180))
79
 {
80
        winkel = 180 - winkel;
81
        m = 1;
82
 }
83
 else if ((winkel >= 180 ) && (winkel <270))
84
 {
85
        winkel = winkel - 180;
86
        m = -1;
87
 }
88
 else if ((winkel >= 270 ) && (winkel < 360))
89
 {
90
        winkel = 360 -winkel ;
91
        m = -1;
92
 }
93
 else if ((winkel < 0) && (winkel > -90))
94
 {
95
        winkel = (abs(winkel));
96
        m = -1;
97
 }
98
else if ((winkel < -90) && (winkel > -180))
99
 {
100
        winkel = 180-(abs(winkel));
101
        m = -1;
102
 }
103
else if ((winkel < -180) && (winkel > -270))
104
 {
105
        winkel = (abs(winkel))-180;
106
        m = +1;
107
 }
108
else if ((winkel < -270) && (winkel > -360))
109
 {
110
        winkel = 360-abs(winkel);
111
        m = +1;
112
 }
113
 else  
114
 {
115
        m = +1;
116
 }
117
 
118
 
119
 //Aus Tabelle Werte holen
120
        switch (winkel)
121
        {
122
                case 1:
123
                        winkel = 17;
124
                        break;
125
                case 2:
126
                        winkel = 35;
127
                        break;
128
                case 3:
129
                        winkel = 52;
130
                        break;
131
                case 4:
132
                        winkel = 70;
133
                        break;
134
                case 5:
135
                        winkel = 87;
136
                        break;
137
                case 6:
138
                        winkel = 105;
139
                        break;
140
                case 7:
141
                        winkel = 122;
142
                        break;
143
                case 8:
144
                        winkel = 139;
145
                        break;
146
                case 9:
147
                        winkel = 156;
148
                        break;
149
                case 10:
150
                        winkel = 174;
151
                        break;
152
                case 11:
153
                        winkel = 191;
154
                        break;
155
                case 12:
156
                        winkel = 208;
157
                        break;
158
                case 13:
159
                        winkel = 225;
160
                        break;
161
                case 14:
162
                        winkel = 242;
163
                        break;
164
                case 15:
165
                        winkel = 259;
166
                        break;
167
                case 16:
168
                        winkel = 276;
169
                        break;
170
                case 17:
171
                        winkel = 292;
172
                        break;
173
                case 18:
174
                        winkel = 309;
175
                        break;
176
                case 19:
177
                        winkel = 326;
178
                        break;
179
                case 20:
180
                        winkel = 342;
181
                        break;
182
                case 21:
183
                        winkel = 359;
184
                        break;
185
                case 22:
186
                        winkel = 375;
187
                        break;
188
                case 23:
189
                        winkel = 391;
190
                        break;
191
                case 24:
192
                        winkel = 407;
193
                        break;
194
                case 25:
195
                        winkel = 423;
196
                        break;
197
                case 26:
198
                        winkel = 438;
199
                        break;
200
                case 27:
201
                        winkel = 454;
202
                        break;
203
                case 28:
204
                        winkel = 469;
205
                        break;
206
                case 29:
207
                        winkel = 485;
208
                        break;
209
                case 30:
210
                        winkel = 500;
211
                        break;
212
                case 31:
213
                        winkel = 515;
214
                        break;
215
                case 32:
216
                        winkel = 530;
217
                        break;
218
                case 33:
219
                        winkel = 545;
220
                        break;
221
                case 34:
222
                        winkel = 559;
223
                        break;
224
                case 35:
225
                        winkel = 574;
226
                        break;
227
                case 36:
228
                        winkel = 588;
229
                        break;
230
                case 37:
231
                        winkel = 602;
232
                        break;
233
                case 38:
234
                        winkel = 616;
235
                        break;
236
                case 39:
237
                        winkel = 630;
238
                        break;
239
                case 40:
240
                        winkel = 643;
241
                        break;
242
                case 41:
243
                        winkel = 656;
244
                        break;
245
                case 42:
246
                        winkel = 682;
247
                        break;
248
                case 43:
249
                        winkel = 682;
250
                        break;
251
                case 44:
252
                        winkel = 695;
253
                        break;
254
                case 45:
255
                        winkel = 707;
256
                        break;
257
                case 46:
258
                        winkel = 719;
259
                        break;
260
                case 47:
261
                        winkel = 731;
262
                case 48:
263
                        winkel = 743;
264
                        break;
265
                case 49:
266
                        winkel = 755;
267
                        break;
268
                case 50:
269
                        winkel = 766;
270
                        break;
271
                case 51:
272
                        winkel = 777;
273
                        break;
274
                case 52:
275
                        winkel = 788;
276
                        break;
277
                case 53:
278
                        winkel = 799;
279
                        break;
280
                case 54:
281
                        winkel = 809;
282
                        break;
283
                case 55:
284
                        winkel = 819;
285
                        break;
286
                case 56:
287
                        winkel = 829;
288
                        break;
289
                case 57:
290
                        winkel = 839;
291
                        break;
292
                case 58:
293
                        winkel = 848;
294
                        break;
295
                case 59:
296
                        winkel = 857;
297
                        break;
298
                case 60:
299
                        winkel = 866;
300
                        break;
301
                case 61:
302
                        winkel = 875;
303
                        break;
304
                case 62:
305
                        winkel = 883;
306
                        break;
307
                case 63:
308
                        winkel = 891;
309
                        break;
310
                case 64:
311
                        winkel = 899;
312
                        break;
313
                case 65:
314
                        winkel = 906;
315
                        break;
316
                case 66:
317
                        winkel = 914;
318
                        break;
319
                case 67:
320
                        winkel = 921;
321
                        break;
322
                case 68:
323
                        winkel = 927;
324
                        break;
325
                case 69:
326
                        winkel = 934;
327
                        break;
328
                case 70:
329
                        winkel = 940;
330
                        break;
331
                case 71:
332
                        winkel = 946;
333
                        break;
334
                case 72:
335
                        winkel = 951;
336
                        break;
337
                case 73:
338
                        winkel = 956;
339
                        break;
340
                case 74:
341
                        winkel = 961;
342
                        break;
343
                case 75:
344
                        winkel = 966;
345
                        break;
346
                case 76:
347
                        winkel = 970;
348
                        break;
349
                case 77:
350
                        winkel = 974;
351
                        break;
352
                case 78:
353
                        winkel = 978;
354
                        break;
355
                case 79:
356
                        winkel = 982;
357
                        break;
358
                case 80:
359
                        winkel = 985;
360
                        break;
361
                case 81:
362
                        winkel = 988;
363
                        break;
364
                case 82:
365
                        winkel = 990;
366
                        break;
367
                case 83:
368
                        winkel = 993;
369
                        break;
370
                case 84:
371
                        winkel = 995;
372
                        break;
373
                case 85:
374
                        winkel = 996;
375
                        break;
376
                case 86:
377
                        winkel = 998;
378
                        break;
379
                case 87:
380
                        winkel = 999;
381
                        break;
382
                case 88:
383
                        winkel = 999;
384
                        break;
385
                case 89:
386
                        winkel = 1000;
387
                        break;
388
                case 90:
389
                        winkel = 1000;
390
                        break;
391
                default:
392
                        winkel = 0;
393
                        break;
394
 
395
        }
396
return (winkel*m);
397
 
398
}
399
 
400