由于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()函数,有兴趣可以去研究。