├── Config.h ├── FFT-互相关时延计算.png ├── FFT-含噪声信号.png ├── FFT-标准正弦信号.png ├── Makefile ├── README.md ├── cos_tb.h ├── main.c ├── plot_result.asv ├── plot_result.m ├── sin_cos_tb_gen.m ├── sin_tb.h ├── test.fft ├── test.xcorr ├── test_fft.txt ├── test_xcorr.txt ├── xcorr.c ├── zx_fft.c ├── zx_fft.h ├── zx_math.c └── zx_math.h /Config.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FileName : Config.h 3 | * Author : xiahouzuoxin 4 | * Date : 2013.08.31 5 | * Version : v1.0 6 | * Brief : place global typedefs and macros here 7 | */ 8 | 9 | #ifndef _CONFIG_H 10 | #define _CONFIG_H 11 | 12 | #include 13 | #include 14 | 15 | /* type redefinition */ 16 | /* 17 | typedef enum 18 | { 19 | FALSE = 0, 20 | TRUE = !FALSE 21 | } BOOL; 22 | */ 23 | 24 | typedef unsigned int UINT16; 25 | typedef unsigned long UINT32; 26 | typedef unsigned char UINT8; 27 | typedef int INT16; 28 | typedef long INT32; 29 | typedef char INT8; 30 | 31 | typedef float FP32; 32 | typedef double FP64; 33 | 34 | #if !defined(__stdint_h) && !defined(_STDINT_H) 35 | typedef unsigned long uint40_t; 36 | typedef unsigned int uint32_t; 37 | typedef unsigned short uint16_t; 38 | typedef unsigned char uint8_t; 39 | typedef long int40_t; 40 | typedef int int32_t; 41 | typedef short int16_t; 42 | //typedef char int8_t; 43 | 44 | typedef float fp32_t; 45 | typedef double fp64_t; 46 | #else 47 | #include 48 | #endif // #ifndef __stdint_h 49 | 50 | typedef long long int64_t; 51 | 52 | typedef struct { 53 | float real; 54 | float imag; 55 | } COMPLEX; 56 | 57 | /* Get a byte or word in specified address */ 58 | #define MEM_B(x) ( *( (UINT8 *)(x) ) ) 59 | #define MEM_W(x) ( *( (UINT32 *)(x) ) ) 60 | 61 | #define UPCASE( c ) ( ((c) >= ''a'' && (c) <= ''z'') ? \ 62 | ( (c) - 0x20) : (c) ) 63 | 64 | /* Check wether a case is a decimal */ 65 | #define DECCHK( c ) ((c) >= ''0'' && (c) <= ''9'') 66 | 67 | /* Check wether a case is a hex */ 68 | #define HEXCHK( c ) ( ((c) >= ''0'' && (c) <= ''9'') || \ 69 | ( (c) >= ''A'' && (c) <= ''F'') || \ 70 | ( (c) >= ''a'' && (c) <= ''f'') ) 71 | 72 | /* ABS/MAX/MIN */ 73 | #define ABS(x) ( ((x) > 0) ? (x) : (-x) ) 74 | #define MIN(a, b) ( ((a) < (b))?(a):(b) ) 75 | #define MAX(a, b) ( ((a) > (b))?(a):(b) ) 76 | 77 | /* Constants */ 78 | #define PI (3.1416f) 79 | #define PI_DIV2 (1.5708f) 80 | 81 | /* Function Define */ 82 | #ifndef ClrBit 83 | #define ClrBit(reg, bit) reg &= ~(1 << bit) 84 | #endif 85 | 86 | #ifndef SetBit 87 | #define SetBit(reg, bit) reg |= (1 << bit) 88 | #endif 89 | 90 | /* Constants */ 91 | #define ZX_SUCCESS (1) 92 | #define ZX_FAILURE (0) 93 | 94 | #define LEVEL_HIGH (1) 95 | #define LEVEL_LOW (0) 96 | 97 | /* ASSERT */ 98 | // uncomment line below to 99 | #define USE_FULL_ASSERT 100 | #ifdef USE_FULL_ASSERT 101 | /** 102 | * @brief The assert_param macro is used for function's parameters check. 103 | * @param expr: If expr is false, it calls assert_failed function 104 | * which reports the name of the source file and the source 105 | * line number of the call that failed. 106 | * If expr is true, it returns no value. 107 | * @retval None 108 | */ 109 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 110 | extern void assert_failed(uint8_t* file, uint32_t line); 111 | #else 112 | #define assert_param(expr) ((void)0) 113 | #endif 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /FFT-互相关时延计算.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiahouzuoxin/fft/8e6c372cc728c06da80be0a89e1601980dac50d2/FFT-互相关时延计算.png -------------------------------------------------------------------------------- /FFT-含噪声信号.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiahouzuoxin/fft/8e6c372cc728c06da80be0a89e1601980dac50d2/FFT-含噪声信号.png -------------------------------------------------------------------------------- /FFT-标准正弦信号.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiahouzuoxin/fft/8e6c372cc728c06da80be0a89e1601980dac50d2/FFT-标准正弦信号.png -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | 2 | SRC_FFT = main.c zx_fft.c zx_math.c 3 | SRC_XCORR = xcorr.c zx_fft.c zx_math.c 4 | 5 | all:test.fft test.xcorr 6 | 7 | test.fft:$(SRC_FFT) 8 | gcc $^ -o $@ 9 | 10 | test.xcorr:$(SRC_XCORR) 11 | gcc $^ -o $@ 12 | 13 | .PHONY:clean run 14 | run: 15 | -./test.fft > test_fft.txt 16 | -./test.xcorr > test_xcorr.txt 17 | clean: 18 | -rm -f test.fft test_fft.txt test_xcorr.txt 19 | 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiahouzuoxin/fft/8e6c372cc728c06da80be0a89e1601980dac50d2/README.md -------------------------------------------------------------------------------- /cos_tb.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiahouzuoxin/fft/8e6c372cc728c06da80be0a89e1601980dac50d2/cos_tb.h -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- 1 | /* 2 | * FileName : main.c 3 | * Author : xiahouzuoxin @163.com 4 | * Version : v1.0 5 | * Date : 2014/4/9 22:27:55 6 | * Brief : 7 | * 8 | * Copyright (C) MICL,USTB 9 | */ 10 | #include "zx_fft.h" 11 | 12 | #define SAMPLE_NODES (128) 13 | COMPLEX x[SAMPLE_NODES]; 14 | 15 | static void MakeInput() 16 | { 17 | int i; 18 | 19 | for ( i=0;i 15 | #include 16 | #include 17 | #include 18 | 19 | #define CH_NUM 3 20 | #define FIFO_SIZE 2048 21 | #define FIFO_SIZE_DIV2 1024 22 | 23 | static COMPLEX x[CH_NUM][FIFO_SIZE]; 24 | 25 | 26 | #define CONJ_REAL(ch1, ch2, i) \ 27 | (x[ch1][i].real*x[ch2][i].real \ 28 | + x[ch1][i].imag*x[ch2][i].imag) 29 | 30 | #define CONJ_IMAG(ch1, ch2, i) \ 31 | (x[ch1][i].real*x[ch2][i].imag \ 32 | - x[ch1][i].imag*x[ch2][i].real) 33 | 34 | #define PEAK_CHECK(delay, max, ch, i, temp) \ 35 | do { \ 36 | max = x[ch][0].real*x[ch][0].real \ 37 | + x[ch][0].imag*x[ch][0].imag; \ 38 | delay = 0; \ 39 | for (i=1; i<50; i++) { \ 40 | temp = x[ch][i].real*x[ch][i].real \ 41 | + x[ch][i].imag*x[ch][i].imag; \ 42 | if (temp > max) { \ 43 | max = temp; \ 44 | delay = i; \ 45 | } \ 46 | } \ 47 | for (i=FIFO_SIZE-1; i>FIFO_SIZE-50; i--) { \ 48 | temp = x[ch][i].real*x[ch][i].real \ 49 | + x[ch][i].imag*x[ch][i].imag; \ 50 | if (temp > max) { \ 51 | max = temp; \ 52 | delay = i; \ 53 | } \ 54 | } \ 55 | if (delay > FIFO_SIZE_DIV2) { \ 56 | delay = delay - FIFO_SIZE; \ 57 | } \ 58 | } while (0) 59 | 60 | 61 | static float t01,t02,t12; 62 | 63 | 64 | static float guass_rand(void) 65 | { 66 | static float V1, V2, S; 67 | static int phase = 0; 68 | float X; 69 | 70 | if ( phase == 0 ) { 71 | do { 72 | float U1 = (float)rand() / RAND_MAX; 73 | float U2 = (float)rand() / RAND_MAX; 74 | 75 | V1 = 2 * U1 - 1; 76 | V2 = 2 * U2 - 1; 77 | S = V1 * V1 + V2 * V2; 78 | } while(S >= 1 || S == 0); 79 | 80 | X = V1 * sqrt(-2 * log(S) / S); 81 | } else { 82 | X = V2 * sqrt(-2 * log(S) / S); 83 | } 84 | 85 | phase = 1 - phase; 86 | 87 | return X; 88 | } 89 | 90 | static void MakeSignal(void) 91 | { 92 | int i = 0; 93 | #define SHIFT 15 94 | 95 | srand(time(NULL)); 96 | for (i=0; i>5)) + guass_rand(); 98 | x[0][i].imag = 0; 99 | x[1][i].real = 0 + guass_rand(); 100 | x[1][i].imag = 0; 101 | x[2][i].real = 0 + guass_rand(); 102 | x[2][i].imag = 0; 103 | } 104 | // for (i=SHIFT; i>5)) + guass_rand(); 107 | x[0][i].imag = 0; 108 | x[1][i].real = x[0][i-SHIFT].real + guass_rand(); 109 | x[1][i].imag = 0; 110 | x[2][i].real = x[1][i].real + guass_rand(); 111 | x[2][i].imag = 0; 112 | } 113 | } 114 | 115 | 116 | int main(void) 117 | { 118 | float temp = 0; 119 | float max = 0; 120 | int ch = 0; 121 | int i = 0; 122 | static COMPLEX rxy_tmp[6]; 123 | 124 | 125 | MakeSignal(); 126 | /* Cross-correlation Step1: FFT */ 127 | // for (ch=0; ch 14 | */ 15 | 16 | #include "zx_fft.h" 17 | 18 | const float sin_tb[] = { // 精度(PI PI/2 PI/4 PI/8 PI/16 ... PI/(2^k)) 19 | 0.000000, 1.000000, 0.707107, 0.382683, 0.195090, 0.098017, 20 | 0.049068, 0.024541, 0.012272, 0.006136, 0.003068, 0.001534, 21 | 0.000767, 0.000383, 0.000192, 0.000096, 0.000048, 0.000024, 22 | 0.000012, 0.000006, 0.000003 23 | }; 24 | 25 | 26 | const float cos_tb[] = { // 精度(PI PI/2 PI/4 PI/8 PI/16 ... PI/(2^k)) 27 | -1.000000, 0.000000, 0.707107, 0.923880, 0.980785, 0.995185, 28 | 0.998795, 0.999699, 0.999925, 0.999981, 0.999995, 0.999999, 29 | 1.000000, 1.000000, 1.000000, 1.000000 , 1.000000, 1.000000, 30 | 1.000000, 1.000000, 1.000000 31 | }; 32 | 33 | /* 34 | * FFT Algorithm 35 | * === Inputs === 36 | * x : complex numbers 37 | * N : nodes of FFT. @N should be power of 2, that is 2^(*) 38 | * === Output === 39 | * the @x contains the result of FFT algorithm, so the original data 40 | * in @x is destroyed, please store them before using FFT. 41 | */ 42 | int fft(TYPE_FFT *x, uint32_t N) 43 | { 44 | int i,j,l,k,ip; 45 | static uint32_t M = 0; 46 | static int le,le2; 47 | static TYPE_FFT_E sR,sI,tR,tI,uR,uI; 48 | 49 | M = floor_log2_32(N); 50 | 51 | /* 52 | * bit reversal sorting 53 | */ 54 | l = N >> 1; 55 | j = l; 56 | ip = N-2; 57 | for (i=1; i<=ip; i++) { 58 | if (i < j) { 59 | tR = x[j].real; 60 | tI = x[j].imag; 61 | x[j].real = x[i].real; 62 | x[j].imag = x[i].imag; 63 | x[i].real = tR; 64 | x[i].imag = tI; 65 | } 66 | k = l; 67 | while (k <= j) { 68 | j = j - k; 69 | k = k >> 1; 70 | } 71 | j = j + k; 72 | } 73 | 74 | /* 75 | * For Loops 76 | */ 77 | for (l=1; l<=M; l++) { /* loop for ceil{log2(N)} */ 78 | //le = (int)pow(2,l); 79 | le = (int)(1 << l); 80 | le2 = (int)(le >> 1); 81 | uR = 1; 82 | uI = 0; 83 | 84 | k = floor_log2_32(le2); 85 | sR = cos_tb[k]; //cos(PI / le2); 86 | sI = -sin_tb[k]; // -sin(PI / le2) 87 | for (j=1; j<=le2; j++) { /* loop for each sub DFT */ 88 | //jm1 = j - 1; 89 | for (i=j-1; i> 1; 126 | for (i=0; i> 2; 136 | for (i=1; i>1)+1; k 10 | 11 | /* 12 | * DSPF_sp_maxval 13 | * @brief 14 | */ 15 | void dsp_max_min_val(const TYPE_MAX* x, int nx, TYPE_MAX *max, TYPE_MAX *min) 16 | { 17 | int i; 18 | 19 | *max = SHRT_MIN; 20 | *min = SHRT_MAX; 21 | for (i = 0; i < nx; i++) { 22 | if (x[i] > *max) { 23 | *max = x[i]; 24 | } else if (x[i] < *min) { 25 | *min = x[i]; 26 | } 27 | } 28 | 29 | return; 30 | } 31 | 32 | /* 33 | * scale 34 | * @brief 35 | * if data not in the same scale, use this function to scale 36 | * them to the same scale, for example [-10000 +10000] 37 | * @input params 38 | * x: input data 39 | * xmax : max value from x[] 40 | * xmin : min value from x[] 41 | * n : size of x[] 42 | * s_low: low boundary of scale 43 | * s_high: high boundary of scale 44 | * @output params 45 | * x: x[] will be changed after scale funtion and return it back 46 | * @retval 47 | * None 48 | */ 49 | void scale(TYPE_SCALE x[], 50 | TYPE_SCALE xmax, 51 | TYPE_SCALE xmin, 52 | int32_t n, 53 | TYPE_SCALE s_low, 54 | TYPE_SCALE s_high) 55 | { 56 | int32_t i = 0; 57 | TYPE_SCALE delta_s = s_high - s_low; 58 | TYPE_SCALE delta_x = xmax - xmin; 59 | 60 | for (i=0; i 0 106 | * @inputs 107 | * @outputs 108 | * @retval 109 | */ 110 | uint32_t floor_log2_32(uint32_t x) 111 | { 112 | x |= (x>>1); 113 | x |= (x>>2); 114 | x |= (x>>4); 115 | x |= (x>>8); 116 | x |= (x>>16); 117 | 118 | return (ones_32(x>>1)); 119 | } 120 | 121 | -------------------------------------------------------------------------------- /zx_math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FileName : zx_math.c 3 | * Author : xiahouzuoxin 4 | * Date : 2013.12.23 5 | * Version : v1.0 6 | * Brief : 7 | */ 8 | #ifndef _ZX_MATH_H 9 | #define _ZX_MATH_H 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | #include "Config.h" 16 | 17 | /* typedefs */ 18 | typedef float TYPE_SCALE; 19 | typedef float TYPE_MAX; 20 | 21 | /* global functions */ 22 | extern void dsp_max_min_val(const TYPE_MAX* x, int nx, TYPE_MAX *max, TYPE_MAX *min); 23 | extern void scale(TYPE_SCALE x[], 24 | TYPE_SCALE xmax, 25 | TYPE_SCALE xmin, 26 | int32_t n, 27 | TYPE_SCALE s_low, 28 | TYPE_SCALE s_high); 29 | extern float cabs(COMPLEX x); 30 | extern int ones_32(uint32_t n); 31 | extern uint32_t floor_log2_32(uint32_t x); 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif 36 | 37 | #endif 38 | 39 | --------------------------------------------------------------------------------