| 33 | | void cossin(double a_p, char x_p, char y_p, char *nx_p, char *ny_p) |
| 34 | | { |
| 35 | | *nx_p = (char) (x_p * cos(a_p) - y_p * sin(a_p)); |
| 36 | | *ny_p = (char) (x_p * sin(a_p) + y_p * cos(a_p)); |
| 37 | | } |
| 38 | | |
| 39 | | short F_PI = (short)((M_PI) * (1<< 7)); |
| 40 | | short ATAN[8] = { |
| 41 | | 0x65, // ATAN(2^-0) |
| 42 | | 0x3B, // ATAN(2^-1) |
| 43 | | 0x1F, // ATAN(2^-2) |
| 44 | | 0x10, // ATAN(2^-3) |
| 45 | | 0x08, // ATAN(2^-4) |
| 46 | | 0x04, // ATAN(2^-5) |
| 47 | | 0x02, // ATAN(2^-6) |
| 48 | | 0x01, // ATAN(2^-7) |
| 49 | | }; |
| 50 | | |
| 51 | | void cordic(short a_p, char x_p, char y_p, char *nx_p, char *ny_p) |
| 52 | | { |
| 53 | | unsigned char i, q; |
| 54 | | short a, x, y, dx, dy; |
| 55 | | |
| 56 | | // conversion en virgule fixe : 7 chiffres après la virgule |
| 57 | | a = a_p; |
| 58 | | x = x_p << 7; |
| 59 | | y = y_p << 7; |
| 60 | | |
| 61 | | // normalisalion de l'angle pour être dans le premier quadrant |
| 62 | | q = 0; |
| 63 | | while (a >= F_PI/2) { |
| 64 | | a = a - F_PI/2; |
| 65 | | q = (q + 1) & 3; |
| 66 | | } |
| 67 | | |
| 68 | | // rotation |
| 69 | | for (i = 0; i <= 7; i++) { |
| 70 | | dx = x >> i; |
| 71 | | dy = y >> i; |
| 72 | | if (a >= 0) { |
| 73 | | x -= dy; |
| 74 | | y += dx; |
| 75 | | a -= ATAN[i]; |
| 76 | | } else { |
| 77 | | x += dy; |
| 78 | | y -= dx; |
| 79 | | a += ATAN[i]; |
| 80 | | } |
| 81 | | } |
| 82 | | |
| 83 | | // produit du résultat par les cosinus des angles : K=0x4E=1001110 |
| 84 | | x = ((x>>6) + (x>>5) + (x>>4) + (x>>1))>>7; |
| 85 | | y = ((y>>6) + (y>>5) + (y>>4) + (y>>1))>>7; |
| 86 | | |
| 87 | | // placement du points dans le quadrant d'origine |
| 88 | | switch (q) { |
| 89 | | case 0: |
| 90 | | dx = x; |
| 91 | | dy = y; |
| 92 | | break; |
| 93 | | case 1: |
| 94 | | dx = -y; |
| 95 | | dy = x; |
| 96 | | break; |
| 97 | | case 2: |
| 98 | | dx = -x; |
| 99 | | dy = -y; |
| 100 | | break; |
| 101 | | case 3: |
| 102 | | dx = y; |
| 103 | | dy = -x; |
| 104 | | break; |
| 105 | | } |
| 106 | | *nx_p = dx; |
| 107 | | *ny_p = dy; |
| 108 | | } |
| 109 | | |
| 110 | | int main() |
| 111 | | { |
| 112 | | FILE *f; |
| 113 | | double t,K; |
| 114 | | int i; |
| 115 | | printf("PI/2 = %x %x\n", F_PI/2i, 128<<7); |
| 116 | | for (i=0, t=1, K=1; i < 8; i++, t=t/2) { |
| 117 | | printf("B_%d = 0x%x\n", i, (short)(round(atan(t)*(1<< 7)))); |
| 118 | | K = K * cos (atan(t)); |
| 119 | | } |
| 120 | | printf("K = %g, 0x%x\n", K, (short)(round(K*(1<< 7)))); |
| 121 | | |
| 122 | | f = fopen("cossin.dat", "w"); |
| 123 | | for (double a = 0; a <= M_PI * 2; a += 1. / 256) { |
| 124 | | char nx_p; |
| 125 | | char ny_p; |
| 126 | | cossin(a, 127, 0, &nx_p, &ny_p); |
| 127 | | fprintf(f, "%4d %4d\n", nx_p, ny_p); |
| 128 | | } |
| 129 | | fclose(f); |
| 130 | | |
| 131 | | f = fopen("cordic.dat", "w"); |
| 132 | | for (short a = 0; a <= 2 * F_PI ; a += 1) { |
| 133 | | char nx_p; |
| 134 | | char ny_p; |
| 135 | | cordic(a, 127, 0, &nx_p, &ny_p); |
| 136 | | fprintf(f, "%4d %4d\n", nx_p, ny_p); |
| 137 | | } |
| 138 | | fclose(f); |
| 139 | | |
| 140 | | return 0; |
| 141 | | } |
| 142 | | }}} |
| 143 | | |
| 144 | | |
| 145 | | |
| 146 | | |