博客

一、相关说明

• 1.地面站：基于QGC编译的Android地面站。
• 2.需求：RTK接入，地面站接收RTK基准站数据，然后将数据打包通过MavLink转发至飞控。
• 3.需求目的：为无人机提供RTK高精度定位提供数据支持。
• 4.基准站数据格式：RTCM数据流。

二、开发方案

目前海星达RTK支持

BlueTooth

及

Wifi

传输数据，但是 BlueTooth收到的数据并非RTCM数据流，无法满足我们的需求。经过与厂家技术开发沟通，Wifi发送的数据是RTCM流数据，那么下面开始通过TCP连接接收数据。

该RTK基准站提供IP及端口，能够支持TCP连接，通过TCP接入开发，实现了以

xD3

开头的RTCM数据的接收：

\xD3\x00\x16""C\x01)3\xF9""B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xDD\x84P
\xD3\x00\x16""C\xC0\x01""B\x93.\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xE4\x99k
\xD3\x00\x16""F@\x01)3\x1E\x82\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x96\x93\xC5
\xD3\x00\x15>\xE0\x01\x03z\x93 \x9F\xC9\f\x8Eyo#\x05\xC3?[|\x00\x00\xC7`3\
xD3\x00\x04HXHDP\xBC$
\xD3\x00\x06?\x00\x01\x00\x00\x00\xAF,\x98

RTCM 3.1数据格式：

三、数据处理关键流程分析

1.创建TCP通信类TCPLink


该类作为TCP连接的客户端。主要任务是实现TCP通信连接，接收服务端(RTK基准站)的数据，再通过

“信号-槽”

机制将数据转发至MAVLinkProtocal类中的receiveBytes函数。

TCP通信具体实现流程略，后期会发表

“QT创建TCP Socket通信”

的文章进行详细阐述。

2.MAVLinkProtocal接收RTCM数据的函数receiveBytes


其中参数link表示当前的TCPLink，参数b表示RTCM数据。

（注：限于篇幅，后文均只展示关键代码，并非全部）

void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
//...
    if (!_linkMgr->containsLink(link)) {
        return;
    }
    QmlObjectListModel& vehicles = *_toolbox->multiVehicleManager()->vehicles();
    //RTK Data
    if (b[0] == 0xD3 && b[1] == 0x00) {
        //parseChar
        decodeInit();
        bool flag = _rtcm_parsingg->addByte(b.toUInt());
        if (flag) {
            //if (_rtcm_parsingg->addByte(b.toUInt())) {
            //gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
            uint8_t * message = _rtcm_parsingg->message();
            QByteArray _parsedB((char *)message, _rtcm_parsingg->messageLength());
        }
        RTCMMavlink * mavlink = new RTCMMavlink(*_toolbox);
        mavlink->RTCMDataUpdateSelf(b, vehicles);
    }
//...
}

2.处理RTCM数据并通过MAVLink协议转发

• 定义RTCMMavlink类，实现数据处理方法RTCMDataUpdateSelf

void RTCMMavlink::RTCMDataUpdateSelf(QByteArray message, QmlObjectListModel& vehicles)
{
    /* statistics */
    _bandwidthByteCounter += message.size();
    qint64 elapsed = _bandwidthTimer.elapsed();
    if (elapsed > 1000) {
        printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f);
        _bandwidthTimer.restart();
        _bandwidthByteCounter = 0;
        qDebug() << "---s000";
    }
    const int maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
    mavlink_gps_rtcm_data_t mavlinkRtcmData;
    memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t));
    if (message.size() < maxMessageLength) {
        qDebug() << "---s111-1";
        mavlinkRtcmData.len = message.size();
        mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3;
        memcpy(&mavlinkRtcmData.data, message.data(), message.size());
        sendMessageToVehicle(mavlinkRtcmData, vehicles);
    } else {
        // We need to fragment
        qDebug() << "---s111-2";
        uint8_t fragmentId = 0;         // Fragment id indicates the fragment within a set
        int start = 0;
        while (start < message.size()) {
            qDebug() << "---s111-2--";
            int length = std::min(message.size() - start, maxMessageLength);
            mavlinkRtcmData.flags = 1;                      // LSB set indicates message is fragmented
            mavlinkRtcmData.flags |= fragmentId++ << 1;     // Next 2 bits are fragment id
            mavlinkRtcmData.flags |= (_sequenceId & 0x1F) << 3;     // Next 5 bits are sequence id
            mavlinkRtcmData.len = length;
            memcpy(&mavlinkRtcmData.data, message.data() + start, length);
            sendMessageToVehicle(mavlinkRtcmData, vehicles);
            start += length;
        }
    }
    ++_sequenceId;
}

• 转发消息至飞控sendMessageToVehicle

void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg, QmlObjectListModel& vehicles)
{
    MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
    for (int i = 0; i < vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
        mavlink_message_t message;
        mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(),
                                              mavlinkProtocol->getComponentId(),
                                              vehicle->priorityLink()->mavlinkChannel(),
                                              &message,
                                              &msg);
        vehicle->sendMessageOnLink(vehicle->priorityLink(), message);
    }
}

最终通过调用Vehicle类的sendMessageOnLink函数，实现将消息发送至飞控。

sendMessageOnLink函数处理操作涉及一系列MAVLink的处理流程，并非本文的重点，所以不进行阐述。

四、总结

因为是刚开始接触Qt开发，很多东西都需要一步步熟悉，包括Qt开发环境、QML编程及C++编程语法等等。另外由于项目具体需求的不明确，RTK对接人员对RTK接入方面也不熟悉，导致其中也走了不少弯路。

当然结果还是好的，经过前前后后一个月的时间，各种学习、各种填坑，最终完成了项目中的这个需求。收获还是挺多的：1.项目具体需求一定要细化；2.相关开发人员之间需要多沟通；3.坚持不懈的态度。