ap/app/zte_amt/fft.c

#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <math.h>

static void fft(short *data,double *x,double *y,unsigned short n,short sign)
{
    unsigned short i,j,k,l,m,n1,n2;
    double c,c1,e,s,s1,t,tr,ti;
    //Calculate i = log2N
    for (i = 0;i < n;i++)
    {
        x[i] = data[i];
        //zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"cal_freq_fft x[%d]=%f,data=%d!\n",i,x[i],data[i]);

    }
    for (j = 1,i = 1; i<20; i++)
    {
        m = i;
        j = 2*j;
        if (j == n)
            break;
    }

    //¼ÆËãµûÐÎͼµÄÊäÈëϱ꣨Âëλµ¹¶Á£©
    n1 = n - 1;
    for (j=0,i=0; i<n1; i++)
    {
        if (i<j)
        {
            tr = x[j];
            ti = y[j];
            x[j] = x[i];
            y[j] = y[i];
            x[i] = tr;
            y[i] = ti;
        }
        k = n/2;
        while (k<(j+1))
        {
            j = j - k;
            k = k/2;
        }
        j = j + k;
    }
    //¼ÆËãÿһ¼¶µÄÊä³ö£¬lΪijһ¼¶£¬iΪͬһ¼¶µÄ²»Í¬Èº£¬Ê¹ÓÃͬһÄڴ棨¼´Î»ÔËË㣩
    n1 = 1;
    for (l=1; l<=m; l++)
    {
        n1 = 2*n1;
        n2 = n1/2;
        e = 3.1415926/n2;
        c = 1.0;
        s = 0.0;
        c1 = cos(e);
        s1 = -sign*sin(e);
        for (j=0; j<n2; j++)
        {
            for (i=j; i<n; i+=n1)
            {
                k = i + n2;
                tr = c*x[k] - s*y[k];
                ti = c*y[k] + s*x[k];
                x[k] = x[i] - tr;
                y[k] = y[i] - ti;
                x[i] = x[i] + tr;
                y[i] = y[i] + ti;
            }
            t = c;
            c = c*c1 - s*s1;
            s = t*s1 + s*c1;
        }
    }
    //Èç¹ûÊÇÇóIFFT£¬ÔÙ³ýÒÔN
    if (sign == -1)
    {
        for (i=0; i<n; i++)
        {
            x[i] /= n;
            y[i] /= n;
        }
    }
}
static void max_fft(double *a,unsigned short n,unsigned short *h)
{
    unsigned short i = 0;
    *h=5;
    for (i=5;i<n;i++)
    {
        if (a[i]>a[*h])
            *h=i;
    }

}

static void abs_fft(double *x,double *y,unsigned short n,double *z)
{
    unsigned short i=0;

    for (i= 0;i<n;i++)
    {
        z[i] =sqrt(x[i]*x[i]+y[i]*y[i]);
    }



}


typedef struct
{
    double freqValue;
    double freqAmp;
}
zDrvVp_Freqfft;

int cal_freq_fft( short *data,zDrvVp_Freqfft *freqfft,unsigned short n, int fs)
{
    unsigned short h = 0;
	unsigned short offset = 0;
    double *x = NULL,*xi = NULL,*xvalue = NULL;
    if ( data == NULL || freqfft == NULL || n <= 0 || fs < 0)
    {
        return -1;
    }

    if (n > 32768 || n < 2) // ÏÞÖÆÔÚ2µÄ1´ÎÃݵ½15´ÎÃÝ
		return -1;

    // ¼ì²énÊÇ·ñ2µÄÃÝ´Î
    do
    {
		offset++;
		h = n >> offset;
    }while (h > 0);

	if( n != (1 << (offset - 1))) return -1;
	
    x=(double*)calloc(n, sizeof(double));
    xi=(double*)calloc(n, sizeof(double));
    xvalue=(double*)calloc(n, sizeof(double));

	if(x && xi && xvalue)
	{
	    fft(data,x,xi,n,1);
	    /*for(i=0;i<1024;i++)
	    {
	    	zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"cal_freq_fft x[%d]=%f,xi[%d]=%f!\n",i,x[i],i,xi[i]);
	    }*/

	    abs_fft(x,xi,n/2,xvalue);
	    max_fft(xvalue,n/2,&h);
	    //zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"h=%d!\n",h);
	    freqfft->freqValue = (h-1)*fs/((n)*1.0);
	    freqfft->freqAmp = xvalue[h]/(n/2);
	    //*freqValue = (h-1)*fs/((n)*1.0);
	    //zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"n=%d!\n",n);
	   // freq = freqfft->freqValue;
	    //zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"fs=%d!\n",fs);
	    //zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"freq=%f!\n",freq);
	    //zOss_Printf(SUBMDL_HAL, PRINT_LEVEL_NORMAL,"freqfft->freqAmp=%f!\n",freqfft->freqAmp);


	    free(x);
	    free(xi);
	    free(xvalue);
		return 0;
	}

	if(x) free(x);
	if(xi) free(xi);
	if(xvalue) free(xvalue);

	
	return -1;
}