- #include <math.h>
- #include <stdio.h>
- #include "string.h"
- typedef struct
- {
- float real;
- float imag;
- } complex_t;
- #ifndef PI
- #define PI (3.14159265)
- #endif
- #define TYPE_FFT_E float /* Type is the same with complex_t member */
- typedef complex_t TYPE_FFT; /* Define complex_t in Config.h */
- typedef unsigned int uint32_t;
- #define SAMPLE_NODES (1024)
- complex_t fft_buff[SAMPLE_NODES];
- //python生成的3个sin混合的波形数组
- float my_sin_wave_table[] =
- {
- 0.000000,13.308217,25.359460,35.142296,42.082633,46.140080,47.788611,47.887148,47.470293,47.507139,48.682920,51.252751,\
- 55.000000,59.307883,63.326941,66.199152,67.286436,66.350198,63.639610,59.866963,56.074110,53.418823,52.928233,55.274323,\
- 60.621778,68.582540,78.287693,88.561119,98.156743,106.007043,111.428222,114.237346,114.756729,113.706291,112.009861,\
- 110.560634,110.000000,110.560634,112.009861,113.706291,114.756729,114.237346,111.428222,106.007043,98.156743,88.561119,\
- 78.287693,68.582540,60.621778,55.274323,52.928233,53.418823,56.074110,59.866963,63.639610,66.350198,67.286436,66.199152,\
- 63.326941,59.307883,55.000000,51.252751,48.682920,47.507139,47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,\
- 25.359460,13.308217,0.000000,-13.308217,-25.359460,-35.142296,-42.082633,-46.140080,-47.788611,-47.887148,-47.470293,\
- -47.507139,-48.682920,-51.252751,-55.000000,-59.307883,-63.326941,-66.199152,-67.286436,-66.350198,-63.639610,-59.866963,\
- -56.074110,-53.418823,-52.928233,-55.274323,-60.621778,-68.582540,-78.287693,-88.561119,-98.156743,-106.007043,-111.428222,\
- -114.237346,-114.756729,-113.706291,-112.009861,-110.560634,-110.000000,-110.560634,-112.009861,-113.706291,-114.756729,\
- -114.237346,-111.428222,-106.007043,-98.156743,-88.561119,-78.287693,-68.582540,-60.621778,-55.274323,-52.928233,\
- -53.418823,-56.074110,-59.866963,-63.639610,-66.350198,-67.286436,-66.199152,-63.326941,-59.307883,-55.000000,\
- -51.252751,-48.682920,-47.507139,-47.470293,-47.887148,-47.788611,-46.140080,-42.082633,-35.142296,-25.359460,\
- -13.308217,-0.000000,13.308217,25.359460,35.142296,42.082633,46.140080,47.788611,47.887148,47.470293,47.507139,\
- 48.682920,51.252751,55.000000,59.307883,63.326941,66.199152,67.286436,66.350198,63.639610,59.866963,56.074110,\
- 53.418823,52.928233,55.274323,60.621778,68.582540,78.287693,88.561119,98.156743,106.007043,111.428222,114.237346,\
- 114.756729,113.706291,112.009861,110.560634,110.000000,110.560634,112.009861,113.706291,114.756729,114.237346,\
- 111.428222,106.007043,98.156743,88.561119,78.287693,68.582540,60.621778,55.274323,52.928233,53.418823,56.074110,\
- 59.866963,63.639610,66.350198,67.286436,66.199152,63.326941,59.307883,55.000000,51.252751,48.682920,47.507139,\
- 47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,25.359460,13.308217,0.000000,-13.308217,-25.359460,\
- -35.142296,-42.082633,-46.140080,-47.788611,-47.887148,-47.470293,-47.507139,-48.682920,-51.252751,-55.000000,\
- -59.307883,-63.326941,-66.199152,-67.286436,-66.350198,-63.639610,-59.866963,-56.074110,-53.418823,-52.928233,\
- -55.274323,-60.621778,-68.582540,-78.287693,-88.561119,-98.156743,-106.007043,-111.428222,-114.237346,-114.756729,\
- -113.706291,-112.009861,-110.560634,-110.000000,-110.560634,-112.009861,-113.706291,-114.756729,-114.237346,-111.428222,\
- -106.007043,-98.156743,-88.561119,-78.287693,-68.582540,-60.621778,-55.274323,-52.928233,-53.418823,-56.074110,-59.866963,\
- -63.639610,-66.350198,-67.286436,-66.199152,-63.326941,-59.307883,-55.000000,-51.252751,-48.682920,-47.507139,-47.470293,\
- -47.887148,-47.788611,-46.140080,-42.082633,-35.142296,-25.359460,-13.308217,-0.000000,13.308217,25.359460,35.142296,\
- 42.082633,46.140080,47.788611,47.887148,47.470293,47.507139,48.682920,51.252751,55.000000,59.307883,63.326941,66.199152,\
- 67.286436,66.350198,63.639610,59.866963,56.074110,53.418823,52.928233,55.274323,60.621778,68.582540,78.287693,88.561119,\
- 98.156743,106.007043,111.428222,114.237346,114.756729,113.706291,112.009861,110.560634,110.000000,110.560634,112.009861,\
- 113.706291,114.756729,114.237346,111.428222,106.007043,98.156743,88.561119,78.287693,68.582540,60.621778,55.274323,\
- 52.928233,53.418823,56.074110,59.866963,63.639610,66.350198,67.286436,66.199152,63.326941,59.307883,55.000000,51.252751,\
- 48.682920,47.507139,47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,25.359460,13.308217,0.000000,-13.308217,\
- -25.359460,-35.142296,-42.082633,-46.140080,-47.788611,-47.887148,-47.470293,-47.507139,-48.682920,-51.252751,-55.000000,\
- -59.307883,-63.326941,-66.199152,-67.286436,-66.350198,-63.639610,-59.866963,-56.074110,-53.418823,-52.928233,-55.274323,\
- -60.621778,-68.582540,-78.287693,-88.561119,-98.156743,-106.007043,-111.428222,-114.237346,-114.756729,-113.706291,\
- -112.009861,-110.560634,-110.000000,-110.560634,-112.009861,-113.706291,-114.756729,-114.237346,-111.428222,-106.007043,\
- -98.156743,-88.561119,-78.287693,-68.582540,-60.621778,-55.274323,-52.928233,-53.418823,-56.074110,-59.866963,-63.639610,\
- -66.350198,-67.286436,-66.199152,-63.326941,-59.307883,-55.000000,-51.252751,-48.682920,-47.507139,-47.470293,-47.887148,\
- -47.788611,-46.140080,-42.082633,-35.142296,-25.359460,-13.308217,-0.000000,13.308217,25.359460,35.142296,42.082633,\
- 46.140080,47.788611,47.887148,47.470293,47.507139,48.682920,51.252751,55.000000,59.307883,63.326941,66.199152,67.286436,\
- 66.350198,63.639610,59.866963,56.074110,53.418823,52.928233,55.274323,60.621778,68.582540,78.287693,88.561119,98.156743,\
- 106.007043,111.428222,114.237346,114.756729,113.706291,112.009861,110.560634,110.000000,110.560634,112.009861,113.706291,\
- 114.756729,114.237346,111.428222,106.007043,98.156743,88.561119,78.287693,68.582540,60.621778,55.274323,52.928233,53.418823,\
- 56.074110,59.866963,63.639610,66.350198,67.286436,66.199152,63.326941,59.307883,55.000000,51.252751,48.682920,47.507139,\
- 47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,25.359460,13.308217,0.000000,-13.308217,-25.359460,-35.142296,\
- -42.082633,-46.140080,-47.788611,-47.887148,-47.470293,-47.507139,-48.682920,-51.252751,-55.000000,-59.307883,-63.326941,\
- -66.199152,-67.286436,-66.350198,-63.639610,-59.866963,-56.074110,-53.418823,-52.928233,-55.274323,-60.621778,-68.582540,\
- -78.287693,-88.561119,-98.156743,-106.007043,-111.428222,-114.237346,-114.756729,-113.706291,-112.009861,-110.560634,\
- -110.000000,-110.560634,-112.009861,-113.706291,-114.756729,-114.237346,-111.428222,-106.007043,-98.156743,-88.561119,\
- -78.287693,-68.582540,-60.621778,-55.274323,-52.928233,-53.418823,-56.074110,-59.866963,-63.639610,-66.350198,-67.286436,\
- -66.199152,-63.326941,-59.307883,-55.000000,-51.252751,-48.682920,-47.507139,-47.470293,-47.887148,-47.788611,-46.140080,\
- -42.082633,-35.142296,-25.359460,-13.308217,-0.000000,13.308217,25.359460,35.142296,42.082633,46.140080,47.788611,47.887148,\
- 47.470293,47.507139,48.682920,51.252751,55.000000,59.307883,63.326941,66.199152,67.286436,66.350198,63.639610,59.866963,\
- 56.074110,53.418823,52.928233,55.274323,60.621778,68.582540,78.287693,88.561119,98.156743,106.007043,111.428222,114.237346,\
- 114.756729,113.706291,112.009861,110.560634,110.000000,110.560634,112.009861,113.706291,114.756729,114.237346,111.428222,\
- 106.007043,98.156743,88.561119,78.287693,68.582540,60.621778,55.274323,52.928233,53.418823,56.074110,59.866963,63.639610,\
- 66.350198,67.286436,66.199152,63.326941,59.307883,55.000000,51.252751,48.682920,47.507139,47.470293,47.887148,47.788611,\
- 46.140080,42.082633,35.142296,25.359460,13.308217,0.000000,-13.308217,-25.359460,-35.142296,-42.082633,-46.140080,-47.788611,\
- -47.887148,-47.470293,-47.507139,-48.682920,-51.252751,-55.000000,-59.307883,-63.326941,-66.199152,-67.286436,-66.350198,\
- -63.639610,-59.866963,-56.074110,-53.418823,-52.928233,-55.274323,-60.621778,-68.582540,-78.287693,-88.561119,-98.156743,\
- -106.007043,-111.428222,-114.237346,-114.756729,-113.706291,-112.009861,-110.560634,-110.000000,-110.560634,-112.009861,\
- -113.706291,-114.756729,-114.237346,-111.428222,-106.007043,-98.156743,-88.561119,-78.287693,-68.582540,-60.621778,-55.274323,\
- -52.928233,-53.418823,-56.074110,-59.866963,-63.639610,-66.350198,-67.286436,-66.199152,-63.326941,-59.307883,-55.000000,\
- -51.252751,-48.682920,-47.507139,-47.470293,-47.887148,-47.788611,-46.140080,-42.082633,-35.142296,-25.359460,-13.308217,\
- -0.000000,13.308217,25.359460,35.142296,42.082633,46.140080,47.788611,47.887148,47.470293,47.507139,48.682920,51.252751,\
- 55.000000,59.307883,63.326941,66.199152,67.286436,66.350198,63.639610,59.866963,56.074110,53.418823,52.928233,55.274323,\
- 60.621778,68.582540,78.287693,88.561119,98.156743,106.007043,111.428222,114.237346,114.756729,113.706291,112.009861,\
- 110.560634,110.000000,110.560634,112.009861,113.706291,114.756729,114.237346,111.428222,106.007043,98.156743,88.561119,\
- 78.287693,68.582540,60.621778,55.274323,52.928233,53.418823,56.074110,59.866963,63.639610,66.350198,67.286436,66.199152,\
- 63.326941,59.307883,55.000000,51.252751,48.682920,47.507139,47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,\
- 25.359460,13.308217,0.000000,-13.308217,-25.359460,-35.142296,-42.082633,-46.140080,-47.788611,-47.887148,-47.470293,\
- -47.507139,-48.682920,-51.252751,-55.000000,-59.307883,-63.326941,-66.199152,-67.286436,-66.350198,-63.639610,-59.866963,\
- -56.074110,-53.418823,-52.928233,-55.274323,-60.621778,-68.582540,-78.287693,-88.561119,-98.156743,-106.007043,-111.428222,\
- -114.237346,-114.756729,-113.706291,-112.009861,-110.560634,-110.000000,-110.560634,-112.009861,-113.706291,-114.756729,\
- -114.237346,-111.428222,-106.007043,-98.156743,-88.561119,-78.287693,-68.582540,-60.621778,-55.274323,-52.928233,-53.418823,\
- -56.074110,-59.866963,-63.639610,-66.350198,-67.286436,-66.199152,-63.326941,-59.307883,-55.000000,-51.252751,-48.682920,\
- -47.507139,-47.470293,-47.887148,-47.788611,-46.140080,-42.082633,-35.142296,-25.359460,-13.308217,-0.000000,13.308217,\
- 25.359460,35.142296,42.082633,46.140080,47.788611,47.887148,47.470293,47.507139,48.682920,51.252751,55.000000,59.307883,\
- 63.326941,66.199152,67.286436,66.350198,63.639610,59.866963,56.074110,53.418823,52.928233,55.274323,60.621778,68.582540,\
- 78.287693,88.561119,98.156743,106.007043,111.428222,114.237346,114.756729,113.706291,112.009861,110.560634,110.000000,\
- 110.560634,112.009861,113.706291,114.756729,114.237346,111.428222,106.007043,98.156743,88.561119,78.287693,68.582540,\
- 60.621778,55.274323,52.928233,53.418823,56.074110,59.866963,63.639610,66.350198,67.286436,66.199152,63.326941,59.307883,\
- 55.000000,51.252751,48.682920,47.507139,47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,25.359460,13.308217,\
- 0.000000,-13.308217,-25.359460,-35.142296,-42.082633,-46.140080,-47.788611,-47.887148,-47.470293,-47.507139,-48.682920,\
- -51.252751,-55.000000,-59.307883,-63.326941,-66.199152,-67.286436,-66.350198,-63.639610,-59.866963,-56.074110,-53.418823,\
- -52.928233,-55.274323,-60.621778,-68.582540,-78.287693,-88.561119,-98.156743,-106.007043,-111.428222,-114.237346,-114.756729,\
- -113.706291,-112.009861,-110.560634,-110.000000,-110.560634,-112.009861,-113.706291,-114.756729,-114.237346,-111.428222,\
- -106.007043,-98.156743,-88.561119,-78.287693,-68.582540,-60.621778,-55.274323,-52.928233,-53.418823,-56.074110,-59.866963,\
- -63.639610,-66.350198,-67.286436,-66.199152,-63.326941,-59.307883,-55.000000,-51.252751,-48.682920,-47.507139,-47.470293,\
- -47.887148,-47.788611,-46.140080,-42.082633,-35.142296,-25.359460,-13.308217,-0.000000,13.308217,25.359460,35.142296,\
- 42.082633,46.140080,47.788611,47.887148,47.470293,47.507139,48.682920,51.252751,55.000000,59.307883,63.326941,66.199152,\
- 67.286436,66.350198,63.639610,59.866963,56.074110,53.418823,52.928233,55.274323,60.621778,68.582540,78.287693,88.561119,\
- 98.156743,106.007043,111.428222,114.237346,114.756729,113.706291,112.009861,110.560634,110.000000,110.560634,112.009861,\
- 113.706291,114.756729,114.237346,111.428222,106.007043,98.156743,88.561119,78.287693,68.582540,60.621778,55.274323,52.928233,\
- 53.418823,56.074110,59.866963,63.639610,66.350198,67.286436,66.199152,63.326941,59.307883,55.000000,51.252751,48.682920,\
- 47.507139,47.470293,47.887148,47.788611,46.140080,42.082633,35.142296,25.359460,13.308217,0.000000,-13.308217,-25.359460,\
- -35.142296,-42.082633,-46.140080,-47.788611,-47.887148,-47.470293,-47.507139,-48.682920,-51.252751,-55.000000,-59.307883,\
- -63.326941,-66.199152,-67.286436,-66.350198,-63.639610,-59.866963,-56.074110,-53.418823,-52.928233,-55.274323,-60.621778,\
- -68.582540,-78.287693,-88.561119,-98.156743,-106.007043,-111.428222,-114.237346,-114.756729,-113.706291,-112.009861,\
- -110.560634,-110.000000,-110.560634,-112.009861,-113.706291,-114.756729,-114.237346,-111.428222,-106.007043,-98.156743,\
- -88.561119,-78.287693,-68.582540,-60.621778,-55.274323,-52.928233,-53.418823,-56.074110,-59.866963,-63.639610,-66.350198,\
- -67.286436,-66.199152,-63.326941,-59.307883,-55.000000,-51.252751,-48.682920,-47.507139,-47.470293,-47.887148,-47.788611,\
- -46.140080,-42.082633,-35.142296,-25.359460,-13.308217,
- };
- //fft算法来自开源 https://github.com/xiahouzuoxin/fft
- const float sin_tb[] = // 精度(PI PI/2 PI/4 PI/8 PI/16 ... PI/(2^k))
- {
- 0.000000, 1.000000, 0.707107, 0.382683, 0.195090, 0.098017,
- 0.049068, 0.024541, 0.012272, 0.006136, 0.003068, 0.001534,
- 0.000767, 0.000383, 0.000192, 0.000096, 0.000048, 0.000024,
- 0.000012, 0.000006, 0.000003
- };
- const float cos_tb[] = // 精度(PI PI/2 PI/4 PI/8 PI/16 ... PI/(2^k))
- {
- -1.000000, 0.000000, 0.707107, 0.923880, 0.980785, 0.995185,
- 0.998795, 0.999699, 0.999925, 0.999981, 0.999995, 0.999999,
- 1.000000, 1.000000, 1.000000, 1.000000, 1.000000, 1.000000,
- 1.000000, 1.000000, 1.000000
- };
- int ones_32(uint32_t n)
- {
- unsigned int c = 0 ;
- for(c = 0; n; ++c)
- {
- n &= (n - 1) ;
- }
- return c ;
- }
- uint32_t floor_log2_32(uint32_t fft_buff)
- {
- fft_buff |= (fft_buff >> 1);
- fft_buff |= (fft_buff >> 2);
- fft_buff |= (fft_buff >> 4);
- fft_buff |= (fft_buff >> 8);
- fft_buff |= (fft_buff >> 16);
- return (ones_32(fft_buff >> 1));
- }
- /*
- * FFT Algorithm
- * === Inputs ===
- * fft_buff : complex numbers
- * N : nodes of FFT. @N should be power of 2, that is 2^(*)
- * === Output ===
- * the @fft_buff contains the result of FFT algorithm, so the original data
- * in @fft_buff is destroyed, please store them before using FFT.
- */
- int fft(TYPE_FFT *fft_buff, uint32_t N)
- {
- int i, j, l, k, ip;
- static uint32_t M = 0;
- static int le, le2;
- static TYPE_FFT_E sR, sI, tR, tI, uR, uI;
- M = floor_log2_32(N);
- /*
- * bit reversal sorting
- */
- l = N >> 1;
- j = l;
- ip = N - 2;
- for(i = 1; i <= ip; i++)
- {
- if(i < j)
- {
- tR = fft_buff[j].real;
- tI = fft_buff[j].imag;
- fft_buff[j].real = fft_buff[i].real;
- fft_buff[j].imag = fft_buff[i].imag;
- fft_buff[i].real = tR;
- fft_buff[i].imag = tI;
- }
- k = l;
- while(k <= j)
- {
- j = j - k;
- k = k >> 1;
- }
- j = j + k;
- }
- /*
- * For Loops
- */
- for(l = 1; l <= M; l++) /* loop for ceil{log2(N)} */
- {
- le = (int)(1 << l);
- le2 = (int)(le >> 1);
- uR = 1;
- uI = 0;
- k = floor_log2_32(le2);
- sR = cos_tb[k];
- sI = -sin_tb[k];
- for(j = 1; j <= le2; j++) /* loop for each sub DFT */
- {
- for(i = j - 1; i < N; i += le) /* loop for each butterfly */
- {
- ip = i + le2;
- tR = fft_buff[ip].real * uR - fft_buff[ip].imag * uI;
- tI = fft_buff[ip].real * uI + fft_buff[ip].imag * uR;
- fft_buff[ip].real = fft_buff[i].real - tR;
- fft_buff[ip].imag = fft_buff[i].imag - tI;
- fft_buff[i].real += tR;
- fft_buff[i].imag += tI;
- } /* Next i */
- tR = uR;
- uR = tR * sR - uI * sI;
- uI = tR * sI + uI * sR;
- } /* Next j */
- } /* Next l */
- return 0;
- }
- /*
- * Inverse FFT Algorithm
- * === Inputs ===
- * fft_buff : complex numbers
- * N : nodes of FFT. @N should be power of 2, that is 2^(*)
- * === Output ===
- * the @fft_buff contains the result of FFT algorithm, so the original data
- * in @fft_buff is destroyed, please store them before using FFT.
- */
- int ifft(TYPE_FFT *fft_buff, uint32_t N)
- {
- int k = 0;
- for(k = 0; k <= N - 1; k++)
- {
- fft_buff[k].imag = -fft_buff[k].imag;
- }
- fft(fft_buff, N); /* using FFT */
- for(k = 0; k <= N - 1; k++)
- {
- fft_buff[k].real = fft_buff[k].real / N;
- fft_buff[k].imag = -fft_buff[k].imag / N;
- }
- return 0;
- }
- static void import_data(void)
- {
- int i;
- for(i = 0; i < SAMPLE_NODES; i++)
- {
- fft_buff[i].real = my_sin_wave_table[i];//取前1024个数进行fft变换
- fft_buff[i].imag = 0.0f;
- }
- }
- int main(int argc, char *argv[])
- {
- int i;
- int f;//频率
- float a;//幅度
- int fd;
- float t;
- printf("FFT\r\n");
- import_data();
- fft(fft_buff, SAMPLE_NODES);
- //fft后的结果在fft_buff
- //将其实部与虚部处理,输出频点与幅度值,导入Excel看效果
- //因为是周期性质,取前半部分即可
- //数据采样频率是360*40,均分到SAMPLE_NODES,则对应频点间隔是 360*40/SAMPLE_NODES
- fd=360*40/SAMPLE_NODES;
- for(i = 0; i < SAMPLE_NODES / 2; i++)
- {
- f = i *fd;
- a = (double)sqrt(fft_buff[i].real * fft_buff[i].real + fft_buff[i].imag * fft_buff[i].imag)/ (SAMPLE_NODES / 2);//转换幅度
- //printf("%d,%f\n", f, a);//>>导入excel查看幅频图效果
- }
- //过滤高频部分
- //将幅度小于某个值的,以30为例过滤
- for(i = 0; i < SAMPLE_NODES; i++)
- {
- a = (double)sqrt(fft_buff[i].real * fft_buff[i].real + fft_buff[i].imag * fft_buff[i].imag)/ (SAMPLE_NODES / 2);
- if(a<30)
- {
- fft_buff[i].real = 0;
- fft_buff[i].imag = 0;
- }
- }
- //再进行逆运算还原
- ifft(fft_buff, SAMPLE_NODES);
- for(i = 0; i < SAMPLE_NODES; i++)
- {
- t=1.0/(360*40)*i;//结合采样频率步进,方便查看波形效果
- //printf("%f,%f\n", t,fft_buff[i].real);//>>导入excel查看还原后的sin效果
- }
- return 0;
- }