File content as of revision 232:130a2b5003e6:

#include "Global.h"
#define NCoef 2
#define DCgain 8192
struct SPI Spi;





unsigned int TempTermLM;
unsigned int ADC5New;

int AMPSUM,SumDelta,HFOSumDelta,DeltaRegul,tempDeltaRegul;
int count10v=0,PLC_EROR, PLC_Flag,HFO_Flag,HFOdelta,HFOregul;
int PlcWormMN, PlcWormPL, PlcWormDelta, PlcWormMNout, PlcWormPLout, PlcWormDeltaout;

int HFOAmpSUM;
int HFOAmp;
int HFOAmpfilt;
int HFOAmpfiltC;
unsigned int ModArraySin	  [64] = {50,55,59,64,68,73,77,81,85,88,91,94,96,98,99,99,100,99,99,98,96,94,91,88,85,81,77,73,68,64,59,55,50,45,41,36,32,27,23,19,16,12,9,7,4,2,1,1,0,1,1,2,4,7,9,12,16,19,23,27,32,36,41,45};
int WormFilt	 			  [64];
int WormFilt2	 			  [64];
int Delt_T4	 			  	  [40];
int Delt_T5	 			  	  [40];
//int AVR_PLC_ERR	 		  [64];
//int ERRPLC;
int tempPlcErr;











void Modulator(void)
{
 LPC_DAC->DACR = (ModArraySin		[(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000; 
}

void ADS_Acum(void)
{
    Spi.ADC_NewData = 0;
  /*Gyro.IN1_Accum	+=  Spi.ADC2;
    Gyro.IN2_Accum	+=  Spi.ADC3;*/

    Spi.ADC1 = (int)(((((Spi.ADC1>>1)*100)*0.0122)-27300)+Gyro.Tmp_OffsetT4);
    Spi.ADC2 = (int)(((((Spi.ADC2>>1)*100)*0.0122)-27300)+Gyro.Tmp_OffsetT5);
    Gyro.Termo 	= Spi.ADC1;
	Gyro.DeltaT = Spi.ADC2;
   /*Gyro.Termo	= Spi.ADC1-44726;
	Gyro.DeltaT	= Spi.ADC2-44726;*/
	Gyro.DeltaTRate=(Spi.ADC2-Spi.ADC1);
    Gyro.TermLM = Spi.ADC1; 
}

void HFOFilt(void)
{
      		HFOAmp =  BUTTER(HFOAmp);
		    
		    HFOdelta=(int)((Gyro.HFO_ref)-(HFOAmp));	  	       
           	HFOSumDelta+=HFOdelta; 
           	HFOregul=HFOSumDelta/(int)(Gyro.HFO_Gain);
          	HFOSumDelta-=Gyro.HFO_Gain*HFOregul;
	        
	        tempDeltaRegul += HFOregul;
	        
	        HFO_Flag=1;	 
}

void PLCFilt(void)
{    static unsigned int countAVR=0;
   
         if (count10v>=64)
 		  {  
 		  		countAVR++;
 		        count10v=0;       
                for(int q=0; q<31; q++)
      	    	{
      	          WormFilt[q]= WormFilt[q]*(-1);
           		}  
		   	    for(int q=0; q<64; q++)
      	    	{
      	    	 WormFilt2[q] =	WormFilt[q];
      	    	 PlcWormDelta+=WormFilt[q];
      	    	 WormFilt[q]=0;
           		}
           		
           	   PlcWormDeltaout =PLCF1hz(PlcWormDelta>>6);
    		   if(countAVR > 63) countAVR=0;
      	   	   PlcWormDelta=0;
          
            SumDelta-= PlcWormDeltaout;
            Gyro.PLC_Eror_count=SumDelta/Gyro.PLC_Gain;
            SumDelta-=Gyro.PLC_Gain*Gyro.PLC_Eror_count;
          	
          	PLC_EROR+=Gyro.PLC_Eror_count;
          	
            PLC_Flag=1;
         }

}

void TempClac(void)
{
    static unsigned int AVR_T_Count=0;
  
	
	
	static  int TempModT4=0;
	static  int TempModT5=0;
	
	static  int OldT4=0;
	static  int OldT5=0,LastTermoMod=0;;
	static  int OldDeltaT4=0;
	static  int OldDeltaT5=0;
	static  int OldMod=0;
	
	
	AVR_T_Count++;
	

	
	TempModT4 = (int)(Gyro.T4-OldT4);
	TempModT5 = (int)(Gyro.T5-OldT5);
	
	OldT4=Gyro.T4;
	OldT5=Gyro.T5;
	
	
	Delt_T4	[AVR_T_Count & 0x1f] += TempModT4;
	Delt_T5	[AVR_T_Count & 0x1f] += TempModT5;

	if(AVR_T_Count==0x5ff)
	{
	  for(int q=0; q<0x1f; q++)
	  	{
	  	Gyro.DeltaT4+=Delt_T4[q];
	  	Delt_T4[q]=0;
	  	Gyro.DeltaT5+=Delt_T5[q];
	  	Delt_T5[q]=0;
	  	}

	  	if(Gyro.DeltaT4==0)Gyro.DeltaT4=OldDeltaT4;
	  	if(Gyro.DeltaT5==0)Gyro.DeltaT5=OldDeltaT5;
	  	
	  	OldDeltaT4=Gyro.DeltaT4;
	  	OldDeltaT5=Gyro.DeltaT5;
        
         
	  	Gyro.TermoMod = Gyro.DeltaT4+Gyro.DeltaT5;
	 
	  
		
		if     (Gyro.TermoMod> 0) Gyro.TermoMod=1;
	 	else if(Gyro.TermoMod==0) Gyro.TermoMod=LastTermoMod ;
		else                      Gyro.TermoMod=(-1);

	  LastTermoMod = Gyro.TermoMod;
      Gyro.DeltaT4=0;
	  Gyro.DeltaT5=0;
	  
      Gyro.TemperNewData=1;
	  AVR_T_Count=0;  
	  	

	  
	}
	
}

void SPI_Exchange(void)  // новая функция чтения, в нецй не должно быть ничего лишнего
{
    unsigned int DummySPI;
    int FilAds;

    ADC5New = LPC_SSP0->DR;// Чтение АЦП
    HFOAmpSUM+=0xffff-ADC5New;

    Spi.ADC4_Accum += LPC_SSP0->DR;
    Spi.ADC3_Accum += LPC_SSP0->DR;
    Spi.ADC2_Accum += LPC_SSP0->DR;
    Spi.ADC1_Accum += LPC_SSP0->DR;
    Spi.ADC5_Accum += ADC5New;	       
    
    FilAds = iir(0xffff-ADC5New);
    WormFilt[CountV64] = FilAds;

    
    while (LPC_SSP0->SR & RX_SSP_notEMPT) 	
    {
    DummySPI = LPC_SSP0->DR;   //если буфер SPI не пуст.//очистить буфер.
    }
    DAC_OutPut();
    if (CountV31 == 0) {						// просто фильтруем по 32 точкам.
         		        						// выставояем бит, что есть новы данные
        Spi.ADC1 = Spi.ADC1_Accum >> 5;			// подгоотавливаем данные (в той эе сетке) те 	 ADC1  0..65535
        Spi.ADC2 = Spi.ADC2_Accum >> 5;
        Spi.ADC3 = Spi.ADC3_Accum >> 5;
        Spi.ADC4 = (Spi.ADC4_Accum >> 5);
        Spi.ADC5 = (Spi.ADC5_Accum >> 5);
            	
			Gyro.T4=Spi.ADC1;
			Gyro.T5=Spi.ADC2;
        Spi.ADC1_Accum  = 0;				    // сбрасывкем аккамулятор
        Spi.ADC2_Accum  = 0;
        Spi.ADC3_Accum  = 0;
        Spi.ADC4_Accum  = 0;
        Spi.ADC5_Accum  = 0;
        Spi.ADC_NewData = 1;
        TempClac();
       }
       if(CountV31==31) 
			{
			HFOAmp=HFOAmpSUM>>5;
			HFOAmpSUM=0;
			HFOFilt();
			}
	   count10v++; 
	  
       if(Time1Hz>3) PLCFilt();
}


void  HFORegul(void)
{
static unsigned int countHFO;
	 countHFO=0;
	 if(Gyro.RgConA&0x2) 
		 {
		   	if(HFO_Flag)
		   	{
		   		HFO_Flag=0;
		  	  	Spi.DAC_A -= HFOregul;
		   		DeltaRegul=0;
        	}
		 }
 	else DeltaRegul=0;
    if(Spi.DAC_A>Gyro.HFO_Min-1)	  Spi.DAC_A=Gyro.HFO_Min-2;
 	else if(Spi.DAC_A<Gyro.HFO_Max+1) Spi.DAC_A=Gyro.HFO_Max+2;
}     


void  PLCRegul(void)
{
static unsigned int Flag_64=0, count;	    
static int  CountFaza,Sin,FlagGain;



 	 if(Gyro.RgConA&0x8)
		{
			if(PLC_Flag)
			 {   
			    PLC_Flag=0;
				if(Gyro.PLCDelay) {	FlagGain=1;	}
	  			else 
	  			{
	  				if(FlagGain) 
	  				{
	  					FlagGain=0;
	  					Gyro.HFO_Gain = GyroP.Str.HFO_Gain;
	  				}
	  				Spi.DAC_B+=Gyro.PLC_Eror_count;
	  			}
	  	 	 }
	  	 	 switch(Gyro.TermoMod) 
	  	 	 {
       		 case -1:
					if(Spi.DAC_B > Gyro.HighTreshold ) 
   						{
   						Gyro.HFO_Gain = GyroP.Str.HFO_Gain_Reset_PLC; 
   						Spi.DAC_B = (Gyro.ResetLevelCool);
   						Gyro.PLCDelay = GyroP.Str.PLCDelay; 
   						}  
   			 break;
   			 
   			 case 1:
   					if(Spi.DAC_B < Gyro.DownTreshold ) 
    					{
    					Gyro.HFO_Gain = GyroP.Str.HFO_Gain_Reset_PLC;
    					Spi.DAC_B = (Gyro.ResetLevelHeat);
    					Gyro.PLCDelay = GyroP.Str.PLCDelay; 
    					}
    		 break;	
    		 }
   	   }   

}





#define NCoefB 4
float iir(float NewSampleB) {
   /* float ACoefB[NCoefB+1] = {
        0.00013312359920503476,
        0.00000000000000000000,
        -0.00026624719841006952,
        0.00000000000000000000,
        0.00013312359920503476
    };

    float BCoefB[NCoefB+1] = {
        1.00000000000000000000,
        -3.95329685155055670000,
        5.87955844350996500000,
        -3.89878590567108720000,
        0.97261396930668409000
    };*/
    
    
    float ACoefB[NCoefB+1] = {
        0.00002732713446345492,
        0.00000000000000000000,
        -0.00005465426892690984,
        0.00000000000000000000,
        0.00002732713446345492
    };

    float BCoefB[NCoefB+1] = {
        1.00000000000000000000,
        -3.97797739472029300000,
        5.95330114200914020000,
        -3.97245819689350290000,
        0.99722704990382360000
    };
    static float yB[NCoefB+1]; //output samples
    static float xB[NCoefB+1]; //input samples
    int n;

    //shift the old samples
    for(n=NCoefB; n>0; n--) {
       xB[n] = xB[n-1];
       yB[n] = yB[n-1];
    }

    //Calculate the new output
    xB[0] = NewSampleB;
    yB[0] = ACoefB[0] * xB[0];
    for(n=1; n<=NCoefB; n++)
    yB[0] += ACoefB[n] * xB[n] - BCoefB[n] * yB[n];

    return yB[0];
}










float BUTTER(float NewSample) {

    float ACoef[NCoef+1] = {
        0.00140639079012754060,
        0.00281278158025508130,
        0.00140639079012754060
    };

    float BCoef[NCoef+1] = {
        1.00000000000000000000,
        -1.88903307939452490000,
        0.89487434461663573000
    };

    static float y[NCoef+1]; //output samples
    static float x[NCoef+1]; //input samples
    int n;

    //shift the old samples
    for(n=NCoef; n>0; n--) 
    {
       x[n] = x[n-1];
       y[n] = y[n-1];
    }

    //Calculate the new output
    x[0] = NewSample;
    y[0] = ACoef[0] * x[0];

    
    for(n=1; n<=NCoef; n++)
    {
    	y[0] += ACoef[n] * x[n] - BCoef[n] * y[n];
  
    }

    return y[0];
}





#define NCoefP 2
float PLCF1hz(float NewSampleP) {
  
   /*  float ACoefP[NCoefP+1] = {
        0.02010938982754319900,
        0.04021877965508639800,
        0.02010938982754319900
    };                                    //    10HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.56101807580071790000,
        0.64135153805756284000
    };*/
  /*  
  float ACoefP[NCoefP+1] = {
        0.00554950417857849340,
        0.01109900835715698700,
        0.00554950417857849340
    };                                    //    5HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.77863177782458480000,
        0.80080264666570744000
    };*/
  /* float ACoefP[NCoefP+1] = {
        0.00024156733395523967,
        0.00048313466791047934,
        0.00024156733395523967
    };                                        //2HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.95557824031503500000,
        0.95654367651120309000
    };*//*
   float ACoefP[NCoefP+1] = {
        0.00000530383940879362,
        0.00001060767881758724,
        0.00000530383940879362
    };                                        //0.125HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.99444641054192680000,
        0.99446178907595384000
    };*/
    /*float ACoefP[NCoefP+1] = {
        0.00024156733395523967,
        0.00048313466791047934,
        0.00024156733395523967
    };                                        //1HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.95557824031503500000,
        0.95654367651120309000
    };*/
    /*
        float ACoefP[NCoefP+1] = {
        0.00013397155178191382,
        0.00026794310356382763,
        0.00013397155178191382
    };                                        //0.75HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.96668138526348500000,
        0.96722742815186025000
    };*/
    float ACoefP[NCoefP+1] = {
        0.00036133932576050832,
        0.00072267865152101664,
        0.00036133932576050832
    };                                        //1.25HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        -1.94447765776709370000,
        0.94597793623228155000
    };
    
    
    /*
     float ACoefP[NCoefP+1] = {
        0.29289321881333569000,
        0.58578643762667137000,
        0.29289321881333569000
    };                                    //     100HZ

    float BCoefP[NCoefP+1] = {
        1.00000000000000000000,
        0.00000000000000004878,
        0.17157287525380990000
    };*/
    static float yP[NCoefP+1]; //output samples
    static float xP[NCoefP+1]; //input samples
    int n;

    //shift the old samples
    for(n=NCoef; n>0; n--) {
       xP[n] = xP[n-1];
       yP[n] = yP[n-1];
    }

    //Calculate the new output
    xP[0] = NewSampleP;
    yP[0] = ACoefP[0] * xP[0];
    for(n=1; n<=NCoefP; n++)
    yP[0] += ACoefP[n] * xP[n] - BCoefP[n] * yP[n];
    
    return yP[0];
}

void DAC_OutPut(void)//выдача в цапы
{
  	/*if(Gyro.RgConA&0x10)*/	Modulator();
    LPC_SSP0->DR=0x5555;
    LPC_SSP0->DR=0x5555;
    LPC_SSP0->DR=0x5555;

    if (CountV31 & 1) 
    {                      	//если нечетный такт то
        LPC_SSP0->DR =  WRITE_DAC0;   			//e.команда для ЦАП_0 передавать.
        LPC_SSP0->DR = (Spi.DAC_A);         	//e. передача 12 бит
    } 
    else 
    { //если такт четный.
        LPC_SSP0->DR = WRITE_DAC1 ; //e.команда для ЦАП_1 передавать.
       LPC_SSP0->DR = (Spi.DAC_B) ;
    }
}