Ubuntu16.04LTS并口收包实例C++编程
记录一次并口收包实例和调试经历。目的是从imu传感实时读取数据。imu通讯合同为:
-115200b码流
-无起始位、无校准位
-imu收到开始m命令包后开始数据发送
-齐齐哈尔两字节:0x5a0xa5
-第三字节为数据为宽度(固定为0x25)
-0x25宽度的数据位(数据位最后一位设置为之前数据位的不进位加和)
-包尾一字节0xaa
以及出现问题以后的剖析解决过程
基础知识
1.[linux系统并口基本操作控制位基础设置方式]
2.[早已存在的并口通讯实例]
3.[防止linux系统内核中的流控制字符造成读取错误]
结合基础知识linux串口波特率,写出了初版代码
设计思路(出错)
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include"serial.h"
//imu
static const uint8_t start[6] = {0xA5,0x5A,0x04,0x01,0x05,0xAA};
static const uint8_t stop[6] = {0xA5,0x5A,0x04,0x02,0x06,0xAA};
static int data_length = 40;
static uint8_t imu_data[40] = { 0 };
int flag=0;
//第一部分代码/
//根据具体的设备修改
const char default_path[] = "/dev/ttyUSB0";
int main(int argc, char *argv[])
{
int fd;
char *path;
uint8_t buf[100] = {0};
//若无输入参数则使用默认终端设备
if (argc > 1)
path = argv[1];
else
path = (char *)default_path;
//获取串口设备描述符
printf("This is tty/usart demo.n");
fd = open(path, O_RDWR | O_NOCTTY|O_NDELAY);//noctty,not influenced by terminal,
// fcntl(fd,F_SETFL,0);
if (fd < 0)
{
printf("Fail to Open %s devicen", path);
return 0;
}
struct termios opt; //BASIC SETTING
// struct termios
//{
// tcflag_t c_iflag; //输入模式标志
// tcflag_t c_oflag; //输出模式标志
// tcflag_t c_cflag; //控制模式标志
// tcflag_t c_lflag; //本地模式标志
// cc_t c_line; //line discipline
// cc_t c_cc[NCC]; //control characters
// }
//清空串口接收缓冲区
tcflush(fd, TCIOFLUSH);
// 获取串口参数opt
tcgetattr(fd, &opt);
//设置串口输出波特率
cfsetospeed(&opt, B115200);
//设置串口输入波特率
cfsetispeed(&opt, B115200);
//设置数据位数 8位数据位
opt.c_cflag &= ~CSIZE;
opt.c_cflag |= CS8;
//校验位 无校验位
opt.c_cflag &= ~PARENB;
opt.c_iflag &= ~INPCK;
//设置停止位 1位停止位
opt.c_cflag &= ~CSTOPB;
opt.c_cflag |= CLOCAL | CREAD;
opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
opt.c_oflag &= ~OPOST;
opt.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
//
opt.c_cc[VTIME]=11;
opt.c_cc[VMIN]=0;
//更新配置
tcsetattr(fd, TCSANOW, &opt);
std::cout << "Device ttyUSB0 is set to 115200bps,8N1n" << std::endl;
//第四部分代码/
tcflush(fd,TCIOFLUSH);
write(fd,start,6);
// fd_set rfds;
while (1)
{
read(fd,buf,sizeof (uint8_t) * data_length);
std::copy(buf,buf+40,imu_data);
if(imu_data[i]==0xa5 && imu_data[i+1]==0x5a && imu_data[i+2]==0x25 ) //&& buf[i+39]==0xaa
{
for(int i = 0;i<data_length;++i)
{
std::cout << std::hex << +imu_data[i] << " ";
}
std::cout<<std::endl;
//the scalar of yaw roll and pitch is 0.1degree
uint16_t yaw = imu_data[3];
yaw = yaw<<8|imu_data[4];
int16_t pitch = imu_data[5];
pitch = pitch<<8|imu_data[6];
int16_t roll = imu_data[7];
roll = roll<<8|imu_data[8];
//after divided by 16384, the scalar of x y z accelaration are g
int16_t x_acc = imu_data[9];
x_acc = x_acc<<8|imu_data[10];
int16_t y_acc = imu_data[11];
y_acc = y_acc<<8|imu_data[12];
int16_t z_acc = imu_data[13];
z_acc = z_acc<<8|imu_data[14];
//after divided by 32.8 the scalar of x y z gyro data are degreee/second
int16_t x_gyro = imu_data[15];
x_gyro = x_gyro<<8| imu_data[16];
int16_t y_gyro = imu_data[17];
y_gyro = y_gyro <<8|imu_data[18];
int16_t z_gyro = imu_data[19];
z_gyro = z_gyro<<8 |imu_data[20];
std::cout<<"yaw = "<<0.1*yaw<<" pitch = "<<0.1*pitch<<" roll = "<<0.1*roll;
std::cout<<std::endl;
std::cout<<std::setprecision(4)<<"x_acc = "<< x_acc/16384<<" y_acc = "<< y_acc/16384<<" z_acc = "<<z_acc/16384;
std::cout<<std::endl;
std::cout<<"x_gyro = "<<x_gyro/32.8<<" y_gyro = "<<y_gyro/32.68<<" z_gyro = "<<z_gyro/32.68;
std::cout<<std::endl;
}
usleep(1000);
}
return 0;
}
发觉当延时函数小的时侯,数据包出现了很严重的丢包现象,有时侯表现为数据包后部数据全部为零且收不到结束位,有的时侯表现为结束位提早出现且数据结果错误。
当延时函数大的时侯linux串口波特率红旗linux下载,数据包常常读取正常,而且实时性破坏特别大redflag linux,但是未能测度合适的时间延时。
针对两种错误的剖析很容易发觉缘由应当不是数据溢出,由于数据包量小,频度不高。并且也在这个方向上做了一些探求。
2.尝试减少imu设备频度,发觉疗效变好。阅读相关资料,发觉错误来自于对设备堵塞读取的错误理解。(例:读取40字节数据,缓存区中只有27字节,读取堵塞被打开,后13字节被手动置零)根本缘由:实时检测并口缓存区,很难通过简单操作实现。
重画代码,问题解决
main.cpp
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include"serial.h"
int main(int argc, char *argv[])
{
using namespace imu;
initIMU();
imu::Data data;
while (1) {
data = getCurrentPos();
std::cout<<"yaw = "<<std::setw(12)<<std::right<<data.yaw<<std::setw(12)<<" pitch = "<<data.pitch<<std::setw(12)<<" roll = "<<data.roll;
std::cout<<std::endl;
std::cout<<"x_acc = "<<std::setw(12)<< data.x_acc<<std::setw(12)<<" y_acc = "<<std::setw(12)<< data.y_acc<<std::setw(12)<<" z_acc = "<<data.z_acc;
std::cout<<std::endl;
std::cout<<"x_gyro = "<<std::setw(12)<<data.x_gyro<<std::setw(12)<<" y_gyro = "<<data.y_gyro<<" z_gyro = "<<data.z_gyro;
std::cout<<std::endl;
}
return 0;
}
serial.cpp
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include"serial.h"
namespace imu{
Data getCurrentPos(){
tcflush(fd,TCIOFLUSH);
while (1)
{
if(packerflag){
if(check != rxbuf[data_length-1]){
std::cout<<"check error"<<(uint16_t)check<<" "<<(uint16_t)(rxbuf[data_length-1])<<std::endl;
recstatus = 0;
ccnt = 0;
packerflag = 0;
check = 0;
tcflush(fd, TCIOFLUSH);
continue;
}
check = 0;
recstatus = 0;
ccnt = 0;
packerflag = 0;
// for(int i = 0;i<data_length;++i)
// {
// std::cout << std::hex << +rxbuf[i] << " ";
// }
// std::cout<<std::endl;
//the scalar of yaw roll and pitch is 0.1degree
int16_t yaw = rxbuf[1];
yaw = yaw<<8|rxbuf[2];
int16_t pitch = rxbuf[3];
pitch = pitch<<8|rxbuf[4];
int16_t roll = rxbuf[5];
roll = roll<<8|rxbuf[6];
//after divided by 16384, the scalar of x y z accelaration are g
int16_t x_acc = rxbuf[7];
x_acc = x_acc<<8|rxbuf[8];
int16_t y_acc = rxbuf[9];
y_acc = y_acc<<8|rxbuf[10];
int16_t z_acc = rxbuf[11];
z_acc = z_acc<<8|rxbuf[12];
//after divided by 32.8 the scalar of x y z gyro data are degreee/second
int16_t x_gyro = rxbuf[13];
x_gyro = x_gyro<<8| rxbuf[14];
int16_t y_gyro = rxbuf[15];
y_gyro = y_gyro <<8|rxbuf[16];
int16_t z_gyro = rxbuf[17];
z_gyro = z_gyro<<8 |rxbuf[18];
data.yaw = 0.1*yaw;
data.pitch = 0.1*pitch;
data.roll = 0.1*roll;
data.x_acc = x_acc/16384.0;
data.y_acc = y_acc/16384.0;
data.z_acc = z_acc/16384.0;
data.x_gyro = x_gyro/32.8;
data.y_gyro = y_gyro/32.8;
data.z_gyro = z_gyro/32.8;
return imu::data;
// std::cout<<"yaw = "<<std::setw(12)<<std::right<<0.1*yaw<<std::setw(12)<<" pitch = "<<0.1*pitch<<std::setw(12)<<" roll = "<<0.1*roll;
// std::cout<<std::endl;
// std::cout<<"x_acc = "<<std::setw(12)<< x_acc/16384.0<<std::setw(12)<<" y_acc = "<<std::setw(12)<< y_acc/16384.0<<std::setw(12)<<" z_acc = "<<z_acc/16384.0;
// std::cout<<std::endl;
// std::cout<<"x_gyro = "<<std::setw(12)<<x_gyro/32.8<<std::setw(12)<<" y_gyro = "<<y_gyro/32.68<<" z_gyro = "<<z_gyro/32.68;
// std::cout<<std::endl;
}
uint8_t tmpch[1] ;
read(fd,tmpch,sizeof(uint8_t));
if(*tmpch == 0xa5&&!recstatus){
prerecstatus = 1;
recstatus = 0;
ccnt = 0;
packerflag = 0 ;
// std::cout<<"prerecstatus"<<std::endl;
continue;
}
if(*tmpch == 0x5a && prerecstatus){
prerecstatus = 0;
recstatus = 1;
ccnt = 0;
check = 0;
// std::cout<<"recstatus"<<std::endl;
continue;
}
if(prerecstatus){
prerecstatus = 0;
continue;
}
if(ccnt ==0&&recstatus){
check += (*tmpch);
data_length= *tmpch;
rxbuf[ccnt++] = *tmpch;
// std::cout<<"data length = "<<(uint16_t)(data_length)<<std::endl;
continue;
}
if(ccnt == data_length && *tmpch == 0xaa){
recstatus = 0;
packerflag = 1;
// std::cout<<"package at "<<(uint16_t)ccnt<<std::endl;
ccnt = 0;
continue;
}
if(ccnt == data_length-1){
rxbuf[ccnt++] = *tmpch;
continue;
}
if(ccnt==data_length){
recstatus = 0;
packerflag = 0;
ccnt = 0;
// std::cout<<"recv end unexpecttedly "<<std::endl;
continue;
}
if(recstatus == 1){
rxbuf[ccnt++] = *tmpch;
check += *tmpch;
continue;
}
}
}
int initIMU(){
char *path;
//获取串口设备描述符
path = (char *)default_path;
printf("This is tty/usart demo.n");
fd = open(path, O_RDWR | O_NOCTTY);//noctty,not influenced by terminal,
//fcntl(fd,F_SETFL,0);
if(fd<0){
printf("Fail to Open %s devicen", path);
return -1;
}
struct termios opt;//BASIC SETTING
//设置串口输出波特率
cfsetospeed(&opt, B115200);
//设置串口输入波特率
cfsetispeed(&opt, B115200);
//设置数据位数 8位数据位
opt.c_cflag &= ~CSIZE;
opt.c_cflag |= CS8;
//校验位 无校验位
opt.c_cflag &= ~PARENB;
opt.c_iflag &= ~INPCK;
//设置停止位 1位停止位
opt.c_cflag &= ~CSTOPB;
opt.c_cflag |= CLOCAL | CREAD;
opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
opt.c_oflag &= ~OPOST;
opt.c_iflag &= ~(BRKINT | ICRNL | ISTRIP | IXON);
//设置停止time
opt.c_cc[VTIME]=20;
opt.c_cc[VMIN]=0;
//更新配置
tcsetattr(fd, TCSANOW, &opt);
std::cout << "Device ttyUSB0 is set to 115200bps,8N1n" << std::endl;
tcflush(fd,TCIOFLUSH);
write(fd,start,6);
tcflush(fd,TCIOFLUSH);
return 0;
}
int closeIMU(){
write(fd,stop,6);
tcflush(fd,TCIOFLUSH);
return 0;
}
int startIMU(){
write(fd,start,6);
tcflush(fd,TCIOFLUSH);
return 0;
}
}
serial.h
#ifndef SERIAL_H_
#define SERIAL_H_
#include
namespace imu{
//imu instruction
static const uint8_t start[6] = {0xA5,0x5A,0x04,0x01,0x05,0xAA};
static const uint8_t stop[6] = {0xA5,0x5A,0x04,0x02,0x06,0xAA};
static int data_length = 40;
//第一部分代码/
//根据具体的设备修改
static char default_path[] = "/dev/ttyUSB0";
static int fd = -1;
static unsigned char prerecstatus;
static unsigned char recstatus;
static unsigned char ccnt = 0;
static unsigned char packerflag = 0;
static uint8_t check = 0;
static unsigned char rxbuf[100];
struct Data{
double yaw;
double roll;
double pitch;
double x_acc;
double y_acc;
double z_acc;
double x_gyro;
double y_gyro;
double z_gyro;
};
static Data data;
Data getCurrentPos();
int initIMU();
int closeIMU();
int startIMU();
}
#endif