Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
Diff: Command.c
- Revision:
- 214:4c70e452c491
- Parent:
- 213:9953db9543d6
- Child:
- 215:b58b887fd367
diff -r 9953db9543d6 -r 4c70e452c491 Command.c --- a/Command.c Wed Jul 26 13:24:39 2017 +0000 +++ b/Command.c Wed Aug 16 09:00:25 2017 +0000 @@ -6,6 +6,8 @@ unsigned int Rate_Flag; unsigned int CountBuFFIn; unsigned int N=0,CRC_N; +unsigned int Rate_7=0; +int iRate_7=0; unsigned int Param1=0; unsigned int Param2=0; unsigned int Param3=0; @@ -123,24 +125,148 @@ { switch(Gyro.ModeOut) { - case 1: if(Gyro.Rate1_Event ) CMD_Rate(); break; - case 2: if(Gyro.Reper_Event ) CMD_Rate2(); break; - case 3: if(Gyro.Event_500Hz ) CMD_Delta_PS(); break; - case 4: if(Gyro.EXT_Latch ) CMD_Delta_PS(); break; - case 5: if(Gyro.Event_500Hz ) CMD_Delta_Bins(); break; - case 6: if(Gyro.EXT_Latch ) CMD_B_Delta(); break; - case 7: if(Gyro.Event_500Hz ) CMD_B_Delta(); break; - case 8: if(Gyro.Rate3_Event ) CMD_Rate3(); break; - case 9: if(Gyro.Reper_Event ){ CMD_Rate2(); Gyro.ModeOut=0;} break; + case 1: if(Gyro.Rate1_Event ) CMD_Rate(); break; + case 2: if(Gyro.Reper_Event ) CMD_Rate2(); break; + case 3: if(Gyro.Event_500Hz ) CMD_Delta_PS(); break; + case 4: if(Gyro.EXT_Latch ) CMD_Delta_PS(); break; + case 5: if(Gyro.Event_500Hz ) CMD_Delta_Bins(); break; + case 6: if(Gyro.EXT_Latch ) CMD_B_Delta(); break; + case 7: if(Gyro.Event_500Hz ) CMD_B_Delta(); break; + case 8: if(Gyro.Rate3_Event ) CMD_Rate3(); break; + case 9: if(Gyro.Reper_Event ){ CMD_Rate2(); Gyro.ModeOut=0;} break; + case 10: if(Gyro.EvenRate7 ){ CMD_Rate7(); Gyro.EvenRate7=0;} break; } } +void BuffClear(void) +{ + for(int q=0; q<60; q++) + { + BuffTemp[q]=0; + } +} +void CMD_Rate7(void) +{ + + // (CountV64)&0x3f]*Gyro.ModAmp туды + //BuffADC_64Point[ (CountFaza + Gyro.PLC_Phase) & 0x3f] + BuffClear(); + + int Temp; + int Temp1; + float Temp2; + int Shift = -11463; + static unsigned int t=16; + + BuffTemp[0] = Gyro.SOC_Out; + BuffTemp[1] = Gyro.My_Addres; + + iRate_7 =(((BuffADC_64Point[(t - 0 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 2] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 3] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков + + iRate_7 = (((BuffADC_64Point[(t - 2 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 4] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 5] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + + iRate_7 =(((BuffADC_64Point[(t - 4 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 6] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 7] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((BuffADC_64Point[(t - 6 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 8] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 9] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((BuffADC_64Point[(t - 8 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 10] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 11] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((BuffADC_64Point[(t - 10 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 12] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 13] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((BuffADC_64Point[(t - 12 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 14] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 15] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((BuffADC_64Point[(t - 14 - Gyro.PLC_Phase)&0x3f]) )+ Shift)&0xffff; + BuffTemp[ 16] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 17] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + + + + + iRate_7 = (((ModArraySin[(t - 0)&0x3f])))- 50; + BuffTemp[ 18] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 19] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 2)&0x3f])))- 50; + BuffTemp[ 20] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 21] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 4)&0x3f])))- 50; + BuffTemp[ 22] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 23] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 6)&0x3f])))- 50; + BuffTemp[ 24] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 25] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 8)&0x3f])))- 50; + BuffTemp[ 26] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 27] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 10)&0x3f])))- 50; + BuffTemp[ 28] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 29] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 12)&0x3f])))- 50; + BuffTemp[ 30] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 31] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (((ModArraySin[(t - 14)&0x3f])))- 50; + BuffTemp[ 32] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 33] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + iRate_7 = (Spi.DAC_B - 0x7fff); + BuffTemp[ 34] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 35] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + Rate_7 = Gyro.PLC_Eror_count; + BuffTemp[ 36] =(Rate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 37] =(Rate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + Gyro.PLC_Eror_count=0; + switch(Gyro.LG_Type) + { + case 1: iRate_7 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; + case 0: Temp2 = (Spi.DAC_A*1.538) - 0xd80f; + iRate_7 = (int)Temp2; break; + } + //Rate_7 = 30000;//(unsigned int)(iRate_7&0xffff); + + BuffTemp[ 38] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков + BuffTemp[ 39] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков; + + Check(BuffTemp, 42); + WriteConN (BuffTemp,42); + if(t<47)t+=16; + else t =16; + +} + + + void CMD_M_Param_R(void) { + unsigned int NP=0; unsigned int Param; + NP = BuffTemp[3]; for (int i=0;i<8;i++) @@ -198,8 +324,8 @@ case 16: Gyro.FrqHZmax = (7680000 / GyroP.Str.FrqHZmax)<<16; break; case 18: temp=((GyroP.Str.VB_Fdf_Hi<<16) | GyroP.Str.VB_Fdf_Lo); - /*sprintf((Time),"%d \r\n", temp/32); - WriteCon(Time);*/ + sprintf((Time),"%d \r\n", temp/32); + WriteCon(Time); temp=temp*20; Gyro.AmpTarget=(unsigned int)(temp); break; @@ -320,8 +446,8 @@ Gyro.CuruAngle = Gyro.CuruAngle + OldCuruAngle; //(72 град/с) - OldCuruAngle=Gyro.CuruAngle & 0x7; //сохраняем 3 бит для след измирений - Temp=Gyro.CuruAngle>>3; //приводим к форме вывода + OldCuruAngle=Gyro.CuruAngle & 0xf; //сохраняем 3 бит для след измирений + Temp=Gyro.CuruAngle>>4; //приводим к форме вывода Gyro.CuruAngle = 0; BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт разности счетчиков @@ -504,8 +630,8 @@ Gyro.CuruAngle = Gyro.CuruAngle + OldCuruAngle; //(72 град/с) - OldCuruAngle=Gyro.CuruAngle & 0x1f; //сохраняем 5 бит для след измирений - Temp=Gyro.CuruAngle>>5; //приводим к форме вывода + OldCuruAngle=Gyro.CuruAngle & 0x3f; //сохраняем 5 бит для след измирений + Temp=Gyro.CuruAngle>>6; //приводим к форме вывода Gyro.CuruAngle = 0; @@ -524,11 +650,12 @@ BuffTemp[ 9] = (Temp >> 0) & 0xff;// 10 --частота расщипления //(unsigned int)((Gyro.DacIn+30000)*0.6667); - if(Gyro.LG_Type) {Temp1 =(int)(((Spi.DAC_A/0.6667)-30000)-0x7fff);} - else + switch(Gyro.LG_Type) { - Temp2 = ((Spi.DAC_A/0.65)-22544)-0x7fff; - Temp1= (int)Temp2; + case 1: Temp1 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break; + + case 0: Temp2 = (Spi.DAC_A*1.538) - 0xd80f; + Temp1= (int)Temp2; break; } //Temp1=Gyro.DacIn/*-0x7fff*/; BuffTemp[10] = (Temp1 >> 8) & 0xff;//выход регулятора гвч; 11 --выход регулятора ГВЧ @@ -605,10 +732,13 @@ { unsigned int Temp; Gyro.Reper_Event=0; - + for(int q=0; q<64; q++) + { + BuffTemp[q]=0; + } BuffTemp[ 0] = Gyro.SOC_Out; BuffTemp[ 1] = Gyro.My_Addres; - + /* Temp=Gyro.CaunPlusReper; BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт счетчика +. @@ -619,6 +749,7 @@ BuffTemp[ 4] =(Temp >> 8) & 0xff;//старший байт счетчика -. BuffTemp[ 5] =(Temp >> 0) & 0xff;//младший байт счетчика -. +*/ Check(BuffTemp, 8); WriteConN (BuffTemp,8); @@ -653,14 +784,12 @@ if(HFO) { Gyro.DacIn=temp; - if(Gyro.LG_Type==1) Spi.DAC_A = (unsigned int)((Gyro.DacIn+30000)*0.6667); + if(Gyro.LG_Type==1) Spi.DAC_A = ((unsigned int)((Gyro.DacIn+0x7011)*0.6667)); else Spi.DAC_A = (unsigned int)((temp)); } else Spi.DAC_B = (unsigned int)temp; - -/* sprintf((Time)," %d %d \r\n", Spi.DAC_A,Gyro.DacIn); - WriteCon(Time); -*/ + + /* sprintf((Time),"%d %d %f \r\n", temp, Spi.DAC_A, (temp+10744)*0.79); @@ -688,11 +817,13 @@ // anser = BuffTemp[3]>>7; temp1 = (((BuffTemp[4]<<8) | BuffTemp[5])&0xFFFF); - Gyro.Frq = (122780000/temp1)<<12; - + Gyro.Frq = (122780000/temp1)<<12; + // Gyro.Frq = (103200000/temp1)<<12; + F_vib=103200000/((Gyro.Frq>>16)*2); temp2 = (((BuffTemp[6]<<8) | BuffTemp[7])&0xFFFF); Gyro.AmpPer = ((((((Gyro.Frq>>12)*200)/16)*temp2)/7680000)/2); - /* sprintf((Time),"%d %d \r\n",temp2,Gyro.AmpPer); + Gyro.Amp = (Gyro.AmpPer)<<15; + /*sprintf((Time),"%d \r\n",Gyro.AmpPer); WriteCon(Time); */ // if(anser) @@ -986,11 +1117,14 @@ case 0xDD: TempParam=(BuffTemp[3]) & 0x9f; //Rate switch(TempParam) { case Rate1 : CMD_Rate(); break; - case Rate2 : Gyro.ModeOut=9; break; + case Rate2 : Gyro.ModeOut=9; break; case Rate3 : CMD_Rate3(); break; case PRate1 : Gyro.ModeOut=1; break; - case PRate2 : Gyro.ModeOut=2; break; - case PRate3 : Gyro.ModeOut=8; break; + case PRate2 : Gyro.ModeOut=2; break; + case PRate3 : Gyro.ModeOut=8; break; + case Rate7 : CMD_Rate7(); break; + case PRate7 : Gyro.ModeOut=10; break; + } break; //DropDelay(); выставка задержки для мультидропа сбрасывается в команде CMD_Maintenance() на () микросекунд; case 0xA0: /*DropDelay();*/ TempParam=(BuffTemp[3]) & 0x90; //Delta_PS @@ -1029,6 +1163,7 @@ unsigned int temp=0; if (ReadCon (Time)) { + int a=0; ///////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////// //////////////////////Выбор FlashMod/////////////////////////// @@ -1055,35 +1190,52 @@ WriteCon(Time); }*/ if (Time[0] == 'V') { // выдача технологическая - Gyro.Debag += 10; - } + F_vib += 1; + LPC_MCPWM->LIM1 = F_vib; + LPC_MCPWM->LIM2 = F_vib; + sprintf((Time),"%d \r\n", F_vib); + WriteCon(Time); + } if (Time[0] == 'v') { // выдача технологическая - Gyro.Debag -= 10; - + F_vib -= 1; + LPC_MCPWM->LIM1 = F_vib; + LPC_MCPWM->LIM2 = F_vib; + } if (Time[0] == 'C') { // выдача технологическая - Gyro.Debag += 1000; - + AmpP += 1; + T_vibP = F_vib/10000; + T_vib_1 = AmpP * T_vibP; + T_vib_2 = T_vibP * (10000-AmpP +1); + LPC_MCPWM->MAT2 = T_vib_2; + LPC_MCPWM->MAT1 = T_vib_1; } if (Time[0] == 'c') { // выдача технологическая - Gyro.Debag -= 1000; - + AmpP -= 1; + T_vibP = F_vib/10000; + T_vib_1 = AmpP * T_vibP; + T_vib_2 = T_vibP * (10000-AmpP+1 ); + + LPC_MCPWM->MAT2 = T_vib_2; + LPC_MCPWM->MAT1 = T_vib_1; } if (Time[0] == 'X') { // выдача технологическая - Spi.DAC_A += 3000; + Spi.DAC_A += 1000; sprintf((Time),"%d \r\n", Spi.DAC_A); WriteCon(Time); } if (Time[0] == 'x') { // выдача технологическая - Spi.DAC_A -= 3000; + Spi.DAC_A -= 1000; sprintf((Time),"%d \r\n", Spi.DAC_A); WriteCon(Time); } if (Time[0] == 'p') { // выдача технологическая - PlcOFF //Gyro.RgConA&0x8=0; - Gyro.LogMod=1; + // PlcOFF //Gyro.RgConA&0x8=0; + + if(Gyro.LogMod) Gyro.LogMod=0; + else Gyro.LogMod=5; // MODFlag=1; - Spi.DAC_B = 32767; + // Spi.DAC_B = 32767; } if (Time[0] == 'o') { // выдача технологическая if(Gyro.LogMod==2)Gyro.LogMod=0; @@ -1091,24 +1243,40 @@ // Spi.DAC_B = 32000; } + + if (Time[0] == 'm') { // выдача технологическая - /* for(q=0; q<32; q++) - { - sprintf((Time),"%d ", Buff_1Point[q]); - WriteCon(Time); - } - sprintf((Time),"\r\n"); - WriteCon(Time); - for(q=0; q<32; q++) - { - sprintf((Time),"%d ", Buff_Restored_sin [q]); - WriteCon(Time); - }*/ - sprintf((Time),"\r\n"); - WriteCon(Time); - } - + for(int q=0; q<64; q++) + { + sprintf((Time),"%d \r\n", BuffADC_64Point[q]); + WriteCon(Time); + } + } + + if (Time[0] == 'n') + { // выдача технологическая + for(int q=0; q<64; q++) + { + sprintf((Time),"%d \r\n", BuffADC_32PointD[q]); + WriteCon(Time); + } + } + if (Time[0] == 'b') + { // выдача технологическая + for(int q=0; q<64; q++) + { + sprintf((Time)," %d \r\n", BuffADC_1Point[ q] ); + WriteCon(Time); + } + } + + + + + + + if (Time[0] == 'u') { // выдача технологическая MODFlag=0; //Mod vibro Hz