第三方RTK接入PX4

        由于px4官方支持的rtk是usb协议的,有些第三方rtk支持串口,因此如果需要通过qgc通过数传透传rtcm的数据,需要对qgc进行二次开发,以下仅对qcg读取rtk部分代码进行分析。

        

         qgc自动连接rtk的相关设置在上图所示界面,对应的代码在LinkManager.cc里面的void LinkManager::_updateAutoConnectLinks(void)函数内(请看代码内中文注释):

   // Iterate Comm Ports
    for (const QGCSerialPortInfo& portInfo: portList) {//这里会扫描所有的端口,将所有的端口信息存储在portInfo里面
        qCDebug(LinkManagerVerboseLog) << "-----------------------------------------------------";
        qCDebug(LinkManagerVerboseLog) << "portName:          " << portInfo.portName();
        qCDebug(LinkManagerVerboseLog) << "systemLocation:    " << portInfo.systemLocation();
        qCDebug(LinkManagerVerboseLog) << "description:       " << portInfo.description();
        qCDebug(LinkManagerVerboseLog) << "manufacturer:      " << portInfo.manufacturer();
        qCDebug(LinkManagerVerboseLog) << "serialNumber:      " << portInfo.serialNumber();
        qCDebug(LinkManagerVerboseLog) << "vendorIdentifier:  " << portInfo.vendorIdentifier();
        qCDebug(LinkManagerVerboseLog) << "productIdentifier: " << portInfo.productIdentifier();

        // Save port name
        currentPorts << portInfo.systemLocation();

        QGCSerialPortInfo::BoardType_t boardType;
        QString boardName;

#ifndef NO_SERIAL_LINK
#ifndef __mobile__
        // check to see if nmea gps is configured for current Serial port, if so, set it up to connect
        if (portInfo.systemLocation().trimmed() == _autoConnectSettings->autoConnectNmeaPort()->cookedValueString()) {
            if (portInfo.systemLocation().trimmed() != _nmeaDeviceName) {
                _nmeaDeviceName = portInfo.systemLocation().trimmed();
                qCDebug(LinkManagerLog) << "Configuring nmea port" << _nmeaDeviceName;
                QSerialPort* newPort = new QSerialPort(portInfo);
                _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt();
                newPort->setBaudRate(static_cast<qint32>(_nmeaBaud));
                qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud;
                // This will stop polling old device if previously set
                _toolbox->qgcPositionManager()->setNmeaSourceDevice(newPort);
                if (_nmeaPort) {
                    delete _nmeaPort;
                }
                _nmeaPort = newPort;
            } else if (_autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt() != _nmeaBaud) {
                _nmeaBaud = _autoConnectSettings->autoConnectNmeaBaud()->cookedValue().toUInt();
                _nmeaPort->setBaudRate(static_cast<qint32>(_nmeaBaud));
                qCDebug(LinkManagerLog) << "Configuring nmea baudrate" << _nmeaBaud;
            }
        } else
#endif
#endif
            if (portInfo.getBoardInfo(boardType, boardName)) {//这个地方通过portInfo存储的信息识别USB端口对应的PID和VID,识别成功之后将boardType赋值
                                                               //每个usb设备厂商会有自己的vid和pid,所以qgc会根据这两个id去判断这个usb是什么设备(是rtk还是数传等等)
                if (portInfo.isBootloader()) {
                    // Don't connect to bootloader
                    qCDebug(LinkManagerLog) << "Waiting for bootloader to finish" << portInfo.systemLocation();
                    continue;
                }
                if (_portAlreadyConnected(portInfo.systemLocation()) || _autoConnectRTKPort == portInfo.systemLocation()) {
                    qCDebug(LinkManagerVerboseLog) << "Skipping existing autoconnect" << portInfo.systemLocation();
                } else if (!_autoconnectPortWaitList.contains(portInfo.systemLocation())) {
                    // We don't connect to the port the first time we see it. The ability to correctly detect whether we
                    // are in the bootloader is flaky from a cross-platform standpoint. So by putting it on a wait list
                    // and only connect on the second pass we leave enough time for the board to boot up.
                    qCDebug(LinkManagerLog) << "Waiting for next autoconnect pass" << portInfo.systemLocation();
                    _autoconnectPortWaitList[portInfo.systemLocation()] = 1;
                } else if (++_autoconnectPortWaitList[portInfo.systemLocation()] * _autoconnectUpdateTimerMSecs > _autoconnectConnectDelayMSecs) {
                    SerialConfiguration* pSerialConfig = nullptr;
                    _autoconnectPortWaitList.remove(portInfo.systemLocation());
                    switch (boardType) {
                    case QGCSerialPortInfo::BoardTypePixhawk:
                        if (_autoConnectSettings->autoConnectPixhawk()->rawValue().toBool()) {
                            pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
                            pSerialConfig->setUsbDirect(true);
                        }
                        break;
                    case QGCSerialPortInfo::BoardTypePX4Flow:
                        if (_autoConnectSettings->autoConnectPX4Flow()->rawValue().toBool()) {
                            pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
                        }
                        break;
                    case QGCSerialPortInfo::BoardTypeSiKRadio:
                        if (_autoConnectSettings->autoConnectSiKRadio()->rawValue().toBool()) {
                            pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
                        }
                        break;
                    case QGCSerialPortInfo::BoardTypeOpenPilot:
                        if (_autoConnectSettings->autoConnectLibrePilot()->rawValue().toBool()) {
                            pSerialConfig = new SerialConfiguration(tr("%1 on %2 (AutoConnect)").arg(boardName).arg(portInfo.portName().trimmed()));
                        }
                        break;
#ifndef __mobile__
                    case QGCSerialPortInfo::BoardTypeRTKGPS://这个地方判断boardType等于RTK
                        if (_autoConnectSettings->autoConnectRTKGPS()->rawValue().toBool() && !_toolbox->gpsManager()->connected()) {
                            qCDebug(LinkManagerLog) << "RTK GPS auto-connected" << portInfo.portName().trimmed();
                            _autoConnectRTKPort = portInfo.systemLocation();
                            _toolbox->gpsManager()->connectGPS(portInfo.systemLocation(), boardName);//这里开始连接RTK基站
                        }
                        break;
#endif
                    default:
                        qWarning() << "Internal error";
                        continue;
                    }
                    if (pSerialConfig) {
                        qCDebug(LinkManagerLog) << "New auto-connect port added: " << pSerialConfig->name() << portInfo.systemLocation();
                        pSerialConfig->setBaud      (boardType == QGCSerialPortInfo::BoardTypeSiKRadio ? 57600 : 115200);
                        pSerialConfig->setDynamic   (true);
                        pSerialConfig->setPortName  (portInfo.systemLocation());
                        pSerialConfig->setAutoConnect(true);

                        SharedLinkConfigurationPtr  sharedConfig(pSerialConfig);
                        createConnectedLink(sharedConfig, boardType == QGCSerialPortInfo::BoardTypePX4Flow);
                    }
                }
            }
    }

        在上图函数中匹配VID和PID之后通过void GPSManager::connectGPS(const QString& device, const QString& gps_type) 开始连接rtk基站:

void GPSManager::connectGPS(const QString& device, const QString& gps_type)
{
    RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings();

    GPSProvider::GPSType type;
    if (gps_type.contains("trimble",  Qt::CaseInsensitive)) {
        type = GPSProvider::GPSType::trimble;
        qCDebug(RTKGPSLog) << "Connecting Trimble device";
    } else if (gps_type.contains("septentrio",  Qt::CaseInsensitive)) {
        type = GPSProvider::GPSType::septentrio;
        qCDebug(RTKGPSLog) << "Connecting Septentrio device";
    } else {
        type = GPSProvider::GPSType::u_blox;
        qCDebug(RTKGPSLog) << "Connecting U-blox device";
    }

    disconnectGPS();
    _requestGpsStop = false;
    _gpsProvider = new GPSProvider(device,
                                   type,
                                   true,    /* enableSatInfo */
                                   rtkSettings->surveyInAccuracyLimit()->rawValue().toDouble(),
                                   rtkSettings->surveyInMinObservationDuration()->rawValue().toInt(),
                                   rtkSettings->useFixedBasePosition()->rawValue().toBool(),
                                   rtkSettings->fixedBasePositionLatitude()->rawValue().toDouble(),
                                   rtkSettings->fixedBasePositionLongitude()->rawValue().toDouble(),
                                   rtkSettings->fixedBasePositionAltitude()->rawValue().toFloat(),
                                   rtkSettings->fixedBasePositionAccuracy()->rawValue().toFloat(),
                                   _requestGpsStop);
    _gpsProvider->start();//RTCM主要的解析和更新逻辑就在这

    //create RTCM device
    _rtcmMavlink = new RTCMMavlink(*_toolbox);

    connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate);

    //test: connect to position update
    connect(_gpsProvider, &GPSProvider::positionUpdate,         this, &GPSManager::GPSPositionUpdate);
    connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate,    this, &GPSManager::GPSSatelliteUpdate);
    connect(_gpsProvider, &GPSProvider::finished,               this, &GPSManager::onDisconnect);
    connect(_gpsProvider, &GPSProvider::surveyInStatus,         this, &GPSManager::surveyInStatus);

    emit onConnect();
}

        RTK基站主要就在_gpsProvider->start()中去运行,也就是GPSProvider::run()函数:

void GPSProvider::run()
{

#ifdef SIMULATE_RTCM_OUTPUT//这里用于模拟固定的RTK基站
        const int fakeMsgLengths[3] = { 30, 170, 240 };
        uint8_t* fakeData = new uint8_t[fakeMsgLengths[2]];
        while (!_requestStop) {
            for (int i = 0; i < 3; ++i) {
                gotRTCMData((uint8_t*) fakeData, fakeMsgLengths[i]);
                msleep(4);
            }
            msleep(100);
        }
        delete[] fakeData;
#endif /* SIMULATE_RTCM_OUTPUT */

    if (_serial) delete _serial;

    _serial = new QSerialPort();
    _serial->setPortName(_device);
    if (!_serial->open(QIODevice::ReadWrite)) {
        int retries = 60;
        // Give the device some time to come up. In some cases the device is not
        // immediately accessible right after startup for some reason. This can take 10-20s.
        while (retries-- > 0 && _serial->error() == QSerialPort::PermissionError) {
            qCDebug(RTKGPSLog) << "Cannot open device... retrying";
            msleep(500);
            if (_serial->open(QIODevice::ReadWrite)) {
                _serial->clearError();
                break;
            }
        }
        if (_serial->error() != QSerialPort::NoError) {
            qWarning() << "GPS: Failed to open Serial Device" << _device << _serial->errorString();
            return;
        }
    }
    _serial->setBaudRate(QSerialPort::Baud9600);
    _serial->setDataBits(QSerialPort::Data8);
    _serial->setParity(QSerialPort::NoParity);
    _serial->setStopBits(QSerialPort::OneStop);
    _serial->setFlowControl(QSerialPort::NoFlowControl);

    unsigned int baudrate;
    GPSBaseStationSupport* gpsDriver = nullptr;

    while (!_requestStop) {

        if (gpsDriver) {
            delete gpsDriver;
            gpsDriver = nullptr;
        }

        /*我这里是使用的u-blox的rtk,所以会配置自动波特率*/
        if (_type == GPSType::trimble) {
            gpsDriver = new GPSDriverAshtech(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
            baudrate = 115200;
        } else if (_type == GPSType::septentrio) {
            gpsDriver = new GPSDriverSBF(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo, 5);
            baudrate = 0; // auto-configure
        } else {
            gpsDriver = new GPSDriverUBX(GPSDriverUBX::Interface::UART, &callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
            baudrate = 0; // auto-configure
        }
        gpsDriver->setSurveyInSpecs(_surveyInAccMeters * 10000.0f, _surveryInDurationSecs);

        if (_useFixedBaseLoction) {
            gpsDriver->setBasePosition(_fixedBaseLatitude, _fixedBaseLongitude, _fixedBaseAltitudeMeters, _fixedBaseAccuracyMeters * 1000.0f);
        }

        /*qgc会自动将rtk基站配置为rtcm3的输出*/
        _gpsConfig.output_mode = GPSHelper::OutputMode::RTCM;
        if (gpsDriver->configure(baudrate, _gpsConfig) == 0) {

            /* reset report */
            memset(&_reportGpsPos, 0, sizeof(_reportGpsPos));

            //In rare cases it can happen that we get an error from the driver (eg. checksum failure) due to
            //bus errors or buggy firmware. In this case we want to try multiple times before giving up.
            int numTries = 0;

            while (!_requestStop && numTries < 3) {
                /*这里当gpsDriver接收到rtcm数据*/
                int helperRet = gpsDriver->receive(GPS_RECEIVE_TIMEOUT);

                if (helperRet > 0) {
                    numTries = 0;

                    if (helperRet & 1) {
                        publishGPSPosition();//更新GPS位置
                        numTries = 0;
                    }

                    if (_pReportSatInfo && (helperRet & 2)) {
                        publishGPSSatellite();//更新GPSW卫星数量
                        numTries = 0;
                    }
                } else {
                    ++numTries;
                }
            }
            if (_serial->error() != QSerialPort::NoError && _serial->error() != QSerialPort::TimeoutError) {
                break;
            }
        }
    }
    qCDebug(RTKGPSLog) << "Exiting GPS thread";
}

        所以当gpsDriver->receive(GPS_RECEIVE_TIMEOUT)接收到数据的时候,他会调用相应的gpsDriver重写的receive函数去解析rtk数据,我这里调用的是GPSDriverUBX::receive(unsigned timeout),因为我使用的u-blox的rtk:

GPSDriverUBX::receive(unsigned timeout)
{
	uint8_t buf[GPS_READ_BUFFER_SIZE];

	/* timeout additional to poll */
	gps_abstime time_started = gps_absolute_time();

	int handled = 0;

	while (true) {
		bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled;

		/* Wait for only UBX_PACKET_TIMEOUT if something already received. */
        //这里将串口数据读到buf
		int ret = read(buf, sizeof(buf), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);

		if (ret < 0) {
			/* something went wrong when polling or reading */
			UBX_WARN("ubx poll_or_read err");
			return -1;

		} else if (ret == 0) {
			/* return success if ready */
			if (ready_to_return) {
				_got_posllh = false;
				_got_velned = false;
				return handled;
			}

		} else {
			//UBX_DEBUG("read %d bytes", ret);

			/* pass received bytes to the packet decoder */
			for (int i = 0; i < ret; i++) {
                //再一个个字节去解析buff
				handled |= parseChar(buf[i]);
				//UBX_DEBUG("parsed %d: 0x%x", i, buf[i]);
			}

			if (_interface == Interface::SPI) {
				if (buf[ret - 1] == 0xff) {
					if (ready_to_return) {
						_got_posllh = false;
						_got_velned = false;
						return handled;
					}
				}
			}
		}

		/* abort after timeout if no useful packets received */
		if (time_started + timeout * 1000 < gps_absolute_time()) {
			UBX_DEBUG("timed out, returning");
			return -1;
		}
	}
}

int	// 0 = decoding, 1 = message handled, 2 = sat info message handled
GPSDriverUBX::parseChar(const uint8_t b)
{
	int ret = 0;

	switch (_decode_state) {

	/* Expecting Sync1 */
    //这里判断rtcm数据帧头
	case UBX_DECODE_SYNC1:
		if (b == UBX_SYNC1) {	// Sync1 found --> expecting Sync2
			UBX_TRACE_PARSER("A");
			_decode_state = UBX_DECODE_SYNC2;

		} else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
			UBX_TRACE_PARSER("RTCM");
			_decode_state = UBX_DECODE_RTCM3;
			_rtcm_parsing->addByte(b);
		}

		break;

	.
    .
    .
    .
    .

	case UBX_DECODE_RTCM3:
        //解析完成之后,触发gotRTCMMessage,从而在前面定义的RTCMMavlink将RTCM数据打包发给px4
		if (_rtcm_parsing->addByte(b)) {
			UBX_DEBUG("got RTCM message with length %i", static_cast<int>(_rtcm_parsing->messageLength()));
			gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
			decodeInit();
		}

		break;

	default:
		break;
	}

	return ret;
}

        了解完qgc整个rtk基站处理流程之后,可以在任意地方去做二次开发,将第三方的rtk通过qgc数传链路接入px4。当然也可以使用MAVSDK将rtk数据透传给px4,详情请参考mavsdk官方文档:

https://mavsdk.mavlink.io/main/en/cpp/api_reference/classmavsdk_1_1_rtk.html

        值得一提的是,想让地面站菜单栏显示rtk的专题跟官方一样,我还没花时间去研究,目前,地面站的GPS状态是根据接收到的gps_info中的fix_tpye去显示的,4对应的DGPS,5对应的浮点解,6对应的固定解:

         二次开发工程中,你也可以通过Mavlink控制台输入gsp status,查看是否有rtcm injected的消息频率,来判断rtcm数据传输是否成功。

        GPSDriverUBX::parseChar(const uint8_t b)只对rtcm数据帧头帧尾做解析,真正解析rtcm整帧数据的函数是parseChar里面的GPSDriverUBX::payloadRxInit()函数,有兴趣可以去研究。

        

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值