File content as of revision 136:19b9e6abb86f:

#include "stdlib.h"
#include "LPC17xx.h"
#include "Global.h"
int tempNH,tempNL;
unsigned int Rate_Flag;
unsigned int CountBuFFIn;
unsigned int N=0,CRC_N;
unsigned int Param1=0;
unsigned int Param2=0;
unsigned int Param3=0;
unsigned int Param4=0;
unsigned int Consol=123;
unsigned int TempParam=1;
unsigned int CountParam=0;




unsigned int pDestT ; 
GyroParam *Flash;
  
 
 void ReadFlash ( void)
 {
  WriteCon("\r\n Pirivet Flash");
  pDestT= (unsigned int) (0x10000);
  Flash = (GyroParam*) pDestT;
  GyroP = *(Flash);
  sprintf((Time)," dffddfd <%07d> ",	GyroP.Str.K_WP_rst_heating);
  WriteCon(Time);
 
 }
 
 
 
 unsigned int memdump( char *base, int n )
{
  
    unsigned int    *p;

    printf( "  memdump from 0x%08X for %d bytes", (unsigned long)base, n );

    p   = (unsigned int *)((unsigned int)base & ~(unsigned int)0x3);

    for ( int i = 0; i < (n >> 2); i++, p++ ) {
        if ( !(i % 4) )
            printf( "\r\n  0x%08X :", (unsigned int)p );
            printf( " 0x%08X", *p );
        }

    printf( "\r\n" );


}


void CMD_M_Param_R(void)
{
   unsigned int NP;
   unsigned int Param;
   BuffTemp[0] =  Gyro.SOC_Out;
   BuffTemp[1] =  Gyro.My_Addres;
   
   NP    = BuffTemp[3]; 
   
   Param = GyroP.Array[NP];
   
   BuffTemp[2] =(Param >> 8) & 0xff;//старший байт требуемого параметра
   BuffTemp[3] =(Param >> 0) & 0xff;//младший байт требуемого параметра
   
   Check(BuffTemp, 6);
   WriteConN (BuffTemp,6);
}
void CMD_M_Param_W(void)
{
   unsigned int NP;
   unsigned int Param;
   
   NP     =  BuffTemp[3];
   Param  = (BuffTemp[4]<<8);
   Param |=  BuffTemp[5];
 
  GyroP.Array[NP] = Param;
     
   
   BuffTemp[0] =  Gyro.SOC_Out;
   BuffTemp[1] =  Gyro.My_Addres;
   BuffTemp[2] =  Gyro.CMD_In;
   BuffTemp[3] =  0x00;//старший байт требуемого параметра
   
   Check(BuffTemp, 6);
   WriteConN (BuffTemp,6);
}
void CMD_Maintenance(void)
{

    BuffTemp[0] =  Gyro.SOC_Out;
    BuffTemp[1] =  Gyro.My_Addres;
    BuffTemp[2] =  Gyro.Firmware_Version;
    BuffTemp[3] = Gyro.GLD_Serial;
    BuffTemp[4]=0x00;
    BuffTemp[5]=0x00;
    Check(BuffTemp, 8);
    WriteConN (BuffTemp,8);
   
}
void CMD_Delta_500Hz(void)
{   
    Gyro.Delta500_Event=0;
    unsigned int Temp;
	BuffTemp[ 0] = Gyro.SOC_Out;
    BuffTemp[ 1] = Gyro.My_Addres;
    
    Temp = Gyro.CuruAngle;
    Gyro.CuruAngle = 0;

    BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт разности счетчиков
    BuffTemp[ 3] =(Temp >> 0) & 0xff;//младший байт разности счетчиков
    
    BuffTemp[ 4] = CountParam;
     switch(CountParam) {
   
   
      
    //F_ras  
     case 0:
           Temp = Gyro.F_ras;
           BuffTemp[5] = (Temp >> 8) & 0xff;
     break;  
     
     case 1:
           BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
    
    
     
     //HFO
     case 2:
           Temp = 0;//HFO
           BuffTemp[5] = (Temp >> 8) & 0xff;
     break;     
     
     case 3:
           Temp = 0;//HFO
           BuffTemp[5] = (Temp >> 0) & 0xff;
     break;  
   
   
     
     //T_Vibro
     case 4:
           Temp = (unsigned int)((7680000*16/(Gyro.Frq>>12))); 
           BuffTemp[5] = (Temp >> 8) & 0xff;
     break;  
    
     case 5:
           BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
    
    
     
     //L_Vibro  
     case 6:
          Temp = (unsigned int)(((7680000*16/200) *Gyro.AmpPer /(Gyro.Frq>>12))); 
          BuffTemp[5]  =  (Temp >> 8) & 0xff;
 
     break;  
   
     case 7:
          BuffTemp[5]  =  (Temp >> 0) & 0xff;
     break;  
     
     
     
     //Напряжение на регуляторе периметра
     case 8:
          Temp = Spi.DAC_B - 0x7fff;
          BuffTemp[5] =  (Temp >> 8) & 0xff;
  
     break;  
   
     case 9:
          BuffTemp[5] =  (Temp >> 0) & 0xff;
     break;  
     
     
     
     //темпкратурный канал 0
     case 10:
          Temp = 100;
          BuffTemp[5] =  (Temp >> 8) & 0xff;

     break;  
    
     case 11:
         BuffTemp[5] =  (Temp >> 0) & 0xff;
     break;
     
       
       
     //темпкратурный канал 1 
     case 12:
        Temp = Gyro.AD_Slow >> 16;
        BuffTemp[5] = (Temp >> 8) & 0xff;
   
     break;  
   
     case 13:
        BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
     
          
      
     //ток 1
     case 14:
        Temp = Gyro.In1;
        BuffTemp[5] = (Temp >> 8) & 0xff;//in1//2
        
     break;  
    
     case 15:
        BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
     
      
      
     //ток 2
     case 16:
        Temp = Gyro.In2;
        BuffTemp[5] = (Temp >> 8) & 0xff;
     break;  
     
     case 17:
        BuffTemp[5] = (Temp >> 0) & 0xff;//in2//3      
     break;  
     
     
     
     //разностный температурный канал Delta Temp
     case 18:
        Temp = Gyro.DeltaT;
        BuffTemp[5] = (Temp >> 8) & 0xff;//дельта
     break;  
    
     case 19:
        BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
     
     
     
     //температурный канал 5
     case 20:
        Temp = Gyro.Termo;//0xa4=164
        BuffTemp[5] =(Temp >> 8) & 0xff;//температура
    
     break;       
     
     case 21:
        BuffTemp[5] =(Temp >> 0) & 0xff;//ADC6   sesnsor 5
     break; 
     } 
    Check(BuffTemp, 6);
    WriteConN (BuffTemp,6);  
   
    CountParam++;
    if(CountParam>21)CountParam=0;//зацикливания буфера от 0 до 21
    
}
void CMD_Delta_Ext(void)
{   
    unsigned int Temp;
    Gyro.EXT_Latch=0;
	BuffTemp[ 0] = Gyro.SOC_Out;
    BuffTemp[ 1] = Gyro.My_Addres;
    Temp = Gyro.CuruAngle;
    Gyro.CuruAngle = 0;

    BuffTemp[ 2] =(Temp >> 8) & 0xff;//старший байт разности счетчиков
    BuffTemp[ 3] =(Temp >> 0) & 0xff;//младший байт разности счетчиков
    
    BuffTemp[ 4] = CountParam;
    switch(CountParam) {
   
      WriteCon("5");
      
    //F_ras  
     case 0:
           Temp = Gyro.F_ras;
           BuffTemp[5] = (Temp >> 8) & 0xff;
              
     break;  
     
     case 1:
           BuffTemp[5] = (Temp >> 0) & 0xff;
          
     break; 
    
    
     
     //HFO
     case 2:
           Temp = 0;//HFO
           BuffTemp[5] = (Temp >> 8) & 0xff;
        
     break;     
     
     case 3:
           Temp = 0;//HFO
           BuffTemp[5] = (Temp >> 0) & 0xff;
             
     break;  
   
   
     
     //T_Vibro
     case 4:
           Temp = (unsigned int)((7680000*16/(Gyro.Frq>>12))); 
           BuffTemp[5] = (Temp >> 8) & 0xff;
             
     break;  
    
     case 5:
           BuffTemp[5] = (Temp >> 0) & 0xff;
             
     break; 
    
    
     
     //L_Vibro  
     case 6:
          Temp = (unsigned int)(((7680000*16/200) *Gyro.AmpPer /(Gyro.Frq>>12))); 
          BuffTemp[5]  =  (Temp >> 8) & 0xff;
 
     break;  
   
     case 7:
          BuffTemp[5]  =  (Temp >> 0) & 0xff;
     break;  
     
     
     
     //Напряжение на регуляторе периметра
     case 8:
          Temp = Spi.DAC_B - 0x7fff;
          BuffTemp[5] =  (Temp >> 8) & 0xff;
  
     break;  
   
     case 9:
          BuffTemp[5] =  (Temp >> 0) & 0xff;
     break;  
     
     
     
     //темпкратурный канал 0
     case 10:
          Temp = 100;
          BuffTemp[5] =  (Temp >> 8) & 0xff;

     break;  
    
     case 11:
         BuffTemp[5] =  (Temp >> 0) & 0xff;
     break;
     
       
       
     //темпкратурный канал 1 
     case 12:
        Temp = Gyro.AD_Slow >> 16;
        BuffTemp[5] = (Temp >> 8) & 0xff;
   
     break;  
   
     case 13:
        BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
     
          
      
     //ток 1
     case 14:
        Temp = Gyro.In1;
        BuffTemp[5] = (Temp >> 8) & 0xff;//in1//2
        
     break;  
    
     case 15:
        BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
     
      
      
     //ток 2
     case 16:
        Temp = Gyro.In2;
        BuffTemp[5] = (Temp >> 8) & 0xff;
     break;  
     
     case 17:
        BuffTemp[5] = (Temp >> 0) & 0xff;//in2//3      
     break;  
     
     
     
     //разностный температурный канал Delta Temp
     case 18:
        Temp = Gyro.DeltaT;
        BuffTemp[5] = (Temp >> 8) & 0xff;//дельта
     break;  
    
     case 19:
        BuffTemp[5] = (Temp >> 0) & 0xff;
     break; 
     
     
     
     //температурный канал 5
     case 20:
        Temp = Gyro.Termo;//0xa4=164
        BuffTemp[5] =(Temp >> 8) & 0xff;//температура
    
     break;       
     
     case 21:
        BuffTemp[5] =(Temp >> 0) & 0xff;//ADC6   sesnsor 5
     break; 
     } 
    Check(BuffTemp, 8);
    WriteConN (BuffTemp,8);  
    CountParam++;
    if(CountParam>21)CountParam=0;//зацикливания буфера от 0 до 21
}

void CMD_Rate2(void)
{
	Gyro.Rate2_Event=0;
    BuffTemp[ 0] = Gyro.SOC_Out;
    BuffTemp[ 1] = Gyro.My_Addres;


    BuffTemp[ 2] =(Gyro.CaunPlus >> 8) & 0xff;//старший байт счетчика +.
    BuffTemp[ 3] =(Gyro.CaunPlus >> 0) & 0xff;//младший байт счетчика +.
 
    BuffTemp[ 4] =(Gyro.CaunMin >> 8) & 0xff;//старший байт счетчика -.
    BuffTemp[ 5] =(Gyro.CaunMin >> 0) & 0xff;//младший байт счетчика -.
  
    Check(BuffTemp, 8);
    WriteConN (BuffTemp,8);
}

void CMD_Rate(void)
{
   
    Gyro.Rate1_Event=0;
   	unsigned int Temp;
    
    Gyro.Firmware_Version=0xff; /// промежуточная затычка
 

    BuffTemp[ 0] = Gyro.SOC_Out;
    BuffTemp[ 1] = Gyro.My_Addres;


    BuffTemp[ 2] =(Gyro.CaunPlus >> 8) & 0xff;//старший байт счетчика +.
    BuffTemp[ 3] =(Gyro.CaunPlus >> 0) & 0xff;//младший байт счетчика +.
 
    BuffTemp[ 4] =(Gyro.CaunMin >> 8) & 0xff;//старший байт счетчика -.
    BuffTemp[ 5] =(Gyro.CaunMin >> 0) & 0xff;//младший байт счетчика -.
 
  
Temp = Gyro.CuruAngle;
Gyro.CuruAngle = 0;
Temp = Gyro.AD_Fast >> 16;
    BuffTemp[ 6] =(Temp >> 8) & 0xff;//старший байт разности счетчиков
    BuffTemp[ 7] =(Temp >> 0) & 0xff;//младший байт разности счетчиков

  
    Temp = Gyro.F_ras;
    BuffTemp[ 8] = (Temp >> 8) & 0xff;//расщипление частота
    BuffTemp[ 9] = (Temp >> 0) & 0xff;//

    BuffTemp[10] = (Temp >> 8) & 0xff;//выход регулятора гвч;
    BuffTemp[11] = (Temp >> 0) & 0xff;

Temp = (Gyro.AD_Slow >> 16)- 0x7fff;
    BuffTemp[12]=(Temp >> 8) & 0xff;////
    BuffTemp[13]=(Temp >> 0) & 0xff;////

 
 	Temp = (unsigned int)((7680000*16/(Gyro.Frq>>12))); 
    BuffTemp[14]  =  (Temp >> 8) & 0xff;
    BuffTemp[15]  =  (Temp >> 0) & 0xff;

    BuffTemp[16] = 0xf;
    BuffTemp[17] = 0x02;
 

 	Temp = (unsigned int)(((7680000*16/200) *Gyro.AmpPer /(Gyro.Frq>>12))); 
    BuffTemp[18]  =  (Temp >> 8) & 0xff;
    BuffTemp[19]  =  (Temp >> 0) & 0xff;

    BuffTemp[20] = 0x00;
    BuffTemp[21] = 0x00;



 Temp = Spi.DAC_B - 0x7fff;
    BuffTemp[22] =  (Temp >> 8) & 0xff;
    BuffTemp[23] =  (Temp >> 0) & 0xff;

 
Temp = Gyro.PLC_Delta >> 16;
    BuffTemp[24] = (Temp >> 8) & 0xff;
    BuffTemp[25] = (Temp >> 0) & 0xff;// ХЗ
    
Temp = 100;
Temp = Gyro.AD_Slow >> 16;
    BuffTemp[26] =  (Temp >> 8) & 0xff;
    BuffTemp[27] =  (Temp >> 0) & 0xff;
    
    Temp = Gyro.AD_Slow >> 16;
    BuffTemp[28] = (Temp >> 8) & 0xff;
    BuffTemp[29] = (Temp >> 0) & 0xff;
	
	Temp = Gyro.In1;
    BuffTemp[30] = (Temp >> 8) & 0xff;//in1//2
    BuffTemp[31] = (Temp >> 0) & 0xff;
    
 	Temp = Gyro.In2;
    BuffTemp[32] = (Temp >> 8) & 0xff;
    BuffTemp[33] = (Temp >> 0) & 0xff;//in2//3
	
	Temp = Gyro.DeltaT;
    BuffTemp[34] = (Temp >> 8) & 0xff;//дельта
    BuffTemp[35] = (Temp >> 0) & 0xff;
	    
	Temp = Gyro.Termo;//0xa4=164

    BuffTemp[36] =(Temp >> 8) & 0xff;//температура
    BuffTemp[37] =(Temp >> 0) & 0xff;//ADC6   sesnsor 5

    BuffTemp[38] =0x00;
    BuffTemp[39] =0x00;
    BuffTemp[40] =0x00;
    BuffTemp[41] =0x00;


    Check(BuffTemp, 44);
    WriteConN (BuffTemp,44);
    
}

void CMD_M_Control_D8()///установка\сброс регистров управления
{
    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
    
/*  
  
 FrqON  
PlcON
 HFOOFF  
HFOON 

 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:
    	 FrqON
    	 break;
    	 
    	 case 0x03:
    	 HFOON
    	 break;
    	 
    	 case 0x01:
    	 PlcON
    	 break;
    	 }
    } 
   else{
   switch (Bit_num){
   	     
   	     case 0x06:
    	 FrqOff
    	 break;
    	 
    	 case 0x03:
    	 HFOOFF
    	 break;
    	 
    	 case 0x01:
    	 PlcOFF
    	 break;
    	}
   } 
    BuffTemp[0] = Gyro.SOC_Out;      //DD
    BuffTemp[1] = Gyro.My_Addres;    //00
    BuffTemp[2] = Gyro.CMD_In;       //D8
    BuffTemp[3] = A<<4;       
    BuffTemp[4] = 0x0;
    BuffTemp[5] = 0x0;
    Check(BuffTemp, CRC_N);
    WriteConN (BuffTemp,CRC_N);
}

void CMD_M_Control_D9()///чтение регистров управления
{
    int bit,NReg;
    BuffTemp[0] = Gyro.SOC_Out;      //DD
    BuffTemp[1] = Gyro.My_Addres;    //00
    BuffTemp[2] = Gyro.CMD_In;       //D9
    if ((Param1 & 0x10) == 0) {
        BuffTemp[3]=0<<4;
        BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
        BuffTemp[5] =  Gyro.RgConA     & 0xff;
    } else {
        BuffTemp[3]=1<<4;
        BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
        BuffTemp[5] =  Gyro.RgConB      & 0xff;
    }
    Check(BuffTemp, CRC_N);
    WriteConN (BuffTemp,CRC_N);
}
// (номер (код) ЦАПа 0...3, старший байт требуемого ЦАПа, младший байт треб ЦАПа)
void CMD_M_Stymul()
{
    int temp;
    int HFO;
    temp=BuffTemp[3];
    Consol = temp&0x3;
    HFO    = temp&0x3;// // (номер (код) ЦАПа 0...3, HFO - 3dac, PLC - 0DAC
    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);
    }
}
void Gph_W()
{
 	Out_G_photo(BuffTemp[4],BuffTemp[5]);    
}

void CMD_M_vib()
{   
    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)
{
    int i=1;
    unsigned int temp,CRC;


    temp=1;
    CRC=0;


    for(; i<Count-2; i++) {
        CRC+=c[i];
    }

    if(c[Count-2]!=((CRC>>8)&0xFF)) {
        temp=0;
        Gyro.RsErrLine = (Gyro.RsErrLine)&=0x2;
    }

    if(c[Count-1]!=((CRC>>0)&0xFF)) {
        temp=0;
//		Gyro.RsErrLine= (Gyro.RsErrLine)=0x2;
    }

    c[Count-2]=(CRC>>8)&0xFF;
    c[Count-1]=(CRC>>0)&0xFF;



    return temp;
}

int Getlengf(void)
{
    unsigned int lengf;
    lengf = 1;
    switch(Gyro.CMD_In) {
        case 0x99://Gyrotainance
            
            lengf=6;
            CRC_N=8;
            break;

        case 0x0A: //m_stymul
            lengf=8;
            break;

        case 0xE9://DeviceMode

            lengf=6;
            CRC_N=8;
            break;
        case 0xE8://DeviceMode

            lengf=6;
            CRC_N=8;
            break;
   
          case 0xA5://DeviceMode

            lengf=6;
            CRC_N=8;
            break;
            
        case 0xDD://m_rate
            lengf=6;
            CRC_N=44;
            break;
            
        case 0xA0://Delta_PS
            lengf=6;
            CRC_N=8;
            break;
            
            
        case 0xB0://DeviceMode

            lengf=6;
            CRC_N=8;
            break;

        case 0xD8://m_control

            lengf=6;
            CRC_N=8;
            break;

        case 0xD9://m_control

            lengf=6;
            CRC_N=8;
            break;
            
        case 0xE4: 
            lengf=8;
            break;
            
            
            case 0xE6: 
            lengf=8;
            break;
     
    }
    
    return lengf;
}

void Read_CMD(void)
{
 
 						
    Gyro.RsErrLine = (Gyro.RsErrLine)& 0xffff;//для сброса ошибки

    CountBuFFIn=ReadChekCon1(BuffTemp);		// чтение данных из консоли

    if(CountBuFFIn==1) { 					// если есть первый байт 
        if (BuffTemp[0] != SOC_In) {
            ReadCon1(BuffTemp);
            Gyro.RsErrLine += 0x100;//WriteCon("\n\r ...Error.... ");
            BuffTemp[99]=Gyro.RsErrLine; 
            
        }
    } else if(CountBuFFIn==2) {		//если второй ,fqn
        if (BuffTemp[1] != Gyro.My_Addres) {
            ReadCon1(BuffTemp); 
            Gyro.RsErrLine += 0x1;//WriteCon("\n\r ...Error_ADDRES.... ");
        }
    } else if(CountBuFFIn==3) {				// если ьоетий байт
        Gyro.CMD_In=BuffTemp[2];
        N=Getlengf(); 
    } 
    else if(CountBuFFIn==4 && (N==6)) 
    {	// N -  длина посылки аскглд(для записи параметров). определяется по коду команды в Getlengf();
        Param1=BuffTemp[3];
    } 
    else if((CountBuFFIn==5)&&(N==7)) 
    {
       
        Param1=BuffTemp[3];
        Param2=BuffTemp[4];

    } 
    else if((CountBuFFIn==6)&&(N==8))
     {
        Param1=BuffTemp[3];
        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))
     {
        ReadCon1(BuffTemp); 
            switch(Gyro.CMD_In) {
                Rate_Flag=0;
               
                case 0x99:
                    Gyro.ModeOut=0;
                    CMD_Maintenance();
                break;
               
                case 0xA0:
                  TempParam=(BuffTemp[3]) & 0x90; 
                  if      (TempParam==Delta_500)        CMD_Delta_500Hz();
                  else if (TempParam==Delta_EXT)        CMD_Delta_Ext();
                  else if (TempParam==PDelta_500)      { Gyro.ModeOut=3; Gyro.Delta500_Event=0; WriteCon("Delta500"); }
                  else if (TempParam==PDelta_EXT)      { Gyro.ModeOut=4; Gyro.DeltaEXT_Event=0; WriteCon("deltaEXT"); }
                    break;
               
                case 0xD8:
                    CMD_M_Control_D8();
                    break;
               
                case 0xE9://чтение параметров
                    CMD_M_Param_R();
                    break;
                    
                case 0xE8://запись параметра
                    CMD_M_Param_W();
                    break;          
               
                case 0xD9:
                    CMD_M_Control_D9();
                    break;
               
                case 0x0A:
                    CMD_M_Stymul();
                    break;
               
                case 0xDD:
                 TempParam=(BuffTemp[3]) & 0x9f; 
                 if      (TempParam == Rate1)        { CMD_Rate(); 		     	                    WriteCon("Rate11");}
                 else if (TempParam == Rate2)        { CMD_Rate2(); 		                        WriteCon("Rate21");}
                 else if (TempParam == PRate1)       { Gyro.ModeOut=1; Gyro.Rate1_Event=0;	        WriteCon("Rate12");}
                 else if (TempParam == PRate2)       { Gyro.ModeOut=2; Gyro.Rate2_Event=0;		    WriteCon("Rate22");}
                 break; 
              
               case 0xE4:
                    CMD_M_vib();
                    break; 
              
               case 0xE6:
                    Gph_W();
                    break; 
              
                    
                    Gyro.RsErrLine = 0;
                    
                      
     }
    
    }
//  else if(CountBuFFIn > 40 )
//     {
//       ReadCon1(BuffTemp); 
//     }

}