test15.c

5402开发板附带的源程序

#include <math.h>
#include <stdlib.h>

#define pi 3.1415925

float fp,fr,fs;
float b_real[50],b_imag[50];
float b1_real[50],b1_imag[50],b2_real[50],b2_imag[50];
float d[50],e[50],g[50];
float f1[4],f2[4],h[50];
float ptr_a[50],ptr_b[50];
float hwdb[50];

void lowpass_input(float *wp,float *ws,float *ap,float *ar,
                   float *f1,float *f2,int *nf)                                                                                       /*初始化*/
{
   fp=100;
   *ap=3;
   fr=300;
   *ar=20;
   fs=1000;
   *wp=tan(pi*fp/fs);
   *ws=tan(pi*fr/fs);
   *f1=-1.0;
   *(f1+1)=1.0;
   *f2=1.0;
   *(f2+1)=1.0;
   *nf=1;
}

void bcg(float ap,float as,float wp,float ws,int *n,float *h)                     /*求H(s)分母系数*/
{
   int i,k;
   float a,p,wc,cs1,cs2;
   float c;

   c=(pow(10.0,0.1*as)-1.0)/(pow(10.0,0.1*ap)-1.0);
   *n=(int)(fabs(log10(c)/log10(ws/wp)/2.0)+0.99999);                                     /*求N*/
   wc=wp;
   a=pow(wc,(double)(*n));

   for(i=0;i<*n;i++)                           /*求极点*/
   {
      p=pi*(0.5+(2.0*i+1.0)/(2.0*(*n)));
      b_real[i]=wc*cos(p);
      b_imag[i]=wc*sin(p);
   }     

   b1_real[0]=-(b_real[0]);
   b1_imag[0]=-(b_imag[0]);
   b1_real[1]=1.0;
   b1_imag[1]=0.0;

   if(*n!=1)
   {
      for(i=1;i<*n;i++)
      {
         for(k=0;k<i;k++)
         {
            cs1=b1_real[k]-b1_real[k+1]*b_real[i];
            cs2=b1_imag[k]-b1_real[k+1]*b_imag[i];
            b2_real[k+1]=cs1+b1_imag[k+1]*b_imag[i];
            b2_imag[k+1]=cs2-b1_imag[k+1]*b_real[i];
         }

         b2_real[0]=-(b1_real[0]*b_real[i]-b1_imag[0]*b_imag[i]);
         b2_imag[0]=-(b1_real[0]*b_imag[i]+b1_imag[0]*b_real[i]);
         b2_real[i+1]=b1_real[i];
         b2_imag[i+1]=b1_imag[i];

         for(k=0;k<=i+1;k++)
         {
            b1_real[k]=b2_real[k];
            b1_imag[k]=b2_imag[k];
            b2_real[k]=0.0;
            b2_imag[k]=0.0;
         }
      }
   }

   for(i=0;i<=*n;i++)
      h[i]=b1_real[i]/a;
}

void pnpe(float *a,int m,int n,float *b,int *mn)                               /*多项式乘方展开系数*/
{
   int i,j,k,nk;
   float c[50];

   *mn=m*n;

   for(i=0;i<*mn+1;i++)
   {
      c[i]=0;
      b[i]=0;
   }

   if(n==0)
      b[0]=1.00;
   else
   {
      for(i=0;i<=m;i++)
         b[i]=a[i];
      if(n==1)
      {
         for(i=0;i<=m;i++)
            b[i]=a[i];
      }
      else
      {
         nk=m+1;
         for(i=1;i<n;i++)
         {
            for(j=0;j<=m;j++)
            {
               for(k=0;k<nk;k++)
                  c[k+j]+=a[j]*b[k];
            }

            nk+=m;

            for(k=0;k<nk;k++)
            {
               b[k]=c[k];
               c[k]=0;
            }
         }
      }
   }
}

void ypmp(float *a,int m,float *b,int n,float *c,int *mn)
{
   int i,j;

   *mn=m+n;

   for(i=0;i<*mn+1;i++)
      c[i]=0;

   for(i=0;i<=m;i++)
   {
      for(j=0;j<=n;j++)
         c[i+j]+=a[i]*b[j];
   }
}

void bsf(float *c,int ni,float *f1,float *f2,int nf,float *ptr_a,
          float *ptr_b,int *no)
{
   int i,j,nd,ne,ng;

   pnpe(f2,nf,ni,ptr_a,no);                                            /*计算分子系数*/

   for(i=0;i<*no+2;i++)
      ptr_b[i]=0;

   for(i=0;i<=ni;i++)                                                           /*计算分母系数*/
   {
      pnpe(f1,nf,i,d,&nd);
      pnpe(f2,nf,ni-i,e,&ne);
      ypmp(d,nd,e,ne,g,&ng);

      for(j=0;j<=ng;j++)
         ptr_b[j]+=c[i]*g[j];
   }
}

main()
{
   int N,nf,ns,nz,i,k;
   float hwdb1_real,hwdb1_imag,hwdb2_real,hwdb2_imag;
   float wp,ws,ap,as,jw,amp1,amp2;

   N=50;

   for(i=0;i<50;i++)
   {
      b_real[i]=0;
      b_imag[i]=0;
      b1_real[i]=0;
      b1_imag[i]=0;
      b2_real[i]=0;
      b2_imag[i]=0;
      d[i]=0;
      e[i]=0;
      g[i]=0;
      ptr_a[i]=0;
      ptr_b[i]=0;
      hwdb[i]=0;
   }

   lowpass_input(&wp,&ws,&ap,&as,f1,f2,&nf);
   bcg(ap,as,wp,ws,&ns,h);            
   bsf(h,ns,f1,f2,nf,ptr_a,ptr_b,&nz);           

   for(k=0;k<N;k++)
    {
      jw=k*pi/N;
      hwdb1_real=0;  hwdb1_imag=0;
      hwdb2_real=0;  hwdb2_imag=0;

      for(i=0;i<=nz;i++)
      {
         hwdb1_real+=ptr_a[i]*cos(i*jw);
         hwdb2_real+=ptr_b[i]*cos(i*jw);
         hwdb1_imag+=ptr_a[i]*sin(i*jw);
         hwdb2_imag+=ptr_b[i]*sin(i*jw);
      }
      amp1=(pow(hwdb1_real,2)+pow(hwdb1_imag,2));
      amp2=(pow(hwdb2_real,2)+pow(hwdb2_imag,2));          
      hwdb[k]=20*log10(amp1/amp2);
      if(hwdb[k]<-200)  hwdb[k]=-200;        
   }
}