www.pudn.com > dtmffft.zip > fft.c
// FFT.C (c) 2004 Howard Long (G6LVB), Hanlincrest Ltd. All rights reserved.
// 72 Princes Gate
// London SW7 2PA
// United Kingdom
// howard@hanlincrest.com
// Free for educational and non-profit use. For commercial use please contact the author.
#include
#include
#include
#include "fft.h"
#define FFT_PI 3.1415926535
static void FFTWindowTerm(PFFTSTRUCT pfs)
{
if (pfs->ps16Window!=NULL)
{
free(pfs->ps16Window);
pfs->ps16Window=NULL;
}
}
static BOOL FFTWindowInit(PFFTSTRUCT pfs)
{
int n;
if ((pfs->ps16Window=malloc(pfs->nLen*sizeof(*pfs->ps16Window)))==NULL)
{
return FALSE;
}
for (n=0;nnLen;n++)
{
double d=0.54-0.46*cos(2*FFT_PI*n/pfs->nLen); // Hamming code
pfs->ps16Window[n]=(S16)floor(d*32767+0.5);
}
return TRUE;
}
static void FFTBitRevTerm(PFFTSTRUCT pfs)
{
if (pfs->ps16BitRev!=NULL)
{
free(pfs->ps16BitRev);
pfs->ps16BitRev=NULL;
}
}
static BOOL FFTBitRevInit(PFFTSTRUCT pfs)
{
int n;
if ((pfs->ps16BitRev=malloc(pfs->nLen*sizeof(*pfs->ps16BitRev)))==NULL)
{
return FALSE;
}
for (n=0;nnLen;n++)
{
int i=0;
int m;
for (m=pfs->nLen/4;m>0;m>>=1)
{
i=(i>>1)+(n & m ? pfs->nLen/2 : 0);
}
pfs->ps16BitRev[n]=(S16)i;
}
/***************/
/*
{
FILE *pf=fopen("bitrev","wb");
fwrite(pfs->ps16BitRev,1,pfs->nLen*sizeof(*pfs->ps16BitRev),pf);
fclose(pf);
}
*/
return TRUE;
}
static void FFTSinCosTerm(PFFTSTRUCT pfs)
{
if (pfs->fscu.pfscs!=NULL)
{
free(pfs->fscu.pfscs);
pfs->fscu.pfscs=NULL;
}
}
static BOOL FFTSinCosInit(PFFTSTRUCT pfs)
{
int n;
if ((pfs->fscu.pfscs=malloc(pfs->nLen*sizeof(*pfs->fscu.pfscs)))==NULL)
{
return FALSE;
}
for (n=0;nnLen/2;n++)
{
double dSin=floor(-32768.0*sin(2*FFT_PI*n/pfs->nLen)+0.5);
double dCos=floor(-32768.0*cos(2*FFT_PI*n/pfs->nLen)+0.5);
if (dSin>32767.5)
{
dSin=32767.0;
}
if (dCos>32767.5)
{
dCos=32767.0;
}
pfs->fscu.pfscs[pfs->ps16BitRev[n]/2].s16Sin=(S16)dSin;
pfs->fscu.pfscs[pfs->ps16BitRev[n]/2].s16Cos=(S16)dCos;
}
/***************/
/*
{
FILE *pf=fopen("sincos","wb");
fwrite(pfs->fscu.pfscs,1,pfs->nLen*sizeof(*pfs->fscu.pfscs),pf);
fclose(pf);
}
*/
return TRUE;
}
void FFTTerm(PFFTSTRUCT pfs)
{
FFTWindowTerm(pfs);
FFTSinCosTerm(pfs);
FFTBitRevTerm(pfs);
pfs->nLen=0;
DeleteCriticalSection(&pfs->cs);
}
BOOL FFTInit(PFFTSTRUCT pfs,int nLen)
{
InitializeCriticalSection(&pfs->cs);
pfs->nLen=nLen;
pfs->fscu.pfscs=NULL;
pfs->ps16BitRev=NULL;
pfs->ps16Window=NULL;
if (!FFTBitRevInit(pfs))
{
FFTTerm(pfs);
return FALSE;
}
if (!FFTSinCosInit(pfs))
{
FFTTerm(pfs);
return FALSE;
}
if (!FFTWindowInit(pfs))
{
FFTTerm(pfs);
return FALSE;
}
return TRUE;
}
static void FFTSetMax(PFFTSTRUCT pfs,S32 s32)
{
EnterCriticalSection(&pfs->cs);
pfs->s32Max=s32;
LeaveCriticalSection(&pfs->cs);
}
S32 FFTGetMax(PFFTSTRUCT pfs)
{
S32 s32;
EnterCriticalSection(&pfs->cs);
s32=pfs->s32Max;
LeaveCriticalSection(&pfs->cs);
return s32;
}
static void FFTSetAverage(PFFTSTRUCT pfs,S32 s32)
{
EnterCriticalSection(&pfs->cs);
pfs->s32Average=s32;
LeaveCriticalSection(&pfs->cs);
}
S32 FFTGetAverage(PFFTSTRUCT pfs)
{
S32 s32;
EnterCriticalSection(&pfs->cs);
s32=pfs->s32Average;
LeaveCriticalSection(&pfs->cs);
return s32;
}
void FFTRun(PFFTSTRUCT pfs,PS16 ps16BufferIn,PS32 ps32BufferOut)
{
{ // Apply windowing on the data
PS16 ps16End=ps16BufferIn+pfs->nLen;
PS16 ps16;
PS16 ps16Window=pfs->ps16Window;
pfs->bClip=FALSE;
for (ps16=ps16BufferIn;ps1629490 || s16<-29490)
{
pfs->bClip=TRUE;
}
*ps16=(S16)(((S32)s16*(S32)(*ps16Window)) >> 15);
ps16Window++;
}
}
/*****************/
/*
// Load up some data for debugging...
{
FILE *pf=fopen("c:\\fftdata","rb");
fread(ps16BufferIn,1,1024,pf);
fclose(pf);
}
*/
{ // FFT proper
PS16 ps16BR1;
PS16 ps16BR2;
int nBFPerGroup=pfs->nLen/4;
PS16 ps16End=ps16BufferIn+pfs->nLen;
while (nBFPerGroup>0)
{
PS16 ps16A=ps16BufferIn;
PS16 ps16B=ps16BufferIn+nBFPerGroup*2;
PFFTSINCOSSTRUCT pfscs=pfs->fscu.pfscs;
while (ps16As16Sin;
S16 s16Cos=pfscs->s16Cos;
PS16 ps16End2=ps16B;
while (ps16A> 15;
S32 s32W=((S32)ps16B[0] * s16Sin - (S32)ps16B[1] * s16Cos) >> 15;
ps16B[0] = (S16)((ps16A[0] + s32V) >> 1);
ps16A[0] = (S16)(ps16B[0] - s32V);
ps16B[1] = (S16)((ps16A[1] - s32W) >> 1);
ps16A[1] = (S16)(ps16B[1] + s32W);
ps16A+=2;
ps16B+=2;
}
ps16A=ps16B;
ps16B+=nBFPerGroup*2;
pfscs++;
}
nBFPerGroup>>=1;
}
ps16BR1=pfs->ps16BitRev+1;
ps16BR2=pfs->ps16BitRev + pfs->nLen / 2 - 1;
while (ps16BR1<=ps16BR2)
{
S16 s16Sin=pfs->fscu.ps16SinCos[ps16BR1[0]];
S16 s16Cos=pfs->fscu.ps16SinCos[ps16BR1[0]+1];
PS16 ps16A=ps16BufferIn + *ps16BR1;
PS16 ps16B=ps16BufferIn + *ps16BR2;
S32 s32HRMinus=ps16A[0]-ps16B[0];
S32 s32HRPlus=s32HRMinus+(ps16B[0]<<1);
S32 s32HIMinus=ps16A[1]-ps16B[1];
S32 s32HIPlus=s32HIMinus+(ps16B[1]<<1);
S32 s32S=((S32)s16Sin*s32HRMinus - (S32)s16Cos*s32HIPlus) >> 15;
S32 s32T=((S32)s16Cos*s32HRMinus + (S32)s16Sin*s32HIPlus) >> 15;
ps16A[0]=(S16)((s32HRPlus + s32S) >> 1);
ps16B[0]=(S16)(ps16A[0] - s32S);
ps16A[1]=(S16)((s32HIMinus + s32T) >> 1);
ps16B[1]=(S16)(ps16A[1] - s32HIMinus);
ps16BR1++;
ps16BR2--;
}
// The 0Hz bin...
ps16BufferIn[0]+=ps16BufferIn[1];
ps16BufferIn[1]=0;
}
// Now fix the data due to bit reversal...
{
int n;
PS32 ps32Out=ps32BufferOut;
PS16 ps16BR=pfs->ps16BitRev;
S64 s64Sum=0;
S32 s32Max=0;
for (n=0;nnLen/2;n++)
{
S32 s32R=ps16BufferIn[ps16BR[0]];
S32 s32I=ps16BufferIn[ps16BR[0]+1];
S32 s32Root;
S32 s32Mask;
S32 s32A2 = s32R*s32R + s32I*s32I;
if (s32A2<0)
{
s32A2=0; // In case of overflow
}
if (s32A2>4194304L)
{
s32Root=32;
do
{
s32Mask=s32A2/s32Root;
s32Root=(s32Root+s32Mask) >> 1;
} while (labs(s32Root-s32Mask) > 1);
s32Root*=16;
}
else
{
s32Root=512;
s32A2*=256;
do
{
s32Mask=s32A2/s32Root;
s32Root=(s32Root+s32Mask) >> 1;
} while (labs(s32Root-s32Mask) > 1);
}
*ps32BufferOut++=s32Root;
s64Sum+=s32Root;
if (s32Root>s32Max)
{
s32Max=s32Root;
}
ps16BR++;
}
FFTSetAverage(pfs,(S32)(s64Sum/(pfs->nLen/2)));
FFTSetMax(pfs,s32Max);
}
}