AD采样实现AC计量之算法实现与流程

最新推荐文章于 2024-03-31 16:12:52 发布

fz835304205

最新推荐文章于 2024-03-31 16:12:52 发布

阅读量4k

点赞数

分类专栏： TI C2000 DSP

本文链接：https://blog.csdn.net/fz835304205/article/details/12913779

版权

TI C2000 DSP 专栏收录该内容

30 篇文章 18 订阅

订阅专栏

网上的资料还是很少的，把半个月的成果给大家分享吧，先上代码，以后有空对其分析。

measure.c

 /****************************************************************************
 *$Author:   ZhengFang     $
 *$Date:     October, 2013 $
 ****************************************************************************/

#include "DSP28x_Project.h"
#include "global.h"

#define SAMPLE_F       8000.0f
#define N              1024
#define WAVE_F         50.0f
#define PI             3.14159265f

#define ZERO_CNT_CYC   100
#define FFTforFreqEn   1


#define IIRLpfB0_U     0.0061f
#define IIRLpfA1_U     (-1.7672f)
#define IIRLpfA2_U     0.7916f



#define	RFFT_STAGES	10
#define	RFFT_SIZE	(1 << RFFT_STAGES) 

#pragma DATA_SECTION(RFFTin1Buff,"EXRAMDATA");  //Buffer alignment for the input array,
float32 RFFTin1Buff[RFFT_SIZE];                 //RFFT_f32u(optional), RFFT_f32(required)
                                                //Output of FFT overwrites input if 
                                                //RFFT_STAGES is ODD
#pragma DATA_SECTION(RFFToutBuff,"EXRAMDATA"); 
float32 RFFToutBuff[RFFT_SIZE];                 //Output of FFT here if RFFT_STAGES is EVEN

#pragma DATA_SECTION(RFFTmagBuff,"EXRAMDATA");
float32 RFFTmagBuff[RFFT_SIZE/2+1];             //Additional Buffer used in Magnitude calc
 
#pragma DATA_SECTION(RFFTF32Coef,"EXRAMDATA");
float32 RFFTF32Coef[RFFT_SIZE];                 //Twiddle buffer

RFFT_F32_STRUCT rfft;



//_RealTimeData RealTimeData;

_CalcCeof CalcCeof;

float32 ElcPowerFreq;

//Uint16 PowerFreqPre = 50000;
//Uint16 PowerFreq = 50000;
#define CALC_POWER          0x0001
#define CALC_FREQUENC       0x0002
#define CALC_HARMONIC       0x0004
#define CALC_URMS           0x0008


_ChannelData ChannelData[CHN_NUM];

//float32 VoltSmpBuf[60] = {0};
//int16 pVolt = 59;
//float32 InstanVolt = 0;
//float32 InstanCurr = 0;
//float32 ShiftPhsVolt = 0;

int16 WaveTmp = 0;
//float32 Urms2Tmp = 0;
//int16 CalcLen = 160*3;
//int16 Urms2Num = 0;

Uint64 SamSum;
Uint64 ErrSum;


volatile int16 FFTProc = 0;
volatile int16 FFTSmpNum = 0;
//volatile int16 ZeroStart = ZERO_CNT_CYC+1;
//volatile int16 PeriodSyn = 0;
//int16 ZeroTimer = 0;
int16 QuasiWin = 160;

#pragma DATA_SECTION(ViewBuffer0,"EXRAMDATA");
int16 ViewBuffer0[2000];
int16 pView = 0;
#pragma DATA_SECTION(ViewBuffer1,"EXRAMDATA");
float32 ViewBuffer1[2000];

	
/*int32 IIrForU[3] = 
{
    0, 0, 0
};
int16 IIrForUout[2] = 
{
    0, 0
};*/

//int16 PhaseInt = 0;
//float32 PhaseFloat = 0.0;
//float32 PhaseFloat2 = 0.0;

#define IIRLpfB0 11        //b0=b2 Q15
#define IIRLpfA1 (-15954)  //Q13
#define IIRLpfA2 7773      //Q13	

//float32 IIrForP[3];
//float32 IIrForQ[3];

//float32 AcPowTmp = 0;
//float32 RePowTmp = 0;


//float32 actpower = 0;
//float32 reactpower = 0;

//const int Quadrant[4] = {1,2,4,3};
const int Quadrant[4] = {0, 90, 270, 180};

void CalcCeofFun(void)
{
    ESTOP0;
    CalcCeof.k = (CalcCeof.u2-CalcCeof.u1)/(CalcCeof.ad2-CalcCeof.ad1);
    CalcCeof.b = CalcCeof.u2 - CalcCeof.ad2*CalcCeof.k;
    ESTOP0;
}
float32 Fre2Queue(Uint16 val)
{
    static Uint16 pAddr = 0;
    float32 FunFreTmp[4] = {50.0, 50.0, 50.0, 50.0};
    float32 res;
    pAddr &= 3;
    FunFreTmp[pAddr] = val;
    pAddr++;
    res = (FunFreTmp[0] + FunFreTmp[1] + FunFreTmp[2] + FunFreTmp[3]) / 4;
    return res;
}
void MakeWave(void)  
{
    int16 i;
    float32 RadStep; //  2*pi*WAVE_F/SAMPLE_F
    float32 Rad;
    float32 Fre = 50.0;
    Rad = 0.0f;
    //ESTOP0;
    RadStep = 2.0*PI*Fre / (SAMPLE_F/2);
	for(i = 0; i < RFFT_SIZE; i++)
	{
		//RFFTin1Buff[i]   = sin(Rad) + cos(Rad*2.3567); //Real input signal
		RFFTin1Buff[i]   = 311*sin(Rad) + 30*sin(3*Rad)+ 50*sin(5*Rad)+ 70*sin(20*Rad);
		//FFTImag_in[i] = cos(Rad*8.345) + sin(Rad*5.789);		
		Rad = Rad + RadStep;
	}
}

void SysFFTCalc(void)
{
    int16 i;
    for (i=0; i < RFFT_SIZE; i++)
    {
      	RFFToutBuff[i] = 0;			   //Clean up output buffer
    }

    for (i=0; i < RFFT_SIZE/2; i++) 
    {
     	RFFTmagBuff[i] = 0;		       //Clean up magnitude buffer
    }
    RFFT_f32u(&rfft);				   //Calculate real FFT
    RFFT_f32_mag(&rfft);				//Calculate magnitude
}
void SysFFTInit(void)
{
    int16 i;
    rfft.FFTSize   = RFFT_SIZE;
    rfft.FFTStages = RFFT_STAGES;   
    rfft.InBuf     = &RFFTin1Buff[0];  //Input buffer
    rfft.OutBuf    = &RFFToutBuff[0];  //Output buffer
    rfft.CosSinBuf = &RFFTF32Coef[0];  //Twiddle factor buffer
    rfft.MagBuf    = &RFFTmagBuff[0];  //Magnitude buffer

    RFFT_f32_sincostable(&rfft);       //Calculate twiddle factor
 
    for(i = 0; i < RFFT_SIZE; i++)
    {
      	RFFToutBuff[i] = 0;			   //Clean up output buffer
    }

    for(i = 0; i < RFFT_SIZE/2; i++) 
    {
     	RFFTmagBuff[i] = 0;		       //Clean up magnitude buffer
    }
}
float32 VoltRmsCalc(float32 *fdata, int16 n)
{
    int i;
    float32 *p = fdata;
    float32 tmp;
    float32 sum = 0;
    for(i = 0; i < n; i++)
    {
        tmp = *p;
        
        sum += tmp * tmp;

    }
    sum/=n;
    sum = __ffsqrtf(sum);
    return sum;
}
#define AD_K  0.06020924f
#define AD_B  (-6.361816f)
void AD2RealVal(int16 * psrc, float32 * pdst)
{
    *pdst = (*psrc) * AD_K + AD_B;
}
void cal_test(void)
{
    volatile float res;
    float32 s[5]={1.11,2.22,3.33,4.44,5.55};
    int t[5] = {1,2,3,4,5};
    res = s[0] * t[0];
    res = s[1] * s[2];
    res = VoltRmsCalc(s, 2);
    res = __ffsqrtf(3.0f); 
    res = sin(PI/4.0f);
}

void HarmonicAnalysis(_ChannelData *pd)
{
    int16 m, n, k, i;
    int16 pitch_i = 0;	 
    //int16 k = 0;
    float32 AmpliTemp = 0.0;
    float32 deltaK = 0.0;
    float32 deltaK2 = 0.0;
    float32 maxValue = 0.0;
    float32 maxValue2 = 0.0;
    float32 fretmp;

    
    for(m = 0; m < 23; m++)
    {
        pd->RealTimeData.HarmAmp[m] = 0.0;
        pd->RealTimeData.HarmFre[m] = 0.0;
    }
    for(m = 0; m < (RFFT_SIZE/2); m++)
    {
        if(RFFTmagBuff[m] > maxValue)
        {
            maxValue = RFFTmagBuff[m];
            pitch_i = m;
        }
    }
    pd->RealTimeData.HarmAmp[0] = 2 * RFFTmagBuff[0] / (RFFT_SIZE); //DC amp = A/N
    if(pitch_i == 0)
    {
        pd->RealTimeData.HarmFre[1] = 0.0;
    }
    else
    {
        if((RFFTmagBuff[pitch_i - 1] == 0) && (RFFTmagBuff[pitch_i + 1] == 0))
        {
            deltaK = 0.0;
        }
        else if(RFFTmagBuff[pitch_i - 1] <= RFFTmagBuff[pitch_i + 1])
        {
        //between HanMaxK and HanMaxK+1
        //deltaK=[2Y(K+1)-Y(K)]/[Y(K+1)+Y(K)]
            deltaK = (float32)(2 *RFFTmagBuff[pitch_i + 1] - RFFTmagBuff[pitch_i]) / (float32)(RFFTmagBuff[pitch_i + 1] + RFFTmagBuff[pitch_i]);
        }
        else
        {
            //deltaK=[Y(K)-2Y(K-1)]/[Y(K)+Y(K-1)]
            deltaK = (float32)(RFFTmagBuff[pitch_i] - 2 * RFFTmagBuff[pitch_i - 1]) / (float32)(RFFTmagBuff[pitch_i] + RFFTmagBuff[pitch_i - 1]);
        }
    } 

    //RealTimeData.HarmFre = ((pitch_i + deltaK)*(float)SAMPLE_F / (2 * RFFT_SIZE));
    pd->RealTimeData.HarmFre[1] = (pitch_i + deltaK)*3.9058f;
    if (deltaK <= 0)
    //delta linit +-0 and +-1
    {
        deltaK2 = 1 + deltaK; //deltaK2 > 0
        if(deltaK > (- 1e-4))
        {
        //deltaK limit -0
        //sinx=x x limit 0 , here use 1e-4
            AmpliTemp = 2 * maxValue;
        }
        else if (deltaK2 < 1e-4)
        {
        //deltaK limit -1
            AmpliTemp = 4 * maxValue;
        }
        else
        {
            AmpliTemp = 2 * PI * maxValue * deltaK / sin(PI *deltaK)*(1-deltaK * deltaK);
        }
         		
    }
    else//deltaK > 0
    {
        deltaK2 = 1-deltaK;
        if (deltaK < 1e-4)
        //deltaK limit +0
            AmpliTemp = 2 * maxValue;
        else if (deltaK2 < 1e-4)
        //deltaK limit +1
            AmpliTemp = 4 * maxValue;
        else
            AmpliTemp = 2 * PI * maxValue * deltaK / sin(PI *deltaK)*(1-deltaK * deltaK);
    }
    if(pd->RealTimeData.HarmFre[1] > 1e-3)
    {
        pd->RealTimeData.HarmAmp[1] = (1.4142 *AmpliTemp) / (RFFT_SIZE); //AC amp = 2*A/N
    }
    else
    {
        pd->RealTimeData.HarmAmp[1] = 0.0;
    }

    //j_step = 2*Frequence*RFFT_SIZE/SAMPLE_F;

    for (n = 2; n < 23; n++)
    {
        //pitch_i = pitch_i + 10;
        pitch_i = pitch_i + 10;
        //pitch_i = j_step*n-2;
        maxValue2 = RFFTmagBuff[pitch_i];
        if(pitch_i < (RFFT_SIZE / 2))
        {
            for(k = pitch_i; k < pitch_i + 4; k++)
            {
             
                if (RFFTmagBuff[k] > maxValue2)
                {
                    maxValue2 = RFFTmagBuff[k];
                    pitch_i = k;
                }
            }
            if((RFFTmagBuff[pitch_i - 1] == 0) && (RFFTmagBuff[pitch_i + 1] == 0))
            {
                deltaK = 0.0;
            }
            else if(RFFTmagBuff[pitch_i - 1] <= RFFTmagBuff[pitch_i + 1])
            {
                deltaK = (float)(2 *RFFTmagBuff[pitch_i + 1] - RFFTmagBuff[pitch_i]) / (float)(RFFTmagBuff[pitch_i + 1] + RFFTmagBuff[pitch_i]);
            }
            else
            {
                deltaK = (float)(RFFTmagBuff[pitch_i] - 2 * RFFTmagBuff[pitch_i - 1]) / (float)(RFFTmagBuff[pitch_i] + RFFTmagBuff[pitch_i - 1]);
            }
            
            if(deltaK <= 0)
            {
                deltaK2 = 1+deltaK;
                if (deltaK > (- 1e-4))
                {
                    AmpliTemp = 2 * maxValue2;
                }
                else if (deltaK2 < (1e-4))
                {
                    AmpliTemp = 4 * maxValue2;
                }
                else
                {
                    AmpliTemp = 2 * PI * maxValue2 * deltaK / sin(PI *deltaK)*(1-deltaK * deltaK);
                }
            }
            else
            {
                deltaK2 = 1-deltaK;
                if(deltaK < 1e-4)
                {
                    AmpliTemp = 2 * maxValue2;
                }
                else if(deltaK2 < 1e-4)
                {
                    AmpliTemp = 4 * maxValue2;
                }
                else
                {
                    AmpliTemp = 2 * PI * maxValue2 * deltaK / sin(PI *deltaK)*(1-deltaK * deltaK);
                }
            }
            fretmp = (pitch_i + deltaK)*3.9058f;
            if(pd->RealTimeData.HarmFre[1] < 40.0)
            {
                pd->RealTimeData.HarmAmp[n] = (1.4142 *AmpliTemp) / (RFFT_SIZE);
                if(pd->RealTimeData.HarmAmp[i] < 1e-1)
                {
                    pd->RealTimeData.HarmAmp[i] = 0.0;
                }
            }
            else
            {
                //fretmp += 0.5;
                i = (int16)(fretmp/pd->RealTimeData.HarmFre[1] + 0.5);
                if(i > 1 && i < 23)
                {
                    pd->RealTimeData.HarmAmp[i] = (1.4142 *AmpliTemp) / (RFFT_SIZE);
                    pd->RealTimeData.HarmFre[i] = fretmp;
                    if(pd->RealTimeData.HarmAmp[i] < 1e-1)
                    {
                        pd->RealTimeData.HarmAmp[i] = 0.0;
                        pd->RealTimeData.HarmFre[i] = 0.0;
                    }
                }
            }
        }
    }
    return ;
}
#if 0
RN(n)
Hamming 0.54－0.46*cos[2*PI*n/(N-1)]         0≤n≤N-1
Hanning 0.5*[1－cos[2*PI*n/(N-1)]]           0≤n≤N-1
Blackman 0.42-0.5*cos[2*PI*n/(N-1)]+0.08cos[4*PI*n/(N-1))*RN(n)
h(i)=sqrt(re(i)*re(i)+im(i)*im(i))
dBh(i)=20*log (h(i)) 
#endif
void HarmonicProcess(void)
{
    Uint16 i;
    
    _ChannelData *pd;

    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_HARMONIC) == 0)
            continue;
    
        for (i = 0; i < RFFT_SIZE; i++)
        {
            RFFTin1Buff[i] = (RFFTin1Buff[i] * HanningWin[i]) / 16384;

        }
        
        SysFFTCalc();
        HarmonicAnalysis(pd);

        #if FFTforFreqEn    
        if((pd->RealTimeData.HarmFre[1] > 45.0) && (pd->RealTimeData.HarmFre[1] < 55.0))
        {
            //SumFrequency = SumFrequency + RealTimeData.HarmFre;
        }
        else 
        {
            pd->RealTimeData.HarmFre[1] = 0;
        }
        #endif 
       
        //ESTOP0;
    	ElcPowerFreq = Fre2Queue(pd->RealTimeData.HarmFre[1]);
    }
}

void CalcFreq(void)
{
    static Uint16 j0 = 0, j1 = 0;
    int16 tempvar, i;
    _ChannelData *pd;
    

    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_FREQUENC) == 0)
            continue;
        if(pd->ZeroStart > 0)
        {
            if((pd->IIrForUout[0] >= 0) && (pd->IIrForUout[1] < 0))
            {
                if(pd->ZeroTimer == 0)
                {
                    pd->ZeroTimer = 1;
                    //ESTOP0;
                    //StartSample = 1;
                    pd->ZeroStart = ZERO_CNT_CYC+1;
                    //RealTimeData.Umax = 0;
                }
                pd->ZeroStart--;
                pd->PeriodSyn = 1;
            }
            if(pd->ZeroTimer)
            {
                pd->ZeroTimer++;
            }
        }
        else
        {
            //ESTOP0;
            if(i == 0)
            {
                j0++;
                if(j0%4 == 0)
                {
                    ESTOP0;
                }
            }
            else
            {
                j1++;
                if(j1%4 == 0)
                {
                    ESTOP0;
                }
            }
            if((pd->ZeroTimer > 14545) && (pd->ZeroTimer < 17777))
            {
                //PowerFreqPre = PowerFreq;
                //PowerFreq = 800000000 / (ZeroTimer-1);
                pd->RealTimeData.Frequency = 800000.0f/(pd->ZeroTimer);
                
                //ESTOP0;
                tempvar = 8000.0f / pd->RealTimeData.Frequency;
                tempvar = tempvar - QuasiWin;
                if ((tempvar > 3) || (tempvar <  - 3))
                {
                    QuasiWin = tempvar+QuasiWin;
                }
                
            }

            pd->ZeroStart = 1;
            pd->ZeroTimer = 0;
            //ZeroCounter = ZERO_CNT_CYC;
        }
    }
}
void Sample2Buff(void)
{
    static Uint16 div = 0;
    //static int16 LastTmp = 0, PreTmp = 0, flag = 1;
    static float32 Rad0 = 0, Rad1 = 0;
    float32 ftmp;
    int16 tmp, i;
    _ChannelData *pd;
    
    for(i = 0; i < CHN_NUM; i++)
    {
        
        pd = &ChannelData[i];
        
        #if 0
        ReadCpldInt(ADC_S1_REG+i, &tmp);
        /*(ABS(tmp-(LastTmp+PreTmp)/2) > 200)
        {
            //ESTOP0;
            if(flag == 0)
            {
                tmp = 2*LastTmp - PreTmp;
            }
            else
            {
                if(flag > 2)
                {
                    PreTmp = LastTmp = tmp;
                }
            }
            flag++;
        }
        else
        {
            PreTmp = LastTmp;
            LastTmp = tmp;
            flag = 0;
        }*/
        #if 0
        ViewBuffer0[pView] = tmp;
        if(++pView > 1999)
        {
            pView = 0;
            ftmp = CalcAvgInt(&ViewBuffer0[500], 1000);
            //CalcCeofFun();
            ESTOP0;
            
        }
        #endif
        /*if(tmp > 8860 || tmp < -8860) //bye-bye-le ^_^ filter the inlegel val
        {
            tmp = LastTmp;   
        }
        else
        {
            LastTmp = tmp;
        }*/
        
        AD2RealVal(&tmp, &ftmp);
        /*SamSum++;
        if(tmp > 2500 || tmp < -2500)
        {
            //ESTOP0;
            ErrSum++;
        }
        if(ErrSum > 10)
        {
            //ESTOP0;
        }*/
        #if 0
        ViewBuffer1[pView] = ftmp;
        if(++pView > 1999)
        {
            pView = 0;
            res = CalcAvgFloat(&ViewBuffer1[500], 1000);
            //CalcCeofFun();
            ESTOP0;
            
        }
        #endif

        #else
        
        if(i == 0)
        {
            Rad0 += 0.03927;
            //Rad0 += 0.03832743;//48.8Hz
            if(Rad0 > 2*PI)
            {
                Rad0 -= 2*PI;
            }
            //ftmp = 311.08f*sin(Rad)+70.7*sin(4*Rad)+56.56*sin(5*Rad)+42.42*sin(13*Rad)+28.28*sin(20*Rad)+10;
            ftmp = 311.127f*sin(Rad0);
        }
        else
        {
            Rad1 += 0.03927;
            //Rad1 += 0.041469;//52.8Hz
            if(Rad1 > 2*PI)
            {
                Rad1 -= 2*PI;
            }
            //ftmp = 7.071f*sin(Rad1+1.0472) /*+ 7.07*sin(8*Rad)*/;//60*
            ftmp = 7.071f*sin(Rad1-3.927);
        }
        #endif
        
        pd->VoltSmpBuf[pd->pVolt] = ftmp;
        
        if(tmp > pd->RealTimeData.Umax)
        {
            pd->RealTimeData.Umax = tmp;
        }
        if(tmp < pd->RealTimeData.Umin)
        {
            pd->RealTimeData.Umin = tmp;
        }
        pd->pVolt--;
        if(pd->pVolt <= -1)
        {
            pd->pVolt = SMP_NUM-1;
        }
        //VoltSmpBuf[pVolt] = WaveTmp;

        //pd++;
        if((pd->CalcConfig & CALC_HARMONIC) == 0)
            continue;
        if((div & BIT0) && (!FFTProc))
        {
            RFFTin1Buff[FFTSmpNum++] = ftmp;
            if(FFTSmpNum >= RFFT_SIZE)
            {
                FFTProc = 1;
                FFTSmpNum = 0;
            }
        }
    }
    div++;
}
void PhaCalForAc(void)
{
    int16 p;
    int16 i;
    _ChannelData *pd;
    
    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];

        p = pd->pVolt + 4;
        if(p > SMP_NUM-1)
        {
            p -= SMP_NUM;
        }
        pd->InstanVolt = pd->VoltSmpBuf[p];

        //pd++;
    }
}
void PhaCalForRe(void)
{
    int16 p0, p1, p2;
    float32 res1, res2;

    int16 i;
    _ChannelData *pd;
    
    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_POWER) == 0)
            continue;
        
        p0 = pd->PhaseInt + pd->pVolt + 3;
        if (p0 > SMP_NUM-1)
            p0 = p0 - SMP_NUM;
        p1 = pd->PhaseInt + pd->pVolt + 4;
        if (p1 > SMP_NUM-1)
            p1 = p1 - SMP_NUM;
        p2 = pd->PhaseInt + pd->pVolt + 5;
        if (p2 > SMP_NUM-1)
            p2 = p2 - SMP_NUM;

        res1 = pd->PhaseFloat2 * (pd->VoltSmpBuf[p2] + pd->VoltSmpBuf[p0] - 2 * pd->VoltSmpBuf[p1]);
        res2 = pd->PhaseFloat * (pd->VoltSmpBuf[p2] - pd->VoltSmpBuf[p0]);
        pd->ShiftPhsVolt = (res1 + res2) / 2 + pd->VoltSmpBuf[p1];

        //pd++;
    }
}

/*
Fs=8000Hz
fp=120Hz
rp=0.05
fs=1500Hz
As=0.02
=>
b0=b2=0.0061
b1=0.0122 
a1=-1.7672
a2=0.7916
*/

void IIRLpfVolt(void)
{
    int16 U_in;

    int16 i;
    _ChannelData *pd;
    
    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_FREQUENC) == 0)
            continue;
        U_in = pd->InstanVolt;
      

        pd->IIrForUout[1] = pd->IIrForUout[0];
        pd->IIrForU[2] = pd->IIrForU[1];
        pd->IIrForU[1] = pd->IIrForU[0];

        pd->IIrForU[0] = U_in - ((IIRLpfA1_U)*(pd->IIrForU[1]) + IIRLpfA2_U * (pd->IIrForU[2]));
        pd->IIrForUout[0] = IIRLpfB0_U *(pd->IIrForU[0] + 2 * pd->IIrForU[1] + pd->IIrForU[2]);

        //pd++;
    }

}

void IIRLpfPower(void)
{
    //y(i)=b0*x(i)+b1*x(i-1)+b2*x(i-2)-a1*y(i-1)-a2*y(i-2)
    // =b0{x(i)+x(i-2)+2x(x-1)}-a1*y(i-1)-a2*y(i-2)		
    float ftmp;

    int16 i;
    _ChannelData *pd;
    
    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_POWER) == 0)
            continue;
        ftmp = pd->InstanVolt * pd->InstanCurr;

        pd->IIrForP[2] = pd->IIrForP[1];
        pd->IIrForP[1] = pd->IIrForP[0];
        pd->IIrForP[0] = ftmp - (IIRLpfA1 *pd->IIrForP[1] + IIRLpfA2 * pd->IIrForP[2]) / 8192; //w延迟器
        pd->AcPowerTmp = IIRLpfB0 *(pd->IIrForP[0] + 2 * pd->IIrForP[1] + pd->IIrForP[2]) / 32768;

        ftmp = pd->ShiftPhsVolt * pd->InstanCurr;

        pd->IIrForQ[2] = pd->IIrForQ[1];
        pd->IIrForQ[1] = pd->IIrForQ[0];
        pd->IIrForQ[0] = ftmp - (IIRLpfA1 *pd->IIrForQ[1] + IIRLpfA2 * pd->IIrForQ[2]) / 8192;
        pd->RePowerTmp = IIRLpfB0 *(pd->IIrForQ[0] + 2 * pd->IIrForQ[1] + pd->IIrForQ[2]) / 32768;

        //pd++;
    }
 
}

void QuasiFunc(void)
{
    float32 ftmp;
    //int32 tmp;
    int16 i;
    _ChannelData *pd;
    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_URMS) == 0)
            continue;
        
        if(pd->PeriodSyn == 1)
        {
            //tmp = CurrVolt;
            pd->Urms2Sum += pd->InstanVolt * pd->InstanVolt;
            //ViewBuffer1[Urms2Num] = CurrVolt;
            //Urms2Tmp += tmp*tmp;
            pd->AcPowSum += pd->AcPowerTmp;
            pd->RePowSum += pd->RePowerTmp;
            pd->Urms2Num++;
            //VoltSmpBufTmp0[SampleNum] = CurrVolt;
            
            
            if(pd->Urms2Num == pd->CalcLen)
            {
                pd->UpdataFlg = 1;
                //if(i == 0)
                    //ESTOP0;
                #if 0
                ftmp = pd->Urms2Sum/(float32)(pd->Urms2Num);
                ftmp = __ffsqrtf(ftmp);
                pd->RealTimeData.Urms = ftmp;
                pd->RealTimeData.AcPower = pd->AcPowSum/pd->Urms2Num;
                pd->RealTimeData.RePower = pd->RePowSum/pd->Urms2Num;
                pd->RealTimeData.AcPower = ABS(pd->RealTimeData.AcPower);
                pd->RealTimeData.RePower = ABS(pd->RealTimeData.RePower);
                ftmp = pd->RealTimeData.AcPower * pd->RealTimeData.AcPower
                    + pd->RealTimeData.RePower * pd->RealTimeData.RePower;
                pd->RealTimeData.ApPower = __ffsqrtf(ftmp);
                pd->RealTimeData.PowerFactor = pd->RealTimeData.AcPower/pd->RealTimeData.ApPower;
                pd->RealTimeData.PhaseAngle = acos(pd->RealTimeData.PowerFactor) * 57.29578f;
                
                pd->Urms2Num = 0;
                pd->Urms2Sum = 0;
                pd->AcPowSum = 0;
                pd->RePowSum = 0;
                pd->PeriodSyn = 0;
                //ESTOP0;
                #endif
            }
        }
    }
}
void UpdatePara2Freq(void)
{
    int16 i;
    float32 ftmp;
    _ChannelData *pd;
    
    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if((pd->CalcConfig & CALC_POWER) == 0)
            continue;
        if((pd->RealTimeData.Frequency > 45.0) && (pd->RealTimeData.Frequency < 55.0))
        {
            //shift90=Fs/f/4
            ftmp = (SAMPLE_F/4.0f) / pd->RealTimeData.Frequency;
            pd->PhaseInt = (int16)(ftmp);
            pd->PhaseFloat = ftmp - pd->PhaseInt;  
            pd->PhaseFloat2 = pd->PhaseFloat * pd->PhaseFloat;
        }
        pd->CalcLen = 160*3;

        //pd++;
    }
}

void SampleIsrProcess(void)
{
    Sample2Buff();
    PhaCalForAc();
    IIRLpfVolt();
    CalcFreq();
    PhaCalForRe();
    ChnConcate();
    IIRLpfPower();
    QuasiFunc();
}
float32 CalcAvgInt(int16 *p, int16 n)
{
    float32 res = 0;
    int16 i;
    int16 *pdata = p;
    for(i = 0; i < n; i++)
    {
        res += *pdata++;
    }
    res /= n;
    return res;
}

float32 CalcAvgFloat(float32 *p, int16 n)
{
    float32 res = 0;
    int16 i;
    float32 *pdata = p;
    for(i = 0; i < n; i++)
    {
        res += *pdata++;
    }
    res /= n;
    return res;
}

void ChnConcate(void)
{
    ChannelData[0].InstanCurr = ChannelData[1].InstanVolt;
}

void CalcChnInit(void)
{
    int16 i;
    _ChannelData *pd;
    

    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        MemSet((Uint16 *)pd, 0, sizeof(_ChannelData));
        pd->pVolt = SMP_NUM-1;
        pd->ZeroStart = ZERO_CNT_CYC+1;
        pd->PhaseInt = 40;
        pd->CalcLen = 160*3;
        pd->FreqChang = 1;
        pd->CalcConfig = CALC_POWER | CALC_FREQUENC | CALC_URMS;
    }
    pd = &ChannelData[1];
    pd->CalcConfig = 0;
    //pd->CalcFlg = CALC_POWER | CALC_URMS;
}
void CalcExec(void)
{
    int i;
    int acdir, redir, q;
    float32 ftmp;
    _ChannelData *pd;
    

    for(i = 0; i < CHN_NUM; i++)
    {
        pd = &ChannelData[i];
        if(pd->UpdataFlg == 1)
        {
            acdir = 0;
            redir = 0;
            ftmp = pd->Urms2Sum/(float32)(pd->Urms2Num);
            ftmp = __ffsqrtf(ftmp);
            pd->RealTimeData.Urms = ftmp;
            pd->RealTimeData.AcPower = pd->AcPowSum/pd->Urms2Num;
            pd->RealTimeData.RePower = pd->RePowSum/pd->Urms2Num;
            //pd->RealTimeData.AcPower = ABS(pd->RealTimeData.AcPower);
            //pd->RealTimeData.RePower = ABS(pd->RealTimeData.RePower);
            ftmp = pd->RealTimeData.AcPower * pd->RealTimeData.AcPower
                + pd->RealTimeData.RePower * pd->RealTimeData.RePower;
            pd->RealTimeData.ApPower = __ffsqrtf(ftmp);
            pd->RealTimeData.PowerFactor = pd->RealTimeData.AcPower/pd->RealTimeData.ApPower;
            pd->RealTimeData.PhaseAngle = acos(ABS(pd->RealTimeData.PowerFactor)) * 57.29578f;
            if(pd->RealTimeData.AcPower < 0)
                acdir = 1;
            if(pd->RealTimeData.RePower < 0)
                redir = 1;
            q = 2 * redir + acdir;
            q = Quadrant[q];
            pd->RealTimeData.PhaseAngle += q;
            pd->Urms2Num = 0;
            pd->Urms2Sum = 0;
            pd->AcPowSum = 0;
            pd->RePowSum = 0;
            

            pd->UpdataFlg = 0;
            
            pd->PeriodSyn = 0;
            
        }
        if(pd->FreqChang == 1)
        {
            UpdatePara2Freq();
            pd->FreqChang = 0;
        }
    }
}
#if 0
void IIRTst(void)
{
    Uint16 j;
    float32 fr = 50.0;

    float32 RadStep; //  2*pi*WAVE_F/SAMPLE_F
    float32 Rad;
    Uint16 p = 0;
    ESTOP0;
    RadStep = 2.0*PI*fr / 8000.0f;
    Rad = 0.0;
    //ZeroStart = ZERO_CNT_CYC+1;
    //ZeroTimer = 0;
    //pVolt = 59;
    for(j = 0; j < 60000; j++)
    {
        #if 0
        for(i = 0; i < 2000; i++)
        {
            VoltSmpBufTmp0[i] = 200*sin((2.0f*PI*fr*((float32)i))/8000.0f)/*+50*sin((2.0f*PI*2*fr*((float32)i))/8000.0f)*/;
           
        }
        #else
        if(j >= 50000)
        {
            j = 0;
        }
        Rad += RadStep;
        if(Rad > 2*PI)
        {
            Rad -= 2*PI;
        }
        WaveTmp = 50*sin(Rad) /*+ 50*sin(5*RadStep*j)*/;
        //VoltSmpBufTmp0[p] = IIRLpf_U_out[0];
        //VoltSmpBufTmp1[p] = WaveTmp;
        p++;
        p %= 2000;
        if(p == 0)
        {
            //ESTOP0;
        }
        SampleIsrProcess();
        #endif
    }
    ESTOP0;
}
#endif

measure.h

#ifndef __MEASURE_H_
#define __MEASURE_H_

#define SMP_NUM    60
#define CHN_NUM    2

typedef struct
{
    float32 Urms;
    float32 Frequency;
    float32 HarmAmp[23];
    float32 HarmFre[23];
    float32 Umax;
    float32 Umin;
    float32 AcPower;
    float32 RePower;
    float32 ApPower;
    float32 PowerFactor;
    float32 PhaseAngle;
    
}_RealTimeData;


typedef struct
{
    float32 VoltSmpBuf[SMP_NUM];
    float32 InstanVolt;
    float32 InstanCurr;
    float32 ShiftPhsVolt;
    float32 Urms2Sum;
    float32 AcPowSum;
    float32 RePowSum;
    float32 AcPowerTmp;
    float32 RePowerTmp;
    float32 PhaseFloat;
    float32 PhaseFloat2;
    float32 IIrForP[3];
    float32 IIrForQ[3];

    int32 IIrForU[3];
    int16 IIrForUout[2];
    int16 pVolt;
    int16 Urms2Num;
    int16 PhaseInt;
    int16 CalcLen;
    
    int16 ZeroTimer;
    volatile int16 FreqChang;
    volatile int16 UpdataFlg;
    volatile int16 ZeroStart;
    volatile int16 PeriodSyn;
    volatile Uint16 CalcConfig;

    _RealTimeData RealTimeData;
    
}_ChannelData;

typedef struct
{
    float32 u1;
    float32 ad1;
    float32 u2;
    float32 ad2;
    float32 k;
    float32 b;
}_CalcCeof;

//extern _RealTimeData RealTimeData;
//extern _ChannelData ChannelData[CHN_NUM];
extern Uint16 HanningWin[1024];
//extern int16 pVolt;
//extern float32 InstanVolt;

//extern volatile int16 PeriodSyn;
extern volatile int16 FFTProc;
extern volatile int16 FFTSmpNum;

float32 VoltRmsCalc(float32 *fdata, int16 n);
void cal_test(void);
float32 Fre2Queue(Uint16 val);
float32 CalcAvgInt(int16 *p, int16 n);
float32 CalcAvgFloat(float32 *p, int16 n);
void Measure_Test(void);
void SysFFTCalc(void);
void SysFFTInit(void);
void AD2RealVal(int16 * psrc, float32 * pdst);
void FFT2Fun(void);
void HarmonicAnalysis(_ChannelData *pd);
void HarmonicProcess(void);
void MakeWave(void);
void CalcFreq(void);
void Sample2Buff(void);
void PhaCalForAc(void);
void IIRLpfVolt(void);
void QuasiFunc(void);
void UpdatePara2Freq(void);
void SampleIsrProcess(void);
void CalcChnInit(void);
void ChnConcate(void);
void CalcExec(void);
void IIRTst(void);


#endif