RTCMUdpInput.cc

Go to the documentation of this file.

#include "RTCMUdpInput.h"
 
#include <QtNetwork/QNetworkDatagram>
#include <QtNetwork/QUdpSocket>
 
#include "QGCLoggingCategory.h"
 
QGC_LOGGING_CATEGORY(RTCMUdpInputLog, "GPS.RTCMUdpInput")
 
RTCMUdpInput::RTCMUdpInput(quint16 port, QObject* parent) : QObject(parent), _port(port) {}
 
RTCMUdpInput::~RTCMUdpInput()
{
    stop();
}
 
bool RTCMUdpInput::start()
{
    stop();
    _rtcmParser.reset();
 
    _socket = new QUdpSocket(this);
    if (!_socket->bind(QHostAddress::AnyIPv4, _port)) {
        qCWarning(RTCMUdpInputLog) << "Failed to bind UDP socket on port" << _port << ":" << _socket->errorString();
        _socket->deleteLater();
        _socket = nullptr;
        return false;
    }
    connect(_socket, &QUdpSocket::readyRead, this, &RTCMUdpInput::_readDatagrams);
 
    if (_port == 0) {
        _port = _socket->localPort();
        emit portChanged();
    }
 
    _running = true;
    emit runningChanged();
    qCDebug(RTCMUdpInputLog) << "Listening for RTCM data on UDP port" << _port;
    return true;
}
 
void RTCMUdpInput::stop()
{
    if (!_running) {
        return;
    }
 
    if (_socket) {
        _socket->close();
        _socket->deleteLater();
        _socket = nullptr;
    }
    _running = false;
    emit runningChanged();
    qCDebug(RTCMUdpInputLog) << "Stopped listening on UDP port" << _port;
}
 
void RTCMUdpInput::setPort(quint16 port)
{
    if (_port == port) {
        return;
    }
 
    _port = port;
    emit portChanged();
 
    if (_running) {
        start();
    }
}
 
void RTCMUdpInput::_readDatagrams()
{
    if (!_socket) {
        return;
    }
    while (_socket->hasPendingDatagrams()) {
        const QNetworkDatagram datagram = _socket->receiveDatagram();
        const QByteArray data = datagram.data();
        if (data.isEmpty()) {
            continue;
        }
 
        if (!_validateRtcm) {
            qCDebug(RTCMUdpInputLog) << "Received RTCM datagram:" << data.size() << "bytes";
            emit rtcmDataReceived(data);
            continue;
        }
 
        // Emit one complete RTCM3 frame per signal so RTCMMavlink assigns a distinct
        // GPS_RTCM_DATA sequence per frame (required for correct MAVLink reassembly).
        int framesFound = 0;
        int framesDropped = 0;
        for (const char ch : data) {
            if (!_rtcmParser.addByte(static_cast<uint8_t>(static_cast<unsigned char>(ch)))) {
                continue;
            }
            if (_rtcmParser.validateCrc()) {
                ++framesFound;
                ++_validFrames;
                emit rtcmDataReceived(_rtcmParser.currentFrame());
            } else {
                ++framesDropped;
                ++_invalidFrames;
            }
            _rtcmParser.reset();
        }
 
        if (framesDropped > 0) {
            qCWarning(RTCMUdpInputLog) << "Dropped" << framesDropped << "RTCM frame(s) - CRC mismatch";
        }
 
        qCDebug(RTCMUdpInputLog) << "Datagram" << data.size() << "bytes -" << "framesFound:" << framesFound
                                 << "framesDropped:" << framesDropped;
 
        const quint64 totalFrames = _validFrames + _invalidFrames;
        if (totalFrames > 0) {
            const double dropPct = 100.0 * _invalidFrames / totalFrames;
            qCDebug(RTCMUdpInputLog) << QString("RTCM frame stats: %1 valid, %2 invalid, %3% dropped")
                                            .arg(_validFrames)
                                            .arg(_invalidFrames)
                                            .arg(dropPct, 0, 'f', 1);
        }
    }
}