Dmitry Kovalev
forkd
Fork of
LGstaandart
by 
Dmitry Kovalev
Diff: Command.c
- Revision:
- 225:f8fee6c586cc
- Parent:
- 224:598aec95415f
- Child:
- 226:4a4d5bd5fcd7
--- a/Command.c Tue Nov 21 07:31:05 2017 +0000
+++ b/Command.c Tue Dec 12 05:53:39 2017 +0000
@@ -39,13 +39,13 @@
}
void DropDelay(void)
-{
- Gyro.DropDelayGLD = DropDelayGLD_0;
- switch(Gyro.My_Addres) {
- case 1: Gyro.DropDelayGLD = DropDelayGLD_1; break;
- case 2: Gyro.DropDelayGLD = DropDelayGLD_2; break;
- case 3: Gyro.DropDelayGLD = DropDelayGLD_3; break;
- }
+ {
+ Gyro.DropDelayGLD = DropDelayGLD_0;
+ switch(Gyro.My_Addres) {
+ case 1: Gyro.DropDelayGLD = DropDelayGLD_1; break;
+ case 2: Gyro.DropDelayGLD = DropDelayGLD_2; break;
+ case 3: Gyro.DropDelayGLD = DropDelayGLD_3; break;
+ }
}
/*
@@ -135,7 +135,7 @@
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;
+ case 10: CMD_Rate7();Gyro.ModeOut=0;break;//if(Gyro.EvenRate7 ){ CMD_Rate7(); Gyro.EvenRate7=0;} break;
case 11: if(Gyro.EvenRate5K ){ M_Rate5K(); Gyro.EvenRate5K--;} break;
}
}
@@ -160,114 +160,66 @@
//BuffADC_64Point[ (CountFaza + Gyro.PLC_Phase) & 0x3f]
BuffClear();
- int Temp;
+ unsigned int Temp;
+ int ADC_Worm[70];
int Temp1;
float Temp2;
static unsigned int t=0;
BuffTemp[0] = Gyro.SOC_Out;
BuffTemp[1] = Gyro.My_Addres;
-
- iRate_7 = Buff_OUT[(t)] ;
- BuffTemp[ 2] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 3] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков
+ MinWorms=BuffADC_10v_F[10];
+ MaxWorms=BuffADC_10v_F[10];
+ for(int q=0; q<64; q++)
+ {
+ if(MinWorms > BuffADC_10v_F[q]) MinWorms = (BuffADC_10v_F[q]);
+ if(MaxWorms < BuffADC_10v_F[q]) MaxWorms = (BuffADC_10v_F[q]);
+ }
+ AmpWorms = (MaxWorms-MinWorms)>>1;
+
+ for(int q=0; q<64; q++)
+ {
+ ADC_Worm[q] = (BuffADC_10v_F[q] - (MinWorms+AmpWorms));// от -128 до 128
+ }
- iRate_7 = Buff_OUT[(t)+8] ;
- BuffTemp[ 4] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 5] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = Buff_OUT[(t)+16];
- BuffTemp[ 6] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 7] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = Buff_OUT[(t)+24];
- BuffTemp[ 8] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 9] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = Buff_OUT[(t)+32] ;
- BuffTemp[10] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[11] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = Buff_OUT[(t)+40] ;
- BuffTemp[12] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[13] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 =Buff_OUT[(t)+ 48];
- BuffTemp[14] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[15] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = Buff_OUT[(t)+56] ;
- BuffTemp[16] =(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[17] =(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
+ for(int q=32; q<64; q++)
+ {
+ ADC_Worm[q] = ADC_Worm[q] * (-1);
+ }
+
+ for(int q=0; q<64; q++)
+ {
+ ADC_Worm[q] = ADC_Worm[q] + AmpWorms+1;
+ Temp = (ADC_Worm[q])>>9;
+ BuffTemp[q+2] = (Temp) & 0xff;//младший байт разности счетчиков
+ }
-
-
-
- iRate_7 = ModArraySin[(t + Gyro.PLC_Phase)&0x3f] - 50;
- BuffTemp[ 18] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 19] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = ModArraySin[(t + 8 + (Gyro.PLC_Phase))&0x3f] - 50;
- BuffTemp[ 20] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 21] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = ModArraySin[(t + 16 + (Gyro.PLC_Phase))&0x3f] - 50;
- BuffTemp[ 22] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 23] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
- iRate_7 = ModArraySin[(t + 24 + (Gyro.PLC_Phase))&0x3f] - 50;
- BuffTemp[ 24] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 25] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = ModArraySin[(t + 32 + (Gyro.PLC_Phase))&0x3f] - 50;
- BuffTemp[ 26] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 27] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = ModArraySin[(t + 40 + (Gyro.PLC_Phase))&0x3f] - 50;
- BuffTemp[ 28] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 29] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = ModArraySin[(t + 48 + (Gyro.PLC_Phase))&0x3f] - 50;
- BuffTemp[ 30] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 31] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
-
- iRate_7 = ModArraySin[(t + 56+ (Gyro.PLC_Phase))&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>>3;
- BuffTemp[ 36] =(Rate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 37] =(Rate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ iRate_7 = ((Spi.DAC_B-0x7fff)&0xffff);
+ BuffTemp[66]=(iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[67]=(iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+
+ Rate_7 = Gyro.PLC_Eror_count>>3;
+ BuffTemp[68]=(Rate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[69]=(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 1: iRate_7 = 0xffff-Spi.DAC_A; break;
case 0: Temp2 = (Spi.DAC_A*1.538) - 0xd80f;
iRate_7 = (int)Temp2;
- //iRate_7 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff);
break;
}
- //Rate_7 = 30000;//(unsigned int)(iRate_7&0xffff);
- BuffTemp[ 38] = (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
- BuffTemp[ 39] = (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
+ Rate_7 = (unsigned int)(iRate_7&0xffff);
+ BuffTemp[70]= (iRate_7 >> 8) & 0xff;//младший байт разности счетчиков
+ BuffTemp[71]= (iRate_7 >> 0) & 0xff;//младший байт разности счетчиков;
- Check(BuffTemp, 42);
- WriteConN (BuffTemp,42);
- /* if(t<60)t+=32;
- else t =0;*/
-
+ Check(BuffTemp,74);
+ WriteConN (BuffTemp,74);
}
@@ -317,12 +269,12 @@
switch(NP)
{
case 0: Gyro.My_Addres = GyroP.Str.My_Addres; break;
- case 1: Gyro.HFO_ref = /*GyroP.Str.HFO_ref>>1;*/(unsigned int)(GyroP.Str.HFO_ref)*0.82; break;
+ case 1: Gyro.HFO_ref = /*GyroP.Str.HFO_ref>>1;*/(unsigned int)(GyroP.Str.HFO_ref) ; break;
case 2: Gyro.HFO_Gain = GyroP.Str.HFO_Gain; break;
- case 3: if(Gyro.LG_Type) Gyro.HFO_Max=((int)(GyroP.Str.DAC_current_Work*0.67)-2000);
+ case 3: if(Gyro.LG_Type) Gyro.HFO_Max=0xffff-GyroP.Str.DAC_current_Work; //((int)(GyroP.Str.DAC_current_Work*0.67)-2000);
else Spi.DAC_A = ((((int)(GyroP.Str.DAC_current_Work+ 0x7fff)&0xffff)+22544)*0.65); break;
- case 4: if(Gyro.LG_Type) Gyro.HFO_Min=((int)(GyroP.Str.DAC_current_Start*0.67)-2000); break;
+ case 4: if(Gyro.LG_Type) Gyro.HFO_Min=0xffff-GyroP.Str.DAC_current_Start; break; //((int)(GyroP.Str.DAC_current_Start*0.67)-2000); break;
case 5: Gyro.TimeToJump = GyroP.Str.TimeToJump; break;
case 6: Gyro.JumpDelta = GyroP.Str.JumpDelta; break;
@@ -340,8 +292,7 @@
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);
+
temp=temp*20;
Gyro.AmpTarget=(unsigned int)(temp);
break;
@@ -368,13 +319,25 @@
case 60: Gyro.GLD_Serial = GyroP.Str.GLD_Serial; break;
- case 10: Gyro.DownTreshold =0x7fff+GyroP.Str.DownTreshold; break;
- case 11: Gyro.HighTreshold =0x7fff+GyroP.Str.HighTreshold; break;
- case 9: Gyro.PLCDelay = GyroP.Str.PLCDelay/10; break;
- /*case 105: Gyro.ResetLevelCool = GyroP.Str.ResetLevelCool; break;
- case 55: Gyro.ResetLevelHeat = GyroP.Str.ResetLevelHeat; break;*/
+ case 10: Gyro.DownTreshold = (unsigned int)((GyroP.Str.DownTreshold-0x7fff)&0xffff); break;
+ case 11: Gyro.HighTreshold = (unsigned int)((GyroP.Str.HighTreshold-0x7fff)&0xffff); break;
+
+ case 9: Gyro.PLCDelay = GyroP.Str.PLCDelay; break;
+
+ case 105:Gyro.ResetLevelCool = (unsigned int)((GyroP.Str.ResetLevelCool-0x7fff)&0xffff); break;
+ case 55: Gyro.ResetLevelHeat = (unsigned int)((GyroP.Str.ResetLevelHeat-0x7fff)&0xffff); break;
+
+
+
+
+ //27
+
+ /// Gyro.PLCDelay = GyroP.Str.PLCDelay;
+ //105
+
+ /*
case 39: Gyro.ResetLevelCool = 0x7fff+GyroP.Str.ResetLevelCool; break;
- case 31: Gyro.ResetLevelHeat = 0x7fff+GyroP.Str.ResetLevelHeat; break; //27
+ case 31: Gyro.ResetLevelHeat = 0x7fff+GyroP.Str.ResetLevelHeat; break; */ //27
//105
@@ -620,7 +583,7 @@
//температурный канал 5
case 20:
OutDeltaPS = 0;
- OutDeltaPS = Gyro.TermLM;//Spi.ADC1;// //Gyro.Termo;//0xa4=164
+ OutDeltaPS = Gyro.Termo;//Spi.ADC1;// //Gyro.Termo;//0xa4=164
BuffTemp[5] =(OutDeltaPS >> 8) & 0xff;//температура
break;
@@ -643,29 +606,19 @@
void M_Rate5K(void)
{
- unsigned int Temp;
- BuffTemp[ 0] = Gyro.SOC_Out; //1 --старт данных
- BuffTemp[ 1] = Gyro.My_Addres; //2 --адрес отвечающего устройствва
-
+ unsigned int Temp;
//Temp=Gyro.CuruAngle; //приводим к форме вывода
BuffTemp[ 3] =(Gyro.CuruAngle >> 8) & 0xff;//старший байт разности счетчиков 7 --младший байт разности счетчиков
BuffTemp[ 2] =(Gyro.CuruAngle >> 0) & 0xff;//младший байт разности счетчиков 8 --старший байт разности счетчиков
Gyro.CuruAngle=0;
- // Temp =(Spi.DAC_B-0x7fff);
+ Temp = ((Spi.DAC_B-0x7fff)&0xffff);
BuffTemp[5] = (Spi.DAC_B >> 8) & 0xff; //23 Выход регулятора рабочего периметра
BuffTemp[4] = (Spi.DAC_B >> 0) & 0xff;
- /* switch(Gyro.LG_Type)
- {
- case 1: Temp =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break;
-
- case 0: // Temp2 = (Spi.DAC_A*1.538) - 0xd80f;
- Temp =(int)(((float)(Spi.DAC_A)/0.64)- 56000); break;
- }
- */
- Temp=Spi.DAC_A;
+
+ Temp = 0xffff - Spi.DAC_A;
BuffTemp[7] =(Temp >> 8) & 0xff;//выход регулятора гвч; 11 --выход регулятора ГВЧ
BuffTemp[6] =(Temp >> 0) & 0xff;
@@ -718,7 +671,7 @@
//(unsigned int)((Gyro.DacIn+30000)*0.6667);
switch(Gyro.LG_Type)
{
- case 1: Temp1 =(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break;
+ case 1: Temp1 = 0xffff - Spi.DAC_A; break;//(int)(((float)(Spi.DAC_A)/0.6667)- 0xefff); break;
case 0: // Temp2 = (Spi.DAC_A*1.538) - 0xd80f;
Temp1 =(int)(((float)(Spi.DAC_A)/0.64)- 56000); break;
@@ -731,6 +684,7 @@
if(Gyro.LG_Type) Temp =(int)(tempDeltaRegul);
else Temp =(unsigned int)((0x7fff-Spi.ADC5)*0.79)<<1;
+ tempDeltaRegul=0;
BuffTemp[12]=(Temp >> 8) & 0xff;//// HFO 13 --сигнал ошибки регулятора ГВЧ
BuffTemp[13]=(Temp >> 0) & 0xff;//// 14 --сигнал ошибки регулятора ГВЧ
@@ -754,7 +708,7 @@
BuffTemp[20] = (Temp >> 8) & 0xff;//регулятор датчика угла поворота //21 старший байт регулятора датчика угла поворота
BuffTemp[21] = (Temp >> 0) & 0xff; //22
- Temp1 =(int)(3300+(0x7fff+(Spi.DAC_B*1.083)));
+ Temp1 =((Spi.DAC_B-0x7fff)&0xffff);///(3300+(0x7fff+(Spi.DAC_B*1.083)));
BuffTemp[22] = (Temp1 >> 8) & 0xff; //23 Выход регулятора рабочего периметра
BuffTemp[23] = (Temp1 >> 0) & 0xff;
//24
@@ -798,6 +752,7 @@
void CMD_Rate2(void)
{
unsigned int Temp;
+ //float y = 52646.45
Gyro.Reper_Event=0;
for(int q=0; q<64; q++)
{
@@ -851,26 +806,14 @@
if(HFO)
{
Gyro.DacIn=temp;
- if(Gyro.LG_Type==1) Spi.DAC_A = ((unsigned int)((Gyro.DacIn+0x7011)*0.6667));
+ if(Gyro.LG_Type==1) Spi.DAC_A = 0xffff-Gyro.DacIn; //((unsigned int)((Gyro.DacIn+0x7011)*0.6667));
else Spi.DAC_A = (unsigned int)(0xffff-Gyro.DacIn); //Spi.DAC_A = ((unsigned int)((Gyro.DacIn+23200)*0.64));
-
- /*Temp2 = (Spi.DAC_A*1.538) - 0xd80f;
- Temp1= (int)Temp2; */
-
}
- else Spi.DAC_B = (unsigned int)temp;
- /*
- sprintf((Time),"%d %d %f \r\n", temp, Spi.DAC_A, (temp+10744)*0.79);
- WriteCon(Time);
- */
-
- /*{
- DACF =(temp*K_DAC)+deltaDAC;
- Spi.DAC_B =(unsigned int)(DACF) (unsigned int)(temp*K_DAC+deltaDAC); // K_DAC);
- }*/
- /* if(Consol)
- {*/
- for(int q=0; q<64; q++)
+
+ else Spi.DAC_B = (unsigned int)((temp-0x7fff)&0xffff);
+ sprintf((Time),"%d %d \r\n", Spi.DAC_B, temp);
+ WriteCon(Time);
+ for(int q=0; q<64; q++)
{
BuffTemp[q]=0;
}
@@ -880,26 +823,18 @@
BuffTemp[3] =0;
Check(BuffTemp, CRC_N);
WriteConN (BuffTemp,CRC_N);
- /* }*/
}
void CMD_M_vib()
{
unsigned int temp1,temp2,anser;
- // anser = BuffTemp[3]>>7;
-
temp1 = (((BuffTemp[4]<<8) | BuffTemp[5])&0xFFFF);
- Gyro.Frq = (122780000/temp1)<<12;
- // Gyro.Frq = (103200000/temp1)<<12;
+ Gyro.Frq = (122780000/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);
Gyro.Amp = (Gyro.AmpPer)<<15;
- /*sprintf((Time),"%d \r\n",Gyro.AmpPer);
- WriteCon(Time); */
-
- // if(anser)
- // {
+
BuffTemp[0] = Gyro.SOC_Out;
BuffTemp[1] = Gyro.My_Addres;
BuffTemp[2] = Gyro.CMD_In;
@@ -907,7 +842,6 @@
BuffTemp[4] = 0x0;
Check(BuffTemp, CRC_N);
WriteConN (BuffTemp,CRC_N);
- // }
}
void CMD_M_Control_D8()///установка\сброс регистров управления
{
@@ -1197,7 +1131,7 @@
case PRate2 : Gyro.ModeOut=2; break;
case PRate3 : Gyro.ModeOut=8; break;
case Rate7 : CMD_Rate7(); break;
- case PRate7 : Gyro.ModeOut=10; break;
+ case PRate7 : Gyro.ModeOut=10; break;
case Rate5K : M_Rate5K(); break;
case PRate5K:
if(Gyro.ModeOut){}
@@ -1240,14 +1174,32 @@
void TechLog(void)
{
unsigned int temp=0;
- /*if (ReadCon (Time))
+ if (ReadCon (Time))
{
int a=0;
+
+
+
+
+ if (Time[0] == 'y')
+ { //Mod 250 Hz PLCRegul250();
+ if( Gyro.Debag) Gyro.Debag=0;
+ else Gyro.Debag=1;
+ }
+ if (Time[0] == 'u')
+ { //Mod 250 Hz PLCRegul250();
+ if( Gyro.Debag2) Gyro.Debag2=0;
+ else Gyro.Debag2=1;
+ }
+ if (Time[0] == 'C') Spi.DAC_A+=200;
+ if (Time[0] == 'c') Spi.DAC_A-=200;
+ if (Time[0] == 'X') Spi.DAC_B+=2000;
+ if (Time[0] == 'x') Spi.DAC_B-=2000;
/////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////
//////////////////////Выбор FlashMod///////////////////////////
////////////////////////////////////////////////////////////////
-
+ /*
if (Time[0] == 'B') Gyro.FlashMod=1;
if (Time[0] == 'R') {Gyro.FlashMod=3;}
if (Time[0] == 'E') {Gyro.FlashMod=4;}
@@ -1284,10 +1236,7 @@
- if (Time[0] == 'C') Spi.DAC_A+=200;
- if (Time[0] == 'c') Spi.DAC_A-=200;
- if (Time[0] == 'X') Spi.DAC_B+=2000;
- if (Time[0] == 'x') Spi.DAC_B-=2000;
+
@@ -1337,7 +1286,7 @@
}
}
-
+
if (Time[0] == 'n')
{ // выдача технологическая
for(int q=0; q<64; q++)
@@ -1364,7 +1313,7 @@
-
+
if (Time[0] == 'u')
{ // выдача технологическая
@@ -1375,14 +1324,7 @@
Gyro.ShowMod2=0; //Mod vibro Hz
}
- if (Time[0] == 'y')
- { //Mod 250 Hz PLCRegul250();
-
- if( Gyro.Debag) Gyro.Debag=0;
- else Gyro.Debag=1;
-
-
- }
+
if (Time[0] == 'f')
{ // выдача технологическая
Gyro.CuruAngleLog = 0; //Mod vibro Hz
@@ -1420,9 +1362,9 @@
Gyro.Gain_Cos = GyroP.Str.Gain_Cos;
Out_G_photo(Gyro.Gain_Sin, Gyro.Gain_Cos);
}
-
+ */
- }*/
+ }
}