C++ 创龙UDP通讯demo

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;
}