RoboMasters 的串口发送函数

RoboMasters 的串口发送函数

应用于Nvidia Jetson TK1 /Nvidia Jetson TX1 与步兵控制板通信

typedef struct {
int fd;
int isFrictionwheelWork;
}Comm;

串口初始化函数:

int openCCOMPort()
{
int fd; /* File descriptor for the port */

fd = open("/dev/ttyTHS2", O_RDWR | O_NOCTTY | O_NDELAY);

//printf("OpenCCOMPort: %d",fd);
if (fd == -1)
{
/*
* Could not open the port.
*/
perror("open_port: Unable to open /dev/ttyUSB0.");
return 5;
}else if(fd == 0){

return 10;
}
else
{
fcntl(fd, F_SETFL, 0);
return (fd);
}

}

int setCCOMPort(int fd, int baudRate, int dataBits, char parity, int stopBits)
{
struct termios newConfig, oldConfig;
int speed;

/*
* 保存原先串口配置
*/
if(tcgetattr(fd, &oldConfig) != 0)
{
perror("tcgetattr");
return 0;
}
newConfig = oldConfig;

/*
* 激活选项
*/
cfmakeraw(&newConfig);

/*
* 设置字符大小
*/
newConfig.c_cflag &= ~CSIZE;

/*
* 设置数据位
*/
switch(dataBits)
{
case 7:
newConfig.c_cflag |= CS7;
break;
default:
case 8:
newConfig.c_cflag |= CS8;
break;
}
/*
* 设置波特率
*/
switch(baudRate)
{
case 2400:
speed = B2400;
break;
case 4800:
speed = B4800;
break;
case 9600:
speed = B9600;
break;
case 19200:
speed = B19200;
break;
case 38400:
speed = B38400;
break;
default:
case 115200:
speed = B115200;
break;
}
cfsetispeed(&newConfig, speed);
cfsetospeed(&newConfig, speed);

/*
* Enable the receiver and set local mode...
*/
newConfig.c_cflag |= (CLOCAL | CREAD);

/*
* 设置奇偶校验位
*/
switch(parity)
{
default:
case 'n':
case 'N':
newConfig.c_cflag &= ~PARENB;
break;
case 'o':
case 'O':
newConfig.c_cflag |= (PARODD | PARENB);
break;
case 'e':
case 'E':
newConfig.c_cflag |= PARENB;
newConfig.c_cflag &= ~PARODD;
break;
case 's':
case 'S':
newConfig.c_cflag &= ~PARENB;
break;
}
/*
* 设置停止位
*/
switch (stopBits)
{
default:
case 1:
newConfig.c_cflag &= ~CSTOPB;
break;
case 2:
newConfig.c_cflag |= CSTOPB;
break;
}

/*
* 激活新配置
*/
if(tcsetattr(fd, TCSANOW, &newConfig) != 0)
{
perror("tcsetattr");
return 0;
}
return 1;
}

void PortInit()
{
RM.fd = openCCOMPort();
setCCOMPort(RM.fd, 115200, 8, 'N', 1);
RM.isFrictionwheelWork = 0;
//printf("PortInit");
}

int sendMsg(Comm com ,char *msg)
{
//int len = strlen(msg);
int len = 0;
for(;msg[len]!='\x80';len++)
{
}
len+=1;
//int n=1;
//printf("%d\n",msg);
//printf("Len: %d\n",len);
//printf("Com.fd: %d\n",com.fd);
int n = write(com.fd, msg, len);
if(n>0)
return 1;
else
return 0;
}
int przTurn(Comm com, int horizontal, int cabrage)
{

printf("COM: Send x %d , y %d \n" , horizontal , cabrage );

char msg[6] = "\xff\x02\x00\x00\x80";
//printf("%d",msg);
if(horizontal > 127 || horizontal <= -127 || cabrage > 127 || cabrage <= -127)
//应该抛出异常
return 0;
if(horizontal < 0)
{
horizontal = -horizontal;
msg[2] = (char)(0x80 + horizontal + (int)msg[2]);
}
else
{
msg[2] = (char)(horizontal + (int)msg[2]);
}

if(cabrage < 0)
{
cabrage = -cabrage;
msg[3] = (char)(0x80 + cabrage + (int)msg[3]);
}
else
{
msg[3] = (char)(cabrage + (int)msg[3]);
}
//printf("Get Location Of Target");
if(!sendMsg(com,msg)){
printf("SENDTOPORT ERROR");
return 0;
}
//printf("Send Message\n");

return 1;
}

串口通信函数:

int sendMsg(Comm com ,char *msg)
{
//int len = strlen(msg);
int len = 0;
for(;msg[len]!='\x80';len++)
{
}
len+=1;
//int n=1;
//printf("%d\n",msg);
//printf("Len: %d\n",len);
//printf("Com.fd: %d\n",com.fd);
int n = write(com.fd, msg, len);
if(n>0)
return 1;
else
return 0;
}

int przTurn(Comm com, int horizontal, int cabrage)
{
printf("COM: Send x %d , y %d \n" , horizontal , cabrage );
char msg[6] = "\xff\x02\x00\x00\x80";
//printf("%d",msg);
if(horizontal > 127 || horizontal <= -127 || cabrage > 127 || cabrage <= -127)
//应该抛出异常
return 0;
if(horizontal < 0)
{
horizontal = -horizontal;
msg[2] = (char)(0x80 + horizontal + (int)msg[2]);
}
else
{
msg[2] = (char)(horizontal + (int)msg[2]);
}

if(cabrage < 0)
{
cabrage = -cabrage;
msg[3] = (char)(0x80 + cabrage + (int)msg[3]);
}
else
{
msg[3] = (char)(cabrage + (int)msg[3]);
}
//printf("Get Location Of Target");
if(!sendMsg(com,msg)){
printf("SENDTOPORT ERROR");
return 0;
}
//printf("Send Message\n");

return 1;
}

发表评论