Diff: QEI.c

Revision:

232:130a2b5003e6

Parent:

231:079835d508ef

--- a/QEI.c	Thu Jan 25 13:59:43 2018 +0000
+++ b/QEI.c	Wed Jan 31 13:41:23 2018 +0000
@@ -16,7 +16,6 @@
 int Pulse_16PointD;
 unsigned int CaunAddPlus	=0;//счетчик ипульсов энкодера"+" за такт ВП
 unsigned int CaunAddMin	    =0;//счетчик ипульсов энкодера"-" за такт ВП
-int TempAngelCompens=0;
 int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера
 int temp32=0;
 int Buff_1Point			[512];			
@@ -40,31 +39,74 @@
        SumTermoCompens   -= SumTermoCompens&0xffc00000;*/
        
        
-int TermoCopensRecalc(unsigned int T4, unsigned int T5)
+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;
-	   int DeltaT4, DeltaT5;
+	   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=Spi.ADC1-OldTempT4;
-	   DeltaT4=abs(DeltaT4);
-	   if(DeltaT4>15) 
+	   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)					//если  влаг персчета то,
 	   {
-	   	sprintf((Time),"Recalc\r\n");
-        WriteCon(Time);
-	   	OldTempT4=Spi.ADC1;
-	   	FlagRecalc=1;
-	   }
-	      
-	   DeltaT5=Spi.ADC2-OldTempT5;
-	   DeltaT5=abs(DeltaT5);
-	   if(DeltaT5>15)
-	   {
-	   	OldTempT5=Spi.ADC2;
+	   	 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; 
 	   }
 
-	   return Gyro.AngelCompens ;
+	       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;
 }
 
 
@@ -175,14 +217,12 @@
         tempReper=1;
         Gyro.CaunMinReperAdd		-=	Dif_QEI;
       }
-      
-      
-      
        
 	 //  Gyro.TermoModAdd=TermoCopensRecalc(Gyro.CurTermoCompens, Gyro.Frq>>11);
 
         
-       Gyro.CuruAngle  	+=  Buff_32Point [CountV31];  
+       Gyro.CuruAngle  	+=  Buff_32Point[CountV31];  
+       //Gyro.AngelCompens=0;
   
 }