Rev 1328 | Rev 1378 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1328 | Rev 1344 | ||
---|---|---|---|
1 | #include <stdlib.h> |
1 | #include <stdlib.h> |
2 | #include <avr/pgmspace.h> |
2 | #include <avr/pgmspace.h> |
3 | #include "mymath.h" |
3 | #include "mymath.h" |
4 | 4 | ||
5 | // discrete mathematics |
5 | // discrete mathematics |
6 | 6 | ||
7 | // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
7 | // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
8 | const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
8 | const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
9 | 9 | ||
10 | int16_t c_sin_8192(int16_t angle) |
10 | int16_t c_sin_8192(int16_t angle) |
11 | { |
11 | { |
12 | int8_t m,n; |
12 | int8_t m,n; |
13 | int16_t sinus; |
13 | int16_t sinus; |
14 | 14 | ||
15 | // avoid negative angles |
15 | // avoid negative angles |
16 | if (angle < 0) |
16 | if (angle < 0) |
17 | { |
17 | { |
18 | m = -1; |
18 | m = -1; |
19 | angle = abs(angle); |
19 | angle = abs(angle); |
20 | } |
20 | } |
21 | else m = +1; |
21 | else m = +1; |
22 | 22 | ||
23 | // fold angle to intervall 0 to 359 |
23 | // fold angle to intervall 0 to 359 |
24 | angle %= 360; |
24 | angle %= 360; |
25 | 25 | ||
26 | // check quadrant |
26 | // check quadrant |
27 | if (angle <= 90) n=1; // first quadrant |
27 | if (angle <= 90) n=1; // first quadrant |
28 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
28 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
29 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
29 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
30 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
30 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
31 | // get lookup value |
31 | // get lookup value |
32 | sinus = pgm_read_word(&pgm_sinlookup[angle]); |
32 | sinus = pgm_read_word(&pgm_sinlookup[angle]); |
33 | // calculate sinus value |
33 | // calculate sinus value |
34 | return (sinus * m * n); |
34 | return (sinus * m * n); |
35 | } |
35 | } |
36 | 36 | ||
37 | // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
37 | // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
38 | int16_t c_cos_8192(int16_t angle) |
38 | int16_t c_cos_8192(int16_t angle) |
39 | { |
39 | { |
40 | return (c_sin_8192(90 - angle)); |
40 | return (c_sin_8192(90 - angle)); |
41 | } |
41 | } |
42 | 42 | ||
43 | 43 | ||
44 | // Arcustangens returns degree in a range of +/. 180 deg |
44 | // Arcustangens returns degree in a range of +/. 180 deg |
45 | const uint8_t pgm_atanlookup[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}; |
45 | const uint8_t pgm_atanlookup[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}; |
46 | 46 | ||
47 | int16_t c_atan2(int16_t y, int16_t x) |
47 | int16_t c_atan2(int16_t y, int16_t x) |
48 | { |
48 | { |
49 | int16_t index, angle; |
49 | int16_t index, angle; |
50 | int8_t m; |
50 | int8_t m; |
51 | 51 | ||
52 | if (!x && !y) return 0; //atan2(0, 0) is undefined |
52 | if (!x && !y) return 0; //atan2(0, 0) is undefined |
53 | 53 | ||
54 | if (y < 0) m = -1; |
54 | if (y < 0) m = -1; |
55 | else m = 1; |
55 | else m = 1; |
56 | 56 | ||
57 | if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
57 | if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
58 | 58 | ||
59 | index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
59 | index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
60 | if (index < 0) index = -index; |
60 | if (index < 0) index = -index; |
61 | 61 | ||
62 | if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
62 | if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
63 | else if (index > 7334) angle = 90; // limit is 90 deg |
63 | else if (index > 7334) angle = 90; // limit is 90 deg |
64 | else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
64 | else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
65 | else if (index > 1465) angle = 88; |
65 | else if (index > 1465) angle = 88; |
66 | else if (index > 1046) angle = 87; |
66 | else if (index > 1046) angle = 87; |
67 | else if (index > 813) angle = 86; |
67 | else if (index > 813) angle = 86; |
68 | else if (index > 664) angle = 85; |
68 | else if (index > 664) angle = 85; |
69 | else if (index > 561) angle = 84; |
69 | else if (index > 561) angle = 84; |
70 | else if (index > 486) angle = 83; |
70 | else if (index > 486) angle = 83; |
71 | else if (index > 428) angle = 82; |
71 | else if (index > 428) angle = 82; |
72 | else if (index > 382) angle = 81; |
72 | else if (index > 382) angle = 81; |
73 | else angle = 80; // (index>345) |
73 | else angle = 80; // (index>345) |
74 | 74 | ||
75 | if (x > 0) return (angle * m); // 1st and 4th quadrant |
75 | if (x > 0) return (angle * m); // 1st and 4th quadrant |
76 | else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
76 | else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
77 | else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
77 | else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
78 | } |
78 | } |
79 | 79 | ||
80 | 80 | ||
81 | // Integer square root |
81 | // Integer square root |
82 | // For details of the algorithm see the article http://www.embedded.com/98/9802fe2.htm |
82 | // For details of the algorithm see the article http://www.embedded.com/98/9802fe2.htm |
83 | uint32_t c_sqrt(uint32_t a) |
83 | uint32_t c_sqrt(uint32_t a) |
84 | { |
84 | { |
85 | uint32_t rem = 0; |
85 | uint32_t rem = 0; |
86 | uint32_t root = 0; |
86 | uint32_t root = 0; |
87 | uint8_t i; |
87 | uint8_t i; |
88 | 88 | ||
89 | for(i = 0; i < 16; i++) |
89 | for(i = 0; i < 16; i++) |
90 | { |
90 | { |
91 | root <<= 1; |
91 | root <<= 1; |
92 | rem = ((rem << 2) + (a >> 30)); |
92 | rem = ((rem << 2) + (a >> 30)); |
93 | a <<= 2; |
93 | a <<= 2; |
94 | root++; |
94 | root++; |
95 | if(root <= rem) |
95 | if(root <= rem) |
96 | { |
96 | { |
97 | rem -= root; |
97 | rem -= root; |
98 | root++; |
98 | root++; |
99 | } |
99 | } |
100 | else root--; |
100 | else root--; |
101 | } |
101 | } |
102 | return (root >> 1); |
102 | return (root >> 1); |
103 | } |
103 | } |
104 | - | ||
105 | uint16_t c_sqrt_int(uint16_t a) |
104 | |
106 | { |
- | |
107 | uint16_t rem = 0; |
- | |
108 | uint16_t root = 0; |
- | |
109 | uint8_t i; |
- | |
110 | for(i = 0; i < 16; i++) |
- | |
111 | { |
- | |
112 | root <<= 1; |
- | |
113 | rem = ((rem << 2) + (a >> 30)); |
- | |
114 | a <<= 2; |
- | |
115 | root++; |
- | |
116 | if(root <= rem) |
- | |
117 | { |
- | |
118 | rem -= root; |
- | |
119 | root++; |
- | |
120 | } |
- | |
121 | else root--; |
- | |
122 | } |
- | |
123 | return (root >> 1); |
- | |
124 | } |
105 | |
125 | 106 | ||
126 | 107 |