LinkTrack UWB定位系统NLink协议解析方法

LinkTrack UWB简介

LinkTrack UWB是集定位，授时，通信于一体的模块，功能上可以类比北斗卫星定位系统。但是该模块的定位功能和通信功能不同同时使用，取决于模块配置的模式。

LinkTrack使用一套自定义的NLink协议，关于NLink协议的具体内容，可以查看NLink协议手册，并且模块提供了NlinkUnpack示例解析代码。该示例解析代码只对完整的一帧数据进行解析，而不会从串口字节流中主动截取出完整的一帧数据。

本文要做的就是使用qt的串口进行NLink协议解析前的数据帧获取工作。

协议解析

在UWBserial类中定义串口成员变量。UWBserial继承自QObject，所以可以把解析到的结果通过信号槽的方式转发到别的类中使用。

QSerialPort *m_serialPort;

在UWBserial类的构造函数中，配置串口参数并打开。这里为了演示，打开了搜索到的第一个串口

QStringList m_serialPortName;
foreach(const QSerialPortInfo &info,QSerialPortInfo::availablePorts())
{
    m_serialPortName << info.portName();
    qDebug()<<"serialPortName:"<<info.portName();
}

m_serialPort = new QSerialPort();
m_serialPort->setPortName(m_serialPortName[0]);

m_serialPort->setBaudRate(QSerialPort::Baud9600);
m_serialPort->setDataBits(QSerialPort::Data8);
m_serialPort->setParity(QSerialPort::NoParity);

m_serialPort->open(QIODevice::ReadWrite);
connect(m_serialPort,SIGNAL(readyRead()),this,SLOT(receiveInfo()));

当串口收到数据后，会调用槽函数receiveInfo()，receiveInfo函数如下：

void UWBserial::receiveInfo()
{
    QByteArray info = m_serialPort->readAll();
    //QByteArray hexData = info.toHex();

    for(int i = 0;i<info.size();i++)
    {
        Data_Receive_Pre(static_cast<unsigned char>(info.at(i)),this);
    }
}

把每个字节按顺序传入到Data_Receive_Pre函数，进行完整一帧数据的截取。
本文以NLINK_LINKTRACK_NODE_FRAME1为例，该数据帧长度可变，帧头固定是0x55,0x03。

void UWBserial::Data_Receive_Pre(u8 data , UWBserial* uwbserial)
{
    static u8 DT_RxBuffer[256],DT_data_cnt = 0;
    static u8 _data_len = 0;
    static u8 state = 0;

    //根据帧头进行解析,使用的是哪个数据帧
    //NLINK_LINKTRACK_NODE_FRAME1
    if(state == 0 && data == 0x55)
    {
        state = 1;
        DT_RxBuffer[0]=data;
    }
    else if(state == 1 && data == 0x03)
    {
        state = 2;
        DT_RxBuffer[1]=data;
    }
    else if(state == 2)
    {
        state = 3;
        _data_len = data-4; //去掉前3个字节后一个校验字节
        DT_RxBuffer[2]=data;
        DT_data_cnt = 0;
    }
    else if(state == 3&&_data_len>0)  //开始记录固定长度的字节
    {
        _data_len--;
        DT_RxBuffer[3+DT_data_cnt++]=data;
        if(_data_len==0)
            state = 4;
    }
    else if(state==4)
    {
        state = 0;
        DT_RxBuffer[3+DT_data_cnt++]=data;
        Data_Receive_Anl(DT_RxBuffer,DT_data_cnt+3,uwbserial);
    }
    else
        state = 0;
}

完整的一帧数据调用LinkTrack提供的示例解析代码解析，并使用消息函数发射出去。

void UWBserial::Data_Receive_Anl(u8 *data_buf, u8 num, UWBserial* uwbserial)
{
    //解析data_buf
    //直接调用SDK提供的函数
    if(verifyNodeFrame1Data(data_buf,num))
    {
        unpackNodeFrame1Data(data_buf);
    }

    //解析出位置，并发送出去
    float x = nodeFrame1Data_.node[0]->pos[0];
    float y = nodeFrame1Data_.node[0]->pos[1];
    float z = nodeFrame1Data_.node[0]->pos[2];

    emit uwbserial->position(x,y,z);
}

注意点

• LinkTrack提供的示例解析代码解析是c语言，而qt是c++，所以在包含头文件时使用如下格式：

extern "C"{
#include "nlink_linktrack_node_frame1.h"
}

• nodeFrame1Data_变量可以直接使用，因为在SDK中已经使用extern声明。