# UWB_serial_parser **Repository Path**: cheng-jiaming-cjm/uwb_serial_parser ## Basic Information - **Project Name**: UWB_serial_parser - **Description**: 主要内容为解析UWB模块（LinkTrack P-B）通过串口发送的定位帧数据，记录了解决思路和过程。 - **Primary Language**: Unknown - **License**: Not specified - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 1 - **Forks**: 4 - **Created**: 2021-11-28 - **Last Updated**: 2023-06-22 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # UWB串口数据解析 ## 1.UWB实现途径：LinkTrack 目前实验室采用的是[深圳市空循环科技有限公司](https://www.nooploop.com/)提供的LinkTrack P-B，相关参数见下表。 LinkTrack P-B ### 1.1 LinkTrack P-B工作模式 （1）LP Mode（局部定位模式）——测距、定位、导航、授时、通信 定位原理与GPS定位相似，应用场合包含单区域定位场合、多区域定位场合以及其他场合。包含多种模式（LP_MODE0、1、4、5、6），主要差别为各个角色的容量与更新频率差异，从而可以适应不同的应用场合需求，但使用方法相同。 在该模式下，可以通过将标签参数配置中的定位输出帧改为NMEA-0183，实现伪GPS。GPS的NMEA-0183输出格式使用的是WG384坐标系，即地球坐标系，而TAG实际输出的数据皆基于基站坐标系，采用的基站坐标系原点是虚拟、固定的（北纬22.5180977度，东经113.9007329度），最终输出的坐标为模拟出的经纬度。因此，如需代替GPS传给无人机，还需结合基站的实际位置做相应转换。根据手册，定位的精度可以保证，一般可直接替换原有载体的GPS接收机。 （2）DR Mode（分布式测距模式）——分布式测距、通信、授时 该模式下只有NODE一种角色，每个节点角色都等价，不区分TAG、ANCHOR、CONSOLE等角色。同时要求运行于同一套系统中的NODE所配置的容量一致。使用NAssistant后，选择其中一个Node，可以观察其与别的节点之间的距离、信号强度等数据。 （3）DT Mode（数传模式）——一对多广播、一对一双向通信 一般配置一个Node为主机MASTER，配置其他的Node为从机SLAVE。将需要发送的数据与传输模式嵌入到NLink_LinkTrack_User_Frame1协议帧，再通过通信接口（UART/USB）发送给MASTER，MASTER会通过无线报文传输给相应的SLAVE，从而实现通信功能。 分为DT_MODE0、DT_MODE1和DT_MODE2。其中DT_MODE1为双向数传模式，支持一个主机与一个从机双向通信；DT_MODE2为广播模式，支持一个主机向无限个从机广播数据；DT_MODE0则为上述两种模式的集成。 （4）三种模式在Role与Protocol上的差异 Mode、Role与Protocol关系图 其中，透明输入指的是通过通信接口（如串口）直接发送未经编码的数传数据。 ![通信结构图](./image/commStructure.jpg) **值得注意**的是： A.手册中还提到了`NLink_LinkTrack_Setting_Frame0`，通过这一协议可以实现读取当前节点 role、id、基站坐标，写入基站坐标功能。但若要进行写操作，需在进行读操作后，将读取到的部分数据嵌入到要写入的数据帧中。同时基站坐标写入只有在 LP Mode 下，写入到 ANCHOR 或 CONSOLE 中才有效，而TAG 只支持基站坐标读取功能。并且对于TAG，其上电默认所有基站坐标数值无效，直到接收到对应有效基站坐标才会更新，可通过读输入帧查询 TAG 接收基站坐标的具体数值。 B.暂时只能获取 UART、USB 的通信协议，CAN 通信协议暂未开放。 （5）LinkTrack P-B使用过程对软件NAssistant的依赖程度 手册中Q&A提到的“所有组网、定位解算均在模块中完成，NAssistant 主要负责监测、显示、控制、配置功能。并不是在终端运行 NAssistant 系统才能正常工作”，是最简练的答案。 而实际上，整个的模式配置过程都是在该软件上进行的，使用软件时既可以实时观察到状态数据，也能进行无线设置（网络内正常组网的节点的查看及操作）、固件更新以及录制、回放与导出。其中，录制将输出.dat文件，实时状态或者回放状态将导出文本数据到本地.xlsx 文件。在厂家提供的使用ROS解析数据中，也提到了需用到NAssistant。为 >本ROS包用于支持Nooploop产品，采用C++编写，只需**根据对应产品手册，通过我们的上位机NAssistant配置好模块，确定设备已正常工作**后，运行本ROS包中产品对应的节点即可获取产品协议一一对应的ROS消息，为您省去处理数据通信及解析的时间 这里可以看出，如果已经安装了ROS，只要稍加改进应该就能在LP模式下用UWB代替GPS了。而如果硬要自己写解析程序，也得对症下药——根据采用的模式与协议进行解析。 ## 2.通信协议解析 ### 2.1关注的通信协议 根据实用目的，暂时只将LP Mode的四种定位输出帧作为解析处理的对象。 （1）NLink_LinkTrack_Tag_Frame0 ![TAG_FRAME0](image/NLINK_LINKTRACK_TAG_FRAME0.png) （2）NLink_LinkTrack_Node_Frame2 ![NODE_FRAME2](image/NLINK_LINKTRACK_NODE_FRAME2_1.png) ![NODE_FRAME2](image/NLINK_LINKTRACK_NODE_FRAME2_2.png) （3）NLink_LinkTrack_Node_Frame3 ![NODE_FRAME3](image/NLINK_LINKTRACK_NODE_FRAME3.png) （4）NMEA-0183 NMEA - 0183 是美国国家海洋电子协会（National Marine Electronics Association）为海用电子设备制定的标准格式。目前业已成了全球导航卫星系统下导航设备统一的 RTCM（Radio Technical Commission for Maritime services）标准协议。该协议采用 ASCII 码来传递信息，串行通信默认参数为：波特率＝4800bps，数据位＝8bit，开始位=1bit，停止位＝1bit，无奇偶校验。 帧格式形如：$aaccc,ddd,ddd,…,ddd*hh(CR)(LF) 其中，“\$”为帧命令起始位；aaccc为地址域，前两位aa为识别符，后三位ccc为语句名；ddd…ddd为数据；“\*”为校验和前缀（也可以作为语句数据结束的标志）；hh为$与\*之间所有字符 ASCII 码的校验和（各字节做异或运算，得到校验和后，再转换 16 进制格式的 ASCII 字符）；(CR)(LF)为帧结束，回车和换行符。 NMEA-0183 常用命令如表所示： | 序号 | 命令 | 说明 | 最大帧长 | | :---: | :--------: | :----------------: | :------: | | **1** | **$XXGGA** | **定位信息** | **72** | | 2 | $XXGSA | 当前卫星信息 | 65 | | 3 | $XXGSV | 可见卫星信息 | 210 | | **4** | **$XXRMC** | **推荐定位信息** | **70** | | 5 | $XXVTG | 地面速度信息 | 34 | | 6 | $XXGLL | 大地坐标信息 | -- | | 7 | $XXZDA | 当前时间(UTC1)信息 | -- | 其中，UTC1即协调世界时，相当于本初子午线(0 度经线)上的时间，北京时间比 UTC 早 8 个小时。XX可以为GP（GPS）、BD（北斗）、GL（GLONASS），有时可以是GN（多星联合）。GPS下各命令的发送次序为\$GPZDA、\$GPGGA、\$GPGLL、\$GPVTG、\$GPGSA、\$GPGSV*3、\$GPRMC。 在这里只关注$GPRMC，格式介绍如下 $GPRMC 用户手册中对NMEA-0183的介绍见8.2，包含GGA 、GSA和RMC。 ### 2.2解析程序 基础参考：厂家所提供的解析程序，见[3]。目前进展如下： - 已将[3]中的程序整合至Qt项目`NLinkOrigin.pro`中，通过删减其中发送ROS消息的功能，完成了项目`NLink_ROSFree.pro`。经验证，在Linux环境中支持使用cmake或qmake编译，运行后可与指定的串口或USB口建立联系；增加了打印解析结果的回调函数，便于调试，暂未增加与其他程序的接口。 - 解析程序中似乎不包含NLink_LinkTrack_Setting_Frame0与NLink_LinkTrack_User_Frame1，此外多出了NLink_LinkTrack_Node_Frame5和NLink_LinkTrack_Node_Frame6。 （1）关于原程序架构 ```C++ int main(int argc, char **argv) { ros::init(argc, argv, "linktrack_parser"); ros::NodeHandle nh; serial::Serial serial; initSerial(&serial); NProtocolExtracter protocol_extraction; linktrack::Init init(&protocol_extraction, &serial); ros::Rate loop_rate(1000); while (ros::ok()) { auto available_bytes = serial.available(); std::string str_received; if (available_bytes) { serial.read(str_received, available_bytes); protocol_extraction.AddNewData(str_received); } ros::spinOnce(); loop_rate.sleep(); } return EXIT_SUCCESS; } ``` 结合main.cpp与其他源文件，整个程序的流程为： - ros与NodeHandle节点 init； - serial init； - linktrack init: 初始化解码的protocl，以及ROS将用到的publisher、subscribler，同时设置callback，保证收到指定帧数据时，能发布相应的message； - 设置ROS的message发布速率后进入循环； - 循环内处理串口信息：解析——判断protocol的handel是否有效，无效则终止程序——callback调用，填充消息并发布，并不时调用`ros::spinOnce()`，查看缓冲区内有无消息，进行轮转； - ROS关闭后跳出循环并return。 （2）原程序使用及相关问题 原程序使用步骤： - 安装serial库：获取代码后，进入文件夹编译项目 ``` make ``` 再安装 ``` sudo make install ``` - 克隆代码并编译 ``` cd catkin_workspace/src git clone --recursive https://github.com/nooploop-dev/nlink_parser.git cd ../ catkin_make source devel/setup.bash ``` 运行LinkTrack ``` roslaunch nlink_parser linktrack.launch ``` 相关问题： - 根据CMkaeLists中的配置，默认将库的链接文件`libserial.so`放在/usr/local中，这使得后续编译时可能会出现共享链接库失败(Cannot open shared object)。建议的解决方法是将/usr/local添加至/etc/ld.so.conf中，并使用/sbin/ldconfig -v更新配置。 - 由于不使用ROS播报，因此未安装ROS的情况下连编译都无法通过。可以安装ROS而利用消息发布、订阅机制，也可以修改源文件，只保留解析部分。事实上也正是按第二种方法进行的。 （3）ROS相关 - 消息自定义、发布和订阅机制：参考[4]中相关链接。而形如`nlink_parser/LinktrackAnchorframe0.h`的头文件，是ROS将.msg文件转化所得，一般放在ROS的workspace里，以头文件的形式被其他可执行文件所包含，最终能实现调用相应的消息类型。因此没有ROS的情况下无法产生，也不需要此类头文件。 - ROS设置串口和通信波特率是在文件夹`lauch`中的`.launch`中定义，串口初始化时再借助ROS库获取，形如 ```C++ auto port_name = ros::param::param("~port_name", "/dev/ttyUSB0"); auto baud_rate = ros::param::param("~baud_rate", 921600); ``` - `utils\nutils`似乎是与RVIZ显示可执行消息转换器有关。 （4）关于报错：No such file or directory 在程序编译和运行的过程中，这是常见的一个问题，而可能的因素有：程序中的文件路径写错、源文件因编码无法被系统读取、动态编译缺少解释器、共享链接库失败、文件本身不存在或者文件损坏、无执行权限（chmod 777 xxx）和系统位数与程序位数不同。而在这次程序的运行中，出现了这样的报错： ``` try to open serial port with /dev/ttyUSB0,921600 Unhandled Exception: IO Exception(2): No such file or directory, file .../src/impl/unix.cc/unix.cc, lineXX. ``` 我曾一度以为这是之前的报错类型，但结合前半段的打印内容，我最终发现这是串口初始化过程中抛出的异常。在调用`serial->open()`时，最终会执行这条语句 ```C++ fd_ = ::open (port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); ``` 以读写方式、不把该文件作为终端设备、设置非阻塞方式打开`port_`表示的设备。此处，`fd_`为设备描述符，linux在操作硬件设备时，屏蔽了硬件的基本细节，只把硬件当做文件来进行操作，所有的操作都是以open函数来开始，用以获取`fd_`，然后后期的其他操作全部控制`fd_`来完成对硬件设备的实际操作。而在修改后的init_serial.cpp中，直接使用了`/dev/ttyUSB0`，这表示电脑上的一个USB口，当电脑没有插入U盘等设备时，是不会有此类设备的。最终就会抛出错误，在源文件中即有定义errno 2对应“No such file or directory”。 （5）关于编译：qmake，cmake和catkin_make - catkin_make：在原程序的编译过程中，使用到了catkin_make，它是一个命令行工具，用以构建catkin workspace中的代码。由于并不打算使用ROS，所以只是粗略查阅，得知catkin_make的本质还是cmake。 - qmake：在使用Qt时，我们使用.pro将源文件整合为可以执行的项目，Qt可以提供良好的文件管理和debug环境，并包含一些内置的类，供用户进行通信（如QUdpSocket）和GUI制作。我们可以直接在程序中按下按钮编译和运行，也可以使用命令行语句，如 ``` qmake ../NLink_ROSFree.pro make -j16 //看不同版本Qt上的Build步骤，似乎也会有make -j8 ./Nlink ``` 前者生成Makefile，后者进行编译，生成许多库对应的.o文件和可执行程序（如./Nlink），最后运行。 - cmake：实验室的SummerCamp中有相关的教程，因此我在此不多说。在出现（4）中的情况时，我尝试了改写CMakeList，以保证编译器能找到源文件~~虽然成功运行后还是在报相同的错误~~。如果感兴趣，想进一步学习CMakeList，可以参考阅读[4]中的相关链接。cmake的命令行语句是 ``` cmake .. make ./linktrack ``` ## 3.实物调试 目前进展如下： - 初步实践了如何使用LinkTrack P-B进行UWB定位，但是在使用数根充电线且不使用架子时摆放很麻烦，所以调试先告一段落。 - 整理时发现了源程序还提供了test，有空将其改写、测试现有程序。 ## Reference [1]相关手册：[选型](reference/LinkTrack_and_Parts_Selection_Guide_Manual_V1.2_zh.pdf); [用户手册](reference/LinkTrack_User_Manual_V2.2_zh.pdf); [通信协议](reference/LinkTrack_Datasheet_V2.2_zh.pdf); [NLink协议](reference/NLink_V1.3.pdf) [2]第三方协议介绍：[NMEA-0183(全)](https://blog.csdn.net/st526403649/article/details/54946529); [NMEA-0183(GPS)与$GPRMC解析](https://blog.csdn.net/weixin_30307267/article/details/95421780?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-0.no_search_link&spm=1001.2101.3001.4242.1) [3]解析程序Demo：[C语言解析协议](https://github.com/nooploop-dev/nlink_unpack); [解析与ROS的配合](https://github.com/nooploop-dev/nlink_parser); [Serial库](https://github.com/nooploop-dev/serial); [他人讲解](https://blog.csdn.net/i_robots/article/details/107410771) ​ 该部分的程序源码已以zip压缩包的形式放于[本仓库中](reference/Origin_zip)。 [4]零散知识点： - ROS：[NodeHandle](https://www.ncnynl.com/archives/201702/1297.html),[英文介绍](https://www.cnblogs.com/siahekai/p/5840318.html); [ROS参数服务器](https://www.cnblogs.com/lvchaoshun/p/6873142.html); [loop_rate与spinOnce](https://blog.csdn.net/qq_37802555/article/details/107670710); [catkin_make](https://www.cnblogs.com/qixianyu/p/6605830.html); [unordered_map](https://www.cnblogs.com/langyao/p/8823092.html); [ROS与话题消息1](https://blog.csdn.net/foreverhehe716/article/details/83055323); [ROS与话题消息2](https://blog.csdn.net/SESESssss/article/details/105362365); - 串口：[串口设备信息获取](https://www.cnblogs.com/xuyaowen/p/Linux-Ls-Serial.html); [/dev/tty解释](https://www.cnblogs.com/wangtianxj/archive/2009/08/27/1555296.html); [打开串口](https://blog.csdn.net/yjxsdzx/article/details/71024212); - CMakeList: [简单用法](https://blog.csdn.net/u010122972/article/details/78216013?spm=1001.2014.3001.5501); [全面用法](https://blog.csdn.net/afei__/article/details/81201039); [SummerCamp](https://gitee.com/pi-lab/SummerCamp/tree/master); - 编译问题：[动态编译的解释器缺失](https://www.coderclan.cc/231.html); [系统与程序位数不符](https://blog.csdn.net/sun927/article/details/46593129/); [动态依赖库](https://blog.csdn.net/u012230668/article/details/89341542); - 其他：[assert](https://www.cnblogs.com/xudong-bupt/p/8420695.html); [std::function函数封装](https://www.cnblogs.com/yinwei-space/p/12708871.html); [decltype](); [句柄](https://blog.csdn.net/qq_39584315/article/details/81909135)