dsPIC6014A写的电网谐波计算程序

/*--------------------------DFT.c---------------------------------*/
//Function:DFT电网谐波分析程序
//Time:20220512
//Generator:
/*----------------------------------------------------------------*/
#include "p30f6014a.h"
#include "sample.h"
#include "math.h"
#include "ModBus.h"

#define TRUE 1
#define FAILURE 0
#define PI  3.1415926
#define VCONST  181
#define ICONST  11149
#define Vconst   (237.0/4601.0)
#define Iconst   (0.53/901.0)
#define Pconst   (107.16*0.99/6819597.0) 

#define RUNLED    PORTDbits.RD11

static int cx=0;

unsigned char temp2[80];

float Va_dft[32],Vb_dft[32],Vc_dft[32],\
      Ia_dft[32],Ib_dft[32],Ic_dft[32];

unsigned char Va_dft_pu[32],Vb_dft_pu[32],Vc_dft_pu[32],\
              Ia_dft_pu[32],Ib_dft_pu[32],Ic_dft_pu[32];

unsigned int Va_rms,Vb_rms,Vc_rms,Ia_rms,Ib_rms,Ic_rms;

unsigned char Va_THD,Vb_THD,Vc_THD,Ia_THD,Ib_THD,Ic_THD;

float Pa,Pb,Pc,cosFA,cosFB,cosFC;
float Sa,Sb,Sc;


/*----------------SIN(),COS(),0~90Degree Value Table---------*/
/*0000000, 2pi/64, 4pi/64, 6pi/64*/
/* 8pi/64,10pi/64,12pi/64,14pi/64*/
/*16pi/64,18pi/64,20pi/64,22pi/64*/
/*24pi/64,26pi/64,28pi/64,30pi/64*/
/*32pi/64                        */
/*
float SinTable[17]={0.0000,0.0980,0.1951,0.2903,\     
                    0.3827,0.4714,0.5556,0.6444,\     
                    0.7071,0.7730,0.8315,0.8819,\     
                    0.9239,0.9569,0.9808,0.9952,\     
                    1.0000 };*/
signed long SinTable[17]={0,100,200,297,\
                          392,483,569,660,\
                          724,792,851,903,\
                          946,980,1004,1019,\
                          1024};
/*
float CosTable[17]={1.0000,\
                    0.9952,0.9808,0.9569,0.9239,\
                    0.8819,0.8315,0.7730,0.7071,\
                    0.6444,0.5556,0.4714,0.3827,\
                    0.2903,0.1951,0.0980,0.0000 };*/

signed long CosTable[17]={1024,\
                          1019,1004,980,946,\
                          903,851,792,724,\
                          660,569,483,392,\
                          297,200,100,0};

signed long  FindSinTable(int n)
{
 while(n>=64)//角度归算到2pi范围内
    {n=n-64;}
 if(n<=16)//if in Scale 0~90d
   { return(SinTable[n]);}
 if((n>16)&&(n<=32))//if in Scale 90~180 deg.
   {return (SinTable[32-n]);}
 if((n>32)&&(n<=48))//if in Scale 180~270 deg.
   {return(-SinTable[n-32]);}
 if(n>48)//if in Scale 270~360 deg. 
   {return(-SinTable[64-n]);}
 }

signed long FindCosTable(int m)
{
 while(m>=64){m=m-64;}//Make in Scale in 360 Deg.
 if(m<=16)//if in Scale 0~90 Deg.
   {return(CosTable[m]);}
 if((m>16)&&(m<=32))//if in Scale 90~180 deg.
   {return(-CosTable[32-m]);}
 if((m>32)&&(m<=48))//if in Scale 180~270 deg.
   {return(-CosTable[m-32]);}
 if(m>48)//if in Scale 270~360 deg. 
   {return(CosTable[64-m]);}
 }
//-----------------sqrt of long int------------------------
//---------------------------------------------------------
signed long sqrt_int(signed long x)
{
 signed long temp=0;
 signed char i;
 for(i=15;i>=0;i--)
 {
  temp=temp|(((signed long)1)<<i);
  if((temp*temp)>x)
    {temp=temp&(~(((signed long)1)<<i));}
  }
 return (temp);
 }

//------------------64点 DFT 通用程序----------------------   
//程序执行时间：136ms
//调用方式：传址调用
//输入：sample[64]  64点采样样本
//输出：1.dft_pu[32]  32点谐波百分值，dft_pu[0]为DC分量，不计
//      2.返回值为基波成分，用于计算基波有效值  
//------------------------------------------------------------

float DFT64(signed long sample[64],unsigned char dft_pu[32])
{
 unsigned char i,j;
 signed long Xreal,Ximp;
 unsigned char count;
 signed long  dft[32];

  for(i=1;i<32;i++)//计算谐波分量的有效值，未标定值
   {
    Xreal=0;Ximp=0;
     for(j=0;j<64;j++)
     {
      count=i*j;
      //Xre=Xre+Ic[j]*cos(2*PI*i*j/64.0);
      Xreal=Xreal+sample[j]*FindCosTable(count);
      //  Xreal=Xreal+__builtin_mulsu(sample[j],FindCosTable(count));
      //Xreal=Xreal+((((sample[j]-sample[32+j])<<12)*FindCosTable(count))>>12);
      //Xim=Xim+Ic[j]*sin(2*PI*i*j/64.0);
      Ximp=Ximp+sample[j]*FindSinTable(count);
       
      // Ximp=Ximp+__builtin_mulsu(sample[j],FindSinTable(count));
      //Ximp=Ximp+((((sample[j]-sample[32+j])<<12)*FindSinTable(count))>>12);
      }
    Xreal=Xreal>>10;
    Ximp=Ximp>>10;
    Nop();
    //dft[i]=sqrt(((float)Xreal)*((float)Xreal)+((float)Ximp)*((float)Ximp));
    dft[i]=sqrt_int(Xreal*Xreal+Ximp*Ximp);
    Nop();
    }
  Nop();
  for(i=2;i<32;i++)//计算谐波分量的百分值
   {
    dft_pu[i]=(unsigned char)((dft[i]*100)/dft[1]);
    }
  Nop();
  dft_pu[1]=100;
  return(dft[1]);
  }

//----------------------------------------------------------
//THD Compute Function
//----------------------------------------------------------

unsigned char THD_compute(unsigned char dft_pu[32])
{
 unsigned char i;
 float THD_sum;
 float temp;
 THD_sum=0;
 for(i=2;i<32;i++)
   {THD_sum=THD_sum+dft_pu[i]*dft_pu[i];}
 temp=sqrt(THD_sum);
 i=temp;
 Nop();
 return(i);
 }

/**********************************************************/
//三相功率Pa，Pb，Pc，cosFa,cosFb,cosFc计算
/***********************************************************/

void P_PF_compute(signed long V[64],signed long I[64],float *P,int *Vrms,int *Irms,float *PF)
{
 float sum_avg,S_temp,P_temp,Q_temp;
 signed long v_head[64];
 int temp;
 unsigned char i;
 //电压有效值的计算
 sum_avg=0;
 for(i=0;i<64;i++)                                                                      
  {
   sum_avg=sum_avg+V[i];
   }
  sum_avg=sum_avg/64.0;
 for(i=0;i<64;i++)
  {
   V[i]=V[i]-sum_avg;//去掉直流分量
   }
 sum_avg=0;
 for(i=0;i<64;i++)
 {
  sum_avg=sum_avg+V[i]*V[i];
  }
 temp=sqrt(sum_avg);
 Nop();
 *Vrms=sqrt(sum_avg)*Vconst;
 //--------------------------------------
 //电流有效值的计算
 sum_avg=0;
 for(i=0;i<64;i++)
  {
   sum_avg=sum_avg+I[i];
   }
 sum_avg=sum_avg/64.0;
 for(i=0;i<64;i++)
 {
  I[i]=I[i]-sum_avg;//去掉直流分量
  }
 
 sum_avg=0;
 for(i=0;i<64;i++)
 {
  sum_avg=sum_avg+I[i]*I[i];
  }
 temp=sqrt(sum_avg);
 Nop();
 *Irms=sqrt(sum_avg)*Iconst*100;
//-------------------------------------------
//功率因数的计算
 Q_temp=0;
 for(i=0;i<64;i++)
 {
  Q_temp=Q_temp+V[i]*I[i];
  }
 //Convert Vbc to Va (head 90 Degree) 

 for(i=0;i<48;i++)
  {v_head[i]=V[i+16];}
 for(i=48;i<64;i++)
  {v_head[i]=V[i-48];} 

 //Va lag 90 Degree
 
 //for(i=0;i<16;i++)
   //{v_head[i]=V[i+48];}
 //for(i=16;i<64;i++)
   //{v_head[i]=V[i-16];}
 P_temp=0;
 for(i=0;i<64;i++)
 {P_temp=P_temp+v_head[i]*I[i];}
 S_temp=sqrt(P_temp*P_temp+Q_temp*Q_temp);
 *PF=P_temp/S_temp;
 *PF=(*PF)/(sqrt(1+(Ia_THD/100.0)*(Ia_THD/100.0)));//功率因数畸变校正
 Nop();
 if((*PF)<0){(*PF)=-(*PF);}
 if(((P_temp>=0)&&(Q_temp>=0))||((P_temp<0)&&(Q_temp<0)))
   {Nop();}
 else
   {*PF=-(*PF);}
   
 }                               
void Pa_Compute(void)
{
 float sum_avg;
 unsigned char i;
 sum_avg=0;
 for(i=0;i<64;i++)
  {
   sum_avg=sum_avg+Va[i];
   }
 sum_avg=sum_avg/64.0;
 for(i=0;i<64;i++)
  {
   Va[i]=Va[i]-sum_avg;
   } 
 //compute Vrms
 sum_avg=0;
 for(i=0;i<64;i++)
 {
  sum_avg=sum_avg+Va[i]*Va[i];
  }
 Va_rms=sqrt(sum_avg)*Vconst;

 sum_avg=0;
 for(i=0;i<64;i++)
  {
   sum_avg=sum_avg+Ia[i];
   }
 sum_avg=sum_avg/64.0;
 for(i=0;i<64;i++)
 {
  Ia[i]=Ia[i]-sum_avg;
  } 
 //compute Irms
 sum_avg=0;
 for(i=0;i<64;i++)
 {
  sum_avg=sum_avg+Ia[i]*Ia[i];
  }
 Ia_rms=sqrt(sum_avg)*Iconst*100;

 Pa=0;
 for(i=0;i<64;i++)
 {
  Pa=Pa+Va[i]*Ia[i];
  }
 Pa=Pa*Pconst;
 Nop();
 Sa=Va_rms*Ia_rms/100.0;
 Nop();
 cosFA=Pa/Sa;
 Nop();
 }
//-------------------------------------------------------
void all_dft(void)
{
 unsigned char i;
  if(sample_flag==TRUE)//64点采样完毕
  {
   TRISDbits.TRISD11=0;
   //RUNLED=!RUNLED;
   //Va_dft_do();
   Va_rms=DFT64(Va,Va_dft_pu)/VCONST;
   if(Va_rms<10)
     {for(i=0;i<32;i++){Va_dft_pu[i]=0;}}
   Va_THD=THD_compute(Va_dft_pu);
   //RUNLED=!RUNLED;
   //Vb_dft_do();
   /*
   Vb_rms=DFT64(Vb,Vb_dft_pu)/VCONST;
   if(Vb_rms<10){for(i=0;i<32;i++){Vb_dft_pu[i]=0;}} 
   Vb_THD=THD_compute(Vb_dft_pu);
   */
   //RUNLED=!RUNLED;
   //Vc_dft_do();
   /*
   Vc_rms=DFT64(Vc,Vc_dft_pu)/VCONST;
   if(Vc_rms<10){for(i=0;i<32;i++){Vc_dft_pu[i]=0;}}
   Vc_THD=THD_compute(Vc_dft_pu);
   RUNLED=!RUNLED;
    */
   //Ia_dft_do();
   Ia_rms=DFT64(Ia,Ia_dft_pu)*100/ICONST; 
   if(Ia_rms<5){for(i=0;i<32;i++){Ia_dft_pu[i]=0;}}
   Ia_THD=THD_compute(Ia_dft_pu);
   //RUNLED=!RUNLED;
   //Ib_dft_do();
   /*
   Ib_rms=DFT64(Ib,Ib_dft_pu)*100/ICONST;
   if(Ib_rms<5){for(i=0;i<32;i++){Ib_dft_pu[i]=0;}}
   Ib_THD=THD_compute(Ib_dft_pu);
   RUNLED=!RUNLED;
    */
   //Ic_dft_do();
   /*
   Ic_rms=DFT64(Ic,Ic_dft_pu)*100/ICONST;
   if(Ic_rms<5){for(i=0;i<32;i++){Ic_dft_pu[i]=0;}}
   Ic_THD=THD_compute(Ic_dft_pu);
    */
 
   P_PF_compute(Va,Ia,&Pa,&Va_rms,&Ia_rms,&cosFA);
   //P_PF_compute(Vb,Ib,&Pb,&Vb_rms,&Ib_rms,&cosFB);
   // P_PF_compute(Vc,Ic,&Pc,&Vc_rms,&Ic_rms,&cosFC);
   //RUNLED=!RUNLED;
   //Send1Byte();
   UartSend();
   //RUNLED=!RUNLED;
   sample_flag=FAILURE;//使能下一个采样周期
   }
 }