zte's code,first commit
Change-Id: I9a04da59e459a9bc0d67f101f700d9d7dc8d681b
diff --git a/ap/app/zte_amt/fft.c b/ap/app/zte_amt/fft.c
new file mode 100644
index 0000000..0abe030
--- /dev/null
+++ b/ap/app/zte_amt/fft.c
@@ -0,0 +1,176 @@
+#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;
+}