PDR/1.Software/PDR 1.01/src/pdr_fft.c

/* Header File Including ------------------------------------------------------------------------ */
#include "pdr_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 -------------------------------------------------------------------------- */
float COMPLEX_abs(myComplex c) {
    return (float)sqrt(c.real * c.real + c.image * c.image);
}

myComplex COMPLEX_add(myComplex a, myComplex b) {
    myComplex c = { 0 };

	c.real = a.real + b.real;
	c.image = a.image + b.image;

    return c;
}

myComplex COMPLEX_sub(myComplex a, myComplex b) {
    myComplex c = { 0 };

	c.real = a.real - b.real;
	c.image = a.image - b.image;

    return c;
}

myComplex COMPLEX_mul(myComplex a, myComplex b) {
    myComplex c;
    c.real = a.real * b.real - a.image * b.image;
    c.image = a.real * b.image + a.image * b.real;
    return c;
}

myComplex COMPLEX_pow(myComplex c, int n) {
    myComplex p = {1, 0};
    int i;
	float real;
    for (i = 0; i < n; ++i) {
        real = p.real * c.real - p.image * c.image;
        p.image = p.real * c.image + p.image * c.real;
        p.real = real;
    }

    return p;
}

/**----------------------------------------------------------------------
* Function    : FFT_init
* Description : ³õʼ»¯gMemery
* Date        : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int FFT_init(int length) {
    myComplex *omega;
    uint16_t *position;
    int bitLength;
    int i;
	int shift;
	int j;
    if (length * (sizeof(myComplex) + 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 = (myComplex *) 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(myComplex *dft, float *signal, int length) {

    if (0 == FFT_init(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] * ((myComplex *)gMemery.buffer)[ik].real;
            dft[k].image += signal[i] * ((myComplex *)gMemery.buffer)[ik].image;
        }
    }
    return length;
}


int FFT_fft(myComplex *fft, float *signal, int length) {
    int i,l,j,m;
    uint16_t *position;
    myComplex *omega;

	//Õý³£Çé¿öÏ£¬lengthÕý³£¡¢Ã»Òç³ö¾Í²»»á½øÈëreturn0
    if (length <= 0 || 0 != (length & (length - 1)) || 0 == FFT_init(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(myComplex));
    for (i = 0; i < length; ++i) {
        if (i < position[i]) {
            SWAP(myComplex, fft[i], fft[position[i]]);
        }
    }

    omega = (myComplex *)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) {
                myComplex delta = COMPLEX_mul(omega[length / l * j], fft[i + m + j]);
                fft[i + m + j] = COMPLEX_sub(fft[i + j], delta);
                fft[i + j] = COMPLEX_add(fft[i + j], delta);
            }
        } 
    }

    return length;
}