Diff: Command.c

Revision:

102:4270092be987

Parent:

98:95b8e79f13e1

Child:

103:e96f08947def

--- a/Command.c	Tue Mar 29 17:00:59 2016 +0000
+++ b/Command.c	Wed Mar 30 13:27:39 2016 +0000
@@ -9,7 +9,7 @@
 unsigned int Param2=0;
 unsigned int Param3=0;
 unsigned int Param4=0;
-
+unsigned int Consol=123;
 /*
 void exec_CMD(void)       //r. === процедура окончательной дешифрации и исполнения команды
 {
@@ -258,31 +258,35 @@
 void CMD_M_Stymul()
 {
     int temp;
+    int HFO;
+    temp=BuffTemp[3];
+    Consol = temp&0x3;
+    HFO    = temp&0x3;
     temp =((BuffTemp[4]<<8) | BuffTemp[5]);
     // temp=0;
     temp=temp&0xFFFF;
+    
+    if(HFO)
+    {
+     Spi.DAC_A=(unsigned int)temp;
+    }
+    else{
     DACF =(temp*K_DAC)+deltaDAC;
     Spi.DAC_B =(unsigned int)(DACF) /*(unsigned int)(temp*K_DAC+deltaDAC)*/; // K_DAC);
-
-    //int temp;
-//	temp =((BuffTemp[4]<<8) | BuffTemp[5]);
-    //temp = 65535 - temp;
-    /*if(temp<1000)
-    {
-    	temp=0;
     }
-    else if(temp>60000)
-    {
-    	temp=65535;
-    }*/
-    //Spi.DAC_B =(((BuffTemp[4]<<8) | BuffTemp[5])*K_DAC+deltaDAC);/*((BuffTemp[4]<<8) | BuffTemp[5])-0xFFFF*///;  //((rcv_buf[4] << 8) | (int)rcv_buf[5]);K_DAC+deltaDAC
-    //BuffTemp[0] = temp;      //DD
-    //WriteConN (BuffTemp,1);
 }
 
 void CMD_M_vib()
-{
- 
+{   LoopOn
+    unsigned int temp1,temp2;
+
+    temp1 =((BuffTemp[4]<<8) | BuffTemp[5]);
+    temp1=temp1&0xFFFF;
+    
+    temp2 =((BuffTemp[6]<<8) | BuffTemp[7]);
+    temp2=temp2&0xFFFF;
+    Gyro.Frq = ((unsigned int) ((7680000*16/temp1)))*16*16*16; 
+    Consol=Gyro.Frq ;
 }
 unsigned int Check(char *c, unsigned int Count)
 {
@@ -359,11 +363,11 @@
             lengf=6;
             CRC_N=8;
             break;
-        case SUBCMD_M_VIB_W: 
+            
+        case 0xE4: 
             lengf=8;
-            CRC_N=10;
             break;
-        break;
+       
     }
     
     return lengf;
@@ -384,7 +388,7 @@
             ReadCon1(BuffTemp);
             Gyro.RsErrLine += 0x100;//WriteCon("\n\r ...Error.... ");
             BuffTemp[99]=Gyro.RsErrLine; 
-            LoopOn
+            
         }
     } else if(CountBuFFIn==2) {		//если второй ,fqn
         if (BuffTemp[1] != Gyro.My_Addres) {
@@ -412,6 +416,13 @@
         Param2=BuffTemp[4];
         Param3=BuffTemp[5];
     } 
+    else if((CountBuFFIn==8)&&(N==10))
+     {
+        Param1=BuffTemp[4];
+        Param2=BuffTemp[5];
+        Param3=BuffTemp[6];
+        Param4=BuffTemp[7];
+    } 
   
     else if(CountBuFFIn > (N-1))
      {
@@ -431,11 +442,12 @@
                     break;
                 case 0xDD:
                     Rate_Flag=1;
-                    break;
+                    break; 
+               case 0xE4:
+                    CMD_M_vib();
+                    break; 
                     Gyro.RsErrLine = 0;
-            
-
-        }
+     }
     }
 }