Diff: Command.c

Revision:

104:ab1cb4ff56b2

Parent:

103:e96f08947def

Child:

105:bd01d8d20fb6

--- a/Command.c	Wed Mar 30 14:20:03 2016 +0000
+++ b/Command.c	Thu Mar 31 10:16:56 2016 +0000
@@ -165,75 +165,56 @@
 
 void CMD_M_Control_D8()///установка\сброс регистров управления
 {
-    int bit,NReg;
+    unsigned int bit,NReg,Pa;
+    unsigned int SR,V,A,Bit_num;
+    SR=0;
+    V=0;
+    A=0;
+    Bit_num=0;
+    
     BuffTemp[0] = Gyro.SOC_Out;      //DD
     BuffTemp[1] = Gyro.My_Addres;    //00
     BuffTemp[2] = Gyro.CMD_In;       //D8
-    //10?
-    if((Param1&0x80)) {
-        bit=1;
-    } else {
-        bit=0;
-    }
-
-    if ((Param1 & 0x10) == 0) {
-        NReg=0;
-        switch(Param1&0xf) { // Gyro.RgConA
-            case 0x0:
-                Gyro.RgConA|=bit;
-                break;
-
-            case 0x1:
-                Gyro.RgConA|=bit<<0x1;
-                break;
-
-            case 0x2:
-                Gyro.RgConA|=bit<<0x2;
-                break;
-
-            case 0x3:
-                Gyro.RgConA|=bit<<0x3;
-                break;
-
-            case 0x4:
-                Gyro.RgConA|=bit<<0x4;
-                break;
+    
+/*  
+  
+ FrqON  
+PlcON
+ HFOOFF  
+HFOON 
 
-            case 0x5:
-                Gyro.RgConA|=bit<<0x5;
-                break;
-
-            case 0x6:
-                Gyro.RgConA|=bit<<0x6;
-                break;
+ PlcOFF  
+ PlcON
+ */
+    Pa = BuffTemp[3];
+    
+    SR = Pa >> 0x7;
+    
+    V  = Pa >> 0x5;
+    V  = V  &  0x3;
+    
+    A  = Pa >> 0x4;
+    A  = A  &  0x1;
+    
+    Bit_num = Pa & 0xf;
+    
+    if(SR) {
+    	 switch (Bit_num){
+    	 case 0x06:
+    	 //FrqOff
+    	 FrqON
+    	 case 0x03:
+    	 //FrqOff
+    	// AHFOON
+    	 }
+    } 
+   else{
+   switch (Bit_num){
+   	 case 0x06:
+    	 FrqOff
+    	}
+   } 
 
-            case 0x7:
-                Gyro.RgConA|=bit<<0x7;
-                break;
-        }
-        BuffTemp[3] = NReg<<4;
-        BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
-        BuffTemp[5] =  Gyro.RgConA     & 0xff;
-    } else {
-        NReg=1;            // Gyro.RgConB
-        switch(Param1&0xf) { // Gyro.RgConB
-            case 0x0:
-                Gyro.RgConB|=bit;
-                break;
-            case 0x1:
-                Gyro.RgConB|=bit<1;
-                break;
-            case 0x2:
-                Gyro.RgConB|=bit<2;
-                break;
-        }
-        BuffTemp[3] =  NReg<<4;
-        BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
-        BuffTemp[5] =  Gyro.RgConB      & 0xff;
-    }
-
-    Check(BuffTemp, CRC_N);
-    WriteConN (BuffTemp,CRC_N);
 }
 
 void CMD_M_Control_D9()///чтение регистров управления