Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

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?

UserRevisionLine numberNew 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