117 lines
3.6 KiB
C
117 lines
3.6 KiB
C
/* Header File Including ------------------------------------------------------------------------ */
|
|
#include "Fft.h"
|
|
|
|
/* Macro Declaration ---------------------------------------------------------------------------- */
|
|
#define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01
|
|
#define SWAP(type, a, b) { type t = a; a = b; b = t; }
|
|
|
|
/* Structure Declaration ------------------------------------------------------------------------ */
|
|
typedef struct FFT_MCB {
|
|
uint8_t type;
|
|
uint32_t parameter[4];
|
|
struct FFT_MCB *next;
|
|
struct FFT_MCB *prev;
|
|
uint8_t buffer[10240];
|
|
} FFT_MCB;
|
|
|
|
/* Global Variable Definition ------------------------------------------------------------------- */
|
|
static FFT_MCB gMemery;
|
|
|
|
/* Function Definition -------------------------------------------------------------------------- */
|
|
/**----------------------------------------------------------------------
|
|
* Function : FftInit
|
|
* Description : ³õʼ»¯¸µÀïÒ¶ÔËËã¿â
|
|
* Date : 2022/10/15 logzhan
|
|
*---------------------------------------------------------------------**/
|
|
static int FftInit(int length) {
|
|
Complex_t *omega;
|
|
uint16_t *position;
|
|
int bitLength;
|
|
int i;
|
|
int shift;
|
|
int j;
|
|
if (length * (sizeof(Complex_t) + sizeof(uint16_t)) > sizeof(gMemery.buffer)) {
|
|
return 0;
|
|
}
|
|
|
|
if (FFT_MCB_TYPE_OMEGA_AND_POSITION == gMemery.type && length == (int)gMemery.parameter[0]) {
|
|
return length;
|
|
}
|
|
|
|
gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION;
|
|
gMemery.parameter[0] = length;
|
|
|
|
omega = (Complex_t *) gMemery.buffer;
|
|
for (i = 0; i < length; ++i) {
|
|
omega[i].real = (float)cos(-2 * M_PI * i / length);
|
|
omega[i].image = (float)sin(-2 * M_PI * i / length);
|
|
}
|
|
|
|
position = (uint16_t *)(omega + length);
|
|
for (bitLength = 0, i = length; (i >> bitLength); ++bitLength);
|
|
for (i = 0; i < length; ++i) {
|
|
j = 0;
|
|
for (shift = 0; shift < bitLength - 1; ++shift) {
|
|
if((i >> shift) & 1) {
|
|
j |= (1 << (bitLength - shift - 2));
|
|
}
|
|
}
|
|
position[i] = (uint16_t)j;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
int FFT_Dft(Complex_t *dft, float *signal, int length) {
|
|
|
|
if (0 == FftInit(length)) return 0;
|
|
|
|
for (int k = 0; k < length; ++k) {
|
|
dft[k].real = 0;
|
|
dft[k].image = 0;
|
|
for(int i = 0; i < length; ++i) {
|
|
int ik = i * k % length;
|
|
dft[k].real += signal[i] * ((Complex_t *)gMemery.buffer)[ik].real;
|
|
dft[k].image += signal[i] * ((Complex_t *)gMemery.buffer)[ik].image;
|
|
}
|
|
}
|
|
return length;
|
|
}
|
|
|
|
|
|
int FFT_Fft(Complex_t *fft, float *signal, int length) {
|
|
int i,l,j,m;
|
|
uint16_t *position;
|
|
Complex_t *omega;
|
|
|
|
//Õý³£Çé¿öÏ£¬lengthÕý³£¡¢Ã»Òç³ö¾Í²»»á½øÈëreturn0
|
|
if (length <= 0 || 0 != (length & (length - 1)) || 0 == FftInit(length)) {
|
|
return 0;
|
|
}
|
|
|
|
for (i = 0; i < length; ++i) {
|
|
fft[i].real = signal[i];
|
|
fft[i].image = 0;
|
|
}
|
|
|
|
position = (uint16_t *)(gMemery.buffer + length * sizeof(Complex_t));
|
|
for (i = 0; i < length; ++i) {
|
|
if (i < position[i]) {
|
|
SWAP(Complex_t, fft[i], fft[position[i]]);
|
|
}
|
|
}
|
|
|
|
omega = (Complex_t *)gMemery.buffer;
|
|
for (l = 2, m = 1; l <= length; l <<= 1, m <<= 1) {
|
|
for (i = 0; i < length; i += l) {
|
|
for (j = 0; j < m; ++j) {
|
|
Complex_t delta = ComplexMul(omega[length / l * j], fft[i + m + j]);
|
|
fft[i + m + j] = ComplexSub(fft[i + j], delta);
|
|
fft[i + j] = ComplexAdd(fft[i + j], delta);
|
|
}
|
|
}
|
|
}
|
|
|
|
return length;
|
|
}
|