Dmitry Kovalev
/
LG2
fork
Fork of LG by
Diff: Command.c
- Revision:
- 210:b02fa166315d
- Parent:
- 209:224e7331a061
- Child:
- 211:ac8251b067d2
diff -r 224e7331a061 -r b02fa166315d Command.c --- a/Command.c Thu Apr 13 14:14:45 2017 +0000 +++ b/Command.c Mon Apr 24 05:43:54 2017 +0000 @@ -190,7 +190,9 @@ case 16: Gyro.FrqHZmax = (7680000 / GyroP.Str.FrqHZmax)<<16; break; case 18: temp=((GyroP.Str.VB_Fdf_Hi<<16) | GyroP.Str.VB_Fdf_Lo); - temp=temp/18.2; + /*sprintf((Time),"%d \r\n", temp/32); + WriteCon(Time);*/ + temp=temp*20; Gyro.AmpTarget=(unsigned int)(temp); break; @@ -287,7 +289,8 @@ WriteConN (BuffTemp,9); } void CMD_Delta_PS(void) -{ int drob; +{ int drob,Temp1; + float Temp2=0; Gyro.EXT_Latch=0; Gyro.Event_500Hz=0; unsigned int Temp; @@ -296,7 +299,7 @@ Gyro.CuruAngle = Gyro.CuruAngle + OldCuruAngle; //(72 град/с) - OldCuruAngle=Gyro.CuruAngle & 0x7; //сохраняем 5 бит для след измирений + OldCuruAngle=Gyro.CuruAngle & 0x7; //сохраняем 3 бит для след измирений Temp=Gyro.CuruAngle>>3; //приводим к форме вывода Gyro.CuruAngle = 0; @@ -319,19 +322,21 @@ //HFO case 2: - Temp = Spi.DAC_A-0x7fff; - BuffTemp[5] = (Temp >> 8) & 0xff; + Temp2 = ((Spi.DAC_A/0.65)-22544)-0x7fff; + Temp1= (int)Temp2; + // Temp = Spi.DAC_A-0x7fff; + BuffTemp[5] = (Temp1 >> 8) & 0xff; break; case 3: - BuffTemp[5] = (Temp >> 0) & 0xff; + BuffTemp[5] = (Temp1 >> 0) & 0xff; break; //T_Vibro case 4: - Temp = (unsigned int)((7675000*16/(Gyro.Frq>>12))); + Temp = (unsigned int)(7680000/(Gyro.Frq>>16)); BuffTemp[5] = (Temp >> 8) & 0xff; break; @@ -343,7 +348,8 @@ //L_Vibro case 6: - Temp = Gyro.L_vibro>>1; + //Temp = Gyro.L_vibro>>1; + Temp = Gyro.L_vibro; BuffTemp[5] = (Temp >> 8) & 0xff; break; @@ -382,7 +388,8 @@ //f case 12: - Temp = Gyro.AD_Slow >> 16; + Temp =(unsigned int)(0x7fff-Spi.ADC5)*0.79; + // Temp = Gyro.AD_Slow >> 16; BuffTemp[5] = (Temp >> 8) & 0xff; break; @@ -395,7 +402,8 @@ //ток 1 case 14: - Temp = Gyro.In1; + //Temp = Gyro.In1; + Temp1 = ((Gyro.In1>>1)*0.800875)-0x3e0b; BuffTemp[5] = (Temp >> 8) & 0xff;//in1//2 break; @@ -408,8 +416,9 @@ //ток 2 case 16: - Temp = Gyro.In2; - BuffTemp[5] = (Temp >> 8) & 0xff; + // Temp = Gyro.In2; + Temp1 = ((Gyro.In2>>1)*0.800875)-0x3e0b; + BuffTemp[5] = (Temp >> 8) & 0xff; break; case 17: @@ -450,45 +459,6 @@ } -void CMD_Rate2(void) -{ - unsigned int Temp; - Gyro.Reper_Event=0; - - BuffTemp[ 0] = Gyro.SOC_Out; - BuffTemp[ 1] = Gyro.My_Addres; - - - Temp=Gyro.CaunPlusReper; - BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт счетчика +. - BuffTemp[ 3] =(Temp >> 0) & 0xff;//младший байт счетчика +. - - //сохраняем 5 бит для след измирений - - Temp= Gyro.CaunMinReper; - BuffTemp[ 4] =(Temp >> 8) & 0xff;//старший байт счетчика -. - BuffTemp[ 5] =(Temp >> 0) & 0xff;//младший байт счетчика -. - - Check(BuffTemp, 8); - WriteConN (BuffTemp,8); -} - -void CMD_Rate3(void) -{ - Gyro.Rate3_Event=0; - BuffTemp[ 0] = Gyro.SOC_Out; - BuffTemp[ 1] = Gyro.My_Addres; - BuffTemp[ 2] = 0xcc;//старший байт счетчика +. - BuffTemp[ 3] = 0xcc;//младший байт счетчика +. - - BuffTemp[ 4] =0xBB;//старший байт счетчика -. - BuffTemp[ 5] =0xBB;//младший байт счетчика -. - - Check(BuffTemp, 8); - WriteConN (BuffTemp,8); -} - - void CMD_Rate(void) { Gyro.Rate1_Event=0; @@ -553,7 +523,7 @@ BuffTemp[13]=(Temp >> 0) & 0xff;//// 14 --сигнал ошибки регулятора ГВЧ //Temp = (unsigned int)((7675000*16/(Gyro.Frq>>12))); - Temp = (unsigned int)(7675000/(Gyro.Frq>>16)); + Temp = (unsigned int)(7680000/(Gyro.Frq>>16)); BuffTemp[14] = (Temp >> 8) & 0xff; //15 период вибропривода.(частота) T_Vibro BuffTemp[15] = (Temp >> 0) & 0xff; //16 период вибропривода. @@ -566,7 +536,7 @@ Temp =Gyro.L_vibro/*>>1*/;//(unsigned int)(((7675000*16/200) * Gyro.AmpPer /(Gyro.Frq>>12))); BuffTemp[18] = (Temp >> 8) & 0xff; //19 длительность импулься вибропривода(амплитуда) BuffTemp[19] = (Temp >> 0) & 0xff; //20 - + Temp = 0x0;//Gyro.L_vibro<<2; BuffTemp[20] = (Temp >> 8) & 0xff;//регулятор датчика угла поворота //21 старший байт регулятора датчика угла поворота @@ -614,6 +584,44 @@ for(int i=0; i<45;i++) {BuffTemp[i]=0;} } +void CMD_Rate2(void) +{ + unsigned int Temp; + Gyro.Reper_Event=0; + + BuffTemp[ 0] = Gyro.SOC_Out; + BuffTemp[ 1] = Gyro.My_Addres; + + + Temp=Gyro.CaunPlusReper; + BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт счетчика +. + BuffTemp[ 3] =(Temp >> 0) & 0xff;//младший байт счетчика +. + + //сохраняем 5 бит для след измирений + + Temp= Gyro.CaunMinReper; + BuffTemp[ 4] =(Temp >> 8) & 0xff;//старший байт счетчика -. + BuffTemp[ 5] =(Temp >> 0) & 0xff;//младший байт счетчика -. + + Check(BuffTemp, 8); + WriteConN (BuffTemp,8); +} + +void CMD_Rate3(void) +{ + Gyro.Rate3_Event=0; + BuffTemp[ 0] = Gyro.SOC_Out; + BuffTemp[ 1] = Gyro.My_Addres; + BuffTemp[ 2] = 0xcc;//старший байт счетчика +. + BuffTemp[ 3] = 0xcc;//младший байт счетчика +. + + BuffTemp[ 4] =0xBB;//старший байт счетчика -. + BuffTemp[ 5] =0xBB;//младший байт счетчика -. + + Check(BuffTemp, 8); + WriteConN (BuffTemp,8); +} + void CMD_M_Stymul() { int temp,Consol=0,HFO,b4; @@ -665,8 +673,9 @@ Gyro.Frq = (122780000/temp1)<<12; temp2 = (((BuffTemp[6]<<8) | BuffTemp[7])&0xFFFF); - Gyro.AmpPer = ((((((Gyro.Frq>>12)*200)/16)*temp2)/7675000)/2); - + Gyro.AmpPer = ((((((Gyro.Frq>>12)*200)/16)*temp2)/7680000)/2); + /* sprintf((Time),"%d %d \r\n",temp2,Gyro.AmpPer); + WriteCon(Time); */ // if(anser) // { @@ -959,10 +968,10 @@ case 0xDD: TempParam=(BuffTemp[3]) & 0x9f; //Rate switch(TempParam) { case Rate1 : CMD_Rate(); break; - case Rate2 : CMD_Rate2(); break; + case Rate2 : CMD_Rate2(); break; case Rate3 : CMD_Rate3(); break; case PRate1 : Gyro.ModeOut=1; break; - case PRate2 : Gyro.ModeOut=2; break; + case PRate2 : Gyro.ModeOut=2; break; case PRate3 : Gyro.ModeOut=8; break; } break; //DropDelay(); выставка задержки для мультидропа сбрасывается в команде CMD_Maintenance() на () микросекунд;