API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 23rd, 2016
139
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 10.34 KB | None | 0 0
raw download clone embed print report
  1. struct fft_sample_8  { s8  r, i; };
  2. struct fft_sample_16 { s16 r, i; };
  3. struct fft_sample_32 { s32 r, i; };
  4.  
  5.  
  6. // Fixed-point multiplication & scaling.Scaling ensures that result remains 16-bit
  7. inline s8 fft_mul_8(s8 a, s8 b) {
  8.     s16 c = ((s16)a * (s16)b) >> 6;
  9.     return (c >> 1) + (c & 0x01);
  10. }
  11. inline s16 fft_mul_16(s16 a, s16 b) {
  12.     if ((a == -32768)&&(b == -32768)) a = -32767;
  13.     s32 c = (s32)a * (s32)b;
  14.     return c >> 15;
  15. }
  16.  
  17. //8-bit sine table
  18. const s8 fft_256_sine_8[192] = {
  19.   0, 3, 6, 9, 12, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 51, 54, 57, 60, 63, 65, 68, 71, 73, 76, 78, 81, 83, 85, 88, 90, 92, 94, 96, 98, 100, 102, 104, 106, 107, 109, 111, 112, 113, 115, 116,
  20.   117, 118, 120, 121, 122, 122, 123, 124, 125, 125, 126, 126, 126, 127, 127, 127, 127, 127, 127, 127, 126, 126, 126, 125, 125, 124, 123, 122, 122, 121, 120, 118, 117, 116, 115, 113, 112, 111, 109, 107, 106, 104, 102, 100, 98, 96, 94, 92,
  21.   90, 88, 85, 83, 81, 78, 76, 73, 71, 68, 65, 63, 60, 57, 54, 51, 49, 46, 43, 40, 37, 34, 31, 28, 25, 22, 19, 16, 12, 9, 6, 3, 0, -3, -6, -9, -12, -16, -19, -22, -25, -28, -31, -34, -37, -40, -43, -46,
  22.   -49, -51, -54, -57, -60, -63, -65, -68, -71, -73, -76, -78, -81, -83, -85, -88, -90, -92, -94, -96, -98, -100, -102, -104, -106, -107, -109, -111, -112, -113, -115, -116, -117, -118, -120, -121, -122, -122, -123, -124, -125, -125, -126, -126,
  23.   -126, -127, -127, -127
  24. };
  25.  
  26. // Forward FFT
  27. void fft256_forw_8(struct fft_sample_8 *samples) {
  28.     u16 l = 1, scale = 0;
  29.     u8  nn = 255, k=7;
  30.  
  31.     // decimation in time - re-order data
  32.     u16 mr = 0;
  33.     for (u8 m=0; m<nn; m++) {
  34.         u16 l = 256;
  35.         do { l >>= 1; } while (mr+l > nn);
  36.         mr = (mr & (l-1)) + l;
  37.         if (mr <= m+1) continue;
  38.         s8 x = samples[m+1].r;
  39.         samples[m+1].r = samples[mr].r;
  40.         samples[mr].r  = x;
  41.         x = samples[m+1].i;
  42.         samples[m+1].i = samples[mr].i;
  43.         samples[mr].i  = x;
  44.     }
  45.  
  46.     while (l < 256) {
  47.         u16 istep = l << 1;
  48.         for (u16 m=0; m<l; ++m) {
  49.             u16 j = m << k;
  50.  
  51.             s8 wr = fft_256_sine_8[j + (256 / 4)];
  52.             s8 wi = -fft_256_sine_8[j];
  53.  
  54.             wr >>= 1; wi >>= 1;
  55.  
  56.             for (u16 i=m; i<256; i+=istep) {
  57.                 u16 j  = i + l;
  58.                 s8  qr = samples[j].r, qi = samples[j].i;
  59.                 s8  tr = fft_mul_8(wr, qr) - fft_mul_8(wi, qi);
  60.                 s8  ti = fft_mul_8(wr, qi) + fft_mul_8(wi, qr);
  61.                 qr = samples[i].r; qi = samples[i].i;
  62.                 qr >>= 1; qi >>= 1;
  63.                 samples[j].r = qr - tr; samples[j].i = qi - ti;
  64.                 samples[i].r = qr + tr; samples[i].i = qi + ti;
  65.             }
  66.         }
  67.         k--;
  68.         l = istep;
  69.     }
  70. }
  71.  
  72. const u8 fft256_win_hanning_8[] PROGMEM = {
  73.     0, 0, 0, 0, 1, 1, 1, 2, 2, 3, 4, 5, 5, 6, 7, 9, 10, 11, 12, 14, 15, 17, 18, 20, 21, 23, 25, 27, 29, 31, 33, 35,
  74.     37, 40, 42, 44, 47, 49, 52, 54, 57, 59, 62, 65, 67, 70, 73, 76, 79, 82, 85, 88, 90, 93, 97, 100, 103, 106, 109, 112, 115, 118, 121, 124,
  75.     127, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 162, 165, 167, 170, 173, 176, 179, 182, 185, 188, 190, 193, 196, 198, 201, 203, 206, 208, 211, 213, 215,
  76.     218, 220, 222, 224, 226, 228, 230, 232, 234, 235, 237, 238, 240, 241, 243, 244, 245, 246, 248, 249, 250, 250, 251, 252, 253, 253, 254, 254, 254, 255, 255, 255,
  77.     255, 255, 255, 255, 254, 254, 254, 253, 253, 252, 251, 250, 250, 249, 248, 246, 245, 244, 243, 241, 240, 238, 237, 235, 234, 232, 230, 228, 226, 224, 222, 220,
  78.     218, 215, 213, 211, 208, 206, 203, 201, 198, 196, 193, 190, 188, 185, 182, 179, 176, 173, 170, 167, 165, 162, 158, 155, 152, 149, 146, 143, 140, 137, 134, 131,
  79.     128, 124, 121, 118, 115, 112, 109, 106, 103, 100, 97, 93, 90, 88, 85, 82, 79, 76, 73, 70, 67, 65, 62, 59, 57, 54, 52, 49, 47, 44, 42, 40,
  80.     37, 35, 33, 31, 29, 27, 25, 23, 21, 20, 18, 17, 15, 14, 12, 11, 10, 9, 7, 6, 5, 5, 4, 3, 2, 2, 1, 1, 1, 0, 0, 0
  81. };
  82. const u8 fft256_win_hamming_8[] PROGMEM = {
  83.     20, 20, 21, 21, 21, 21, 22, 22, 23, 23, 24, 25, 25, 26, 27, 28, 29, 30, 32, 33, 34, 36, 37, 39, 40, 42, 43, 45, 47, 49, 51, 53,
  84.     55, 57, 59, 61, 63, 66, 68, 70, 73, 75, 77, 80, 82, 85, 88, 90, 93, 95, 98, 101, 104, 106, 109, 112, 115, 118, 120, 123, 126, 129, 132, 135,
  85.     138, 141, 143, 146, 149, 152, 155, 158, 161, 163, 166, 169, 172, 174, 177, 180, 183, 185, 188, 190, 193, 196, 198, 200, 203, 205, 208, 210, 212, 214, 216, 219,
  86.     221, 223, 225, 227, 228, 230, 232, 234, 235, 237, 238, 240, 241, 242, 244, 245, 246, 247, 248, 249, 250, 251, 251, 252, 253, 253, 254, 254, 254, 255, 255, 255,
  87.     255, 255, 255, 255, 254, 254, 254, 253, 253, 252, 251, 251, 250, 249, 248, 247, 246, 245, 244, 242, 241, 240, 238, 237, 235, 234, 232, 230, 228, 227, 225, 223,
  88.     221, 219, 216, 214, 212, 210, 208, 205, 203, 200, 198, 196, 193, 190, 188, 185, 183, 180, 177, 174, 172, 169, 166, 163, 161, 158, 155, 152, 149, 146, 143, 141,
  89.     138, 135, 132, 129, 126, 123, 120, 118, 115, 112, 109, 106, 104, 101, 98, 95, 93, 90, 88, 85, 82, 80, 77, 75, 73, 70, 68, 66, 63, 61, 59, 57,
  90.     55, 53, 51, 49, 47, 45, 43, 42, 40, 39, 37, 36, 34, 33, 32, 30, 29, 28, 27, 26, 25, 25, 24, 23, 23, 22, 22, 21, 21, 21, 21, 20
  91. };
  92. const u8 fft256_win_blackman_8[] PROGMEM = {
  93.     2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 6, 6, 7, 7, 8, 9, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18,
  94.     19, 21, 22, 23, 25, 26, 28, 29, 31, 33, 34, 36, 38, 40, 42, 44, 46, 49, 51, 53, 56, 58, 61, 63, 66, 69, 71, 74, 77, 80, 83, 86,
  95.     89, 92, 95, 99, 102, 105, 109, 112, 115, 119, 122, 126, 129, 133, 136, 140, 143, 147, 150, 154, 158, 161, 165, 168, 172, 175, 179, 182, 185, 189, 192, 195,
  96.     198, 201, 205, 208, 210, 213, 216, 219, 222, 224, 227, 229, 231, 234, 236, 238, 240, 241, 243, 245, 246, 248, 249, 250, 251, 252, 253, 253, 254, 254, 255, 255,
  97.     255, 255, 255, 254, 254, 253, 253, 252, 251, 250, 249, 248, 246, 245, 243, 241, 240, 238, 236, 234, 231, 229, 227, 224, 222, 219, 216, 213, 210, 208, 205, 201,
  98.     198, 195, 192, 189, 185, 182, 179, 175, 172, 168, 165, 161, 158, 154, 150, 147, 143, 140, 136, 133, 129, 126, 122, 119, 115, 112, 109, 105, 102, 99, 95, 92,
  99.     89, 86, 83, 80, 77, 74, 71, 69, 66, 63, 61, 58, 56, 53, 51, 49, 46, 44, 42, 40, 38, 36, 34, 33, 31, 29, 28, 26, 25, 23, 22, 21,
  100.     19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 9, 8, 7, 7, 6, 6, 5, 5, 4, 4, 4, 3, 3, 3, 2, 2, 2, 2, 2, 2, 2
  101. };
  102. const u8 fft256_win_nuttall_8[] PROGMEM = {
  103.     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 5,
  104.     5, 6, 6, 7, 8, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 19, 20, 22, 23, 25, 27, 28, 30, 32, 34, 36, 39, 41, 43, 46, 48, 51,
  105.     54, 57, 60, 63, 66, 69, 72, 76, 79, 83, 86, 90, 94, 98, 101, 105, 109, 113, 117, 122, 126, 130, 134, 138, 143, 147, 151, 155, 160, 164, 168, 172,
  106.     176, 180, 185, 189, 192, 196, 200, 204, 208, 211, 215, 218, 221, 224, 227, 230, 233, 235, 238, 240, 242, 244, 246, 248, 249, 251, 252, 253, 254, 254, 255, 255,
  107.     255, 255, 255, 254, 254, 253, 252, 251, 249, 248, 246, 244, 242, 240, 238, 235, 233, 230, 227, 224, 221, 218, 215, 211, 208, 204, 200, 196, 192, 189, 185, 180,
  108.     176, 172, 168, 164, 160, 155, 151, 147, 143, 138, 134, 130, 126, 122, 117, 113, 109, 105, 101, 98, 94, 90, 86, 83, 79, 76, 72, 69, 66, 63, 60, 57,
  109.     54, 51, 48, 46, 43, 41, 39, 36, 34, 32, 30, 28, 27, 25, 23, 22, 20, 19, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 8, 7, 6, 6,
  110.     5, 5, 4, 4, 3, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
  111. };
  112.  
  113. // amplify by window
  114. void fft256_win_8(struct fft_sample_8 *samples, const u8 *win){
  115.     u8 i=0;
  116.     do {
  117.         samples->r = ((s16)samples->r * (s16)(*win)) / 256;
  118.         samples++; win++; i++;
  119.     } while (i);
  120. }
  121.  
  122. // square root
  123. u8 sqrt16( u16 x ){
  124.     u8 b = ((u8)(x >> 8)) ? 0x80 : 8, result=0;
  125.     while (b){
  126.         u8 g = result | b;
  127.         if ((u16)g * g <= x) result = g;
  128.         b >>= 1;
  129.     }
  130.     return result;
  131. }
  132.  
  133. // calculate real amplitudes
  134. void fft_amp_8(struct fft_sample_8 *samples, u16 n, u8 (*sqrt16)(u16)){
  135.     while (n--){
  136.         samples->r = sqrt16( (s16)samples->r * samples->r + (s16)samples->i * samples->i );
  137.         samples++;
  138.     }
  139. }
  140.  
  141. const s16 fft_256_sine_16[192] PROGMEM = {
  142.     0, 804, 1608, 2410, 3212, 4011, 4808, 5602, 6393, 7179, 7962, 8739, 9512, 10278, 11039, 11793, 12539, 13279, 14010, 14732, 15446, 16151, 16846, 17530, 18204, 18868, 19519, 20159, 20787, 21403, 22005, 22594,
  143.     23170, 23731, 24279, 24811, 25329, 25832, 26319, 26790, 27245, 27683, 28105, 28510, 28898, 29268, 29621, 29956, 30273, 30571, 30852, 31113, 31356, 31580, 31785, 31971, 32137, 32285, 32412, 32521, 32609, 32678, 32728, 32757,
  144.     32767, 32757, 32728, 32678, 32609, 32521, 32412, 32285, 32137, 31971, 31785, 31580, 31356, 31113, 30852, 30571, 30273, 29956, 29621, 29268, 28898, 28510, 28105, 27683, 27245, 26790, 26319, 25832, 25329, 24811, 24279, 23731,
  145.     23170, 22594, 22005, 21403, 20787, 20159, 19519, 18868, 18204, 17530, 16846, 16151, 15446, 14732, 14010, 13279, 12539, 11793, 11039, 10278, 9512, 8739, 7962, 7179, 6393, 5602, 4808, 4011, 3212, 2410, 1608, 804,
  146.     0, -804, -1608, -2410, -3212, -4011, -4808, -5602, -6393, -7179, -7962, -8739, -9512, -10278, -11039, -11793, -12539, -13279, -14010, -14732, -15446, -16151, -16846, -17530, -18204, -18868, -19519, -20159, -20787, -21403, -22005, -22594,
  147.     -23170, -23731, -24279, -24811, -25329, -25832, -26319, -26790, -27245, -27683, -28105, -28510, -28898, -29268, -29621, -29956, -30273, -30571, -30852, -31113, -31356, -31580, -31785, -31971, -32137, -32285, -32412, -32521, -32609, -32678, -32728, -32757
  148. };
  149. void fft256_forw_16(struct fft_sample_16 *samples) {
  150.     u16 l = 1;
  151.     u8  nn = 255, k=7;
  152.  
  153.     // decimation in time - re-order data
  154.     u16 mr = 0;
  155.     for (u8 m=0; m<nn; m++) {
  156.         u16 l = 256;
  157.         do { l >>= 1; } while (mr+l > nn);
  158.         mr = (mr & (l-1)) + l;
  159.         if (mr <= m+1) continue;
  160.         s16 x = samples[m+1].r;
  161.         samples[m+1].r = samples[mr].r;
  162.         samples[mr].r  = x;
  163.         x = samples[m+1].i;
  164.         samples[m+1].i = samples[mr].i;
  165.         samples[mr].i  = x;
  166.     }
  167.  
  168.     while (l < 256) {
  169.         u16 istep = l << 1;
  170.         for (u16 m=0; m<l; ++m) {
  171.             u16 j = m << k;
  172.  
  173.             s16 wr =  fft_256_sine_16[j + (256 / 4)];
  174.             s16 wi = -fft_256_sine_16[j];
  175.  
  176.             wr /= 2; wi /= 2;
  177.  
  178.             for (u16 i=m; i<256; i+=istep) {
  179.                 u16 j  = i + l;
  180.                 s16 qr = samples[j].r, qi = samples[j].i;
  181.                 s16 tr = fft_mul_16(wr, qr) - fft_mul_16(wi, qi);
  182.                 s16 ti = fft_mul_16(wr, qi) + fft_mul_16(wi, qr);
  183.                 qr = samples[i].r; qi = samples[i].i;
  184.                 qr /= 2; qi /= 2;
  185.                 samples[j].r = qr - tr; samples[j].i = qi - ti;
  186.                 samples[i].r = qr + tr; samples[i].i = qi + ti;
  187.             }
  188.         }
  189.         k--;
  190.         l = istep;
  191.     }
  192. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!