C++ 创龙UDP通讯demo

#include <iostream>
#include <vector>
#include <string>

static uint8_t checksum(uint8_t *buff,int size)
{
    uint8_t ldr = 0;
    for(int i = 0;i<size;i++)
    {
        ldr ^= buff[i];
    }
    return ldr;
}

bool decode(uint8_t *inbuff,int inbuffsize,uint8_t *outbuff,int &outbuffsize)
{
    std::string out;
    //zklx::GeneralX::buff_to_string((char *)inbuff,inbuffsize,out);
    //dzlog_debug(" %s",out.c_str());
    if(inbuff[0] != 0x7e || inbuff[inbuffsize -1] != 0xe7 || inbuffsize < 7)
    {
        return false;
    }
    int size = 0;
    int i,j = 0;

    for(i = 0;i<inbuffsize;)
    {
        if(inbuff[i] == 0xee)
        {
            i++;
        }
        outbuff[j++] = inbuff[i];
        i++;
        size++;
    }
    outbuffsize = size;

    uint8_t ldr = checksum(outbuff + 1,size - 3);
    if(ldr != outbuff[size - 2])
    {
        return false;
    }
    return true;
}

void encode(uint8_t *inbuff,int inbuffsize,uint8_t *outbuff,int &outbuffsize)
{
    int i = 0;
    int n = 0;
    uint8_t ldr = checksum(inbuff,inbuffsize);

    outbuff[n++]  = 0x7e;
    for(i = 0;i<inbuffsize;i++)
    {
        if((inbuff[i] == 0x7e) || (inbuff[i] == 0xe7) || (inbuff[i] == 0xee))
        {
            outbuff[n++] = 0xee;
            outbuff[n++] = inbuff[i];
        }else{
            outbuff[n++] = inbuff[i];
        }
    }
    if((ldr == 0x7e) || (ldr == 0xe7) || (ldr == 0xee))
    {
        outbuff[n++] = 0xee;
        outbuff[n++] = ldr;
    }else{
        outbuff[n++] = ldr;
    }
    outbuff[n++] = 0xe7;
    outbuffsize = n;
}


int main()
{
    //两轴转台自检指令
    uint8_t command[256] = {0x01, 0x11, 0x03, 0x31, 0xA1};
    //两轴转台
    uint8_t out_command[256] = {0};
    int out_size;
    encode(command, 5, out_command, out_size);
    
    for(int i = 0; i < out_size; i ++)
    {
        std::cout <<"0x"<< std::hex << (int)out_command[i] << " ";
    }
    std::cout << "" << std::endl;

    uint8_t command_1[256] = {0x01,0x11,0x03,0x31, 0xA4, 0x01};
    encode(command_1, 6, out_command, out_size);
    
    for(int i = 0; i < out_size; i ++)
    {
        std::cout <<"0x"<< std::hex << (int)out_command[i] << " ";
    }
    std::cout << "" << std::endl;

    uint8_t command_2[256] = {0x01,0x11,0x03,0x31, 0xA5, 0x10, 0x27, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00};
    encode(command_2, 13, out_command, out_size);
    
    for(int i = 0; i < out_size; i ++)
    {
        std::cout <<"0x"<< std::hex << (int)out_command[i] << " ";
    }
    std::cout << "" << std::endl;

    uint8_t command_3[256] = {0x03,0x31,0x01,0x11, 0xD1, 0x00, 0x00, 0x00, 0x00, 0x00};
    encode(command_3, 10, out_command, out_size);
    
    
    for(int i = 0; i < out_size; i ++)
    {
        std::cout <<"0x"<< std::hex << (int)out_command[i] << " ";
    }
    std::cout << "" << std::endl;

    uint8_t command_4[256] = {0x03,0x31,0x01,0x11,0xD2,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
    encode(command_4, 14, out_command, out_size);
   
    for(int i = 0; i < out_size; i ++)
    {
        std::cout <<"0x"<< std::hex << (int)out_command[i] << " ";
    }
    std::cout << "" << std::endl;
    return 1;
}