bpsk.c

可以直接用在DSP1.0或2.0芯片上

#include <stdio.h>
#include <math.h>


void tra(float *x,float *y)
 {
  float t;
  t=(*x);
  (*x)=(*y);
  (*y)=t;
 }


void FFT(float *xr,float *xi,int n,int inv)
{
int i,j,a,b,k,m;
int ep,arg,mt,s0,s1;
float sign,pr,pi,ph;
float *c,*s;

c=(float *)calloc(n,sizeof(float));
if(c==NULL) exit(1);
s=(float *)calloc(n,sizeof(float));
if(s==NULL) exit(1);

     j=0;
     if(inv==0)
     {
     sign=1.0;
     for(i=0;i<n;i++)
      {
          xr[i]=xr[i]/n;
          xi[i]=xi[i]/n;
      }
     }
     else sign=-1.0;

     for(i=0;i<n-1;i++)
     {
         if(i<j)
         {    tra(&xr[i],&xr[j]);
          tra(&xi[i],&xi[j]);
         }
         k=n/2;
         while(k<=j)
         {   j=j-k;
         k=k/2;
         }
         j=j+k;
     }

     ep=0;
     i=n;
     while(i!=1)
     {
        ep=ep+1;
        i=i/2;
     }
     ph=2*M_PI/n;

     for(i=0;i<n;i++)
     {
      s[i]=sign*sin(ph*i);
      c[i]=cos(ph*i);
     }
     a=2;
     b=1;
     for(mt=1;mt<=ep;mt++)
     {   s0=n/a;
         s1=0;
         for(k=0;k<b;k++)
         {   i=k;
              while(i<n)
              { arg=i+b;
                  if(k==0)
                  {pr=xr[arg];
                   pi=xi[arg];
                   }
                   else
                  {
                   pr=xr[arg]*c[s1]-xi[arg]*s[s1];
                   pi=xr[arg]*s[s1]+xi[arg]*c[s1];
                   }

                   xr[arg]=xr[i]-pr;
                   xi[arg]=xi[i]-pi;
                   xr[i]=xr[i]+pr;
                   xi[i]=xi[i]+pi;
                   i=i+a;
               }
               s1=s1+s0;
      }
      a=2*a;
      b=2*b;
     }
     free(c);
     free(s);

}

main()
{
   float sample[64]={
-13665
,-13204
,-12253
,-9002
,-5086
,142
,5041
,9813
,13034
,14875
,14586
,12539
,8580
,3534
,-2308
,-7882
,-12676
,-15689
,-16787
,-15581
,-12412
,-7463
,-1436
,4949
,10726
,15290
,17750
,18027
,15888
,11583
,5614
,-1119
,-7829
,-13437
,-17297
,-18782
,-17845
,-14576
,-9409
,-3035
,3662
,9821
,14572
,17294
,17688
,15800
,11824
,6464
,344
,-5549
,-10438
,-13694
,-14968
,-14227
,-11662
,-7773
,-3169
,1527
,5571
,8552
,10042
,10034
,8720
,6358};
   int i;
   int number=64,L=128,M=64;
   int pos,pos2;
   float baud_est,srate=23.4375;
   float amp[64],amr[64];
   float amq[64]={0},ami[64]={0};
   float spec2[128];
   float delta;
   float fl,fh;
   float ar,ai,wr,wi;
   float gr[128],hr[128],qr[128],dr[128],cr[128],Xr[64];
   float gi[128],hi[128],qi[128],di[128],ci[128],Xi[64];
   float precise[64];
   float fz[64];

   for(i=0; i<number; i++)
   {
     amp[i]=fabs(sample[i]);
     amr[i]=fabs(sample[i]);
   }

   FFT(amp,amq,number,1);
   
   for(i=0; i<number; i++)
   {
     spec2[i]=sqrt(amp[i]*amp[i]+amq[i]*amq[i]);
   }

   pos=1;
   for(i=1;i<(number+1)/2;i++)
   {
     if(spec2[pos]<spec2[i])
     {
      pos=i;
     }
   }
   baud_est=pos*srate/number;

   delta=0.05;
   fl=baud_est*(1-delta);
   fh=baud_est*(1+delta);
   ar=cos(2*M_PI*fl/srate);
   ai=sin(2*M_PI*fl/srate);
   wr=cos(-2*M_PI*(fh-fl)/(M*srate));
   wi=sin(-2*M_PI*(fh-fl)/(M*srate));

   dr[0]=cos(-1*M_PI*(fh-fl)/(M*srate))*ar+sin(-1*M_PI*(fh-fl)/(M*srate))*ai;
   di[0]=sin(-1*M_PI*(fh-fl)/(M*srate))*ar-cos(-1*M_PI*(fh-fl)/(M*srate))*ai;
   cr[0]=1;
   ci[0]=0;
   gr[0]=cr[0]*amr[0]-ci[0]*ami[0];
   gi[0]=cr[0]*ami[0]+ci[0]*amr[0];
    for(i=1;i<number;i++)
    {
     dr[i]=dr[i-1]*wr-di[i-1]*wi;
     di[i]=dr[i-1]*wi+di[i-1]*wr;
     cr[i]=cr[i-1]*dr[i-1]-ci[i-1]*di[i-1];
     ci[i]=cr[i-1]*di[i-1]+ci[i-1]*dr[i-1];
     gr[i]=cr[i]*amr[i]-ci[i]*ami[i];
     gi[i]=cr[i]*ami[i]+ci[i]*amr[i];
    }

    for (i=number;i<L;i++)
    {
     gr[i]=0;
     gi[i]=0;
    }

    FFT(gr,gi,L,1);

    dr[0]=cos(M_PI*(fh-fl)/(M*srate));
    di[0]=sin(M_PI*(fh-fl)/(M*srate));
    hr[0]=1;
    hi[0]=0;

    for(i=1;i<number;i++)
    {
     if(i<M)
     {
      dr[i]=dr[i-1]*wr+di[i-1]*wi;
      di[i]=di[i-1]*wr-dr[i-1]*wi;
      hr[i]=hr[i-1]*dr[i-1]-hi[i-1]*di[i-1];
      hi[i]=hr[i-1]*di[i-1]+hi[i-1]*dr[i-1];
      hr[L-i]=hr[i];
      hi[L-i]=hi[i];
      }
      else
      {
      hr[i]=hr[i-1]*dr[i-1]-hi[i-1]*di[i-1];
      hi[i]=hr[i-1]*di[i-1]+hi[i-1]*dr[i-1];
      }
    }

    for(i=M;i<=L-number;i++)
    {
     hr[i]=0;
     hi[i]=0;
    }

    FFT(hr,hi,L,1);
 
    for(i=0;i<L;i++)
    {
     qr[i]=hr[i]*gr[i]-hi[i]*gi[i];
     qi[i]=hr[i]*gi[i]+hi[i]*gr[i];
    }

    FFT(qr,qi,L,0);

    dr[0]=cos(-1*M_PI*(fh-fl)/(M*srate));
    di[0]=sin(-1*M_PI*(fh-fl)/(M*srate));
    cr[0]=1;
    ci[0]=0;
    Xr[0]=cr[0]*qr[0]-ci[0]*qi[0];
    Xi[0]=cr[0]*qi[0]+ci[0]*qr[0];

    for(i=1;i<M;i++)
    {
     dr[i]=dr[i-1]*wr-di[i-1]*wi;
     di[i]=dr[i-1]*wi+di[i-1]*wr;
     cr[i]=cr[i-1]*dr[i-1]-ci[i-1]*di[i-1];
     ci[i]=cr[i-1]*di[i-1]+ci[i-1]*dr[i-1];
     Xr[i]=cr[i]*qr[i]-ci[i]*qi[i];
     Xi[i]=cr[i]*qi[i]+ci[i]*qr[i];
    }

    for(i=0;i<M;i++)
    {
     precise[i]=sqrt(Xr[i]*Xr[i]+Xi[i]*Xi[i]);
    }

    pos2=0;
    for(i=0;i<M;i++)
    {
     if(precise[pos2]<precise[i])
     {
      pos2=i;
     }
    }
     pos2=pos2+1;

    baud_est=pos2*(fh-fl)/M+fl;

    for(i=0;i<M;i++)
    {
     fz[i]=i*(fh-fl)/M+fl;
     }

      printf("%f ",baud_est);

    getch();

}