|
N-sim
Emulation and simulation of Wireless Sensor Networks |
|
Home Project Page Download CVS Installation Configuration Plug-ins |
|
Hosted by |
/home/brennan/n-sim/Vaike/linux/system-addons/apps/int_fft.c
Go to the documentation of this file.00001 /* fix_fft.c - Fixed-point Fast Fourier Transform */ 00002 /* 00003 fix_fft() perform FFT or inverse FFT 00004 window() applies a Hanning window to the (time) input 00005 fix_loud() calculates the loudness of the signal, for 00006 each freq point. Result is an integer array, 00007 units are dB (values will be negative). 00008 iscale() scale an integer value by (numer/denom). 00009 fix_mpy() perform fixed-point multiplication. 00010 Sinewave[1024] sinewave normalized to 32767 (= 1.0). 00011 Loudampl[100] Amplitudes for lopudnesses from 0 to -99 dB. 00012 Low_pass Low-pass filter, cutoff at sample_freq / 4. 00013 00014 00015 All data are fixed-point short integers, in which 00016 -32768 to +32768 represent -1.0 to +1.0. Integer arithmetic 00017 is used for speed, instead of the more natural floating-point. 00018 00019 For the forward FFT (time -> freq), fixed scaling is 00020 performed to prevent arithmetic overflow, and to map a 0dB 00021 sine/cosine wave (i.e. amplitude = 32767) to two -6dB freq 00022 coefficients; the one in the lower half is reported as 0dB 00023 by fix_loud(). The return value is always 0. 00024 00025 For the inverse FFT (freq -> time), fixed scaling cannot be 00026 done, as two 0dB coefficients would sum to a peak amplitude of 00027 64K, overflowing the 32k range of the fixed-point integers. 00028 Thus, the fix_fft() routine performs variable scaling, and 00029 returns a value which is the number of bits LEFT by which 00030 the output must be shifted to get the actual amplitude 00031 (i.e. if fix_fft() returns 3, each value of fr[] and fi[] 00032 must be multiplied by 8 (2**3) for proper scaling. 00033 Clearly, this cannot be done within the fixed-point short 00034 integers. In practice, if the result is to be used as a 00035 filter, the scale_shift can usually be ignored, as the 00036 result will be approximately correctly normalized as is. 00037 00038 00039 TURBO C, any memory model; uses inline assembly for speed 00040 and for carefully-scaled arithmetic. 00041 00042 Written by: Tom Roberts 11/8/89 00043 Made portable: Malcolm Slaney 12/15/94 malcolm@interval.com 00044 00045 Timing on a Macintosh PowerBook 180.... (using Symantec C6.0) 00046 fix_fft (1024 points) 8 ticks 00047 fft (1024 points - Using SANE) 112 Ticks 00048 fft (1024 points - Using FPU) 11 00049 00050 */ 00051 00052 /* FIX_MPY() - fixed-point multiplication macro. 00053 This macro is a statement, not an expression (uses asm). 00054 BEWARE: make sure _DX is not clobbered by evaluating (A) or DEST. 00055 args are all of type fixed. 00056 Scaling ensures that 32767*32767 = 32767. */ 00057 00058 #define FIX_MPY(DEST,A,B) DEST = ((long)(A) * (long)(B))>>15 00059 00060 #define N_WAVE 1024 /* dimension of Sinewave[] */ 00061 #define LOG2_N_WAVE 10 /* log2(N_WAVE) */ 00062 #define N_LOUD 100 /* dimension of Loudampl[] */ 00063 #ifndef fixed 00064 #define fixed short 00065 #endif 00066 00067 extern fixed Sinewave[N_WAVE]; /* placed at end of this file for clarity */ 00068 extern fixed Loudampl[N_LOUD]; 00069 int db_from_ampl(fixed re, fixed im); 00070 fixed fix_mpy(fixed a, fixed b); 00071 00072 /* 00073 fix_fft() - perform fast Fourier transform. 00074 00075 if n>0 FFT is done, if n<0 inverse FFT is done 00076 fr[n],fi[n] are real,imaginary arrays, INPUT AND RESULT. 00077 size of data = 2**m 00078 set inverse to 0=dft, 1=idft 00079 */ 00080 int fix_fft(fixed fr[], fixed fi[], int m, int inverse) 00081 { 00082 int mr,nn,i,j,l,k,istep, n, scale, shift; 00083 fixed qr,qi,tr,ti,wr,wi/*,t*/; 00084 00085 n = 1<<m; 00086 00087 if(n > N_WAVE) 00088 return -1; 00089 00090 mr = 0; 00091 nn = n - 1; 00092 scale = 0; 00093 00094 /* decimation in time - re-order data */ 00095 for(m=1; m<=nn; ++m) { 00096 l = n; 00097 do { 00098 l >>= 1; 00099 } while(mr+l > nn); 00100 mr = (mr & (l-1)) + l; 00101 00102 if(mr <= m) continue; 00103 tr = fr[m]; 00104 fr[m] = fr[mr]; 00105 fr[mr] = tr; 00106 ti = fi[m]; 00107 fi[m] = fi[mr]; 00108 fi[mr] = ti; 00109 } 00110 00111 l = 1; 00112 k = LOG2_N_WAVE-1; 00113 while(l < n) { 00114 if(inverse) { 00115 /* variable scaling, depending upon data */ 00116 shift = 0; 00117 for(i=0; i<n; ++i) { 00118 j = fr[i]; 00119 if(j < 0) 00120 j = -j; 00121 m = fi[i]; 00122 if(m < 0) 00123 m = -m; 00124 if(j > 16383 || m > 16383) { 00125 shift = 1; 00126 break; 00127 } 00128 } 00129 if(shift) 00130 ++scale; 00131 } else { 00132 /* fixed scaling, for proper normalization - 00133 there will be log2(n) passes, so this 00134 results in an overall factor of 1/n, 00135 distributed to maximize arithmetic accuracy. */ 00136 shift = 1; 00137 } 00138 /* it may not be obvious, but the shift will be performed 00139 on each data point exactly once, during this pass. */ 00140 istep = l << 1; 00141 for(m=0; m<l; ++m) { 00142 j = m << k; 00143 /* 0 <= j < N_WAVE/2 */ 00144 wr = Sinewave[j+N_WAVE/4]; 00145 wi = -Sinewave[j]; 00146 if(inverse) 00147 wi = -wi; 00148 if(shift) { 00149 wr >>= 1; 00150 wi >>= 1; 00151 } 00152 for(i=m; i<n; i+=istep) { 00153 j = i + l; 00154 tr = fix_mpy(wr,fr[j]) - 00155 fix_mpy(wi,fi[j]); 00156 ti = fix_mpy(wr,fi[j]) + 00157 fix_mpy(wi,fr[j]); 00158 qr = fr[i]; 00159 qi = fi[i]; 00160 if(shift) { 00161 qr >>= 1; 00162 qi >>= 1; 00163 } 00164 fr[j] = qr - tr; 00165 fi[j] = qi - ti; 00166 fr[i] = qr + tr; 00167 fi[i] = qi + ti; 00168 } 00169 } 00170 --k; 00171 l = istep; 00172 } 00173 00174 return scale; 00175 } 00176 00177 00178 /* window() - apply a Hanning window */ 00179 void window(fixed fr[], int n) 00180 { 00181 int i,j,k; 00182 00183 j = N_WAVE/n; 00184 n >>= 1; 00185 for(i=0,k=N_WAVE/4; i<n; ++i,k+=j) 00186 FIX_MPY(fr[i],fr[i],16384-(Sinewave[k]>>1)); 00187 n <<= 1; 00188 for(k-=j; i<n; ++i,k-=j) 00189 FIX_MPY(fr[i],fr[i],16384-(Sinewave[k]>>1)); 00190 } 00191 00192 /* fix_loud() - compute loudness of freq-spectrum components. 00193 n should be ntot/2, where ntot was passed to fix_fft(); 00194 6 dB is added to account for the omitted alias components. 00195 scale_shift should be the result of fix_fft(), if the time-series 00196 was obtained from an inverse FFT, 0 otherwise. 00197 loud[] is the loudness, in dB wrt 32767; will be +10 to -N_LOUD. 00198 */ 00199 void fix_loud(fixed loud[], fixed fr[], fixed fi[], int n, int scale_shift) 00200 { 00201 int i, max; 00202 00203 max = 0; 00204 if(scale_shift > 0) 00205 max = 10; 00206 scale_shift = (scale_shift+1) * 6; 00207 00208 for(i=0; i<n; ++i) { 00209 loud[i] = db_from_ampl(fr[i],fi[i]) + scale_shift; 00210 if(loud[i] > max) 00211 loud[i] = max; 00212 } 00213 } 00214 00215 /* db_from_ampl() - find loudness (in dB) from 00216 the complex amplitude. 00217 */ 00218 int db_from_ampl(fixed re, fixed im) 00219 { 00220 static long loud2[N_LOUD] = {0}; 00221 long v; 00222 int i; 00223 00224 if(loud2[0] == 0) { 00225 loud2[0] = (long)Loudampl[0] * (long)Loudampl[0]; 00226 for(i=1; i<N_LOUD; ++i) { 00227 v = (long)Loudampl[i] * (long)Loudampl[i]; 00228 loud2[i] = v; 00229 loud2[i-1] = (loud2[i-1]+v) / 2; 00230 } 00231 } 00232 00233 v = (long)re * (long)re + (long)im * (long)im; 00234 00235 for(i=0; i<N_LOUD; ++i) 00236 if(loud2[i] <= v) 00237 break; 00238 00239 return (-i); 00240 } 00241 00242 /* 00243 fix_mpy() - fixed-point multiplication 00244 */ 00245 fixed fix_mpy(fixed a, fixed b) 00246 { 00247 FIX_MPY(a,a,b); 00248 return a; 00249 } 00250 00251 /* 00252 iscale() - scale an integer value by (numer/denom) 00253 */ 00254 int iscale(int value, int numer, int denom) 00255 { 00256 #ifdef DOS 00257 asm mov ax,value 00258 asm imul WORD PTR numer 00259 asm idiv WORD PTR denom 00260 00261 return _AX; 00262 #else 00263 return (long) value * (long)numer/(long)denom; 00264 #endif 00265 } 00266 00267 /* 00268 fix_dot() - dot product of two fixed arrays 00269 */ 00270 fixed fix_dot(fixed *hpa, fixed *pb, int n) 00271 { 00272 fixed *pa = 0; 00273 long sum; 00274 register fixed a,b; 00275 //unsigned int seg,off; 00276 00277 00278 sum = 0L; 00279 while(n--) { 00280 a = *pa++; 00281 b = *pb++; 00282 FIX_MPY(a,a,b); 00283 sum += a; 00284 } 00285 00286 if(sum > 0x7FFF) 00287 sum = 0x7FFF; 00288 else if(sum < -0x7FFF) 00289 sum = -0x7FFF; 00290 00291 return (fixed)sum; 00292 00293 00294 } 00295 00296 00297 fixed Sinewave[1024] = { 00298 0, 201, 402, 603, 804, 1005, 1206, 1406, 00299 1607, 1808, 2009, 2209, 2410, 2610, 2811, 3011, 00300 3211, 3411, 3611, 3811, 4011, 4210, 4409, 4608, 00301 4807, 5006, 5205, 5403, 5601, 5799, 5997, 6195, 00302 6392, 6589, 6786, 6982, 7179, 7375, 7571, 7766, 00303 7961, 8156, 8351, 8545, 8739, 8932, 9126, 9319, 00304 9511, 9703, 9895, 10087, 10278, 10469, 10659, 10849, 00305 11038, 11227, 11416, 11604, 11792, 11980, 12166, 12353, 00306 12539, 12724, 12909, 13094, 13278, 13462, 13645, 13827, 00307 14009, 14191, 14372, 14552, 14732, 14911, 15090, 15268, 00308 15446, 15623, 15799, 15975, 16150, 16325, 16499, 16672, 00309 16845, 17017, 17189, 17360, 17530, 17699, 17868, 18036, 00310 18204, 18371, 18537, 18702, 18867, 19031, 19194, 19357, 00311 19519, 19680, 19840, 20000, 20159, 20317, 20474, 20631, 00312 20787, 20942, 21096, 21249, 21402, 21554, 21705, 21855, 00313 22004, 22153, 22301, 22448, 22594, 22739, 22883, 23027, 00314 23169, 23311, 23452, 23592, 23731, 23869, 24006, 24143, 00315 24278, 24413, 24546, 24679, 24811, 24942, 25072, 25201, 00316 25329, 25456, 25582, 25707, 25831, 25954, 26077, 26198, 00317 26318, 26437, 26556, 26673, 26789, 26905, 27019, 27132, 00318 27244, 27355, 27466, 27575, 27683, 27790, 27896, 28001, 00319 28105, 28208, 28309, 28410, 28510, 28608, 28706, 28802, 00320 28897, 28992, 29085, 29177, 29268, 29358, 29446, 29534, 00321 29621, 29706, 29790, 29873, 29955, 30036, 30116, 30195, 00322 30272, 30349, 30424, 30498, 30571, 30643, 30713, 30783, 00323 30851, 30918, 30984, 31049, 00324 31113, 31175, 31236, 31297, 00325 31356, 31413, 31470, 31525, 31580, 31633, 31684, 31735, 00326 31785, 31833, 31880, 31926, 31970, 32014, 32056, 32097, 00327 32137, 32176, 32213, 32249, 32284, 32318, 32350, 32382, 00328 32412, 32441, 32468, 32495, 32520, 32544, 32567, 32588, 00329 32609, 32628, 32646, 32662, 32678, 32692, 32705, 32717, 00330 32727, 32736, 32744, 32751, 32757, 32761, 32764, 32766, 00331 32767, 32766, 32764, 32761, 32757, 32751, 32744, 32736, 00332 32727, 32717, 32705, 32692, 32678, 32662, 32646, 32628, 00333 32609, 32588, 32567, 32544, 32520, 32495, 32468, 32441, 00334 32412, 32382, 32350, 32318, 32284, 32249, 32213, 32176, 00335 32137, 32097, 32056, 32014, 31970, 31926, 31880, 31833, 00336 31785, 31735, 31684, 31633, 31580, 31525, 31470, 31413, 00337 31356, 31297, 31236, 31175, 31113, 31049, 30984, 30918, 00338 30851, 30783, 30713, 30643, 30571, 30498, 30424, 30349, 00339 30272, 30195, 30116, 30036, 29955, 29873, 29790, 29706, 00340 29621, 29534, 29446, 29358, 29268, 29177, 29085, 28992, 00341 28897, 28802, 28706, 28608, 28510, 28410, 28309, 28208, 00342 28105, 28001, 27896, 27790, 27683, 27575, 27466, 27355, 00343 27244, 27132, 27019, 26905, 26789, 26673, 26556, 26437, 00344 26318, 26198, 26077, 25954, 25831, 25707, 25582, 25456, 00345 25329, 25201, 25072, 24942, 24811, 24679, 24546, 24413, 00346 24278, 24143, 24006, 23869, 23731, 23592, 23452, 23311, 00347 23169, 23027, 22883, 22739, 22594, 22448, 22301, 22153, 00348 22004, 21855, 21705, 21554, 21402, 21249, 21096, 20942, 00349 20787, 20631, 20474, 20317, 20159, 20000, 19840, 19680, 00350 19519, 19357, 19194, 19031, 18867, 18702, 18537, 18371, 00351 18204, 18036, 17868, 17699, 17530, 17360, 17189, 17017, 00352 16845, 16672, 16499, 16325, 16150, 15975, 15799, 15623, 00353 15446, 15268, 15090, 14911, 14732, 14552, 14372, 14191, 00354 14009, 13827, 13645, 13462, 13278, 13094, 12909, 12724, 00355 12539, 12353, 12166, 11980, 11792, 11604, 11416, 11227, 00356 11038, 10849, 10659, 10469, 10278, 10087, 9895, 9703, 00357 9511, 9319, 9126, 8932, 8739, 8545, 8351, 8156, 00358 7961, 7766, 7571, 7375, 7179, 6982, 6786, 6589, 00359 6392, 6195, 5997, 5799, 5601, 5403, 5205, 5006, 00360 4807, 4608, 4409, 4210, 4011, 3811, 3611, 3411, 00361 3211, 3011, 2811, 2610, 2410, 2209, 2009, 1808, 00362 1607, 1406, 1206, 1005, 804, 603, 402, 201, 00363 0, -201, -402, -603, -804, -1005, -1206, -1406, 00364 -1607, -1808, -2009, -2209, -2410, -2610, -2811, -3011, 00365 -3211, -3411, -3611, -3811, -4011, -4210, -4409, -4608, 00366 -4807, -5006, -5205, -5403, -5601, -5799, -5997, -6195, 00367 -6392, -6589, -6786, -6982, -7179, -7375, -7571, -7766, 00368 -7961, -8156, -8351, -8545, -8739, -8932, -9126, -9319, 00369 -9511, -9703, -9895, -10087, -10278, -10469, -10659, -10849, 00370 -11038, -11227, -11416, -11604, -11792, -11980, -12166, -12353, 00371 -12539, -12724, -12909, -13094, -13278, -13462, -13645, -13827, 00372 -14009, -14191, -14372, -14552, -14732, -14911, -15090, -15268, 00373 -15446, -15623, -15799, -15975, -16150, -16325, -16499, -16672, 00374 -16845, -17017, -17189, -17360, -17530, -17699, -17868, -18036, 00375 -18204, -18371, -18537, -18702, -18867, -19031, -19194, -19357, 00376 -19519, -19680, -19840, -20000, -20159, -20317, -20474, -20631, 00377 -20787, -20942, -21096, -21249, -21402, -21554, -21705, -21855, 00378 -22004, -22153, -22301, -22448, -22594, -22739, -22883, -23027, 00379 -23169, -23311, -23452, -23592, -23731, -23869, -24006, -24143, 00380 -24278, -24413, -24546, -24679, -24811, -24942, -25072, -25201, 00381 -25329, -25456, -25582, -25707, -25831, -25954, -26077, -26198, 00382 -26318, -26437, -26556, -26673, -26789, -26905, -27019, -27132, 00383 -27244, -27355, -27466, -27575, -27683, -27790, -27896, -28001, 00384 -28105, -28208, -28309, -28410, -28510, -28608, -28706, -28802, 00385 -28897, -28992, -29085, -29177, -29268, -29358, -29446, -29534, 00386 -29621, -29706, -29790, -29873, -29955, -30036, -30116, -30195, 00387 -30272, -30349, -30424, -30498, -30571, -30643, -30713, -30783, 00388 -30851, -30918, -30984, -31049, -31113, -31175, -31236, -31297, 00389 -31356, -31413, -31470, -31525, -31580, -31633, -31684, -31735, 00390 -31785, -31833, -31880, -31926, -31970, -32014, -32056, -32097, 00391 -32137, -32176, -32213, -32249, -32284, -32318, -32350, -32382, 00392 -32412, -32441, -32468, -32495, -32520, -32544, -32567, -32588, 00393 -32609, -32628, -32646, -32662, -32678, -32692, -32705, -32717, 00394 -32727, -32736, -32744, -32751, -32757, -32761, -32764, -32766, 00395 -32767, -32766, -32764, -32761, -32757, -32751, -32744, -32736, 00396 -32727, -32717, -32705, -32692, -32678, -32662, -32646, -32628, 00397 -32609, -32588, -32567, -32544, -32520, -32495, -32468, -32441, 00398 -32412, -32382, -32350, -32318, -32284, -32249, -32213, -32176, 00399 -32137, -32097, -32056, -32014, -31970, -31926, -31880, -31833, 00400 -31785, -31735, -31684, -31633, -31580, -31525, -31470, -31413, 00401 -31356, -31297, -31236, -31175, -31113, -31049, -30984, -30918, 00402 -30851, -30783, -30713, -30643, -30571, -30498, -30424, -30349, 00403 -30272, -30195, -30116, -30036, -29955, -29873, -29790, -29706, 00404 -29621, -29534, -29446, -29358, -29268, -29177, -29085, -28992, 00405 -28897, -28802, -28706, -28608, -28510, -28410, -28309, -28208, 00406 -28105, -28001, -27896, -27790, -27683, -27575, -27466, -27355, 00407 -27244, -27132, -27019, -26905, -26789, -26673, -26556, -26437, 00408 -26318, -26198, -26077, -25954, -25831, -25707, -25582, -25456, 00409 -25329, -25201, -25072, -24942, -24811, -24679, -24546, -24413, 00410 -24278, -24143, -24006, -23869, -23731, -23592, -23452, -23311, 00411 -23169, -23027, -22883, -22739, -22594, -22448, -22301, -22153, 00412 -22004, -21855, -21705, -21554, -21402, -21249, -21096, -20942, 00413 -20787, -20631, -20474, -20317, -20159, -20000, -19840, -19680, 00414 -19519, -19357, -19194, -19031, -18867, -18702, -18537, -18371, 00415 -18204, -18036, -17868, -17699, -17530, -17360, -17189, -17017, 00416 -16845, -16672, -16499, -16325, -16150, -15975, -15799, -15623, 00417 -15446, -15268, -15090, -14911, -14732, -14552, -14372, -14191, 00418 -14009, -13827, -13645, -13462, -13278, -13094, -12909, -12724, 00419 -12539, -12353, -12166, -11980, -11792, -11604, -11416, -11227, 00420 -11038, -10849, -10659, -10469, -10278, -10087, -9895, -9703, 00421 -9511, -9319, -9126, -8932, -8739, -8545, -8351, -8156, 00422 -7961, -7766, -7571, -7375, -7179, -6982, -6786, -6589, 00423 -6392, -6195, -5997, -5799, -5601, -5403, -5205, -5006, 00424 -4807, -4608, -4409, -4210, -4011, -3811, -3611, -3411, 00425 -3211, -3011, -2811, -2610, -2410, -2209, -2009, -1808, 00426 -1607, -1406, -1206, -1005, -804, -603, -402, -201, 00427 }; 00428 00429 00430 fixed Loudampl[100] = { 00431 32767, 29203, 26027, 23197, 20674, 18426, 16422, 14636, 00432 13044, 11626, 10361, 9234, 8230, 7335, 6537, 5826, 00433 5193, 4628, 4125, 3676, 3276, 2920, 2602, 2319, 00434 2067, 1842, 1642, 1463, 1304, 1162, 1036, 923, 00435 823, 733, 653, 582, 519, 462, 412, 367, 00436 327, 292, 260, 231, 206, 184, 164, 146, 00437 130, 116, 103, 92, 82, 73, 65, 58, 00438 51, 46, 41, 36, 32, 29, 26, 23, 00439 20, 18, 16, 14, 13, 11, 10, 9, 00440 8, 7, 6, 5, 5, 4, 4, 3, 00441 3, 2, 2, 2, 2, 1, 1, 1, 00442 1, 1, 1, 0, 0, 0, 0, 0, 00443 0, 0, 0, 0, 00444 }; 00445 00446 00447 00448