| 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 |  |  |