Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1687 | - | 1 | /****************************************************************/ |
| 2 | /* */ |
||
| 3 | /* NG-Video 5,8GHz */ |
||
| 4 | /* */ |
||
| 5 | /* Copyright (C) 2011 - gebad */ |
||
| 6 | /* */ |
||
| 7 | /* This code is distributed under the GNU Public License */ |
||
| 8 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
| 9 | /* */ |
||
| 10 | /****************************************************************/ |
||
| 11 | |||
| 12 | #include <stdio.h> |
||
| 13 | #include <stdlib.h> |
||
| 14 | #include <math.h> |
||
| 15 | #include <avr/pgmspace.h> |
||
| 16 | |||
| 17 | #include "mymath.h" |
||
| 18 | |||
| 19 | // http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2832_Bit.29 |
||
| 20 | unsigned int sqrt32(uint32_t q) |
||
| 21 | { uint16_t r, mask; |
||
| 22 | uint16_t i = 8*sizeof (r) -1; |
||
| 23 | |||
| 24 | r = mask = 1 << i; |
||
| 25 | for (; i > 0; i--) { |
||
| 26 | mask >>= 1; |
||
| 27 | |||
| 28 | if (q < (uint32_t) r*r) |
||
| 29 | r -= mask; |
||
| 30 | else |
||
| 31 | r += mask; |
||
| 32 | } |
||
| 33 | |||
| 34 | if (q < (uint32_t) r*r) |
||
| 35 | r -= 1; |
||
| 36 | |||
| 37 | return r; |
||
| 38 | } |
||
| 39 | |||
| 40 | // aus Orginal MK source code |
||
| 41 | // sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
||
| 42 | const uint16_t sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, \ |
||
| 43 | 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, \ |
||
| 44 | 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, \ |
||
| 45 | 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, \ |
||
| 46 | 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, \ |
||
| 47 | 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, \ |
||
| 48 | 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, \ |
||
| 49 | 8161, 8172, 8181, 8187, 8191, 8192}; |
||
| 50 | |||
| 51 | //double c_cos_8192(int16_t angle) |
||
| 52 | int16_t c_cos_8192(int16_t angle) |
||
| 53 | { |
||
| 54 | int8_t m,n; |
||
| 55 | int16_t sinus; |
||
| 56 | |||
| 57 | angle = 90 - angle; |
||
| 58 | // avoid negative angles |
||
| 59 | if (angle < 0) |
||
| 60 | { |
||
| 61 | m = -1; |
||
| 62 | angle = -angle; |
||
| 63 | } |
||
| 64 | else m = +1; |
||
| 65 | |||
| 66 | // fold angle to interval 0 to 359 |
||
| 67 | angle %= 360; |
||
| 68 | |||
| 69 | // check quadrant |
||
| 70 | if (angle <= 90) n = 1; // first quadrant |
||
| 71 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
||
| 72 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
||
| 73 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
||
| 74 | // get lookup value |
||
| 75 | sinus = sinlookup[angle]; |
||
| 76 | // calculate sinus value |
||
| 77 | return (sinus * m * n); |
||
| 78 | } |
||
| 79 | |||
| 80 | /* ---------------------------------------------------------------------------------------------- |
||
| 81 | |||
| 82 | atan2.S |
||
| 83 | |||
| 84 | Author: Reiner Patommel |
||
| 85 | |||
| 86 | atan2.S uses CORDIC, an algorithm which was developed in 1956 by Jack Volder. |
||
| 87 | CORDIC can be used to calculate trigonometric, hyperbolic and linear |
||
| 88 | functions and is until today implemented in most pocket calculators. |
||
| 89 | The algorithm makes only use of simple integer arithmetic. |
||
| 90 | |||
| 91 | The CORDIC equations in vectoring mode for trigonometric functions are: |
||
| 92 | Xi+1 = Xi - Si * (Yi * 1 / 2^i) |
||
| 93 | Yi+1 = Yi + Si * (Xi * 1 / 2^i) |
||
| 94 | Zi+1 = Zi - Si * atan(1 / 2^i) |
||
| 95 | where Si = +1 if Yi < 0 else Si = -1 |
||
| 96 | The rotation angles are limited to -PI/2 and PI/2 i.e. |
||
| 97 | -90 degrees ... +90 degrees |
||
| 98 | |||
| 99 | For the calculation of atan2(x,y) we need a small pre-calculated table of |
||
| 100 | angles or radians with the values Tn = atan(1/2^i). |
||
| 101 | We rotate the vector(Xo,Yo) in steps to the x-axis i.e. we drive Y to 0 and |
||
| 102 | accumulate the rotated angles or radians in Z. The direction of the rotation |
||
| 103 | will be positive or negative depending on the sign of Y after the previous |
||
| 104 | rotation and the rotation angle will decrease from step to step. (The first |
||
| 105 | step is 45 degrees, the last step is 0.002036 degrees for n = 15). |
||
| 106 | |||
| 107 | After n rotations the variables will have the following values: |
||
| 108 | Yn = ideally 0 |
||
| 109 | Xn = c*sqrt(Xo^2+Yo^2) (magnitude of the vector) |
||
| 110 | Zn = Zo+atan(Yo/Xo) (accumulated rotation angles or radians) |
||
| 111 | |||
| 112 | c, the cordic gain, is the product Pn of sqrt(1+2^(-2i)) and amounts to |
||
| 113 | approx. 1.64676 for n = 15. |
||
| 114 | |||
| 115 | In order to represent X, Y and Z as integers we introduce a scaling factor Q |
||
| 116 | which is chosen as follows: |
||
| 117 | 1. We normalize Xo and Yo (starting values) to be less than or equal to 1*Q and |
||
| 118 | set Zo = 0 i.e. Xomax = 1*Q, Yomax = 1*Q, Zo = 0. |
||
| 119 | 2. With Xo = 1*Q and Yo = 1*Q, Xn will be Xnmax = Q*c*sqrt(2) = 2.329*Q |
||
| 120 | 3. In order to boost accuracy we only cover the rotation angles between 0 and PI/2 |
||
| 121 | i.e. X and Z are always > 0 and therefore can be unsigned. |
||
| 122 | (We do the quadrant correction afterwards based on the initial signs of x and y) |
||
| 123 | 4. If X is an unsigned int, Xnmax = 65536, and Q = 65536/2.329 = 28140. |
||
| 124 | i.e. |
||
| 125 | Xnmax = 65536 --> unsigned int |
||
| 126 | Ynmax = +/- 28140 --> signed int |
||
| 127 | Znmax = PI/2 * 28140 = 44202 --> unsigned int |
||
| 128 | The systematic error is 90/44202 = 0.002036 degrees or 0.0000355 rad. |
||
| 129 | |||
| 130 | Below is atan2 and atan in C: */ |
||
| 131 | |||
| 132 | #define getAtanVal(x) (uint16_t)pgm_read_word(&atantab[x]) |
||
| 133 | //uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3,2,1}; // T 16 |
||
| 134 | // Genauigkeit etwas eingeschränkt, da Servos nur in 1° Abständen positioniert werden |
||
| 135 | uint16_t atantab[T] PROGMEM = {22101,13047,6894,3499,1756,879,440,220,110,55,27,14,7,3}; |
||
| 136 | |||
| 137 | // max. Wert für y oder x ist 76314 |
||
| 138 | int16_t my_atan2(int32_t y, int32_t x) |
||
| 139 | { unsigned char i; |
||
| 140 | uint32_t x1; |
||
| 141 | int32_t y1; |
||
| 142 | uint32_t angle = 0; |
||
| 143 | uint32_t tmp; |
||
| 144 | |||
| 145 | x1 = abs(x); |
||
| 146 | y1 = abs(y); |
||
| 147 | |||
| 148 | if (y1 == 0) { |
||
| 149 | if (x < 0) |
||
| 150 | return (180); |
||
| 151 | else |
||
| 152 | return 0; |
||
| 153 | } |
||
| 154 | |||
| 155 | if (x1 < y1) { |
||
| 156 | x1 = SCALED(x1) / y1; |
||
| 157 | y1 = Q; |
||
| 158 | } |
||
| 159 | else { |
||
| 160 | y1 = SCALED(y1) / x1; |
||
| 161 | x1 = Q; |
||
| 162 | } |
||
| 163 | |||
| 164 | for (i = 0; i < T-1; i++) { |
||
| 165 | tmp = x1 >> i; |
||
| 166 | if (y1 < 0) { |
||
| 167 | x1 -= y1 >> i; |
||
| 168 | y1 += tmp; |
||
| 169 | angle -= getAtanVal(i); |
||
| 170 | } |
||
| 171 | else { |
||
| 172 | x1 += y1 >> i; |
||
| 173 | y1 -= tmp; |
||
| 174 | angle += getAtanVal(i); |
||
| 175 | } |
||
| 176 | } |
||
| 177 | |||
| 178 | angle = angle/RQ; // RAD_TO_DEG * angle / Q |
||
| 179 | if (x <= 0) |
||
| 180 | angle = 180 - angle; |
||
| 181 | if (y <= 0) |
||
| 182 | return(-angle); |
||
| 183 | return(angle); |
||
| 184 | } |