Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
QEI.c@232:130a2b5003e6, 2018-01-31 (annotated)
- Committer:
- Kovalev_D
- Date:
- Wed Jan 31 13:41:23 2018 +0000
- Revision:
- 232:130a2b5003e6
- Parent:
- 231:079835d508ef
static termocorr
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 1:f2adcae3d304 | 1 | #include "Global.h" |
Kovalev_D | 231:079835d508ef | 2 | //#include "math.h" |
igor_v | 0:8ad47e2b6f00 | 3 | |
igor_v | 21:bc8c1cec3da6 | 4 | int Dif_QEI; |
igor_v | 42:6fc307c4963e | 5 | |
igor_v | 21:bc8c1cec3da6 | 6 | int FFF=0;//для заплатки |
igor_v | 0:8ad47e2b6f00 | 7 | |
Kovalev_D | 197:7a05523bf588 | 8 | unsigned int tempmod=64,tempmod2=1; |
Kovalev_D | 211:ac8251b067d2 | 9 | unsigned int tempReper,tempReper2; |
Kovalev_D | 86:398da56ef751 | 10 | int Pulse_8Point; |
Kovalev_D | 86:398da56ef751 | 11 | int Pulse_16Point; |
Kovalev_D | 86:398da56ef751 | 12 | int Pulse_32Point; |
Kovalev_D | 183:7e200f4d9b16 | 13 | int Pulse_64Point; |
Kovalev_D | 211:ac8251b067d2 | 14 | int Pulse_96Point; |
Kovalev_D | 211:ac8251b067d2 | 15 | int Pulse_128Point; |
Kovalev_D | 86:398da56ef751 | 16 | int Pulse_16PointD; |
Kovalev_D | 191:40028201ddad | 17 | unsigned int CaunAddPlus =0;//счетчик ипульсов энкодера"+" за такт ВП |
Kovalev_D | 192:d32c8cf7bcd9 | 18 | unsigned int CaunAddMin =0;//счетчик ипульсов энкодера"-" за такт ВП |
Kovalev_D | 112:4a96133a1311 | 19 | int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера |
Kovalev_D | 207:d1ce992f5d17 | 20 | int temp32=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 21 | int Buff_1Point [512]; |
Kovalev_D | 183:7e200f4d9b16 | 22 | int Buff_32Point [32]; |
Kovalev_D | 211:ac8251b067d2 | 23 | int Buff_64Point [32]; |
Kovalev_D | 211:ac8251b067d2 | 24 | int Buff_96Point [32]; |
Kovalev_D | 211:ac8251b067d2 | 25 | int Buff_128Point [32]; |
igor_v | 43:3f7c8a0ea59c | 26 | int Buff_16Point [32]; |
igor_v | 43:3f7c8a0ea59c | 27 | int Buff_16PointD [32]; |
igor_v | 43:3f7c8a0ea59c | 28 | int Buff_8Point [32]; |
igor_v | 43:3f7c8a0ea59c | 29 | int Buff_Restored_sin [32]; |
Kovalev_D | 196:f76dbc081e63 | 30 | int Buff_Restored_sin2 [32]; |
Kovalev_D | 231:079835d508ef | 31 | |
Kovalev_D | 231:079835d508ef | 32 | /* float i = 0.123; |
Kovalev_D | 231:079835d508ef | 33 | float j = 1.00000000000; |
Kovalev_D | 231:079835d508ef | 34 | static int SumTermoCompens=0,TermoCompens=0; |
Kovalev_D | 231:079835d508ef | 35 | int TermoCompensCalc; |
Kovalev_D | 231:079835d508ef | 36 | TermoCompensCalc = (TermoComp<<22)/(100 * Frq); |
Kovalev_D | 231:079835d508ef | 37 | SumTermoCompens += TermoCompensCalc; |
Kovalev_D | 231:079835d508ef | 38 | Gyro.AngelCompens += SumTermoCompens>>22; |
Kovalev_D | 231:079835d508ef | 39 | SumTermoCompens -= SumTermoCompens&0xffc00000;*/ |
Kovalev_D | 231:079835d508ef | 40 | |
Kovalev_D | 231:079835d508ef | 41 | |
Kovalev_D | 232:130a2b5003e6 | 42 | int TermoCopensRecalc(int T4, int T5, unsigned int multiplier) |
Kovalev_D | 231:079835d508ef | 43 | { |
Kovalev_D | 232:130a2b5003e6 | 44 | static unsigned int test; |
Kovalev_D | 231:079835d508ef | 45 | static int OldTempT4=0,OldTempT5=0,OldTempAVR; |
Kovalev_D | 231:079835d508ef | 46 | static int TempAVR; |
Kovalev_D | 232:130a2b5003e6 | 47 | static unsigned int FlagRecalc=0,DeltaT4, DeltaT5, DeltaT, TempL, TempR, TempD; |
Kovalev_D | 232:130a2b5003e6 | 48 | static float DeltaL, DeltaR,DeltaD; |
Kovalev_D | 232:130a2b5003e6 | 49 | static float TempAngelCompens=0; |
Kovalev_D | 232:130a2b5003e6 | 50 | static float SumAngelCompens=0; |
Kovalev_D | 232:130a2b5003e6 | 51 | static int IntegerAngelCompens=0; |
Kovalev_D | 231:079835d508ef | 52 | |
Kovalev_D | 232:130a2b5003e6 | 53 | DeltaT4=T4-OldTempT4; // ищем дельту между текущей температурой Т4 и тимпературой прошлого пересчета термокомпенс. состовляющей. |
Kovalev_D | 232:130a2b5003e6 | 54 | DeltaT4=abs(DeltaT4); // убираем знак дельты температуры. |
Kovalev_D | 232:130a2b5003e6 | 55 | if(DeltaT4> GyroP.Str.Step) // если дельта больше шага пересчета то, |
Kovalev_D | 232:130a2b5003e6 | 56 | { |
Kovalev_D | 232:130a2b5003e6 | 57 | |
Kovalev_D | 232:130a2b5003e6 | 58 | OldTempT4=T4; // записываем текущую температуру как темп. прошлого пересчета. |
Kovalev_D | 232:130a2b5003e6 | 59 | FlagRecalc=1; // выставляем флаг о необходимости пересчитать термокомпенс. поправку числа |
Kovalev_D | 232:130a2b5003e6 | 60 | } |
Kovalev_D | 232:130a2b5003e6 | 61 | |
Kovalev_D | 232:130a2b5003e6 | 62 | // тоже самое что и для Т4, без выставления флага пересчета. |
Kovalev_D | 232:130a2b5003e6 | 63 | DeltaT5=T5-OldTempT5; |
Kovalev_D | 232:130a2b5003e6 | 64 | DeltaT5=abs(DeltaT5); |
Kovalev_D | 232:130a2b5003e6 | 65 | if(DeltaT5> GyroP.Str.Step) |
Kovalev_D | 232:130a2b5003e6 | 66 | { |
Kovalev_D | 232:130a2b5003e6 | 67 | OldTempT5=T5; |
Kovalev_D | 232:130a2b5003e6 | 68 | } |
Kovalev_D | 232:130a2b5003e6 | 69 | if(FlagRecalc) //если влаг персчета то, |
Kovalev_D | 231:079835d508ef | 70 | { |
Kovalev_D | 232:130a2b5003e6 | 71 | for(int q=0; q<14; q++) |
Kovalev_D | 232:130a2b5003e6 | 72 | { |
Kovalev_D | 232:130a2b5003e6 | 73 | if(TermoCorrStatic.TermoTempStatic.StaticTermoCorr_Temp_Arr[q] < T4 ) //пробегаем по масиву температур. |
Kovalev_D | 232:130a2b5003e6 | 74 | { |
Kovalev_D | 232:130a2b5003e6 | 75 | } |
Kovalev_D | 232:130a2b5003e6 | 76 | else |
Kovalev_D | 232:130a2b5003e6 | 77 | { |
Kovalev_D | 232:130a2b5003e6 | 78 | TempL = TermoCorrStatic.TermoTempStatic.StaticTermoCorr_Temp_Arr[q - 1]; //подготавливаем левую границу температуры. |
Kovalev_D | 232:130a2b5003e6 | 79 | DeltaL = TermoCorrStatic.TermoDeltaStatic.StaticTermoCorr_Delta_Arr[q - 1] * multiplier; //подготавливаем левую границу значения поправки и приводим к времени вызова функции. |
Kovalev_D | 232:130a2b5003e6 | 80 | |
Kovalev_D | 232:130a2b5003e6 | 81 | TempR = TermoCorrStatic.TermoTempStatic.StaticTermoCorr_Temp_Arr[q]; //подготавливаем правую границу температуры. |
Kovalev_D | 232:130a2b5003e6 | 82 | DeltaR = TermoCorrStatic.TermoDeltaStatic.StaticTermoCorr_Delta_Arr[q] * multiplier; //подготавливаем правую границу значения поправки и приводим к времени вызова функции. |
Kovalev_D | 232:130a2b5003e6 | 83 | q=15; |
Kovalev_D | 232:130a2b5003e6 | 84 | } |
Kovalev_D | 232:130a2b5003e6 | 85 | } |
Kovalev_D | 232:130a2b5003e6 | 86 | |
Kovalev_D | 232:130a2b5003e6 | 87 | TempD = TempR - TempL; //расчитываем значение термокомпенс. сост. |
Kovalev_D | 232:130a2b5003e6 | 88 | TempAngelCompens = DeltaL+(T4-TempL)*((DeltaR-DeltaL)/TempD); //при текущей температуре. |
Kovalev_D | 232:130a2b5003e6 | 89 | FlagRecalc=0; |
Kovalev_D | 231:079835d508ef | 90 | } |
Kovalev_D | 231:079835d508ef | 91 | |
Kovalev_D | 232:130a2b5003e6 | 92 | if(GyroP.Str.TermoMode==1) |
Kovalev_D | 232:130a2b5003e6 | 93 | { |
Kovalev_D | 232:130a2b5003e6 | 94 | SumAngelCompens += TempAngelCompens; //копим статическую термокомпенсационную составляющую числа |
Kovalev_D | 232:130a2b5003e6 | 95 | IntegerAngelCompens =(SumAngelCompens); //смотрим накопился ли импульс |
Kovalev_D | 232:130a2b5003e6 | 96 | SumAngelCompens -= IntegerAngelCompens; //если накопился, то списываем его из суматора. |
Kovalev_D | 232:130a2b5003e6 | 97 | Gyro.AngelCompens += (IntegerAngelCompens<<6); //добовляем накопившиеся количество целых импульсов к общей термокомпенс. сост. числа, и приводим к разрядной сетке 1 имп. = 64 ппг. |
Kovalev_D | 232:130a2b5003e6 | 98 | } |
Kovalev_D | 232:130a2b5003e6 | 99 | if(GyroP.Str.TermoMode==2) |
Kovalev_D | 232:130a2b5003e6 | 100 | { |
Kovalev_D | 232:130a2b5003e6 | 101 | SumAngelCompens += TempAngelCompens; //сумматор |
Kovalev_D | 232:130a2b5003e6 | 102 | if((SumAngelCompens)>0.25) //если накопили четверть импульса то, |
Kovalev_D | 232:130a2b5003e6 | 103 | { |
Kovalev_D | 232:130a2b5003e6 | 104 | Gyro.AngelCompens += 16; //добовляем четверть импульса (привед. к разрядной сетке) к общей термокомпенс. сост. числа. |
Kovalev_D | 232:130a2b5003e6 | 105 | SumAngelCompens -= 0.25; //списываем четверть импулься из суматора. |
Kovalev_D | 232:130a2b5003e6 | 106 | } |
Kovalev_D | 232:130a2b5003e6 | 107 | } |
Kovalev_D | 232:130a2b5003e6 | 108 | |
Kovalev_D | 232:130a2b5003e6 | 109 | return 0; |
Kovalev_D | 231:079835d508ef | 110 | } |
Kovalev_D | 231:079835d508ef | 111 | |
Kovalev_D | 231:079835d508ef | 112 | |
igor_v | 21:bc8c1cec3da6 | 113 | void D_QEI(void) |
igor_v | 21:bc8c1cec3da6 | 114 | { |
Kovalev_D | 231:079835d508ef | 115 | |
Kovalev_D | 209:224e7331a061 | 116 | |
igor_v | 21:bc8c1cec3da6 | 117 | Dif_QEI=0; |
igor_v | 21:bc8c1cec3da6 | 118 | Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера. |
Kovalev_D | 207:d1ce992f5d17 | 119 | |
Kovalev_D | 207:d1ce992f5d17 | 120 | |
Kovalev_D | 207:d1ce992f5d17 | 121 | |
Kovalev_D | 197:7a05523bf588 | 122 | //LPC_QEI->CON = 0xF; |
igor_v | 21:bc8c1cec3da6 | 123 | Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.() |
Kovalev_D | 208:19150d2b528f | 124 | /* temp32 = Dif_QEI; |
Kovalev_D | 207:d1ce992f5d17 | 125 | Gyro.CuruAngle32 += temp32; |
Kovalev_D | 208:19150d2b528f | 126 | temp32=0;*/ |
igor_v | 21:bc8c1cec3da6 | 127 | Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения. |
Kovalev_D | 207:d1ce992f5d17 | 128 | |
Kovalev_D | 211:ac8251b067d2 | 129 | |
Kovalev_D | 211:ac8251b067d2 | 130 | |
igor_v | 21:bc8c1cec3da6 | 131 | if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль |
igor_v | 21:bc8c1cec3da6 | 132 | if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль |
Kovalev_D | 211:ac8251b067d2 | 133 | |
Kovalev_D | 211:ac8251b067d2 | 134 | /* sprintf((Time)," %d %d %d \r\n",Gyro.CaunPlusReperAdd,Gyro.CaunMinReperAdd, Dif_QEI); |
Kovalev_D | 211:ac8251b067d2 | 135 | WriteCon(Time);*/ |
Kovalev_D | 108:030cdde08314 | 136 | Buff_1Point[CountV255] = Dif_QEI ; // накопление в буфер еденичных значений приращения по каждому такту. |
Kovalev_D | 208:19150d2b528f | 137 | |
Kovalev_D | 208:19150d2b528f | 138 | |
Kovalev_D | 205:775d54fdf646 | 139 | |
igor_v | 21:bc8c1cec3da6 | 140 | ////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D | 108:030cdde08314 | 141 | Pulse_8Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 142 | Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов |
Kovalev_D | 112:4a96133a1311 | 143 | Buff_8Point[CountV31] = (Pulse_8Point); |
igor_v | 0:8ad47e2b6f00 | 144 | |
igor_v | 21:bc8c1cec3da6 | 145 | Pulse_16Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 146 | Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов |
Kovalev_D | 112:4a96133a1311 | 147 | Buff_16Point[CountV31] = (Pulse_16Point ); |
igor_v | 21:bc8c1cec3da6 | 148 | |
igor_v | 21:bc8c1cec3da6 | 149 | Pulse_32Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 150 | Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов |
Kovalev_D | 112:4a96133a1311 | 151 | Gyro.Cnt_Dif = (Pulse_32Point); |
Kovalev_D | 112:4a96133a1311 | 152 | Buff_32Point[CountV31] = (Pulse_32Point ); |
Kovalev_D | 209:224e7331a061 | 153 | |
Kovalev_D | 231:079835d508ef | 154 | /* Pulse_64Point += Buff_1Point[CountV255]; |
Kovalev_D | 211:ac8251b067d2 | 155 | Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов |
Kovalev_D | 183:7e200f4d9b16 | 156 | Buff_64Point[CountV31] = (Pulse_64Point ); |
Kovalev_D | 211:ac8251b067d2 | 157 | |
Kovalev_D | 211:ac8251b067d2 | 158 | Pulse_96Point += Buff_1Point[CountV255]; |
Kovalev_D | 211:ac8251b067d2 | 159 | Pulse_96Point -= Buff_1Point[(CountV255-96) & 0xff]; // заполнение буфера накопленых приращений за 96 тактов |
Kovalev_D | 211:ac8251b067d2 | 160 | Buff_96Point[CountV31] = (Pulse_96Point ); |
Kovalev_D | 211:ac8251b067d2 | 161 | |
Kovalev_D | 211:ac8251b067d2 | 162 | Pulse_128Point += Buff_1Point[CountV255]; |
Kovalev_D | 211:ac8251b067d2 | 163 | Pulse_128Point -= Buff_1Point[(CountV255-128) & 0xff]; // заполнение буфера накопленых приращений за 128 тактов |
Kovalev_D | 211:ac8251b067d2 | 164 | Buff_128Point[CountV31] = (Pulse_128Point ); |
Kovalev_D | 193:a0fe8bfc97e4 | 165 | |
igor_v | 0:8ad47e2b6f00 | 166 | |
igor_v | 21:bc8c1cec3da6 | 167 | Pulse_16PointD += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 168 | Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз |
igor_v | 21:bc8c1cec3da6 | 169 | Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; // |
Kovalev_D | 169:140743e3bb96 | 170 | Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // |
Kovalev_D | 231:079835d508ef | 171 | Buff_16PointD[CountV31] = Pulse_16PointD ;*/ |
Kovalev_D | 183:7e200f4d9b16 | 172 | |
Kovalev_D | 214:4c70e452c491 | 173 | |
Kovalev_D | 214:4c70e452c491 | 174 | |
Kovalev_D | 214:4c70e452c491 | 175 | |
Kovalev_D | 214:4c70e452c491 | 176 | |
Kovalev_D | 214:4c70e452c491 | 177 | |
Kovalev_D | 214:4c70e452c491 | 178 | |
Kovalev_D | 214:4c70e452c491 | 179 | |
Kovalev_D | 214:4c70e452c491 | 180 | |
Kovalev_D | 214:4c70e452c491 | 181 | |
Kovalev_D | 214:4c70e452c491 | 182 | |
Kovalev_D | 211:ac8251b067d2 | 183 | Buff_Restored_sin [CountV31] = (Buff_16Point[CountV31])*2 - Buff_32Point[CountV31]; |
Kovalev_D | 209:224e7331a061 | 184 | |
Kovalev_D | 214:4c70e452c491 | 185 | if((Buff_Restored_sin [CountV31]) > 0) CaunAddPlus += Buff_Restored_sin [CountV31]; // счетчик положительных импульсов |
Kovalev_D | 209:224e7331a061 | 186 | else CaunAddMin -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды |
Kovalev_D | 209:224e7331a061 | 187 | |
Kovalev_D | 211:ac8251b067d2 | 188 | |
Kovalev_D | 209:224e7331a061 | 189 | switch(CountV31){ |
Kovalev_D | 209:224e7331a061 | 190 | case 31: |
Kovalev_D | 209:224e7331a061 | 191 | Gyro.CaunMinRate = Gyro.CaunMinRateAdd; |
Kovalev_D | 209:224e7331a061 | 192 | Gyro.CaunPlusRate = Gyro.CaunPlusRateAdd; |
Kovalev_D | 209:224e7331a061 | 193 | Gyro.CaunMinRateAdd = 0; |
Kovalev_D | 209:224e7331a061 | 194 | Gyro.CaunPlusRateAdd = 0; |
Kovalev_D | 209:224e7331a061 | 195 | break; |
Kovalev_D | 211:ac8251b067d2 | 196 | |
Kovalev_D | 209:224e7331a061 | 197 | break; |
Kovalev_D | 209:224e7331a061 | 198 | } |
Kovalev_D | 231:079835d508ef | 199 | |
Kovalev_D | 211:ac8251b067d2 | 200 | |
Kovalev_D | 211:ac8251b067d2 | 201 | if(Dif_QEI > 0) |
Kovalev_D | 211:ac8251b067d2 | 202 | { |
Kovalev_D | 211:ac8251b067d2 | 203 | if(tempReper) |
Kovalev_D | 211:ac8251b067d2 | 204 | { |
Kovalev_D | 211:ac8251b067d2 | 205 | Gyro.CaunMinReper = Gyro.CaunMinReperAdd; |
Kovalev_D | 211:ac8251b067d2 | 206 | Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; |
Kovalev_D | 211:ac8251b067d2 | 207 | Gyro.CaunMinReperAdd = 0; |
Kovalev_D | 211:ac8251b067d2 | 208 | Gyro.CaunPlusReperAdd = 0; |
Kovalev_D | 211:ac8251b067d2 | 209 | Gyro.Reper_Event=1; |
Kovalev_D | 211:ac8251b067d2 | 210 | // Gyro.CuruAngle += Gyro.CaunPlusReper-Gyro.CaunMinReper; |
Kovalev_D | 211:ac8251b067d2 | 211 | tempReper=0; |
Kovalev_D | 211:ac8251b067d2 | 212 | } |
Kovalev_D | 211:ac8251b067d2 | 213 | Gyro.CaunPlusReperAdd += Dif_QEI; |
Kovalev_D | 211:ac8251b067d2 | 214 | } |
Kovalev_D | 211:ac8251b067d2 | 215 | else |
Kovalev_D | 211:ac8251b067d2 | 216 | { |
Kovalev_D | 211:ac8251b067d2 | 217 | tempReper=1; |
Kovalev_D | 211:ac8251b067d2 | 218 | Gyro.CaunMinReperAdd -= Dif_QEI; |
Kovalev_D | 211:ac8251b067d2 | 219 | } |
Kovalev_D | 231:079835d508ef | 220 | |
Kovalev_D | 231:079835d508ef | 221 | // Gyro.TermoModAdd=TermoCopensRecalc(Gyro.CurTermoCompens, Gyro.Frq>>11); |
Kovalev_D | 210:b02fa166315d | 222 | |
Kovalev_D | 231:079835d508ef | 223 | |
Kovalev_D | 232:130a2b5003e6 | 224 | Gyro.CuruAngle += Buff_32Point[CountV31]; |
Kovalev_D | 232:130a2b5003e6 | 225 | //Gyro.AngelCompens=0; |
Kovalev_D | 231:079835d508ef | 226 | |
igor_v | 21:bc8c1cec3da6 | 227 | } |
Kovalev_D | 211:ac8251b067d2 | 228 | |
Kovalev_D | 231:079835d508ef | 229 | |
Kovalev_D | 193:a0fe8bfc97e4 | 230 |