?? dsp280x_fft.c
字號:
/* system header define */
#include "IQmathLib.h"
#include "math.h"
#include "fft.h"
/* constant definition */
#define PIE 3.1415926535L
#define N1 512
#define N2 256
#define N8 64
#define N256K 25600
#define N128K 12800
#define ZM_RNG 8
#define _BKD 0
#define _FWD 1
#define _HER 0xFFFF
/* IQ variable definition */
_iq qRetVal = 0;
_iq qWn = 0;
_iq qWn1 = 0;
_iq qPie = 0;
_iq qRVal = 0;
_iq qIVal = 0;
_iq qTemp = 0;
_iq qRVal_1 = 0;
long FFT_Execute(Uint16 *ptSrc, long *ptDest);
void FFT_Execute_1(long *src, long *targ, long tsize, long fsize);
long FFT_test(float *ptSrc, float *ptDest);
long FFT_Zoom(Uint16 *ptSrc, long *ptDest, long maxRef);
long FFT_Execute_2(Uint16 *ptSrc, long *ptDest);
Uint16 FFT_Dsp(int dir,long m,float *x,float *y);
long FFT_test(float *ptSrc, float *ptDest)
{
long maxAdr;
float fmaxAdr;
float *xr, *yi;
Uint16 i;
float fxr[8] = { 1,2,3,4,2,2,5,1};
float fxi[8] = { 0,0,0,0,0,0,0,0};
for(i=0;i<512;i++)
*(ptDest+i) = 0;
xr = (float *) ptSrc;
yi = (float *) ptDest;
#if 1
FFT_Dsp(1,9,xr,yi);
#else
maxAdr = FFT_Execute_2(ptSrc, ptDest);
//maxAdr = FFT_Execute(ptSrc, ptDest);
maxAdr = FFT_Zoom(ptSrc, ptDest, maxAdr);
#endif
return maxAdr;
}
Uint16 FFT_Dsp(int dir,long m,float *x,float *y)
{
long n,i,i1,j,k,i2,l,l1,l2;
float c1,c2,tx,ty,t1,t2,u1,u2,z;
long rsum,isum;
long *ptrOut;
ptrOut = (long *)x;
/* Calculate the number of points */
n = 1;
for (i=0;i<m;i++)
n *= 2;
/* Do the bit reversal */
i2 = n >> 1;
j = 0;
for (i=0;i<n-1;i++) {
if (i < j) {
tx = x[i];
ty = y[i];
x[i] = x[j];
y[i] = y[j];
x[j] = tx;
y[j] = ty;
}
k = i2;
while (k <= j) {
j -= k;
k >>= 1;
}
j += k;
}
/* Compute the FFT */
c1 = -1.0;
c2 = 0.0;
l2 = 1;
for (l=0;l<m;l++) {
l1 = l2;
l2 <<= 1;
u1 = 1.0;
u2 = 0.0;
for (j=0;j<l1;j++) {
for (i=j;i<n;i+=l2) {
i1 = i + l1;
t1 = u1 * x[i1] - u2 * y[i1];
t2 = u1 * y[i1] + u2 * x[i1];
x[i1] = x[i] - t1;
y[i1] = y[i] - t2;
x[i] += t1;
y[i] += t2;
}
z = u1 * c1 - u2 * c2;
u2 = u1 * c2 + u2 * c1;
u1 = z;
}
c2 = (float)sqrt((1.0 - c1) / 2.0);
if (dir == 1)
c2 = -c2;
c1 = (float)sqrt((1.0 + c1) / 2.0);
}
for(i=0;i<n;i++)
{
rsum = (long)(x[i]*4096);
isum = (long)(y[i]*4096);
rsum = (labs(rsum)+5)/4096;
isum = (labs(isum)+5)/4096;
ptrOut[i] = rsum + isum;
}
return(1);
}
long FFT_Execute_2(Uint16 *ptSrc, long *ptDest)
{
long i,j,temp,i50;
long rVal, iVal, rsum=0;
long maxVal, maxAdr;
// 2 * pi /512
// = pi * /256
qPie = _IQ((float)PIE);
qTemp = _IQ((float)8);
qWn1 = _IQdiv(qPie, qTemp);
qTemp = _IQ((float)1600);
qWn1 = _IQdiv(qWn1, qTemp);
//
for(i=0;i<N2;i++){
rVal = 0;
iVal = 0;
i50 = i * 50;
for(j=0;j<N1;j++){
// w = w * i *j
temp = i50 * j;
qWn = _IQmpyI32(qWn1, temp);
//A= x(j)*Cos(2) , B=x(j)*Sin(w)*img
qRVal = _IQcos(qWn);
qIVal = _IQsin(qWn);
temp = ((long) *(ptSrc+j)) & 0xFFFF;
qTemp = temp << 19;
qRVal = _IQmpy(qRVal, qTemp);
qIVal = _IQmpy(qIVal, qTemp);
rVal += (qRVal >> 9);
iVal += (qIVal >> 9);
}
rsum = _IQ10mag(rVal,iVal);
rsum >>= 10;
*(ptDest+i) = rsum;
}
//search max value on frequency domain
maxVal = *(ptDest+1);
maxAdr = (long)(ptDest+1);
for(i=2;i<N2;i++){
if(*(ptDest+i) > maxVal){
maxVal = *(ptDest+i);
maxAdr = (long)(ptDest+i);
}
}
maxAdr -=((long)(ptDest));
return (maxAdr/2);
}
long FFT_Zoom(Uint16 *ptSrc, long *ptDest, long maxRef)
{
long i,j,temp, i2;
long rVal, iVal, rsum=0;
long maxVal, maxAdr, minRang, maxRang, midRang;
Uint16 maxDir;
float fmaxAdr;
maxRef *= 50;
minRang = maxRef-ZM_RNG;
maxRang = maxRef+ZM_RNG;
// 2 * pi /512
// = pi * /256
qPie = _IQ((float)PIE);
qTemp = _IQ((float)8);
qWn1 = _IQdiv(qPie, qTemp);
qTemp = _IQ((float)1600);
qWn1 = _IQdiv(qWn1, qTemp);
if(maxRef > 50 && maxRef < 3300){
//search max value direct
for(i=minRang;i<(maxRang+1);i++){
rVal = 0;
iVal = 0;
i2 = i;
for(j=0;j<N1;j++){
// w = w * i *j
temp = i2 * j;
qWn = _IQmpyI32(qWn1, temp);
//A= x(j)*Cos(2) , B=x(j)*Sin(w)*img
qRVal = _IQcos(qWn);
qIVal = _IQsin(qWn);
temp = ((long) *(ptSrc+j)) & 0xFFFF;
qTemp = temp << 19;
qRVal = _IQmpy(qRVal, qTemp);
qIVal = _IQmpy(qIVal, qTemp);
rVal += (qRVal >> 9);
iVal += (qIVal >> 9);
}
rsum = _IQ10mag(rVal,iVal);
rsum >>= 10;
*((ptDest-minRang)+i) = rsum;
}
//search max value on frequency domain
maxVal = *(ptDest);
maxAdr = (long)(ptDest);
//
for(i=0;i<(ZM_RNG * 2 + 1);i++){
if(*(ptDest+i) > maxVal){
maxVal = *(ptDest+i);
maxAdr = (long)(ptDest+i);
} /* for(){ */
} /* for(){ */
//
midRang = (long)(ptDest+ZM_RNG);
if(maxAdr > midRang){
maxDir = _FWD;
minRang = maxRef;
maxRang = minRang+64;
}
else if(maxAdr < midRang){
maxDir = _BKD;
maxRang = maxRef;
minRang = maxRang-64;
}
else if(maxAdr == midRang){
maxDir = _HER;
maxAdr = maxRef;
}
} /*if(maxRef > 50 && maxRef < 3300){*/
else if(maxRef <= 50){
maxDir = _FWD;
minRang = 50;
maxRang = minRang+64;
}
else if(maxRef >= 3300){
maxDir = _BKD;
maxRang = 3300;
minRang = maxRang-64;
}
//
if(maxDir != _HER){
for(i=minRang;i<(maxRang+1);i++){
rVal = 0;
iVal = 0;
i2 = i ;
for(j=0;j<N1;j++){
// w = w * i *j
temp = i2 * j;
qWn = _IQmpyI32(qWn1, temp);
//A= x(j)*Cos(2) , B=x(j)*Sin(w)*img
qRVal = _IQcos(qWn);
qIVal = _IQsin(qWn);
temp = ((long) *(ptSrc+j)) & 0xFFFF;
qTemp = temp << 19;
qRVal = _IQmpy(qRVal, qTemp);
qIVal = _IQmpy(qIVal, qTemp);
rVal += (qRVal >> 9);
iVal += (qIVal >> 9);
}
rsum = _IQ10mag(rVal,iVal);
rsum >>= 10;
*((ptDest-minRang)+i) = rsum;
}
//
//search max value on frequency domain
maxVal = *(ptDest);
maxAdr = (long)(ptDest);
for(i=1;i<64;i++){
if(*(ptDest+i) > maxVal){
maxVal = *(ptDest+i);
maxAdr = (long)(ptDest+i);
}
}
#if 0
//
maxAdr -=((long)(ptDest));
fmaxAdr = maxRef;
if(maxDir == _FWD)
fmaxAdr = (fmaxAdr+(maxAdr/2)*0.5);
else if(maxDir == _BKD)
fmaxAdr = (fmaxAdr-(64.0-(maxAdr/2))*0.5);
#else
//
maxAdr -=((long)(ptDest));
if(maxDir == _FWD)
maxAdr = (maxRef+(maxAdr/2));
else if(maxDir == _BKD)
maxAdr = (maxRef-(64-maxAdr/2));
#endif
}
return (maxAdr);
}
long FFT_Execute(Uint16 *ptSrc, long *ptDest)
{
long i,j,temp;
long rVal, iVal, rsum=0;
long maxVal, maxAdr;
// 2 * pi /512
// = pi * /256
qPie = _IQ((float)PIE);
qTemp = _IQ((float)N2);
qWn1 = _IQdiv(qPie, qTemp);
//
for(i=0;i<N2;i++){
rVal = 0;
iVal = 0;
for(j=0;j<N1;j++){
// w = w * i *j
temp = i * j;
qWn = _IQmpyI32(qWn1, temp);
//A= x(j)*Cos(2) , B=x(j)*Sin(w)*img
qRVal = _IQcos(qWn);
qIVal = _IQsin(qWn);
temp = ((long) *(ptSrc+j)) & 0xFFFF;
qTemp = temp << 15;
qRVal = _IQmpy(qRVal, qTemp);
qIVal = _IQmpy(qIVal, qTemp);
rVal += (qRVal >> 5);
iVal += (qIVal >> 5);
}
#if 0
rsum = _IQ10abs(rVal);
isum = _IQ10abs(iVal);
*(ptDest+i) = (rsum+isum)>>10;
#else
rsum = _IQ10mag(rVal,iVal);
rsum >>= 10;
*(ptDest+i) = rsum;
#endif
}
//search max value on frequency domain
maxVal = *(ptDest+1);
maxAdr = (long)(ptDest+1);
for(i=2;i<N2;i++){
if(*(ptDest+i) > maxVal){
maxVal = *(ptDest+i);
maxAdr = (long)(ptDest+i);
}
}
maxAdr -=((long)(ptDest));
return (maxAdr/2);
}
void FFT_Execute_1(long *src, long *targ, long tsize, long fsize)
{
double pi = 3.14159;
double i,j,n,w,w1;
long maxVal, maxAdr, rval, ival,rsum, isum,fcount, tcount;
long *ptrTemp, *ptrTdata;
ptrTemp = targ;
n = (double)fsize/2.0;
w1 = pi / n; // pi / 256 ( = 2 * pi /512)
fsize /= 2; // 256 = 512 / 2
i=0;
for(fcount=0;fcount<fsize;fcount++){
ptrTdata = src;
j=0;
rsum = 0;
isum = 0;
for(tcount=0;tcount<tsize;tcount++){
// X[fcount]: w[fcount] = 2 * pi * tcount * fcount / fsize
w = w1*j*i;
rval = (long)(10240.0*(cos(w)));
ival = (long)(10240.0*(sin(w)));
rval = (*ptrTdata * rval);
ival = (*ptrTdata * ival);
rsum += (rval/10240);
isum += (ival/10240);
ptrTdata++;
j++;
}
rsum = labs(rsum);
isum = labs(isum);
*ptrTemp = rsum+isum;
ptrTemp++;
i++;
}
//search max value on frequency domain
ptrTemp = (targ+1);
maxVal = *ptrTemp;
maxAdr = (long)(ptrTemp);
for(i=1;i<fsize;i++){
if(*ptrTemp > maxVal){
maxVal = *ptrTemp;
maxAdr = (long)(ptrTemp);
}
ptrTemp++;
}
maxAdr = (maxAdr - ((long)targ))/2*50;
ptrTemp = targ;
}
#if 0
void FFT_Execute(long *ptSrc, long *ptDest)
{
long i,j,temp, val1, val2, val3;
long rVal, iVal, rsum=0, isum=0;
// 2 * pi /512
// = pi * /256
qPie = _IQ((float)PIE);
qTemp = _IQ((float)N2);
qWn1 = _IQdiv(qPie, qTemp);
//
for(i=0;i<N2;i++){
rVal = 0;
iVal = 0;
for(j=0;j<N1;j++){
// w = w * i *j
temp = i * j;
qWn = _IQmpyI32(qWn1, temp);
//A= x(j)*Cos(2) , B=x(j)*Sin(w)*img
qRVal = _IQcos(qWn);
qIVal = _IQsin(qWn);
val1 = _IQmpyI32int(qRVal, (long)65536);
val2 = _IQmpyI32int(qIVal, (long)65536);
//
temp = ((long) *(ptSrc+j));
val1 = temp * val1;
val2 = temp * val2;
val1 /= (65536);
val2 /= (65536);
//
rVal += val1;
iVal += val2;
}
rsum = labs(rVal);
isum = labs(iVal);
*(ptDest+i) = rsum+isum;
}
}
#endif
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -