调用单相SOGI锁相环锁相不精确的问题

#include "DSP281x_Device.h"
#include "DSP281x_Examples.h"
#include "Solar_IQ.h"
#include <math.h>
#define PI 3.1415926
#define Ts 0.00005
#define GRID_FREQ 50
#define N  400

// Variable defs
long ia1,ia,i1,ig,ig_ref;
long us1,us2,us,factor_us[2]={0,0},us_0,uss,uss0;
SPLL_1ph_SOGI_IQ spll1;
long id1[3]={0,0,0},ed1[3]={0,0,0}; // Current Loop output and error
float kc1=4.000246740102,kc3=1.999753275118,kp=0.002,kr=10;  // Current loop PR
int  i=0;
long g1,g2,g3,M;
float ig_r=1;

ioport   unsigned int  port00;
ioport   unsigned int  port01;
ioport   unsigned int  port02;
ioport   unsigned int  port03;

interrupt void T1_Compare_isr(void);
void AD_Sampling(void);
void Current_Loop(void);
void SPWM(void);
void DA_Viewer(void);

void main(void)
{

	// Step 1. Initialize System Control:
	// PLL, WatchDog, enable Peripheral Clocks
	// This example function is found in the DSP281x_SysCtrl.c file.
	InitSysCtrl();

	// Step 2. Initalize GPIO:
	// This example function is found in the DSP281x_Gpio.c file and
	// illustrates how to set the GPIO to it's default state.
	InitGpio();

	// Step 3. Clear all interrupts and initialize PIE vector table:
	// Disable CPU interrupts
	DINT;

	// Initialize PIE control registers to their default state.
	// The default state is all PIE interrupts disabled and flags
	// are cleared.
	// This function is found in the DSP281x_PieCtrl.c file.
	InitPieCtrl();

	// Disable CPU interrupts and clear all CPU interrupt flags:
	IER=0x0000;
	IFR=0x0000;

	// Initialize the PIE vector table with pointers to the shell Interrupt
	// Service Routines (ISR).
	// This will populate the entire table, even if the interrupt
	// is not used in this example.  This is useful for debug purposes.
	// The shell ISR routines are found in DSP281x_DefaultIsr.c.
	// This function is found in DSP281x_PieVect.c.
	InitPieVectTable();

	// Interrupts that are used in this example are re-mapped to
	// ISR functions found within this file.
	EALLOW;  // This is needed to write to EALLOW protected registers
	PieVectTable.T1PINT=&T1_Compare_isr;
	EDIS;    // This is needed to disable write to EALLOW protected registers

	// Step 4. Initialize all the Device Peripherals:
	// This function is found in DSP281x_InitPeripherals.c
	// InitPeripherals(); // Not required for this example
	InitEv();
	InitAdc();

	// Step 5. User specific code, enable interrupts:

	//SPLL 1ph SOGI Method initialization
	SPLL_1ph_SOGI_IQ_init(GRID_FREQ,_IQ((float)(Ts)),&spll1);
	SPLL_1ph_SOGI_IQ_coeff_update(((float)(Ts)), (float)(2*PI*GRID_FREQ),&spll1);

	PieCtrlRegs.PIEIER2.all=M_INT4;      // PIE interrupt enable
	IER|=M_INT2;

	// Enable global Interrupts and higher priority real-time debug events:
	EINT;                                // Enable Global interrupt INTM
	ERTM;                                // Enable Global realtime interrupt DBGM
	EvaRegs.T1CON.bit.TENABLE = 1;       // Timer1 number counter enable

	// Step 6. Just sit and loop forever:
	for(;;);

}

void AD_Sampling(void)
{
	us1=(AdcRegs.ADCRESULT0>>4);
	us2=(us1-2079);
	us=_IQ(us2);

	ia1=(AdcRegs.ADCRESULT1>>4);
	ia=(ia1-2060);
	i1=_IQ(ia);

	us_0=_IQmpy(us,_IQ(0.12784601));
	spll1.u[0]=us_0;
}


void Current_Loop(void)
{
	ig=_IQmpy(i1,_IQ(0.002052103));
	ig_ref=_IQmpy(_IQ(ig_r),spll1.sin);      //There is a problem here.
	ed1[2]=ig_ref-ig;
	id1[2]=_IQmpy(_IQ(kc3),id1[1])-id1[0]+_IQmpy((_IQ(kp)-_IQ(2*kr*Ts/kc1)),ed1[0])-_IQmpy(_IQ(kp*kc3),ed1[1])+_IQmpy((_IQ(kr*2*Ts/kc1)+_IQ(kp)),ed1[2]);
	ed1[0]=ed1[1];
	ed1[1]=ed1[2];
    id1[0]=id1[1];
    id1[1]=id1[2];
    if(id1[1]>=131072)//0.5--131072��0.8--183501
    	{
          id1[1]=131072;
    	}
    	if(id1[1]<=-131072)
    	{
          id1[1]=-131072;
    	}
}

void SPWM(void)
{
	if(id1[2]>=0)
		{
	        M=_IQdiv(id1[2],(_IQ(1)+id1[2]));     // Continous mode
	      //M=id1[2];                        // Discontinous mode
			g1=(int)((long)3750*M>>18);
			g2=3751;
			//g2=(int)((long)3750*M>>18);
			g3=0;
		}
	if(id1[2]<0)
		{
			M=_IQdiv((-id1[2]),(_IQ(1)-id1[2]));  // Continous mode
	      //M=-id1[2];                       // Discontinous mode
			g1=(int)((long)3750*M>>18);
			g2=0;
			//g2=3751;
			g3=3751;
	    }

	i++;
	if(i>=N)
	    {
	        i=0;
	    }
    if(g1<=0)
	g1=0;
	if(g1>=3750)
	g1=3750;
    if(g2<=0)
	g2=0;
	if(g2>=3750)
	g2=3750;
	if(g3<=0)
	g3=0;
	if(g3>=3750)
	g3=3750;

    EvaRegs.CMPR1=g1;
    EvaRegs.CMPR2=g2;
	EvaRegs.CMPR3=g3;
}

void DA_Viewer(void)
{
	//port00=(ualpha>>11)+2048;
	//port01=(ubeta>>11)+2048;
    port02=(spll1.theta[0]>>12)+2048;
    //port03=(sin_out>>11)+2048;
}

interrupt void T1_Compare_isr(void)
{
	AD_Sampling();
	SPLL_1ph_SOGI_IQ_FUNC(&spll1);
	Current_Loop();
	SPWM();
	DA_Viewer();
    EvaRegs.EVAIFRA.bit.T1PINT=1;
    PieCtrlRegs.PIEACK.bit.ACK2=1;
    EINT;
}