Rev 2147 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2147 | Rev 2212 | ||
|---|---|---|---|
| Line 46... | Line 46... | ||
| 46 | return r; |
46 | return r; |
| 47 | } |
47 | } |
| Line 48... | Line 48... | ||
| 48 | 48 | ||
| 49 | // aus Orginal MK source code |
49 | // aus Orginal MK source code |
| 50 | // sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
50 | // sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
| 51 | const uint16_t sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
51 | const int16_t sinlookup[91] = { 0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
| 52 | 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
52 | 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
| 53 | 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
53 | 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
| 54 | 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
54 | 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
| 55 | 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
55 | 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
| 56 | 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
56 | 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
| 57 | 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
57 | 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
| Line 58... | Line -... | ||
| 58 | 8161, 8172, 8181, 8187, 8191, 8192}; |
- | |
| - | 58 | 8161, 8172, 8181, 8187, 8191, 8192}; |
|
| 59 | 59 | ||
| 60 | //double c_cos_8192(int16_t angle) |
60 | |
| - | 61 | int16_t c_cos_8192(int16_t angle) |
|
| - | 62 | { |
|
| - | 63 | return (c_sin_8192(90 - angle)); |
|
| - | 64 | } |
|
| - | 65 | ||
| - | 66 | ||
| - | 67 | ||
| 61 | int16_t c_cos_8192(int16_t angle) |
68 | int16_t c_sin_8192(int16_t angle) |
| 62 | { |
69 | { |
| Line 63... | Line -... | ||
| 63 | int8_t m,n; |
- | |
| 64 | int16_t sinus; |
70 | int8_t m, n; |
| 65 | 71 | int16_t sinus; |
|
| 66 | angle = 90 - angle; |
72 | |
| 67 | // avoid negative angles |
73 | // avoid negative angles |
| 68 | if (angle < 0) |
74 | if (angle < 0) |
| 69 | { |
75 | { |
| 70 | m = -1; |
76 | m = -1; |
| Line 71... | Line 77... | ||
| 71 | angle = -angle; |
77 | angle = -angle; |
| 72 | } |
78 | } |
| Line 73... | Line 79... | ||
| 73 | else m = +1; |
79 | else m = +1; |
| 74 | 80 | ||
| 75 | // fold angle to interval 0 to 359 |
81 | // fold angle to interval 0 to 359 |
| 76 | angle %= 360; |
82 | angle %= 360; |
| 77 | 83 | ||
| 78 | // check quadrant |
84 | // check quadrant |
| 79 | if (angle <= 90) n = 1; // first quadrant |
85 | if (angle <= 90) n = 1; // first quadrant |
| 80 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
86 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
| 81 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
87 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
| 82 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
88 | else {angle = 360 - angle; n = -1;} //fourth quadrant |