oldexamplecode
Dependencies: mbed
DFT.cpp@0:6863633bf8a4, 2017-03-24 (annotated)
- Committer:
- rik
- Date:
- Fri Mar 24 11:22:30 2017 +0000
- Revision:
- 0:6863633bf8a4
oldexamplecode;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rik | 0:6863633bf8a4 | 1 | |
rik | 0:6863633bf8a4 | 2 | #include "DFT.h" |
rik | 0:6863633bf8a4 | 3 | |
rik | 0:6863633bf8a4 | 4 | |
rik | 0:6863633bf8a4 | 5 | // Preprocesses the data array and performs a dft on it, ARRAY MUST BE 1024 LONG |
rik | 0:6863633bf8a4 | 6 | void performFFT(float* datam, int datalength){ |
rik | 0:6863633bf8a4 | 7 | if (datalength == 1024){ |
rik | 0:6863633bf8a4 | 8 | // - Calculating and substracting the mean - Takes ~2N additions |
rik | 0:6863633bf8a4 | 9 | subMean(datam, datalength); |
rik | 0:6863633bf8a4 | 10 | // - Applying a hanning window - N multiplications |
rik | 0:6863633bf8a4 | 11 | applyHanningTable(datam, datalength); |
rik | 0:6863633bf8a4 | 12 | // - Re-indexing the array - 2N operations |
rik | 0:6863633bf8a4 | 13 | reindexData(datam, datalength); |
rik | 0:6863633bf8a4 | 14 | // It then applies a radix-2 Cooley Tukey FFT to create a DFT of the original signal |
rik | 0:6863633bf8a4 | 15 | cooleyTukeyRadix2(datam, datalength); |
rik | 0:6863633bf8a4 | 16 | // Remove the DC component and null everything above the nyquist |
rik | 0:6863633bf8a4 | 17 | afterProcessing(datam, datalength); |
rik | 0:6863633bf8a4 | 18 | } |
rik | 0:6863633bf8a4 | 19 | // Thus preprocessing takes 5N operations |
rik | 0:6863633bf8a4 | 20 | // FFT itself takes NlogN operations |
rik | 0:6863633bf8a4 | 21 | // Calculating to complex is N operations |
rik | 0:6863633bf8a4 | 22 | // Calulating the complex back to magnitude is ~4N operations |
rik | 0:6863633bf8a4 | 23 | // Total operation time is thus ~10N + NlogN complex operations |
rik | 0:6863633bf8a4 | 24 | } |
rik | 0:6863633bf8a4 | 25 | |
rik | 0:6863633bf8a4 | 26 | // Performs a radix-2 cooley tukey FFT on the data array |
rik | 0:6863633bf8a4 | 27 | void cooleyTukeyRadix2(float* data, int datalength){ |
rik | 0:6863633bf8a4 | 28 | |
rik | 0:6863633bf8a4 | 29 | complex_num fftdata[1024]; |
rik | 0:6863633bf8a4 | 30 | complex_num fftbuffer[512]; |
rik | 0:6863633bf8a4 | 31 | complex_num twiddlebuffer; |
rik | 0:6863633bf8a4 | 32 | |
rik | 0:6863633bf8a4 | 33 | complex_num minusone; |
rik | 0:6863633bf8a4 | 34 | minusone.real = -1; |
rik | 0:6863633bf8a4 | 35 | minusone.imaginary = 0; |
rik | 0:6863633bf8a4 | 36 | |
rik | 0:6863633bf8a4 | 37 | // Convert data to complex data |
rik | 0:6863633bf8a4 | 38 | for (int i = 0; i < datalength; i++){ |
rik | 0:6863633bf8a4 | 39 | fftdata[i].real = data[i]; |
rik | 0:6863633bf8a4 | 40 | fftdata[i].imaginary = 0; |
rik | 0:6863633bf8a4 | 41 | } |
rik | 0:6863633bf8a4 | 42 | |
rik | 0:6863633bf8a4 | 43 | for (int stage = 0; stage < 10; stage++){ |
rik | 0:6863633bf8a4 | 44 | |
rik | 0:6863633bf8a4 | 45 | unsigned int step = 1<<(stage+1); |
rik | 0:6863633bf8a4 | 46 | |
rik | 0:6863633bf8a4 | 47 | for (unsigned int i = 0; i < 1024; i = i+step){ // Crack it up in parts, i.e. for stage0 -> 512, stage1 -> 256 , 2>128, 3>64, 4>32, 5>16, 6>8, 7>4, 8>2, 9>1 --> 1024/2^stage OR IN STEPS: stage0 -> 2, stage1 -> 4, 2>8, 3>16, 4,32, 5>64, ......... 9>512 --> 1024 - 1024/2^stage+1 |
rik | 0:6863633bf8a4 | 48 | // Buffer the first 'half' of the thing, which is the first half step |
rik | 0:6863633bf8a4 | 49 | for (unsigned int k = 0; k < step / 2; k++){ |
rik | 0:6863633bf8a4 | 50 | fftbuffer[k].real = fftdata[i+k].real; |
rik | 0:6863633bf8a4 | 51 | fftbuffer[k].imaginary = fftdata[i+k].imaginary; |
rik | 0:6863633bf8a4 | 52 | } |
rik | 0:6863633bf8a4 | 53 | |
rik | 0:6863633bf8a4 | 54 | // Apply twiddle factors to the seconds half (half step) depending on stage and numbering |
rik | 0:6863633bf8a4 | 55 | for (unsigned int k = 0; k < step / 2; k++){ |
rik | 0:6863633bf8a4 | 56 | // Read in the correct twiddle factor |
rik | 0:6863633bf8a4 | 57 | // Twiddle factor is recalculated with k*(1024 / 2^stage+1) which can be recalculated to 1<< |
rik | 0:6863633bf8a4 | 58 | twiddlebuffer.imaginary = twiddleLookupTable_Imaginary[k * (1<<(9-stage))]; |
rik | 0:6863633bf8a4 | 59 | twiddlebuffer.real = twiddleLookupTable_Real[k * (1 << (9 - stage))]; |
rik | 0:6863633bf8a4 | 60 | |
rik | 0:6863633bf8a4 | 61 | fftdata[i + k + step / 2] = complex_multiply(fftdata[i + k + step / 2], twiddlebuffer); |
rik | 0:6863633bf8a4 | 62 | } |
rik | 0:6863633bf8a4 | 63 | |
rik | 0:6863633bf8a4 | 64 | // Recalculate the first half by addition of the first and recalculated half |
rik | 0:6863633bf8a4 | 65 | for (unsigned int k = 0; k < step / 2; k++) |
rik | 0:6863633bf8a4 | 66 | fftdata[i + k] = complex_add(fftdata[i+k], fftdata[i+k+step/2]); |
rik | 0:6863633bf8a4 | 67 | |
rik | 0:6863633bf8a4 | 68 | |
rik | 0:6863633bf8a4 | 69 | // Multiply the second half with minus 1 |
rik | 0:6863633bf8a4 | 70 | for (unsigned int k = 0; k < step / 2; k++) |
rik | 0:6863633bf8a4 | 71 | fftdata[i + step / 2 + k] = complex_multiply(fftdata[i + step/2+k], minusone); |
rik | 0:6863633bf8a4 | 72 | |
rik | 0:6863633bf8a4 | 73 | // Add the buffered old values, with the new second half values |
rik | 0:6863633bf8a4 | 74 | for (unsigned int k = 0; k < step / 2; k++) |
rik | 0:6863633bf8a4 | 75 | fftdata[i + step / 2 + k] = complex_add(fftdata[i + step/2 + k], fftbuffer[k]); |
rik | 0:6863633bf8a4 | 76 | } |
rik | 0:6863633bf8a4 | 77 | } |
rik | 0:6863633bf8a4 | 78 | |
rik | 0:6863633bf8a4 | 79 | // Convert complex data back to magnitude data |
rik | 0:6863633bf8a4 | 80 | for (int i = 0; i < 1024; i++) |
rik | 0:6863633bf8a4 | 81 | data[i] = complex_magnitude(fftdata[i]); |
rik | 0:6863633bf8a4 | 82 | } |
rik | 0:6863633bf8a4 | 83 | |
rik | 0:6863633bf8a4 | 84 | // Calculates and subtracts the signal mean - Takes ~2N operations |
rik | 0:6863633bf8a4 | 85 | void subMean(float* data, int datalength){ |
rik | 0:6863633bf8a4 | 86 | float mean = 0; |
rik | 0:6863633bf8a4 | 87 | float total = 0; |
rik | 0:6863633bf8a4 | 88 | // Because the data is between 0 and 1, we can just sum and then device by datalength |
rik | 0:6863633bf8a4 | 89 | // Normally you would devide each data sample by the datalength and sum all of those (to prevent overflow), so this saves N times multiplications |
rik | 0:6863633bf8a4 | 90 | for (int i = 0; i < datalength; i++) |
rik | 0:6863633bf8a4 | 91 | total += data[i]; |
rik | 0:6863633bf8a4 | 92 | |
rik | 0:6863633bf8a4 | 93 | mean = total / datalength; |
rik | 0:6863633bf8a4 | 94 | |
rik | 0:6863633bf8a4 | 95 | for (int i = 0; i < datalength; i++) |
rik | 0:6863633bf8a4 | 96 | data[i] = data[i] - mean; |
rik | 0:6863633bf8a4 | 97 | |
rik | 0:6863633bf8a4 | 98 | } |
rik | 0:6863633bf8a4 | 99 | |
rik | 0:6863633bf8a4 | 100 | // Reindexes the array in preperation of the FFT implemented using a LUT, 2N operations |
rik | 0:6863633bf8a4 | 101 | void reindexData(float* data, int datalength){ |
rik | 0:6863633bf8a4 | 102 | |
rik | 0:6863633bf8a4 | 103 | float reordereddata[1024]; |
rik | 0:6863633bf8a4 | 104 | |
rik | 0:6863633bf8a4 | 105 | for (int i = 0; i < datalength; i++){ |
rik | 0:6863633bf8a4 | 106 | reordereddata[reversebit_lut_i[i]] = data[i]; |
rik | 0:6863633bf8a4 | 107 | } |
rik | 0:6863633bf8a4 | 108 | |
rik | 0:6863633bf8a4 | 109 | // Copy all the reorderddata to the original array (datalength*int size in bytes) |
rik | 0:6863633bf8a4 | 110 | memcpy(data, reordereddata, datalength*4); |
rik | 0:6863633bf8a4 | 111 | } |
rik | 0:6863633bf8a4 | 112 | |
rik | 0:6863633bf8a4 | 113 | // Removes the DC component and null everything above the nyquist frequency |
rik | 0:6863633bf8a4 | 114 | void afterProcessing(float* data, int datalength){ |
rik | 0:6863633bf8a4 | 115 | data[0] = 0; // Set DC component to 0 |
rik | 0:6863633bf8a4 | 116 | |
rik | 0:6863633bf8a4 | 117 | for (int i = datalength-1; i > datalength / 2; i--) // Null everything above the nyquist frequency |
rik | 0:6863633bf8a4 | 118 | data[i] = 0; |
rik | 0:6863633bf8a4 | 119 | |
rik | 0:6863633bf8a4 | 120 | // FIlter 50hz |
rik | 0:6863633bf8a4 | 121 | filterfreq(50, data, datalength); |
rik | 0:6863633bf8a4 | 122 | } |
rik | 0:6863633bf8a4 | 123 | |
rik | 0:6863633bf8a4 | 124 | // Filters a frequency and its multiples |
rik | 0:6863633bf8a4 | 125 | void filterfreq(int frequency, float* data, int datalength){ |
rik | 0:6863633bf8a4 | 126 | int niggr = 512; |
rik | 0:6863633bf8a4 | 127 | int freqperbin = 10; |
rik | 0:6863633bf8a4 | 128 | // Calculate which bin contains the frequency |
rik | 0:6863633bf8a4 | 129 | int bin = frequency / freqperbin; |
rik | 0:6863633bf8a4 | 130 | |
rik | 0:6863633bf8a4 | 131 | // Null bin frequencies and the two surrounding bins, plus its multiples adjecent |
rik | 0:6863633bf8a4 | 132 | do{ |
rik | 0:6863633bf8a4 | 133 | if (bin - 1 >= 0) // Negative elements = you gonna have a bad time |
rik | 0:6863633bf8a4 | 134 | data[bin - 1] = 0; |
rik | 0:6863633bf8a4 | 135 | data[bin] = 0; |
rik | 0:6863633bf8a4 | 136 | data[bin + 1] = 0; // Since element 512-1024 also exist, we can null it even if it isnt really usefull |
rik | 0:6863633bf8a4 | 137 | bin *= 2; |
rik | 0:6863633bf8a4 | 138 | } while (bin<niggr); |
rik | 0:6863633bf8a4 | 139 | } |
rik | 0:6863633bf8a4 | 140 | |
rik | 0:6863633bf8a4 | 141 | // Calculate total magnitude, frequency magnitude component integrating module (FMCIM) |
rik | 0:6863633bf8a4 | 142 | int calcPSD(float* data, int datalength){ |
rik | 0:6863633bf8a4 | 143 | float knoedel=0; |
rik | 0:6863633bf8a4 | 144 | for (int i = 0; i < datalength; i++){ |
rik | 0:6863633bf8a4 | 145 | knoedel += (float)data[i]; |
rik | 0:6863633bf8a4 | 146 | } |
rik | 0:6863633bf8a4 | 147 | return (int)knoedel; |
rik | 0:6863633bf8a4 | 148 | } |