Rev 796 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
796 | joko | 1 | /* |
2 | |||
3 | Copyright 2007, Niklas Nold |
||
4 | |||
5 | This program (files math.c and math.h) is free software; you can redistribute it and/or modify |
||
6 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
||
7 | either version 3 of the License, or (at your option) any later version. |
||
8 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
||
9 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
10 | GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
||
11 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
12 | |||
13 | 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 |
||
14 | */ |
||
15 | |||
16 | #include "main.h" |
||
17 | |||
18 | |||
19 | const uint8_t pgm_atan[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
||
20 | |||
21 | //############################################################################ |
||
22 | // Arkustangens2 im Gradmaß |
||
23 | signed int atan2_i(signed int x, signed int y) |
||
24 | //############################################################################ |
||
25 | { |
||
26 | int i,angle; |
||
27 | int8_t m; |
||
28 | |||
29 | if (!x && !y) return 0; //atan2 = 0 für x und y = 0 |
||
30 | |||
31 | if (y < 0) m=-1; |
||
32 | else m=1; |
||
33 | |||
34 | if (!x) return (90*m); // atan2 = 90° für x = 0 |
||
35 | |||
36 | i = abs(((long)y*64) / x); // Berechne i für die Lookup table (Schrittweite atan(x) ist 0,015625 -> y *64) |
||
37 | |||
38 | if (i<346) angle = pgm_read_byte(&pgm_atan[i]); // Lookup für 1° bis 79° |
||
39 | else if (i>7334) angle = 90; // Grenzwert ist 90° |
||
40 | else if (i>2444) angle = 89; // 89° bis 80° über Wertebereiche |
||
41 | else if (i>1465) angle = 88; |
||
42 | else if (i>1046) angle = 87; |
||
43 | else if (i>813) angle = 86; |
||
44 | else if (i>664) angle = 85; |
||
45 | else if (i>561) angle = 84; |
||
46 | else if (i>486) angle = 83; |
||
47 | else if (i>428) angle = 82; |
||
48 | else if (i>382) angle = 81; |
||
49 | else angle = 80; // (i>345) |
||
50 | |||
51 | if (x > 0) return (angle*m); // Quadrant I und IV |
||
52 | else if ((x < 0) && (m > 0)) return (-angle + 180); // Quadrant II |
||
53 | else return (angle - 180); // x < 0 && y < 0 Quadrant III |
||
54 | } |
||
55 | |||
56 | /* |
||
57 | const uint16_t pgm_sinus_i[91] PROGMEM = { |
||
58 | 0, 18, 36, 54, 71, 89, 107, 125, 143, 160, 178, 195, 213, 230, 248, |
||
59 | 265, 282, 299, 316, 333, 350, 367, 384, 400, 416, 433, 449, 465, 481, 496, |
||
60 | 512, 527, 543, 558, 573, 587, 602, 616, 630, 644, 658, 672, 685, 698, 711, |
||
61 | 724, 737, 749, 761, 773, 784, 796, 807, 818, 828, 839, 849, 859, 868, 878, |
||
62 | 887, 896, 904, 912, 920, 928, 935, 943, 949, 956, 962, 968, 974, 979, 984, |
||
63 | 989, 994, 998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024, |
||
64 | 1024 |
||
65 | }; //für div 1024 = div 2^10 = (>>10) |
||
66 | */ |
||
67 | const uint16_t pgm_sinus_i[91] PROGMEM = { |
||
68 | 0,71,143,214,286,357,428,499,570,641,711,782,852,921,991, |
||
69 | 1060,1129,1198,1266,1334,1401,1468,1534,1600,1666,1731,1796,1860,1923,1986, |
||
70 | 2048,2110,2171,2231,2290,2349,2408,2465,2522,2578,2633,2687,2741,2793,2845, |
||
71 | 2896,2946,2996,3044,3091,3138,3183,3228,3271,3314,3355,3396,3435,3474,3511, |
||
72 | 3547,3582,3617,3650,3681,3712,3742,3770,3798,3824,3849,3873,3896,3917,3937, |
||
73 | 3956,3974,3991,4006,4021,4034,4046,4056,4065,4074,4080,4086,4090,4094,4095, |
||
74 | 4096 |
||
75 | }; //für div 4096 = div 2^12 = (>>12) |
||
76 | |||
77 | |||
78 | //############################################################################ |
||
79 | // Kosinusfunktion im Gradmaß |
||
80 | signed int cos_i(signed int winkel) |
||
81 | //############################################################################ |
||
82 | { |
||
83 | return (sin_i(90-winkel)); |
||
84 | } |
||
85 | |||
86 | //############################################################################ |
||
87 | // Sinusfunktion im Gradmaß |
||
88 | signed int sin_i(signed int winkel) |
||
89 | //############################################################################ |
||
90 | { |
||
91 | short int m,n; |
||
92 | signed int sinus; |
||
93 | |||
94 | if (winkel < 0) |
||
95 | { |
||
96 | m = -1; |
||
97 | winkel = abs(winkel); |
||
98 | } |
||
99 | else m = +1; |
||
100 | |||
101 | // Quadranten auswerten |
||
102 | if (winkel <= 90) n=1; |
||
103 | else if ((winkel > 90) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;} |
||
104 | else if ((winkel > 180) && (winkel <= 270)) {winkel = winkel - 180; n = -1;} |
||
105 | else {winkel = 360 - winkel; n = -1;} //if ((winkel > 270) && (winkel <= 360)) |
||
106 | |||
107 | sinus = pgm_read_word(&pgm_sinus_i[winkel]); |
||
108 | |||
109 | return (sinus*m*n); |
||
110 | } |
||
111 |