Dmitry Kovalev
fork
Diff: Command.c
- Revision:
- 210:b02fa166315d
- Parent:
- 209:224e7331a061
- Child:
- 211:ac8251b067d2
--- 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() на () микросекунд;