File content as of revision 232:130a2b5003e6:

#include "Global.h"
//#include "math.h"

int Dif_QEI;

int FFF=0;//для заплатки

unsigned int tempmod=64,tempmod2=1;
unsigned int tempReper,tempReper2;
int Pulse_8Point;
int Pulse_16Point;
int Pulse_32Point;
int Pulse_64Point;
int Pulse_96Point;
int Pulse_128Point;
int Pulse_16PointD;
unsigned int CaunAddPlus	=0;//счетчик ипульсов энкодера"+" за такт ВП
unsigned int CaunAddMin	    =0;//счетчик ипульсов энкодера"-" за такт ВП
int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера
int temp32=0;
int Buff_1Point			[512];			
int Buff_32Point		[32];		
int Buff_64Point		[32];
int Buff_96Point		[32];
int Buff_128Point		[32];	
int Buff_16Point		[32];
int Buff_16PointD		[32];
int Buff_8Point			[32];
int Buff_Restored_sin 	[32];
int Buff_Restored_sin2 	[32];

	/* float i = 0.123;	 
	   float j = 1.00000000000;
	   static int SumTermoCompens=0,TermoCompens=0;
	   int TermoCompensCalc;
	   TermoCompensCalc = (TermoComp<<22)/(100 * Frq);
	   SumTermoCompens 	 += TermoCompensCalc;
	   Gyro.AngelCompens += SumTermoCompens>>22;
       SumTermoCompens   -= SumTermoCompens&0xffc00000;*/
       
       
int TermoCopensRecalc(int T4, int T5, unsigned int multiplier)
{
	   static unsigned int  test;
	   static int OldTempT4=0,OldTempT5=0,OldTempAVR;
	   static int TempAVR;
	   static unsigned int FlagRecalc=0,DeltaT4, DeltaT5,  DeltaT, TempL,  TempR, TempD;
	   static float DeltaL, DeltaR,DeltaD;
	   static float TempAngelCompens=0;
	   static float SumAngelCompens=0;
	   static int IntegerAngelCompens=0;
	   
	   DeltaT4=T4-OldTempT4;			// ищем дельту между текущей температурой Т4 и тимпературой прошлого пересчета термокомпенс. состовляющей.
	   DeltaT4=abs(DeltaT4);			// убираем знак дельты температуры.
	   if(DeltaT4> GyroP.Str.Step)      // если дельта больше шага пересчета то,
	   	 	{
	   	 		
	   			OldTempT4=T4;			// записываем текущую температуру как темп. прошлого пересчета.
	   			FlagRecalc=1;			// выставляем флаг о необходимости пересчитать термокомпенс. поправку числа
	   		} 
	   
	   // тоже самое что и для Т4, без выставления флага пересчета.
	   DeltaT5=T5-OldTempT5;
	   DeltaT5=abs(DeltaT5);
	   if(DeltaT5> GyroP.Str.Step)
	   		{
	   			OldTempT5=T5;
	 		}
	   if(FlagRecalc)					//если  влаг персчета то,
	   {
	   	 for(int q=0; q<14; q++)
      	    	{
      	          if(TermoCorrStatic.TermoTempStatic.StaticTermoCorr_Temp_Arr[q] < T4 ) 								//пробегаем по масиву температур.
      	          	{
      	          	}
      	          	else
      	          	{
      	          		TempL  = TermoCorrStatic.TermoTempStatic.StaticTermoCorr_Temp_Arr[q - 1];						//подготавливаем левую границу температуры.
      	          		DeltaL = TermoCorrStatic.TermoDeltaStatic.StaticTermoCorr_Delta_Arr[q - 1] * multiplier; 		//подготавливаем левую границу значения поправки и приводим к времени вызова функции.
      	          		
      	         		TempR  = TermoCorrStatic.TermoTempStatic.StaticTermoCorr_Temp_Arr[q];							//подготавливаем правую границу температуры.
      	         		DeltaR = TermoCorrStatic.TermoDeltaStatic.StaticTermoCorr_Delta_Arr[q] * multiplier; 	        //подготавливаем правую границу значения поправки и приводим к времени вызова функции.		
     			        q=15;
      	          	}
           		 }  
           	
           TempD  =	 TempR - TempL;											//расчитываем значение термокомпенс. сост.
           TempAngelCompens = DeltaL+(T4-TempL)*((DeltaR-DeltaL)/TempD);	//при текущей температуре.
	       FlagRecalc=0; 
	   }

	       if(GyroP.Str.TermoMode==1)
	       {
	       		SumAngelCompens += TempAngelCompens;							//копим статическую термокомпенсационную составляющую числа
           		IntegerAngelCompens =(SumAngelCompens);							//смотрим накопился ли импульс
           		SumAngelCompens -=  IntegerAngelCompens;						//если накопился, то списываем его из суматора.
           		Gyro.AngelCompens += (IntegerAngelCompens<<6);					//добовляем накопившиеся количество целых импульсов к общей термокомпенс. сост. числа, и приводим к разрядной сетке 1 имп. = 64 ппг.
			}
	       if(GyroP.Str.TermoMode==2)
	       {
	       		SumAngelCompens += TempAngelCompens;							//сумматор
	       		if((SumAngelCompens)>0.25)										//если накопили четверть импульса то,
	       		{
	       			Gyro.AngelCompens += 16;									//добовляем четверть импульса (привед. к разрядной сетке) к общей термокомпенс. сост. числа.
	       			SumAngelCompens -= 0.25;									//списываем четверть импулься из суматора.
	       		}
			}	

	   return 0;
}


void D_QEI(void)
{
 
  
    Dif_QEI=0;
    Cur_QEI  =  LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
    
    
    
    //LPC_QEI->CON = 0xF;			
    Dif_QEI  =  (Cur_QEI - Last_QEI);  // получение приращения.()
  /*  temp32   =   Dif_QEI;
    Gyro.CuruAngle32 += temp32;
    temp32=0;*/
    Last_QEI =  Cur_QEI;               // запись текущего значения энкодера в регистр предыдущего значения.
    
   

    if (Dif_QEI < -0xfff)  Dif_QEI += 0x10000;   // обработка прохода значения через ноль
    if (Dif_QEI >  0xfff)  Dif_QEI -= 0x10000;    // обработка прохода значения через ноль
    
   /*  sprintf((Time)," %d %d %d \r\n",Gyro.CaunPlusReperAdd,Gyro.CaunMinReperAdd, Dif_QEI);
     WriteCon(Time);*/
    Buff_1Point[CountV255] = Dif_QEI ; 	// накопление в буфер еденичных значений приращения по каждому такту.
    
 

    //////////////////////////////////////////////////////////////////////////////////////////////
        Pulse_8Point += Buff_1Point[CountV255];
        Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff];                    // заполнение буфера накопленых приращений за 8 тактов
        Buff_8Point[CountV31] = (Pulse_8Point);

        Pulse_16Point += Buff_1Point[CountV255];
        Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff];									// заполнение буфера накопленых приращений за 16 тактов
        Buff_16Point[CountV31] =  (Pulse_16Point );

        Pulse_32Point += Buff_1Point[CountV255];
        Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff];  								// заполнение буфера накопленых приращений за 32 тактов
        Gyro.Cnt_Dif  =  (Pulse_32Point);
        Buff_32Point[CountV31] = (Pulse_32Point );
                
       /* Pulse_64Point += Buff_1Point[CountV255];
        Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff];  								// заполнение буфера накопленых приращений за  64 тактов
        Buff_64Point[CountV31] = (Pulse_64Point );
        
        Pulse_96Point += Buff_1Point[CountV255];
        Pulse_96Point -= Buff_1Point[(CountV255-96) & 0xff];  								// заполнение буфера накопленых приращений за 96 тактов
        Buff_96Point[CountV31] = (Pulse_96Point );
        
        Pulse_128Point += Buff_1Point[CountV255];
        Pulse_128Point -= Buff_1Point[(CountV255-128) & 0xff];  								// заполнение буфера накопленых приращений за 128 тактов
        Buff_128Point[CountV31] = (Pulse_128Point );


        Pulse_16PointD += Buff_1Point[CountV255];
        Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff];									// заполнение буфера накопленых приращений за 16 тактов Двойныз
        Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff];									//
        Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff];									// 
        Buff_16PointD[CountV31] = Pulse_16PointD ;*/
     
     
     

        
        
        
        
        
     
     
        Buff_Restored_sin  [CountV31] = (Buff_16Point[CountV31])*2 - Buff_32Point[CountV31];
    
        if((Buff_Restored_sin [CountV31]) > 0)	CaunAddPlus += Buff_Restored_sin [CountV31];  // счетчик положительных импульсов
    	else 									CaunAddMin  -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды
    	

    	switch(CountV31){
        case 31: 
        Gyro.CaunMinRate = Gyro.CaunMinRateAdd;
        Gyro.CaunPlusRate = Gyro.CaunPlusRateAdd;
        Gyro.CaunMinRateAdd  = 0; 
        Gyro.CaunPlusRateAdd = 0;
        break;

        break;
        }
      
        
      if(Dif_QEI > 0)  
      {
      	if(tempReper)
      	{
      		Gyro.CaunMinReper  = Gyro.CaunMinReperAdd;
	        Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
	        Gyro.CaunMinReperAdd  = 0; 
	        Gyro.CaunPlusReperAdd = 0;
	        Gyro.Reper_Event=1;
	      //  Gyro.CuruAngle  += Gyro.CaunPlusReper-Gyro.CaunMinReper;
	        tempReper=0;
	    }
      	Gyro.CaunPlusReperAdd	+=	Dif_QEI;
      }
      else 
      { 
        tempReper=1;
        Gyro.CaunMinReperAdd		-=	Dif_QEI;
      }
       
	 //  Gyro.TermoModAdd=TermoCopensRecalc(Gyro.CurTermoCompens, Gyro.Frq>>11);

        
       Gyro.CuruAngle  	+=  Buff_32Point[CountV31];  
       //Gyro.AngelCompens=0;
  
}