git.piffa.net Git - arduino/blob - books/ArduinoNextSteps-master/ArduinoNextSteps/sketch_13_07_FFT_Freq/fix_fft.cpp

   1 #include <avr/pgmspace.h>\r
   2 #include "fix_fft.h"\r
   3 #include <WProgram.h>\r
   4 \r
   5 /* fix_fft.c - Fixed-point in-place Fast Fourier Transform  */\r
   6 /*\r
   7   All data are fixed-point short integers, in which -32768\r
   8   to +32768 represent -1.0 to +1.0 respectively. Integer\r
   9   arithmetic is used for speed, instead of the more natural\r
  10   floating-point.\r
  11 \r
  12   For the forward FFT (time -> freq), fixed scaling is\r
  13   performed to prevent arithmetic overflow, and to map a 0dB\r
  14   sine/cosine wave (i.e. amplitude = 32767) to two -6dB freq\r
  15   coefficients. The return value is always 0.\r
  16 \r
  17   For the inverse FFT (freq -> time), fixed scaling cannot be\r
  18   done, as two 0dB coefficients would sum to a peak amplitude\r
  19   of 64K, overflowing the 32k range of the fixed-point integers.\r
  20   Thus, the fix_fft() routine performs variable scaling, and\r
  21   returns a value which is the number of bits LEFT by which\r
  22   the output must be shifted to get the actual amplitude\r
  23   (i.e. if fix_fft() returns 3, each value of fr[] and fi[]\r
  24   must be multiplied by 8 (2**3) for proper scaling.\r
  25   Clearly, this cannot be done within fixed-point short\r
  26   integers. In practice, if the result is to be used as a\r
  27   filter, the scale_shift can usually be ignored, as the\r
  28   result will be approximately correctly normalized as is.\r
  29 \r
  30   Written by:  Tom Roberts  11/8/89\r
  31   Made portable:  Malcolm Slaney 12/15/94 malcolm@interval.com\r
  32   Enhanced:  Dimitrios P. Bouras  14 Jun 2006 dbouras@ieee.org\r
  33   Modified for 8bit values David Keller  10.10.2010\r
  34 */\r
  35 \r
  36 \r
  37 #define N_WAVE  256    /* full length of Sinewave[] */\r
  38 #define LOG2_N_WAVE 8   /* log2(N_WAVE) */\r
  39 \r
  40 \r
  41 \r
  42 \r
  43 /*\r
  44   Since we only use 3/4 of N_WAVE, we define only\r
  45   this many samples, in order to conserve data space.\r
  46 */\r
  47 \r
  48 \r
  49 \r
  50 const prog_int8_t Sinewave[N_WAVE-N_WAVE/4] PROGMEM = {\r
  51 0, 3, 6, 9, 12, 15, 18, 21,\r
  52 24, 28, 31, 34, 37, 40, 43, 46,\r
  53 48, 51, 54, 57, 60, 63, 65, 68,\r
  54 71, 73, 76, 78, 81, 83, 85, 88,\r
  55 90, 92, 94, 96, 98, 100, 102, 104,\r
  56 106, 108, 109, 111, 112, 114, 115, 117,\r
  57 118, 119, 120, 121, 122, 123, 124, 124,\r
  58 125, 126, 126, 127, 127, 127, 127, 127,\r
  59 \r
  60 127, 127, 127, 127, 127, 127, 126, 126,\r
  61 125, 124, 124, 123, 122, 121, 120, 119,\r
  62 118, 117, 115, 114, 112, 111, 109, 108,\r
  63 106, 104, 102, 100, 98, 96, 94, 92,\r
  64 90, 88, 85, 83, 81, 78, 76, 73,\r
  65 71, 68, 65, 63, 60, 57, 54, 51,\r
  66 48, 46, 43, 40, 37, 34, 31, 28,\r
  67 24, 21, 18, 15, 12, 9, 6, 3,\r
  68 \r
  69 0, -3, -6, -9, -12, -15, -18, -21,\r
  70 -24, -28, -31, -34, -37, -40, -43, -46,\r
  71 -48, -51, -54, -57, -60, -63, -65, -68,\r
  72 -71, -73, -76, -78, -81, -83, -85, -88,\r
  73 -90, -92, -94, -96, -98, -100, -102, -104,\r
  74 -106, -108, -109, -111, -112, -114, -115, -117,\r
  75 -118, -119, -120, -121, -122, -123, -124, -124,\r
  76 -125, -126, -126, -127, -127, -127, -127, -127,\r
  77 \r
  78 /*-127, -127, -127, -127, -127, -127, -126, -126,\r
  79 -125, -124, -124, -123, -122, -121, -120, -119,\r
  80 -118, -117, -115, -114, -112, -111, -109, -108,\r
  81 -106, -104, -102, -100, -98, -96, -94, -92,\r
  82 -90, -88, -85, -83, -81, -78, -76, -73,\r
  83 -71, -68, -65, -63, -60, -57, -54, -51,\r
  84 -48, -46, -43, -40, -37, -34, -31, -28,\r
  85 -24, -21, -18, -15, -12, -9, -6, -3, */\r
  86 };\r
  87 \r
  88 \r
  89 \r
  90 \r
  91 \r
  92 \r
  93 /*\r
  94   FIX_MPY() - fixed-point multiplication & scaling.\r
  95   Substitute inline assembly for hardware-specific\r
  96   optimization suited to a particluar DSP processor.\r
  97   Scaling ensures that result remains 16-bit.\r
  98 */\r
  99 inline char FIX_MPY(char a, char b)\r
 100 {\r
 101   \r
 102   //Serial.println(a);\r
 103  //Serial.println(b);\r
 104   \r
 105   \r
 106     /* shift right one less bit (i.e. 15-1) */\r
 107     int c = ((int)a * (int)b) >> 6;\r
 108     /* last bit shifted out = rounding-bit */\r
 109     b = c & 0x01;\r
 110     /* last shift + rounding bit */\r
 111     a = (c >> 1) + b;\r
 112 \r
 113           /*\r
 114           Serial.println(Sinewave[3]);\r
 115           Serial.println(c);\r
 116           Serial.println(a);\r
 117           while(1);*/\r
 118 \r
 119     return a;\r
 120 }\r
 121 \r
 122 /*\r
 123   fix_fft() - perform forward/inverse fast Fourier transform.\r
 124   fr[n],fi[n] are real and imaginary arrays, both INPUT AND\r
 125   RESULT (in-place FFT), with 0 <= n < 2**m; set inverse to\r
 126   0 for forward transform (FFT), or 1 for iFFT.\r
 127 */\r
 128 int fix_fft(char fr[], char fi[], int m, int inverse)\r
 129 {\r
 130     int mr, nn, i, j, l, k, istep, n, scale, shift;\r
 131     char qr, qi, tr, ti, wr, wi;\r
 132 \r
 133     n = 1 << m;\r
 134 \r
 135     /* max FFT size = N_WAVE */\r
 136     if (n > N_WAVE)\r
 137           return -1;\r
 138 \r
 139     mr = 0;\r
 140     nn = n - 1;\r
 141     scale = 0;\r
 142 \r
 143     /* decimation in time - re-order data */\r
 144     for (m=1; m<=nn; ++m) {\r
 145           l = n;\r
 146           do {\r
 147                 l >>= 1;\r
 148           } while (mr+l > nn);\r
 149           mr = (mr & (l-1)) + l;\r
 150 \r
 151           if (mr <= m)\r
 152                 continue;\r
 153           tr = fr[m];\r
 154           fr[m] = fr[mr];\r
 155           fr[mr] = tr;\r
 156           ti = fi[m];\r
 157           fi[m] = fi[mr];\r
 158           fi[mr] = ti;\r
 159     }\r
 160 \r
 161     l = 1;\r
 162     k = LOG2_N_WAVE-1;\r
 163     while (l < n) {\r
 164           if (inverse) {\r
 165                 /* variable scaling, depending upon data */\r
 166                 shift = 0;\r
 167                 for (i=0; i<n; ++i) {\r
 168                     j = fr[i];\r
 169                     if (j < 0)\r
 170                           j = -j;\r
 171                     m = fi[i];\r
 172                     if (m < 0)\r
 173                           m = -m;\r
 174                     if (j > 16383 || m > 16383) {\r
 175                           shift = 1;\r
 176                           break;\r
 177                     }\r
 178                 }\r
 179                 if (shift)\r
 180                     ++scale;\r
 181           } else {\r
 182                 /*\r
 183                   fixed scaling, for proper normalization --\r
 184                   there will be log2(n) passes, so this results\r
 185                   in an overall factor of 1/n, distributed to\r
 186                   maximize arithmetic accuracy.\r
 187                 */\r
 188                 shift = 1;\r
 189           }\r
 190           /*\r
 191             it may not be obvious, but the shift will be\r
 192             performed on each data point exactly once,\r
 193             during this pass.\r
 194           */\r
 195           istep = l << 1;\r
 196           for (m=0; m<l; ++m) {\r
 197                 j = m << k;\r
 198                 /* 0 <= j < N_WAVE/2 */\r
 199                 wr =  pgm_read_word_near(Sinewave + j+N_WAVE/4);\r
 200 \r
 201 /*Serial.println("asdfasdf");\r
 202 Serial.println(wr);\r
 203 Serial.println(j+N_WAVE/4);\r
 204 Serial.println(Sinewave[256]);\r
 205 \r
 206 Serial.println("");*/\r
 207 \r
 208 \r
 209                 wi = -pgm_read_word_near(Sinewave + j);\r
 210                 if (inverse)\r
 211                     wi = -wi;\r
 212                 if (shift) {\r
 213                     wr >>= 1;\r
 214                     wi >>= 1;\r
 215                 }\r
 216                 for (i=m; i<n; i+=istep) {\r
 217                     j = i + l;\r
 218                     tr = FIX_MPY(wr,fr[j]) - FIX_MPY(wi,fi[j]);\r
 219                     ti = FIX_MPY(wr,fi[j]) + FIX_MPY(wi,fr[j]);\r
 220                     qr = fr[i];\r
 221                     qi = fi[i];\r
 222                     if (shift) {\r
 223                           qr >>= 1;\r
 224                           qi >>= 1;\r
 225                     }\r
 226                     fr[j] = qr - tr;\r
 227                     fi[j] = qi - ti;\r
 228                     fr[i] = qr + tr;\r
 229                     fi[i] = qi + ti;\r
 230                 }\r
 231           }\r
 232           --k;\r
 233           l = istep;\r
 234     }\r
 235     return scale;\r
 236 }\r
 237 \r
 238 /*\r
 239   fix_fftr() - forward/inverse FFT on array of real numbers.\r
 240   Real FFT/iFFT using half-size complex FFT by distributing\r
 241   even/odd samples into real/imaginary arrays respectively.\r
 242   In order to save data space (i.e. to avoid two arrays, one\r
 243   for real, one for imaginary samples), we proceed in the\r
 244   following two steps: a) samples are rearranged in the real\r
 245   array so that all even samples are in places 0-(N/2-1) and\r
 246   all imaginary samples in places (N/2)-(N-1), and b) fix_fft\r
 247   is called with fr and fi pointing to index 0 and index N/2\r
 248   respectively in the original array. The above guarantees\r
 249   that fix_fft "sees" consecutive real samples as alternating\r
 250   real and imaginary samples in the complex array.\r
 251 */\r
 252 int fix_fftr(char f[], int m, int inverse)\r
 253 {\r
 254     int i, N = 1<<(m-1), scale = 0;\r
 255     char tt, *fr=f, *fi=&f[N];\r
 256 \r
 257     if (inverse)\r
 258           scale = fix_fft(fi, fr, m-1, inverse);\r
 259     for (i=1; i<N; i+=2) {\r
 260           tt = f[N+i-1];\r
 261           f[N+i-1] = f[i];\r
 262           f[i] = tt;\r
 263     }\r
 264     if (! inverse)\r
 265           scale = fix_fft(fi, fr, m-1, inverse);\r
 266     return scale;\r
 267 } \r
 268 \r