嵌入式通讯数据转化及实现

  底层板子和电脑通讯，需要一种相互之间约定规则，也就是通讯协议。常用的数据协议组成基本：数据头+校验+数据+数据尾。数据头一般包括标志位/数据有效位/数据类型/时间等，校验一般常用的抑或运算，累加求和等，数据尾一般就是数据的结束标志。本文主要是探讨记录，python程序与arduino之间的串口通讯问题。其中上层借鉴ROS的串口协议进行处理实现。

---

ＲＯＳ通讯协议

  主要是为了解决与ROS对接问题，所以研究分析ROSserial 的具体实现，用于借鉴。
协议包格式
1st Byte - Sync Flag (Value: 0xff)
2nd Byte - Sync Flag / Protocol version
3rd Byte - Message Length (N) - Low Byte
4th Byte - Message Length (N) - High Byte
5th Byte - Checksum over message length
6th Byte - Topic ID - Low Byte
7th Byte - Topic ID - High Byte
N Bytes - Serialized Message Data
Byte N+8 - Checksum over Topic ID and Message Data

  不同ROS发行版对应不同协议版本字段定义（0xff：ROS Groovy, 0xfe on ROS Hydro, Indigo, and Jade.）。Topics ID 0-100为系统功能专用主题使用, 这些主题类似于消息 rosserial_msgs/TopicInfo 中定义的那些特定主题。长度和data的checksum字段用于确保包的完整性，data的checksum可以按照如下公式计算：
255 - ( (Topic ID Low Byte + Topic ID High Byte + data byte values) % 256)

 ROS代码实现：
（SerialClient.py 文档内）

class SerialClient:
{
...
def send(self, topic, msg):
# Send a message on a particular topic to the device. 
    with self.mutex:
       length = len(msg)
        if self.buffer_in > 0 and length >　self.buffer_in:
                rospy.logerr("Message from ROS network dropped: message larger than buffer.")
                print msg
                return -1
            else:
#modified frame : header(2 bytes) + msg_len(2 bytes) + msg_len_chk(1 byte) + topic_id(2 bytes) + msg(x bytes) + msg_topic_id_chk(1 byte)
# second byte of header is protocol version
                 msg_len_checksum = 255 - ( ((length&255) + (length>>8))%256 )
                 msg_checksum = 255 - ( ((topic&255) + (topic>>8) + sum([ord(x) for x in msg]))%256 )
                 data = "\xff" + self.protocol_ver  + chr(length&255) + chr(length>>8) + chr(msg_len_checksum) + chr(topic&255) + chr(topic>>8)
                 data = data + msg + chr(msg_checksum)
                 self.port.write(data)
                 return length
...

def run(self):
#Forward recieved messages to appropriate publisher. 
        data = ''
        while not rospy.is_shutdown():
            if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3):
                if (self.synced == True):
                    rospy.logerr("Lost sync with device, restarting...")
                else:
                    rospy.logerr("Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino")
                self.lastsync_lost = rospy.Time.now()
                self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "no sync with device")
                self.requestTopics()
                self.lastsync = rospy.Time.now()
# This try-block is here because we make multiple calls to read(). Any one of them can throw
# an IOError if there's a serial problem or timeout. In that scenario, a single handler at the
# bottom attempts to reconfigure the topics.
            try:
                if self.port.inWaiting() < 1:
                    time.sleep(0.001)
                    continue

                flag = [0,0]
                flag[0] = self.tryRead(1)
                if (flag[0] != '\xff'):
                    continue

                flag[1] = self.tryRead(1)
                if ( flag[1] != self.protocol_ver):
                    self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
                    rospy.logerr("Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client")
                    protocol_ver_msgs = {'\xff': 'Rev 0 (rosserial 0.4 and earlier)', '\xfe': 'Rev 1 (rosserial 0.5+)', '\xfd': 'Some future rosserial version'}
                    if (flag[1] in protocol_ver_msgs):
                        found_ver_msg = 'Protocol version of client is ' + protocol_ver_msgs[flag[1]]
                    else:
                        found_ver_msg = "Protocol version of client is unrecognized"
                    rospy.loginfo("%s, expected %s" % (found_ver_msg, protocol_ver_msgs[self.protocol_ver]))
                    continue

                msg_len_bytes = self.tryRead(2)
                msg_length, = struct.unpack("<h", msg_len_bytes)

                msg_len_chk = self.tryRead(1)
                msg_len_checksum = sum(map(ord, msg_len_bytes)) + ord(msg_len_chk)

                if msg_len_checksum % 256 != 255:
                    rospy.loginfo("wrong checksum for msg length, length %d" %(msg_length))
                    rospy.loginfo("chk is %d" % ord(msg_len_chk))
                    continue

# topic id (2 bytes)
                topic_id_header = self.tryRead(2)
                topic_id, = struct.unpack("<h", topic_id_header)

                try:
                    msg = self.tryRead(msg_length)
                except IOError:
                    self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Packet Failed : Failed to read msg data")
                    rospy.loginfo("Packet Failed :  Failed to read msg data")
                    rospy.loginfo("msg len is %d",len(msg))
                    raise

# checksum for topic id and msg
                chk = self.tryRead(1)
                checksum = sum(map(ord, topic_id_header) ) + sum(map(ord, msg)) + ord(chk)

                if checksum % 256 == 255:
                    self.synced = True
                    try:
                        self.callbacks[topic_id](msg)
                    except KeyError:
                        rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
                    rospy.sleep(0.001)
                else:
                    rospy.loginfo("wrong checksum for topic id and msg")

            except IOError:
# One of the read calls had an issue. Just to be safe, request that the client
# reinitialize their topics.
                self.requestTopics()

}

  上两段代码是从ROS程序中找到的串口生成发送指令，以及读取解析指令的程序。其中
struct.unpack("<h", topic_id_header)
struct.pack()
是python 的一个struct模块，实现对数据进行字节转化解包与打包的作用。
参考博客

  在代码中还发现了一下函数，单未找到具体实现方法在哪里，所以记录下，留待后续大神指点。跟人感觉应该实现的功能与struct相同，可能针对数据类型不一样，不过仅仅个人猜测尚未证实。
message.serialize(data)
message.deserialize(data_buffer)

  通过struct.pack(fat,vel,vel…)将数据生成为二进制字节流。下位机接受串口字节流数据，根据上位机序列化的数据，将二进制转化生成对应数据。由于下位机采用的Arduino uno ,所以根据支持的数据格式，实际float和double实际上是相同精度和类型,对应的４个字节表示一个float/double类型数据。另外arduino板子采用小端存储，所以上位机转字节流包的时候需要采用小端进行转化。
  Arduino 数据类型

 图片来源

  struct 类型表：

  需要解决以下问题：
１、４byte与float(double)数据互转
２、１byte 与char 数据
３、２byte 转化为int数据

  通过参考资料有两种普遍推荐的，一种是指针强转，一种借助union特性。指针的具体可以参照文献进行尝试，由于未知原因，第一次尝试指针强转时未能成功。现在回头想一下，个人分析感觉应该是数据大小端存储的原因导致无法解析出数据。后续应用了union的方式解决了问题，此处记录我所使用的union方法。

  查询c语言union联合体，简单来说就是，基地址相同，不同数据类型根据自己的所占空间（字节）共用一块内存对数据进行存储。根据读取指定方式对内存中存储的数据进行解析。

union <name>
{
    <datatype>  <1st variable name>;
    <datatype>  <2nd variable name>;
    .
    .
    .
    <datatype>  <nth variable name>;
} <union variable name>;

  数据小端／大端模式存储情况对数据存储的影响。首先定义一个union联合体，然后存入一个２byte的数据，看下具体在实际内存中的存储。具体程序可以参考这篇不错的博文。

a = 0x1234

小端模式

大端模式

对于arduino想存储不同数目的指令，所以需要一个类似vector容器来动态的改变数组的大小。解决方案，有链表，另外也可以添加参考资料12的c++for arduino链接库（又说mega2560一下的可能会出现内存不足的可能，尚未实验）。
参考资料
参考资料１
参考资料２
参考资料３
参考资料４
参考资料５
参考资料６
参考资料７
参考资料８
参考资料９
参考资料10
参考资料11
参考资料12
（数据序列化相关参数，具体协议待补全）

ROSserial 安装

　命令对应对应的ros版本进行apt-get方式安装
　sudo apt-get install ros-kinetic-serial
　

测试是否安装成功
roscd serial

安装rosserial-arduino
sudo apt-get install ros-kinetic-rosserial-arduino

安装rosserial
sudo apt-get install ros-kinetic-rosserial

进入arduino IDE安装目录库文件夹~/Arduino/libraries安装对应的库.
rosrun rosserial_arduino make_libraries.py ./

（未完待续）