自动驾驶实战系列(四)——自动驾驶中的串口通信及协议解析

本来这部分内容在第三篇已经讲过,但是近期写了一款IMU驱动,涉及到一些ROS下串口通信的知识,网上能够看到的一些代码写的觉觉都是demo,稳定性比较差,所以这里准备总结一套通用的模板,后续可以自己修改着来适配自己的机器,无论是单独的GPS、IMU还是组合导航,均可以按以下讲解去写。感谢哈工大锦博,对我指点颇多。

一、数据协议

1、NMEA

​ NMEA 0183是美国国家海洋电子协会(National Marine Electronics Association )为海用电子设备制定的标准格式。目前业已成了GPS导航设备统一的RTCM标准协议。GPS接收机根据NMEA-0183协议的标准规范,将位置、速度等信息通过串口传送到PC机、PDA等设备。大多数常见的GPS接收机、GPS数据处理软件、导航软件都遵守或者至少兼容这个协议。NMEA-0183协议定义的语句非常多,但是常用的或者说兼容性最广的语句只有$GPGGA、$GPGSA、$GPGSV、$GPRMC、$GPVTG、$GPGLL等。下面贴一个GPGGA的字段说明示例。

1
$GPGGA, 092204.999, 4250.5589, S, 14718.5084, E, 1, 04, 24.4, 19.7, M,,,, 0000*1F
  • 字段0:$GPGGA,语句ID,Global Positioning System Fix Data定位信息。

  • 字段1:UTC 时间,hhmmss.sss,时分秒格式

  • 字段2:纬度ddmm.mmmm,度分格式(前导位数不足则补0)

  • 字段3:纬度N(北纬)或S(南纬)

  • 字段4:经度dddmm.mmmm,度分格式(前导位数不足则补0)

  • 字段5:经度E或W

  • 字段6:GPS状态,0=未定位,1=非差分定位,2=差分定位,3=无效PPS,6=正在估算

  • 字段7:正在使用的卫星数量(00 - 12)(前导位数不足则补0)

  • 字段8:HDOP水平精度因子(0.5 - 99.9)

  • 字段9:海拔高度(-9999.9 - 99999.9)

  • 字段10:地球椭球面相对大地水准面的高度

  • 字段11:差分时间(从最近一次接收到差分信号开始的秒数,如果不是差分定位将为空)

  • 字段12:差分站ID号0000 - 1023(前导位数不足则补0,如果不是差分定位将为空)

  • 字段13:校验值

实例

星网宇达M2系列,Novatel等等

2、Modbus RTU

Modbus 一个工业上常用的通讯协议,已经成为工业通讯领域的业界标准,最初的ModBus仅支持串口,分为RTU和ACSII两种信号传输模式(一般基于RS485串口通信媒介)。而随着时代的变迁,ModBus新增了TCP版本,可以通过以太网进行通讯。ModBus采用半双工的通讯方式,由1个子站和多个从站组成,允许多个设备连接在同一个网络上进行通讯。上述三类对应的物理层:

  • ModBus TCP,以太网,RJ45。

  • ModBus RTU/ASCII,异步串行通讯,RS232/485/422、光纤、无线

  • ModBus Plus,高速令牌传递网络,同轴电缆或光缆。

参考博客:Modbus通讯协议(一)——概述

在自动驾驶领域,使用的modbus协议一般是RTU传输方式,两者的主要区别如下

  • ModBus ASCII协议拥有开始和结束标记,ModBus RTU没有。

  • ASCII协议的可读性强、传输效率低。

  • RTU采用CRC校验,而ASCII采用LRC校验

实例

组合导航INS设备

我手上拿到的这款INS设备输出数据有三路输出、输入接口

  • RS232:差分4G定位
  • RS422: 记录原始数据,即给用户使用的INS融合数据
  • CAN口:转发CAN口,可接四轮轮速,档位和方向盘转角

这里是R422口输出融合的ins数据,输出波特率默认为230400,这里可以通过上位机软件重新设定波特率和频率。

这是一个衡量符号传输速率的参数。它表示每秒钟传送的符号的个数,波特率和距离成反比,高波特率常常用于放置的很近的仪器间的通信。

​ 正常情况下从RS232差分口输出的是GPGGA/GPRMC协议格式的数据,如果想要GNSS原始数据,可以从差分口分两路出来,一路连接DTU,另外一路串口输出GGA和RMC格式数据。

RS232和RS485的区别在于,RS232是要用在近距离传输上最大距离为30M,RS485用在长距离传输最大距离1200M。

​ RS422端口输出的是给用户的融合数据,这里是BD DB 0B为帧头的定长二进制数据。(有些厂商,比如星网宇达,直接输出类似差分口nmea数据,GPGGA/GTIMU/GPFPD等,传输的时候都是逐字节传,用char字符数组来接收数据,切割字符串即可)

某IMU设备

这是一款IMU设备,也是串口通信,采用modbus rtu协议。

112

二、协议解析

下面分别给出modbus和nmea协议解析的示例代码和注意事项。

Modbus RTU

首先是某INS设备,我们可以看其参考文档里面输出数据格式表。

image-20200109185614504

以以上协议为例分析,帧头为BD DB 0B,一帧完整的数据58字节,最后一个字节为异异或校验位,从serial流中读取字节。可以通过帧头+长度+异或校验来解析一帧完整的数据。详细步骤就是,在buffer中去查找帧头,连帧头共58帧封装成一帧完整的数据,按字段和厂商提供的换算方式进行解析。最后用ROS发布出来,或sensor_msg/Imu、sensor_msg/NavSatFix标准格式,或者自定义msg格式。

下面是某IMU的角度读取格式表,类似上面的INS设备,这里帧头是0x00,定长,17字节为一帧完整的数据。解析的时候还是帧头+长度+校验,这里的校验是CRC16,循环冗余校验码,具体的算法流程我没有仔细研究,但会直接给出代码。

image-20200326201130271

这里我们在工程实现上,我们用一个循环缓冲区去读buffer,然后解析一帧发布一帧。

首先初始化串口的一些基本参数,注意波特率需要和上位机软件设定的一致

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
bool InitPort(string port, string frame_id, int baudrate, int parity, int rionrate) {

// init some param
yaw.header.frame_id = frame_id;
yaw.orientation.w = 1.0;
yaw.orientation.x = 0.0;
yaw.orientation.y = 0.0;
yaw.orientation.z = 0.0;

have_new_data = false;// 一帧数据解析完的标志位

// 设定奇偶校验位
serial::parity_t parity_;
switch (parity) {
case 0:
parity_ = serial::parity_none;
break;
case 1:
parity_ = serial::parity_odd; // 奇校验
break;
case 2:
parity_ = serial::parity_even; // 偶校验
break;
case 3:
parity_ = serial::parity_mark;
break;
case 4:
parity_ = serial::parity_space;
break;
default:
ROS_ERROR_STREAM("invalid serial parity setting, parity set is "
<< parity);
break;
}

//设置串口属性,并打开串口
try {
ser.setPort(port);
ser.setBaudrate(baudrate);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
ser.setTimeout(to);
ser.open();
}
catch (serial::IOException &e) {
ROS_ERROR_STREAM("Unable to open port ");
return false;
}

这里注意,modbus在解析的时候,由于是半双工通信,所以需要丛机向主机请求数据,主机响应。

发布imu话题

1
2
ros::Publisher yaw_data_pub = nh_priv.advertise<sensor_msgs::Imu>(imu_topic, 1); //缓冲区不要太大
ros::Rate loopRate(200); //这里的发布频率需要比串口传输数据的频率快

初始化一个循环队列,把buffer中每一个字节的数据往里面装,之后在循环读取->解析数据即可

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
Queue quene();//写一个循环队列的类

while (ros::ok()) {
if (quene.ser.available()) { //我的serial::serial ser是全局变量
size_t m = quene.ser.available();
if (m != 0) {
uint8_t buffer[1024];
m = quene.ser.read(buffer, m);//读串口,装到buffer里面

for (int i = 0; i < m; i++) {
if (quene.EnQueue(buffer[i])) // 缓冲区满了返回false
{
cout << "EnQueue error" << endl;
}
}

quene.HandleData(); //此函数解析串口数据
if (quene.have_new_data) { //解析完新的一帧就发布出去
yaw_data_pub.publish(quene.publishNewData(true, true));
quene.have_new_data = false;
}
}
} else {
cout << "waiting data." << endl;
}

//处理ROS的信息,比如订阅消息,并调用回调函数
ros::spinOnce();
loopRate.sleep();
}

最后是解析数据帧,这里有一个常用的写法

1
2
3
4
5
6
7
//按字节读16进制数据,按高低位拼成4字节
frame_len = ((uint32_t) GetQueue(i++));
frame_len |= ((uint32_t) GetQueue(i++)) << 8;
frame_len |= ((uint32_t) GetQueue(i++)) << 16;
frame_len |= ((uint32_t) GetQueue(i++)) << 24;

zw = (static_cast<double>(frame_len) - 150000) * 0.01;

CRC16校验,需要知道这一帧的每一个字节数据,建议单独保存在一个uint8_t数组中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
uint8_t tmp[15] = {0};
// 把每一位数据添加到tmp数组,代码类似,自己改一下
if (i + 15 < len) {
tmp[1] = 0x03;
tmp[2] = GetQueue(i++);//这里帧头和规定的第二字节数据已经做出判断

for (int j = 0; j < 11; ++j) { //之后的每一个字节和队列中的下标一致
tmp[3 + j] = GetQueue(i + j);
}
}

uint16_t getCode = GetCrcData(tmp, 15);//判断数据帧的校验和是否与CRC16计算的一致
if (crc_check == GetCrcData(tmp, 15)) {
have_new_data = true;
}

uint16_t GetCrcData(uint8_t *buffer, uint32_t len) {
uint16_t wcrc = 0XFFFF;//16位crc寄存器预置
uint8_t temp;
uint32_t i = 0, j = 0;//计数
for (i = 0; i < len; i++)//循环计算每个数据
{
temp = *buffer & 0X00FF;//将八位数据与crc寄存器亦或
buffer++;//指针地址增加,指向下个数据
wcrc ^= temp;//将数据存入crc寄存器
for (j = 0; j < 8; j++)//循环计算数据的
{
if (wcrc & 0X0001)//判断右移出的是不是1,如果是1则与多项式进行异或。
{
wcrc >>= 1;//先将数据右移一位
wcrc ^= 0XA001;//与上面的多项式进行异或
} else//如果不是1,则直接移出
{
wcrc >>= 1;//直接移出
}
}
}
uint8_t CRC_L;//定义数组
uint8_t CRC_H;
CRC_L = wcrc & 0xff;//crc的低八位
CRC_H = wcrc >> 8;//crc的高八位
return ((CRC_H << 8) | CRC_L);
}

总结,在此类串口通信时,需要注意以下两点

  • 丢弃错误帧,保证发布出来的数据完全无误(帧头+长度+异或校验)
  • ros的发布频率必须快于imu串口(否则可能导致数据延迟)

NMEA 0183

星网宇达M2

image-20200109190555697

​ 一般来说,如果直接读取差分口数据,类似星网宇达(无DTU),则可直接获取类似以上$GPGGA开头的nmea数据。这种相对来说比较容易,因为一行一帧数据,只需要校验每一行数据是否正常,无需手动通过帧头帧尾去分封帧。而是逐行读取,按字段分割即可,这里感谢自动化所飞哥提供的代码,网上也可以搜到很多类似。

1
2
3
4
5
6
7
8
9
10
11
12
13
while not rospy.is_shutdown():
g = 9.81
if (ser.read() != '$'):
continue
line0 = ser.readline() #逐行读取,按','分割
cs = line0[-4:-2]
cs1 = int(cs, 16)
cs2 = checksum(line0)
if (cs1 != cs2):
continue
line = line0.replace("\r\n", " ")
line = line.replace("*", ",")
words = string.split(line, ",")

GTIMU/GPFPD协议解析

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
# 协议字段解析
if words[0] == "GTIMU":
GPSWeek = string.atoi(words[1])
GPSTime = string.atof(words[2])
GyroX = string.atof(words[3])
GyroY = string.atof(words[4])
GyroZ = string.atof(words[5])
AccX = string.atof(words[6])
AccY = string.atof(words[7])
AccZ = string.atof(words[8])
Tpr = string.atof(words[9])
elif words[0] == "GPFPD":
GPSWeek = string.atoi(words[1])
GPSTime = string.atof(words[2])
Heading = string.atof(words[3])
Pitch = string.atof(words[4])
Roll = string.atof(words[5])
Lattitude = string.atof(words[6])
Longitude = string.atof(words[7])
Altitude = string.atof(words[8])
Ve = string.atof(words[9])
Vn = string.atof(words[10])
Vu = string.atof(words[11])
Baseline = string.atof(words[12])
NSV1 = string.atoi(words[13])
NSV2 = string.atoi(words[14])
Status = words[15]

if (ser.read() != '$'):
continue

Imu /Gps数据发布

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
q_avg =tf.transformations.quaternion_from_euler(numpy.deg2rad(Roll), numpy.deg2rad(Pitch), numpy.deg2rad(Heading))  

# imu data
Imumsg.angular_velocity.x = numpy.deg2rad(GyroX)
Imumsg.angular_velocity.y = numpy.deg2rad(GyroY)
Imumsg.angular_velocity.z = numpy.deg2rad(GyroZ)
Imumsg.linear_acceleration.x = AccX*g
Imumsg.linear_acceleration.y = AccY*g
Imumsg.linear_acceleration.z = AccZ*g
Imumsg.orientation.w = q_avg[3]
Imumsg.orientation.x = q_avg[0]
Imumsg.orientation.y = q_avg[1]
Imumsg.orientation.z = q_avg[2]
Imumsg.header.stamp = rospy.Time.now()
Imumsg.header.frame_id = 'imu_node'
Imumsg.header.seq = seq
# gps data
GPSmsg.latitude = Lattitude
GPSmsg.longitude = Longitude
GPSmsg.altitude = Altitude
#GPSmsg.status.status = Status
GPSmsg.header.stamp = rospy.Time.now()
GPSmsg.header.frame_id = 'gps_node'
GPSmsg.header.seq = seq

Velmsg.twist.linear.x = math.sqrt(Ve**2 + Vn ** 2)
Velmsg.twist.linear.y = 0
Velmsg.twist.linear.z = 0
Velmsg.twist.angular.x = 0
Velmsg.twist.angular.y = 0
Velmsg.twist.angular.z = numpy.deg2rad(GyroZ)
# pub
if (Status == '00'):
print("init")
if (Status == '01'):
print("coarse alignment")
if(Status == '02'):
print("fine alignment")
if(Status == '03'):
print("gps direction")
if(Status == '04'):
print("gps localization")
if(Status == '05'):
print("rtk")
print('pub')
pub_gps.publish(GPSmsg)
pub_imu.publish(Imumsg)
pub_vel.publish(Velmsg)
seq = seq + 1
rate.sleep()

三、致谢

感谢:一清UDI、导远电子程工、中科院自动化所飞哥、主线科技刘工、哈工大刘博等

-------------    本文结束  感谢您的阅读    -------------
胡想成 wechat
欢迎关注个人公众号!
您的支持,是我装逼的最大动力!

本文标题:自动驾驶实战系列(四)——自动驾驶中的串口通信及协议解析

文章作者:胡想成

发布时间:2020年03月23日 - 17:03

最后更新:2020年06月05日 - 17:06

原始链接:xchu.net/2020/03/23/46serial-driver/

版权声明:本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0许可协议,转载请注明出处!