[求助] 关于DSP数字锁相放大器的设计

有木有大神帮忙，我的毕设是关于DSP数字锁相放大器的设计，我现在编程编不好，达不到效果，很是愁人，望大神解决啊。。。。QQ 870612404

/*****************head file********************/  

主要是锁相放大算法那一块实现不了，程序如下
/*****************head file********************/  
#include "DSP2833x_Device.h"     // DSP2833x Headerfile Include File
#include "DSP2833x_Examples.h"   // DSP2833x Examples Include File
#include "math.h"
#include "stdio.h"
#include "stdlib.h"
// Determine when the shift to right justify the data takes place
// Only one of these should be defined as 1.
// The other two should be defined as 0.
#define POST_SHIFT   0  // Shift results after the entire sample table is full
#define INLINE_SHIFT 1  // Shift results as the data is taken from the results regsiter
#define NO_SHIFT     0  // Do not shift the results

// ADC start parameters
#if (CPU_FRQ_150MHZ)     // Default - 150 MHz SYSCLKOUT
  #define ADC_MODCLK 0x3 // HSPCLK = SYSCLKOUT/2*ADC_MODCLK2 = 150/(2*3)   = 25.0 MHz
#endif
#if (CPU_FRQ_100MHZ)
  #define ADC_MODCLK 0x2 // HSPCLK = SYSCLKOUT/2*ADC_MODCLK2 = 100/(2*2)   = 25.0 MHz
#endif
#define ADC_CKPS   0x0   // ADC module clock = HSPCLK/1      = 25.5MHz/(1)   = 25.0 MHz
#define ADC_SHCLK  0x1   // S/H width in ADC module periods                  = 2 ADC cycle
#define AVG        1000  // Average sample limit
#define ZOFFSET    0x00  // Average Zero offset
#define BUF_SIZE   512  // Sample buffer size
#define N   180
#define M   140
#define pi 3.1416
#define b  34 //h(n)的序列长度
#define a  40 //h(n)的序列长度
#define c  2*N+b-1  
float array_sin[180];
float array_cos[180];
float SampleTable1[BUF_SIZE]={0};
float SampleTable2[BUF_SIZE]={0};  
float SampleTable3[BUF_SIZE]={0};
Uint16 SampleTable[BUF_SIZE];
float SampleValue[BUF_SIZE];
float y1[N-1]={0.0};           //输出序列
float y2[c]={0.0};
void InitEPwm1Example();
volatile unsigned int adconvover=0;
interrupt void epwm1_timer_isr(void);
Uint16 EPwm1TimerIntCount;
Uint16 array_index;
const float L[34] = {
                0,   0.0002864978729488, 0.001264624162909, 0.003083310527426,
   0.005841204980407, 0.009575017244339,  0.01425214897781,  0.01976834915372,
    0.02595074975146,  0.03256621452731,  0.03933450837311,   0.0459454018102,
    0.05207849738032,  0.05742432989344,  0.06170517100719,  0.06469397151492,
    0.06623000282249,  0.06623000282249,  0.06469397151492,  0.06170517100719,
    0.05742432989344,  0.05207849738032,   0.0459454018102,  0.03933450837311,
    0.03256621452731,  0.02595074975146,  0.01976834915372,  0.01425214897781,
   0.009575017244339, 0.005841204980407, 0.003083310527426, 0.001264624162909,
  0.0002864978729488,0
};
const float H[40] = {
  -0.001305096054213,-0.001478350963911, -0.00188527402024,-0.002559610054874,
  -0.003537780947164,-0.004860232357888,-0.006573547322234,-0.008733738690038,
   -0.01141141434653, -0.01470003343206, -0.01872949362692, -0.02368939446207,
   -0.02987095372283, -0.03774760142235,  -0.0481434027665, -0.06262584473804,
   -0.08457160676635,  -0.1227895346972,  -0.2096342344354,  -0.6364784631899,
     0.6364784631899,   0.2096342344354,   0.1227895346972,  0.08457160676635,
    0.06262584473804,   0.0481434027665,  0.03774760142235,  0.02987095372283,
    0.02368939446207,  0.01872949362692,  0.01470003343206,  0.01141141434653,
   0.008733738690038, 0.006573547322234, 0.004860232357888, 0.003537780947164,
   0.002559610054874,  0.00188527402024, 0.001478350963911, 0.001305096054213
};

void LinearConvolution();
void sin_start()
{
        
        int n=0;
        float interval =2*pi/180;
        
        for (n=0;n<180;n++)
        {
                array_sin[n]=sin(interval*n);
        array_cos[n]=cos(interval*n);
        }        
}

/***************main function**********************/
main()
{
   Uint16 i;  
   
   
   
// Clear SampleTable
   for (i=0; i<200; i++)
   {
     SampleTable[i] = 0;
   //  SampleValue[i] = 0;
   }

   InitSysCtrl();              //初始化系统函数
   InitGpio();                 // 该例中跳过
   DINT;
   IER = 0x0000;               //禁止CPU中断
   IFR = 0x0000;               //清除CPU中断标志
   InitPieCtrl();              //初始化PIE控制寄存器
   InitPieVectTable();         //初始化PIE中断向量表

  //服务程序
    EALLOW;           // This is needed to write to EALLOW protected register
    //PieVectTable.ADCINT = &adc_isr;
    PieVectTable.EPWM1_INT = &epwm1_timer_isr;
    EDIS;
    InitAdc();         //初始化ADC模块
    InitEPwm1Example();//初始化EPwm1模块

    array_index=0;
    EPwm1TimerIntCount=0;

    //开中断
    PieCtrlRegs.PIEIER3.bit.INTx1 = 1;//使能PIE中断，EPWM-T1定时器中断位于INT3.1
    IER |= M_INT3;   //开CPU中断
    EINT;            // 使能全局中断
    ERTM;            // 使能实时中断
    // 等待ADC中断
    for(;;)
     {
       }
}
   


  void InitEPwm1Example()
  {
    EALLOW;
    SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 0;// 在系统初始化函数中已经使能  
                                     //EPWM模块，此处只需设置禁止同步。
    EDIS;   //1.设置TB模块
    EPwm1Regs.TBPRD = 12;             //1/(5k*140)ms //设置计数周期=600TBCLK
    EPwm1Regs.CMPA.half.CMPA = 12500;    // 比较器A = 400TBCLK
    EPwm1Regs.CMPB = 500;                //比较器 B = 500 TBCLK
    EPwm1Regs.TBPHS.half.TBPHS = 0x0000; // 设置相位寄存器置零
    EPwm1Regs.TBCTR = 0x0000;            // 清除TB计数器

    // Setup TBCLK = SYSCLKOUT / (HSPCLKDIV * CLKDIV)=150/4*4
    EPwm1Regs.TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;
      //计数方式设置00 //Up-count mode;01Down-    count mode;10 Up-down-count mode
    EPwm1Regs.TBCTL.bit.PHSEN = TB_DISABLE;    // 禁止相位加载
    EPwm1Regs.TBCTL.bit.PRDLD = TB_SHADOW;     //从阴影寄存器加载计数值
    EPwm1Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_DISABLE;//禁用同步输出，在头文
                                  //件DSP2833x-ePwm-defines.h中有定义
    EPwm1Regs.TBCTL.bit.HSPCLKDIV =TB_DIV4;
    EPwm1Regs.TBCTL.bit.CLKDIV = TB_DIV4;

    //设置比较模块
    EPwm1Regs.CMPCTL.bit.SHDWAMODE = CC_SHADOW; // 映射模式Load registers  
                                            //every ZERO
    EPwm1Regs.CMPCTL.bit.SHDWBMODE = CC_SHADOW;
    EPwm1Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR = Zero
    EPwm1Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO;

    // 设置动作模块
    EPwm1Regs.AQCTLA.bit.CAU = AQ_SET;
    EPwm1Regs.AQCTLA.bit.CAD = AQ_CLEAR;
    EPwm1Regs.AQCTLB.bit.CBU = AQ_SET;
    EPwm1Regs.AQCTLB.bit.CBD = AQ_CLEAR;

   //事件触发子模块设置
   //1.开始转换脉冲SOCA设置
   EPwm1Regs.ETSEL.bit.SOCAEN = 1; // 使能ADC开始转换A(EPWMxSOCA）脉冲
   EPwm1Regs.ETSEL.bit.SOCASEL = 2; //设置EPWMxSOCA何时生成，2,TBCTR=TBPRD
   EPwm1Regs.ETPS.bit.SOCAPRD = 3; //第三个中断事件A发生，即EPWMxSOCA生成
   //2.EPWM中断设置
   EPwm1Regs.ETSEL.bit.INTSEL = ET_CTR_PRD;//EPWM中断设置，此处设置为周期中断
   EPwm1Regs.ETSEL.bit.INTEN = 1; //使能EPWM中断
   EPwm1Regs.ETPS.bit.INTPRD = ET_1ST;//第一个事件发生时产生中断

   EALLOW;
   SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1;// Enable TBCLK within the ePWM
   EDIS;
   }

   void InitAdc(void)
  {  
   Uint16 i;

   AdcRegs.ADCTRL1.bit.RESET=1; //复位AD 模块
   for(i=0;i<9;i++)
   {asm(" NOP");}
   AdcRegs.ADCTRL1.bit.RESET=0;
   AdcRegs.ADCTRL1.bit.SUSMOD=0; //仿真暂停方式，0为忽略。
   AdcRegs.ADCTRL1.bit.ACQ_PS=15; //设定窗口大小SC=ADCTRL1[11:8] + 1 ;
                               //16ADC CLOCKS
   AdcRegs.ADCTRL1.bit.CPS=1; //内核时钟预定标器，1为二分频
   AdcRegs.ADCTRL1.bit.CONT_RUN=1;//连续运行方式，0:启停方式.1:连续转换方式.
   AdcRegs.ADCTRL1.bit.SEQ_CASC=1; //**级联序列发生器工作方式：0 双序列模   
                                //式;1 级联模式

    //ADCTRL3对电源和采样方式进行设置
    AdcRegs.ADCTRL3.bit.ADCBGRFDN=3;//带隙参考电路上电
    for(i=0;i<10000;i++)
    {asm(" NOP");}
    AdcRegs.ADCTRL3.bit.ADCPWDN=1;//除带隙慰嫉缏吠獾钠渌鸄DC模块上电
    for(i=0;i<5000;i++)
    {asm(" NOP");}
   AdcRegs.ADCTRL3.bit.ADCCLKPS=7; // ADC Core clock divider ：//nSPCLK/[2*(ADCTRL1[7]：                                      //CPS+ 1)]=12.5MnZ
    AdcRegs.ADCTRL3.bit.SMODE_SEL=1; //采样方式。顺序采样0/同步采样1

   AdcRegs.ADCMAXCONV.bit.MAX_CONV1=0x07; //设定转换数量,共8同步采样（每
                                       //次采样两个通道），共采样16路
    AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0x0; //采样ADCINA0和ADCINB0
    AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 0x1; //采样ADCINA1和ADCINB1
   AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 0x2; //采样ADCINA2和ADCINB2
   AdcRegs.ADCCHSELSEQ1.bit.CONV03 = 0x3; //采样ADCINA3和ADCINB3
   AdcRegs.ADCCHSELSEQ3.bit.CONV08 = 0x4; //采样ADCINA4和ADCINB4
   AdcRegs.ADCCHSELSEQ3.bit.CONV09 = 0x5; //采样ADCINA5和ADCINB5
   AdcRegs.ADCCHSELSEQ3.bit.CONV10 = 0x6; //采样ADCINA6和ADCINB6
   AdcRegs.ADCCHSELSEQ3.bit.CONV11 = 0x7; //采样ADCINA7和ADCINB7

   //ADC状态和标志寄存器ADCST（清除中断标志）
   AdcRegs.ADCST.bit.INT_SEQ1_CLR=1;//中断清除位
   AdcRegs.ADCST.bit.INT_SEQ2_CLR=1;

    //AdcRegs.ADCTRL2.all=0x2800;
   AdcRegs.ADCTRL2.bit.EPWM_SOCB_SEQ=0;
   AdcRegs.ADCTRL2.bit.RST_SEQ1=0;// 复位序列发生器1
    //SEQ1
   AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1=0;//SEQ1中断使能：0禁止，1使能
   AdcRegs.ADCTRL2.bit.INT_MOD_SEQ1=0;//SEQ1中断方式：0每个SEQ1序列转换
                                  //束，置位SEQ1的中断标志
   AdcRegs.ADCTRL2.bit.EPWM_SOCA_SEQ1=1;//SEQ1的EPWM SOCA使能位，1使能
   AdcRegs.ADCTRL2.bit.EXT_SOC_SEQ1=0;//SEQ1的外部SOC信号位
   //SEQ2
   AdcRegs.ADCTRL2.bit.RST_SEQ2=0;    //复位SEQ2
   AdcRegs.ADCTRL2.bit.SOC_SEQ2=0;    //启动SEQ2的转换触发。仅适用于双序
                                   //列发生器模式
   AdcRegs.ADCTRL2.bit.INT_ENA_SEQ2=0;
   AdcRegs.ADCTRL2.bit.INT_MOD_SEQ2=0;
   AdcRegs.ADCTRL2.bit.EPWM_SOCB_SEQ2=0;
   }

   //**********EPWM1的定时器中断****************
   interrupt void epwm1_timer_isr(void)//1S 采样
  {
  
   Uint16 j,k,mode;
   float temp1 = 0;
   float temp2 = 0;
  
  // EPwm1TimerIntCount++;              //中断次数
   //if(EPwm1TimerIntCount==50)
// {
   //EPwm1TimerIntCount=0;
   //DELAY_US(5L);
   sin_start();
   SampleTable[array_index]= ( (AdcRegs.ADCRESULT0)>>4);
   SampleValue[array_index] =  SampleTable[array_index]*3.0/4096;
    // If 40 conversions have been logged, start over
    if(array_index == 140)
    {
   
  
            LinearConvolution(M,a,SampleValue,H,y1);//线性卷积
                mode=0;        
                for(k = 0;k                 {
                         for(j = 0;j                          {
                        
                                 temp1 += y1[j]*array_sin[j+k];
                                 temp2 += y1[j]*array_cos[j+k];
                         }                       
                 if(mode==0)        
                         {
                         SampleTable1[N-1-k] = temp1/(N-k);
                         SampleTable2[N-1-k] = temp2/(N-k);
             }
             else        
                         {
                         SampleTable1[N-1-k] = temp1/N;
                         SampleTable2[N-1-k] = temp2/N;
             }
                 }                 
              temp1 = 0;
                  temp2 = 0;
                  
                  for(k=0;k                   {
                          SampleTable3[N-1-k]=2*sqrt(pow(SampleTable1[N-1-k],2)+pow(SampleTable2[N-1-k],2));
                  }
                  
                 for(k = 0;k < N-1;k++)
                 {
                         for(j = 0;j < N-k;j++)
                         {
                                 temp1 += y1[j+k]*array_sin[j];
                                 temp2 += y1[j+k]*array_cos[j];
                         }
                 if(mode==0)        
                  {
                         SampleTable1[N-1+k] = temp1/(N-k);
                          SampleTable2[N-1+k] = temp2/(N-k);
          }

                 else
                  {
                    SampleTable1[N-1+k] = temp1/N;
                         SampleTable2[N-1+k] = temp2/N;
                  }
            }
        
                 for(k=0;k                  {
                         SampleTable3[N-1+k]=2*sqrt(pow(SampleTable1[N-1+k],2)+pow(SampleTable2[N-1+k],2));
                        
                 }

          LinearConvolution(2*N-1,b,SampleTable3,L,y2);//线性卷积
          array_index= 0;         
                        
    }
   else array_index++;
  
   //AdcRegs.ADCTRL2.bit.SOC_SEQ1=0;
   //EPwm1Regs.ETSEL.bit.SOCAEN = 0;
   // Reinitialize for next ADC sequence
   AdcRegs.ADCTRL2.bit.RST_SEQ1 = 1; // Reset SEQ1
   //清除定时器中断标志，并使能中断
   EPwm1Regs.ETCLR.bit.INT = 1;
   PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;
   // AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1; // Clear INT SEQ1 bit
   //PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; // Acknowledge interrupt toPIE
    return;
  }

void LinearConvolution(Uint16 xn,Uint16 hn,float *x,float *h,float *y)  
{
   Uint16 i,j,k,l;
   Uint16 yn;   //输出序列y的长度
   yn=xn+hn-1;
   for(i=0;i    k=yn-1;
   for(i=hn-1;i>0;i--)      //将*h作为被乘数
    {
       l=k;
       for(j=xn-1;j>0;j--)  //数组x[n]的1~(xn-1)与h[i]逐一相乘
            {
           y[l]+=h[i]*x[j];
                   l--;
                }
           y[l]+=x[0]*h[i];     
           k--;
        }
        l=k;
   for(j=xn-1;j>0;j--)
        {
       y[l]+=h[0]*x[j];
           l--;
    }
        y[l]+=x[0]*h[0];
}
  
//===========================================================================
// No more.
//===========================================================================