fft.c

VC写的对DTMF信号的识别程序

// FFT.C (c) 2004 Howard Long (G6LVB), Hanlincrest Ltd. All rights reserved.
// 72 Princes Gate
// London SW7 2PA
// United Kingdom
// [e-mail]
// Free for educational and non-profit use. For commercial use please contact the author.

#include <windows.h>
#include <math.h>

#include <stdio.h>

#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;n<pfs->nLen;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;n<pfs->nLen;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;n<pfs->nLen/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;ps16<ps16End;ps16++)
		{
			S16 s16=*ps16;

			if (s16>29490 || 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 (ps16A<ps16End)
			{
				S16 s16Sin=pfscs->s16Sin;
				S16 s16Cos=pfscs->s16Cos;
				PS16 ps16End2=ps16B;

				while (ps16A<ps16End2)
				{
					S32 s32V=((S32)ps16B[0] * s16Cos + (S32)ps16B[1] * s16Sin) >> 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;n<pfs->nLen/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);
	}
}