yt-bak2.15.c

1、监控行业云台控制代码 2、对Pelco-P,Pelco-D进行解析

        
    unsigned char Address_buf[TCVYTCodeLength];
    unsigned char Command_buf[TCVYTCodeLength];   
    int  Address_count = 0;
    int  Command_count = 0;

    char *DataList[64];
    int   DataList_count = 0;
    char *TempList[64];
    int   TempList_count = 0;
    
    unsigned char Mod = 0;
    unsigned char Mask = 0xFF;
    unsigned char Dec  = 0;
    unsigned char Wait  = 20;
    unsigned char WaitThread = 0;     
    int Value, Sum, sumIndex, Xor, xorIndex;
          
    printf("SendYTCommand -- 1!\n");
    //--------------------------------------malloc buffer
    s = malloc(128);

    //for(i = 0; i < 64; i++)
    //{
    //      DataList[i] = malloc(8);
    //}
    //--------------------------------------
    //Create Adrress Data -->Address_buf, Address_count
    //--------------------------------------
    sprintf(addr, "Address%2d", Address);
    if(Address < 10)
          addr[7] = '0';
          
    for(i = 0; i < 16; i++)
    {
          ss = TMYTCtrlInfo[Index]->codAddr[i];
          if( strstr(ss, addr) )
          {
               debug("find addr :%s ", ss);
               break;
          }
    }

    ss = strrchr(ss, '=');
    if(ss == NULL)
    {
       ret = -1;
       goto SendFreeAll;
    }
	
    memcpy(s, ss + 1, strlen(ss) - 1);
    debug("a:%s ", s);
    if(s == NULL)
    {
       ret = -1;
       goto SendFreeAll;
    }

    Address_count = 1;
    ss = strtok(s, ",");
	debug("a1:%s ", s);
	debug("a2:%s ", ss);
    sscanf(ss, "%x", &data);	
    debug("i:%d  \n", data);
    Address_buf[0] = data;
    
    while( (ss = strtok(NULL, ",") ) )
    {
          sscanf(ss, "%x", &data);
          debug("i:%d", data);
          Address_buf[Address_count] = data;
          Address_count ++;
    }
    printf("Address_count==%d\n", Address_count);    
    //--------------------------------------
    //Create Command Data -->Command_buf, Command_count
    //--------------------------------------
    for(i = 0; i < TMYTCtrlInfo[Index]->CtrlNum; i++)                 //find '"Command" in codCtrl
    {
          ss = TMYTCtrlInfo[Index]->codCtrl[i];
          if( strstr(ss, Command) )
          {     
               debug("find :%s ", ss);
               break;
          }
    }
    
    if(i >= TMYTCtrlInfo[Index]->CtrlNum)                             //find '"Command" in codCmd
    {
         for(i = 0; i < TMYTCtrlInfo[Index]->CmdNum; i++)
         {
               ss = TMYTCtrlInfo[Index]->codCmd[i];
               if( strstr(ss, Command) )
               {
                    debug("find :%s ", ss);
                    break;
               }
         }
         
         if(i >= TMYTCtrlInfo[Index]->CmdNum)
         {
            ret = -2;
            goto SendFreeAll;
         }         
    }

    ss = strchr(ss, '=');
    if(ss == NULL)
    {
       ret = -2;
       goto SendFreeAll;
    }
    
    memcpy(s, ss + 1, strlen(ss) - 1);
    //printf("1s:%s ", s);
    //printf("2ss:%s ", ss);
    
    if(ss == NULL)
    {
       ret = -2;
       goto SendFreeAll;
    }

    ss = strtok(s, ",");
    DataList[0] = ss;
    DataList_count = 1;
    
    while( (ss = strtok(NULL, ",") ) )
    {
          DataList[DataList_count] = ss;
          DataList_count ++;
          //debug("%d(%s)  ", DataList_count, DataList[DataList_count]);          
    }
    debug("ss:%s (%d)", s, DataList_count);

    //--------------------------------
    //Create Send Data main loop
    //--------------------------------
    for(i = 0; i < DataList_count; i++)
    {
		Info = DataList[i];
		debug("%d(%s)  ", i, Info);
          
		if(Info[0] == '%'){      //取模
			sscanf(Info+1, "%x", &Value);
			Mod = Value;
		}
		else if(Info[0] == '~'){  //减处理(减校验和)
			sscanf(Info+1, "%x", &Value);
			Dec = Value;
		}
		else if(Info[0] == 'M'){  //Mask处理(与操作)
			sscanf(Info+1, "%x", &Value);
			Mask = Value;
		}
		else if(Info[0] == '#'){  //连续指令间隔时间
	             	sscanf(Info+1, "%x", &Value);
			Wait = Value;
		}
		else if(Info[0] == '&'){  //连续发码间隔时间
	    	        sscanf(Info+1, "%x", &Value);
			WaitThread = Value;
		}
		else if(Info[0] == '^'){    //
	             	//printf("ccAddress_count==%d\n", Address_count);    
			for(j=0; j<Address_count; j++)
             		{
				Command_buf[Command_count] = Address_buf[j];
				Command_count++;
			}
		}
		else if(Info[0] == '!'){    //云台预置点
	    	        sscanf(Info+1, "%x", &Value);				//2004.11.25 CYG Add
			Command_buf[Command_count] = PresetNo + Value;         	
			Command_count++;
		}
		else if(Info[0] == '+'){  //求校验和
			Temp = Info + 1;
            //debug("++=: %s - %s\n", Temp, Info);
            //-----------------
            ss = strtok(Temp, ".");
            TempList[0] = ss;
            TempList_count = 1;

            while( (ss = strtok(NULL, ".") ) )
            {
                   TempList[TempList_count] = ss;
                   TempList_count ++;
                   debug("++=: %s - %d\n", ss, TempList_count);
            }
             
            //printf("+x=: %s - %d\n", TempList[0], TempList_count);
            //-----------------               			
			if(TempList_count == 1)
			{
				sscanf(TempList[0], "%x", &Value);
				Sum = 0;
				for(j=0;j<Value;j++)
	           	{
				    sumIndex = Command_count - j - 1;
				    if(sumIndex<0)
					{
						debug( "PTZ command code error 1!\n" );
	     				return -1;
				    }
				    Sum += Command_buf[sumIndex];
				}
				if(Mod != 0)
					Command_buf[Command_count] = Sum%Mod;
				else
					Command_buf[Command_count] = Sum;
				if(Mask != 0xFF)
					Command_buf[Command_count] &= Mask;
				if(Dec != 0)
					Command_buf[Command_count] = Dec - Command_buf[Command_count];
				Command_count++;
	       }
	       else
	       {  
				Sum = 0;
				for(j=0; j<TempList_count; j++)
				{
					//计算固定累加值(2004-07-20)
					Temp = TempList[j];
					if(Temp[strlen(Temp) - 1] == "+")
					{
						//Temp = TempList[j].Left(TempList[j].GetLength()-1);
                        memcpy(s, Temp, strlen(Temp) - 1);                          
						sscanf(s, "%x", &Value);
						Sum+=Value;
					}
					else
	                {
						sscanf(TempList[j], "%x", &Value);
						sumIndex = Command_count - Value;
						if(sumIndex < 0)
               			{
							debug( "PTZ command code error 2!\n" );
							return -1;
						}
						Sum += Command_buf[sumIndex];
					}
				}
				if(Mod!=0)
					Command_buf[Command_count] = Sum % Mod;
				else
					Command_buf[Command_count] = Sum;
				if(Mask!=0xFF)
					Command_buf[Command_count] &= Mask;
				if(Dec!=0)
					Command_buf[Command_count] = Dec - Command_buf[Command_count];
                         
				Command_count++;
			}          
		}
		else if(Info[0] == '-'){  //求校验和(取反)
			Temp = Info + 1;
                              
		             //-----------------
		             ss = strtok(Temp, ".");
		             TempList[0] = ss;
		             TempList_count = 1;
	
		             while( (ss = strtok(NULL, ".") ) )
		             {
		                   TempList[TempList_count] = ss;
		                   TempList_count ++;
		             }
		             //-----------------
		             for(j = 0 ; j< TempList_count; j++)
					 {
		                  debug("--=: %s - %d\n", TempList[j], TempList_count);
					 }
                  
			    if(TempList_count == 1)
				{
					sscanf(TempList[0],"%x",&Value);
					Sum = 0;
					for(j=0; j<Value; j++)
                	{
						sumIndex = Command_count - j - 1;
						if(sumIndex<0)
						{
							printf( "PTZ command code error 3!\n" );
							return -1;
						}
						Sum += Command_buf[sumIndex];
					}
					if(Mod != 0)
						Command_buf[Command_count] = ~(Sum%Mod);
					else
						Command_buf[Command_count] = ~(Sum);
					if(Mask != 0xFF)
						Command_buf[Command_count] &= Mask;
					if(Dec != 0)
						Command_buf[Command_count] = Dec - Command_buf[Command_count];
					Command_count++;
				}
				else
				{  //
					Sum = 0;
					for(j=0; j<TempList_count; j++)
                	{
						sscanf(TempList[j], "%x", &Value);
						sumIndex = Command_count - Value;
						if(sumIndex < 0)
						{
							debug( "PTZ command code error 4!\n" );
							return -1;
						}
						Sum += Command_buf[sumIndex];
                   		//printf("Sum ==%x %x %x\n", Command_buf[sumIndex], Command_count, Value);  
					}
					if(Mod != 0)
						Command_buf[Command_count] = ~(Sum%Mod);
					else
						Command_buf[Command_count] = ~(Sum);
					if(Mask != 0xFF)
						Command_buf[Command_count] &= Mask;
					if(Dec != 0)
						Command_buf[Command_count] = Dec - Command_buf[Command_count];
					Command_count++;
				}          
		}
		else if(Info[0] == '='){  //求异或校验值
				Temp = Info + 1; //Info.Right(Info.GetLength()-1);
				sscanf(Temp, "%x", &Value);
				xorIndex = Command_count - Value;
             	//printf("Value == %d %d %x\n", Value, Command_count, Command_buf[xorIndex]);                 
				if(xorIndex < 0)
		        {
					debug( "PTZ command code error 5!\n" );
                	return;
				}
				Xor = Command_buf[xorIndex];
				for(j=1; j<Value; j++)
		        {
					xorIndex ++;
					if(xorIndex >= Command_count)
                	{
						debug( "PTZ command code error 6!\n" );
						return;
					}
					Xor ^= Command_buf[xorIndex];                    
				}
				Command_buf[Command_count] = Xor;
				Command_count++;
			}
		else if(Info[0] == '|'){     //连续指令
          
		}
		//ASCII数值处理(2004-08-21)
		else if( strlen(Info) >=3 && Info[0] =='[' && Info[strlen(Info) - 1]==']')
		{
			memcpy(s, Info +1, strlen(Info) - 2);
			if(strstr(s, "STX") !=0)
			{
				Value = 0x02;
			}
			else if(strstr(s, "ETX") !=0)
			{
				Value = 0x03;
			}
			else
			{
				sscanf(s,"%c",&Value);
			}
               
			Command_buf[Command_count] = Value;
			Command_count++;
		}
		//直接转换成16
		else
		{
			sscanf(Info, "%x", &Value);
			Command_buf[Command_count] = Value;
			Command_count++;
		}          
    }
    
    printf("\n");                                    
    printf("Send data =: ");
    for(i = 0; i < Command_count; i++)
    {
          printf(" %x", Command_buf[i]);		  
    }
	write(fd, Command_buf, Command_count);  
	
    printf("\n");                                
SendFreeAll:
    //--------------------free all
    free(s);
    //--------------------

    return 0;      
}


    //printf("%s ", strtok(s, s1) );
    //while( (p = strtok(NULL, s1) ) )
    //   printf("%s ", p);
    //printf("ss\n");
    //