calculate.c

此代码应用与f2812的通讯程序!!方便用户编程!

#include "Device.h"

#pragma DATA_SECTION(fft, "Var");
CFFT32  fft = CFFT32_32P_DEFAULTS;

//FFT计算三相电压,三相电流
void CalcAll(void)
{  
	Calc_Complex(ID_Yc_Ua, ID_Yc_Ub, &Yc_Temp[ID_Yc_Ua], &Yc_Temp[ID_Yc_Ub],
		EEPROM_Data[ID_PARM_K_Ua], EEPROM_Data[ID_PARM_K_Ub]);

	Calc_Complex(ID_Yc_Uc, ID_Yc_I1a, &Yc_Temp[ID_Yc_Uc], &Yc_Temp[ID_Yc_I1a], 
		EEPROM_Data[ID_PARM_K_Uc], EEPROM_Data[ID_PARM_K_I1a]);	
	
	Calc_Complex(ID_Yc_I1b, ID_Yc_I1c, &Yc_Temp[ID_Yc_I1b], &Yc_Temp[ID_Yc_I1c],
		EEPROM_Data[ID_PARM_K_I1b], EEPROM_Data[ID_PARM_K_I1c]);

	Calc_Complex(ID_Yc_I0, ID_Yc_I2a, &Yc_Temp[ID_Yc_I0], &Yc_Temp[ID_Yc_I2a],
		EEPROM_Data[ID_PARM_K_I0], EEPROM_Data[ID_PARM_K_I2a]);

	Calc_Complex(ID_Yc_I2b, ID_Yc_I2c, &Yc_Temp[ID_Yc_I2b], &Yc_Temp[ID_Yc_I2c], 
		EEPROM_Data[ID_PARM_K_I2b], EEPROM_Data[ID_PARM_K_I2c]);
}

//计算三相有无功率
void GetUIPQ() 
{   
	GetPQ(ID_Yc_Ua,ID_Yc_I1a, ID_Yc_P1a, ID_Yc_Q1a, ID_K_PQ1_A);
	GetPQ(ID_Yc_Ub,ID_Yc_I1b, ID_Yc_P1b, ID_Yc_Q1b, ID_K_PQ1_B);
	GetPQ(ID_Yc_Uc,ID_Yc_I1c, ID_Yc_P1c, ID_Yc_Q1c, ID_K_PQ1_C);

	GetPQ(ID_Yc_Ua,ID_Yc_I2a, ID_Yc_P2a, ID_Yc_Q2a, ID_K_PQ2_A);
	GetPQ(ID_Yc_Ub,ID_Yc_I2b, ID_Yc_P2b, ID_Yc_Q2b, ID_K_PQ2_B);
	GetPQ(ID_Yc_Uc,ID_Yc_I2c, ID_Yc_P2c, ID_Yc_Q2c, ID_K_PQ2_C);
	
	Yc_Temp[ID_Yc_P1z] = Yc_Temp[ID_Yc_P1a] + Yc_Temp[ID_Yc_P1b] + Yc_Temp[ID_Yc_P1c];//P1z
	Yc_Temp[ID_Yc_Q1z] = Yc_Temp[ID_Yc_Q1a] + Yc_Temp[ID_Yc_Q1b] + Yc_Temp[ID_Yc_Q1c];//Q1z
	Yc_Temp[ID_Yc_P2z] = Yc_Temp[ID_Yc_P2a] + Yc_Temp[ID_Yc_P2b] + Yc_Temp[ID_Yc_P2c];//P2z
	Yc_Temp[ID_Yc_Q2z] = Yc_Temp[ID_Yc_Q2a] + Yc_Temp[ID_Yc_Q2b] + Yc_Temp[ID_Yc_Q2c];//Q2z

}


//
void DoYcSum()
{  
 /*	int    i ,*pYc;
	long   *pYcSum; 
	
  	//int f_phase;
	//long a, b;
	pYc    = Yc_Temp;
	pYcSum = Yc_Sum ;
		
	for(i = 0; i < YC_NUM; i++)
	{
		if (YcTab[i] == -1) break;
		*(pYcSum + YcTab[i]) += *(pYc + YcTab[i]);
	}

	sumi++;

	if(sumi == 16)
	{
		sumi   = 0;
		pYc    = Yc_Ave;
		pYcSum = Yc_Sum;
		
		for(i = 0; i < 65; i++)
		{
			if (YcTab[i] == -1) break;
			if (YcTab[i] == ID_Yc_Fre) continue;
			
			*(pYc + YcTab[i]) = *(pYcSum + YcTab[i])>>4;//求平均（除16）
			*(pYcSum + YcTab[i]) = 0;//清零			
		}
*/		
		
/*
		//===============相位补偿=========================
		//相位补偿公式  
		//(P+iQ)Exp(iA) = (P + iQ)*(CosA + iSinA) 
		//              = (P*CosA - Q*SinA) + i(P*SinA+Q*CosA)/
		//================================================
		for (i = 0; i < 4; i++)
		{
			f_phase = K_Phase[i] + 100;
			if (f_phase > 200)  f_phase = 200;//phase =  100;
			if (f_phase < 0)    f_phase = 0;  //phase = -100;
			
			a = ((long)Yc_Ave[ID_Yc_P1a+i]*CosTab[f_phase] - (long)Yc_Ave[ID_Yc_Q1a+i]*SinTab[f_phase]) >> 14;
			b = ((long)Yc_Ave[ID_Yc_P1a+i]*SinTab[f_phase] + (long)Yc_Ave[ID_Yc_Q1a+i]*CosTab[f_phase]) >> 14;
			Yc_Ave[ID_Yc_P1a+i] = a;
			Yc_Ave[ID_Yc_Q1a+i] = b;
		}
		//第二路
		for (i = 0; i < 4; i++)
		{
			f_phase = K_Phase[i+4] + 100;
			if (f_phase > 200)  f_phase = 200;//phase =  100;
			if (f_phase < 0)    f_phase = 0;  //phase = -100;
			
			a = (long)(Yc_Ave[ID_Yc_P2a+i]*CosTab[f_phase] - (long)Yc_Ave[ID_Yc_Q2a+i]*SinTab[f_phase]) >> 14;
			b = (long)(Yc_Ave[ID_Yc_P2a+i]*SinTab[f_phase] + (long)Yc_Ave[ID_Yc_Q2a+i]*CosTab[f_phase]) >> 14;
			Yc_Ave[ID_Yc_P2a+i] = a;
			Yc_Ave[ID_Yc_Q2a+i] = b;
		}
		*/

//	}
}

void DDcount()
{   
	//DD_ONE   3,60,000L
  //  static int  bcdd = 0;      //  保存电度标志 
	
	if( Yc_Temp[ID_Yc_P1z] > 10)//第一路正向有功
	{	
		DDu_Temp[ID_DD_DPP1] += Yc_Temp[ID_Yc_P1z];
		if( DDu_Temp[ID_DD_DPP1] > DD_ONE )
		{
			DDu[ID_DD_DPP1]++;
			DDu_Temp[ID_DD_DPP1] -= DD_ONE;
		//	bcdd=1;
		}		
	}
	
	if( Yc_Temp[ID_Yc_P1z] < -10)//第一路反向有功
	{	
		DDu_Temp[ID_DD_DPN1] -= Yc_Temp[ID_Yc_P1z];
		if( DDu_Temp[ID_DD_DPN1] > DD_ONE )
		{
			DDu[ID_DD_DPN1]++;
			DDu_Temp[ID_DD_DPN1] -= DD_ONE;
		//	bcdd=1;
		}		
	}
	
    if(Yc_Temp[ID_Yc_P2z] > 10)//第二路正向有功
	{	
		DDu_Temp[ID_DD_DPP2] += Yc_Temp[ID_Yc_P2z];
		if( DDu_Temp[ID_DD_DPP2] > DD_ONE )
		{
			DDu[ID_DD_DPP2]++;
			DDu_Temp[ID_DD_DPP2] -= DD_ONE;
		//	bcdd=1;
		}		
	}
	
	if(Yc_Temp[ID_Yc_P2z] < -10)//第二路反向有功
	{	
		DDu_Temp[ID_DD_DPN2] -=Yc_Temp[ID_Yc_P2z];
		if(DDu_Temp[ID_DD_DPN2] > DD_ONE)
		{
			DDu[ID_DD_DPN2]++;
			DDu_Temp[ID_DD_DPN2] -= DD_ONE;
		//	bcdd=1;
		}
	}
	
	if(Yc_Temp[ID_Yc_Q1z] > 10)//第一路正向无功
	{
		DDu_Temp[ID_DD_DQP1] += Yc_Temp[ID_Yc_Q1z];
		if( DDu_Temp[ID_DD_DQP1]> DD_ONE)
		{
			DDu[ID_DD_DQP1] ++;
			DDu_Temp[ID_DD_DQP1] -= DD_ONE;
		//	bcdd=1;
		}
	}
	if(Yc_Temp[ID_Yc_Q1z] < -10)//第一路反向无功
	{
		DDu_Temp[ID_DD_DQN1] -= Yc_Temp[ID_Yc_Q1z];
		if( DDu_Temp[ID_DD_DQN1] > DD_ONE)
		{
			DDu[ID_DD_DQN1] ++;
			DDu_Temp[ID_DD_DQN1] -= DD_ONE;
		//	bcdd=1;
		}
	}
	
	if(Yc_Temp[ID_Yc_Q2z] > 10)//第二路正向无功
	{
		DDu_Temp[ID_DD_DQP2] += Yc_Temp[ID_Yc_Q2z];
		if( DDu_Temp[ID_DD_DQP2]> DD_ONE)
		{
			DDu[ID_DD_DQP2] ++;
			DDu_Temp[ID_DD_DQP2] -= DD_ONE;
		//	bcdd=1;
		}
	}
	if(Yc_Temp[ID_Yc_Q2z] < -10)//第二路反向无功
	{
		DDu_Temp[ID_DD_DQN2] -= Yc_Temp[ID_Yc_Q2z];
		if( DDu_Temp[ID_DD_DQN2] > DD_ONE)
		{
			DDu[ID_DD_DQN2] ++;
			DDu_Temp[ID_DD_DQN2] -= DD_ONE;
		//	bcdd=1;
		}
	}	
}


void Calc_Complex(int chn_1, int chn_2, int* pResult1_xb1, int* pResult2_xb1, int k1, int k2)
{
	long a,b,resSqrt; 
	  
	fft.ipcbptr = fri;
	fft.init(&fft);

	CFFT32_brev2(Ach[chn_1], fri, N/2);
	CFFT32_brev2(Ach[chn_2], fri+1, N/2);  //PRE-HALF OF INPUT
	CFFT32_brev2(Ach[chn_1]+1, fri+N, N/2);
	CFFT32_brev2(Ach[chn_2]+1, fri+N+1, N/2);  //BACK-HALF OF INPUT
	
	//fft运算及谱分析
	fft.calc(&fft);
	
	LineComplex[chn_1][REAL] = (int)(fri[2] + fri[62]) >> 1;//fr[k]=fri[2*K];fi[k]=fri[2*K+1];
	LineComplex[chn_1][IMAG] = (int)(fri[63]- fri[3] ) >> 1;
	LineComplex[chn_2][REAL] = (int)(fri[3] + fri[63]) >> 1;
	LineComplex[chn_2][IMAG] = (int)(fri[2] - fri[62]) >> 1; 
	
	//调零
	if(LineComplex[chn_1][REAL] > -100 && LineComplex[chn_1][REAL] < 100) LineComplex[chn_1][REAL] = 0;
	if(LineComplex[chn_1][IMAG] > -100 && LineComplex[chn_1][IMAG] < 100) LineComplex[chn_1][IMAG] = 0;
	if(LineComplex[chn_2][REAL] > -100 && LineComplex[chn_2][REAL] < 100) LineComplex[chn_2][REAL] = 0;
	if(LineComplex[chn_2][IMAG] > -100 && LineComplex[chn_2][IMAG] < 100) LineComplex[chn_2][IMAG] = 0;
	
	a = (long)LineComplex[chn_1][REAL] * LineComplex[chn_1][REAL];
	b = (long)LineComplex[chn_1][IMAG] * LineComplex[chn_1][IMAG];
	resSqrt = qsqrt(a + b);
	*pResult1_xb1 = (resSqrt * k1) >> 13;
	

	a = (long)LineComplex[chn_2][REAL] * LineComplex[chn_2][REAL];
	b = (long)LineComplex[chn_2][IMAG] * LineComplex[chn_2][IMAG];
	resSqrt = qsqrt(a + b);	
	*pResult2_xb1 = (resSqrt * k2) >> 13;
	
	return ;
}

void GetPQ(int chn_1, int chn_2, int ID_YcP, int ID_YcQ, int ID_K)
{
	long a, b;
	long p, q;
	
	//CALCULATE P
	a = (long)LineComplex[chn_1][REAL] * LineComplex[chn_2][REAL];
	b = (long)LineComplex[chn_1][IMAG] * LineComplex[chn_2][IMAG];
	a >>= 8;
	b >>= 8;
	p = (((a+b)>>2)*K_PQ[ID_K])>>12; //功率系数已经在开始已经移3位
	//p /=10;

	//CALCULATE Q
	a = (long)LineComplex[chn_1][REAL] * LineComplex[chn_2][IMAG];
	b = (long)LineComplex[chn_2][REAL] * LineComplex[chn_1][IMAG];
	a >>= 8;
	b >>= 8;
	q = (((a-b)>>2)*K_PQ[ID_K])>>12;
//	q /=10;
	
	Yc_Temp[ID_YcP] = p;
	Yc_Temp[ID_YcQ] = q;
}

/*
void AdjPhase(int group)
{
	int *pYcP,  *pYcQ, *pPhase;
	int i, j, temp1, temp2;
	long a, b;
	long p,q;    
	
	pYcP   = &Yc_Ave[ID_Yc_P1a] + 14*group;
	pYcQ   = &Yc_Ave[ID_Yc_Q1a] + 14*group;
	pPhase = &Yc_Parm_Phase[0] + 4*group;
	
	for(j = 0; j < 4; j++)
	{
		p = *pYcP;
		q = *pYcQ;
		//Auto Adjust Phase
		for(i = 0; i < SIN_NUM; i++)
		{
			temp1 = p - q;
			a = (p*CosTab[i] - q*SinTab[i]) >> 14;
			b = (p*SinTab[i] + q*CosTab[i]) >> 14;
			temp2 = a - b;
			
			if (temp1 < DIM && temp1 > -DIM)
			{
				*pPhase = 0;
				break;
			}
			if(temp1*temp2<=0 && temp2<=DIM && temp2>=-DIM)
			{
				*pPhase = i - 100;
				*pYcP = a;
				*pYcQ = b;
				break;
			}
		}
		pYcP++;
		pYcQ++;
		pPhase++;
	}
}*/
//======================== NO MORE ========================