expiirokhp01rt.c

c5402addfilter程序,实现addfilter功能

extern void InitC5402(void);
extern void OpenMcBSP(void);
extern void CloseMcBSP(void);
extern void READAD50(void);
extern void WRITEAD50(void);

/***********************************************************************
** Main Function Program
***********************************************************************/
  #include "stdio.h"
  #include "math.h"
  #define pi 3.1415926 
    
  double fs,nlpass,nlstop,nhpass,nhstop,a[3],b[3],x,y;

  void biir2hpdes(double fs, double nhpass, double nhstop, double a[], double b[]);
  
  void biir2hpdes(double fs, double nhpass, double nhstop, double a[], double b[])

  {
    int i,u;
	double wp,omp,gsa,t;
    
    wp=nhpass*2*pi;
	omp=tan(wp/2.0);
	
	gsa=omp*omp;
	for (i=0; i<=2; i++)
	  {
		u=i%2;
		a[i]=gsa*pow(2,u)-sqrt(2)*omp*(i-1)+pow(-2,u);
	  }
	
	for (i=0; i<=2; i++)
	  {	u=i%2;
		b[i]=pow(-2,u);
	  }
	t=a[0];
	for (i=0; i<=2; i++)
	  {	a[i]=a[i]/t;
		b[i]=b[i]/t;
	  }
  }
 
void main(void)
{
		int k=0;
		int x_ad,y_da;
		int *px = (int*)0x3000;
		int *py = (int*)0x3100;
		float w2,w1,w0;
		w2=w1=w0=0.0;
		
//	for (;;)
	{	    		
		//InitC5402(); /* initialize C5402 DSP */
		OpenMcBSP();
		
		fs=16000;
		/*  IIR LP  filter  */
		nhpass=0.3;
		nhstop=0.1;
		biir2hpdes(fs,nlpass,nlstop,a,b);
        
        for(;;)
        {
			READAD50();
			
			x_ad=*px;
			x=x_ad/32768.0;

			w2=x-a[1]*w1-a[2]*w0;
      		y=b[0]*w2+b[1]*w1+b[2]*w0;
      		w0=w1;
      		w1=w2;
      	
      		y_da=(int)(y*32768.0);
      		*py=y_da;
      			
			k++;

			WRITEAD50();

		} 
	}
				
		/*void PROCESSING(void)
		{
		do the necessery operations here!*/
}

/***********************************************************************
** End of File –– exp08.c
***********************************************************************/