Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
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; }