9#include <QtNetwork/QSslSocket>
10#include <QtNetwork/QSslError>
15#include <QtPositioning/QGeoCoordinate>
16#include <QtCore/QDateTime>
18#include <QtCore/QCoreApplication>
19#include <QtCore/QDebug>
20#include <QtCore/QtGlobal>
22#include <QtQml/QQmlEngine>
23#include <QtQml/QJSEngine>
33 return NTRIPManager::instance();
45NTRIPManager::NTRIPManager(QObject* parent)
47 , _ntripStatus(tr(
"Disconnected"))
49 qCDebug(NTRIPLog) <<
"NTRIPManager created";
50 _startupTimer.start();
54 QObject* parentObj =
qgcApp() ?
static_cast<QObject*
>(
qgcApp()) : static_cast<QObject*>(this);
57 _rtcmMavlink->setObjectName(QStringLiteral(
"RTCMMavlink"));
58 qCDebug(NTRIPLog) <<
"NTRIP Created RTCMMavlink helper";
63 NTRIPSettings* settings = SettingsManager::instance()->ntripSettings();
68 fact->setRawValue(
false);
72 this, [
this, fact]() {
73 if (!_forcedOffOnce) {
74 const bool wantOn = fact->rawValue().toBool();
76 qCDebug(NTRIPLog) <<
"NTRIP Startup: coercing ntripServerConnectEnabled back to false";
77 fact->setRawValue(
false);
85 _settingsCheckTimer =
new QTimer(
this);
86 connect(_settingsCheckTimer, &QTimer::timeout,
this, &NTRIPManager::_checkSettings);
87 _settingsCheckTimer->start(1000);
89 _ggaTimer =
new QTimer(
this);
90 _ggaTimer->setInterval(5000);
91 connect(_ggaTimer, &QTimer::timeout,
this, &NTRIPManager::_sendGGA);
93 connect(
qApp, &QCoreApplication::aboutToQuit,
this, &NTRIPManager::stopNTRIP, Qt::QueuedConnection);
99NTRIPManager::~NTRIPManager()
101 qCDebug(NTRIPLog) <<
"NTRIPManager destroyed";
105 if (_instance ==
this) {
117 _state = WaitingForPreamble;
120 _lengthBytesRead = 0;
127 case WaitingForPreamble:
131 _state = ReadingLength;
132 _lengthBytesRead = 0;
137 _lengthBytes[_lengthBytesRead++] = byte;
138 _buffer[_bytesRead++] = byte;
139 if (_lengthBytesRead == 2) {
141 _messageLength = ((_lengthBytes[0] & 0x03) << 8) | _lengthBytes[1];
142 if (_messageLength > 0 && _messageLength < 1021) {
143 _state = ReadingMessage;
151 _buffer[_bytesRead++] = byte;
152 if (_bytesRead >= _messageLength + 3) {
159 _crcBytes[_crcBytesRead++] = byte;
160 if (_crcBytesRead == 3) {
171 if (_messageLength >= 2) {
173 return ((_buffer[3] << 4) | (_buffer[4] >> 4)) & 0xFFF;
180 const QString& username,
181 const QString& password,
182 const QString& mountpoint,
183 const QString& whitelist,
188 , _hostAddress(hostAddress)
190 , _username(username)
191 , _password(password)
192 , _mountpoint(mountpoint)
193 , _useSpartn(useSpartn)
199 if (!whitelist.isEmpty()) {
200 qCDebug(NTRIPLog) <<
"NTRIP SPARTN enabled; ignoring RTCM whitelist:" << whitelist;
204 _spartnNeedHeaderStrip =
true;
208 qCWarning(NTRIPLog) <<
"NTRIP SPARTN is enabled but port is" << _port <<
"(expected 2102 for TLS)";
212 for (
const auto& msg : whitelist.split(
',')) {
213 int msg_int = msg.toInt();
215 _whitelist.append(msg_int);
217 qCDebug(NTRIPLog) <<
"NTRIP whitelist:" << _whitelist;
218 if (_whitelist.empty()) {
219 qCDebug(NTRIPLog) <<
"NTRIP whitelist is empty; all RTCM message IDs will be forwarded.";
224 _rtcmParser->
reset();
227 _state = NTRIPState::uninitialised;
237 _stopping.store(
true);
240 QObject::disconnect(_readyReadConn);
241 _socket->disconnectFromHost();
249 _rtcmParser =
nullptr;
259void NTRIPTCPLink::_hardwareConnect()
261 if (_stopping.load()) {
265 qCDebug(NTRIPLog) <<
"NTRIP connectToHost" << _hostAddress <<
":" << _port <<
" mount=" << _mountpoint
266 <<
" tls=" << (_useSpartn ?
"yes" :
"no");
271 QSslSocket* sslSocket =
new QSslSocket(
this);
274 _socket =
new QTcpSocket(
this);
277 _socket->setSocketOption(QAbstractSocket::KeepAliveOption, 1);
278 _socket->setSocketOption(QAbstractSocket::LowDelayOption, 1);
279 _socket->setReadBufferSize(0);
281 QObject::connect(_socket, &QTcpSocket::errorOccurred,
this, [
this](QAbstractSocket::SocketError code) {
283 if (_stopping.load() || !_socket) {
284 qCDebug(NTRIPLog) <<
"NTRIP suppressing socket error during intentional stop:" << int(code);
288 QString msg = _socket->errorString();
291 if (code == QAbstractSocket::RemoteHostClosedError && _state == NTRIPState::waiting_for_http_response) {
292 const QByteArray leftover = _socket->readAll();
293 if (!leftover.isEmpty()) {
294 qCWarning(NTRIPLog) <<
"NTRIP peer closed; trailing bytes before close:" << leftover;
295 msg += QString(
" (peer closed after %1 bytes)").arg(leftover.size());
298 msg +=
" (hint: port 2102 expects HTTPS/TLS; current socket is plain TCP)";
299 }
else if (!_mountpoint.isEmpty()) {
300 msg +=
" (peer closed before HTTP response; check mountpoint and credentials)";
305 qCWarning(NTRIPLog) <<
"NTRIP socket error code:" << int(code)
307 <<
" state:" << int(_socket->state())
308 <<
" peer:" << _socket->peerAddress().toString() <<
":" << _socket->peerPort()
309 <<
" local:" << _socket->localAddress().toString() <<
":" << _socket->localPort();
311 }, Qt::DirectConnection);
314 QObject::connect(_socket, &QTcpSocket::disconnected,
this, [
this]() {
316 if (_stopping.load() || !_socket) {
317 qCDebug(NTRIPLog) <<
"NTRIP suppressing disconnect signal during intentional stop";
321 const QByteArray trailing = _socket->readAll();
323 if (!trailing.isEmpty()) {
324 reason = QString::fromUtf8(trailing).trimmed();
325 qCWarning(NTRIPLog) <<
"NTRIP disconnected; trailing bytes:" << trailing;
327 reason =
"Server disconnected";
328 qCWarning(NTRIPLog) <<
"NTRIP disconnected cleanly by server";
331 const qint64 t0 = this->property(
"ntrip_postok_t0_ms").toLongLong();
332 const bool saw = this->property(
"ntrip_saw_rtcm").toBool();
333 const qint64 dt = (t0 > 0) ? (QDateTime::currentMSecsSinceEpoch() - t0) : -1;
334 qCWarning(NTRIPLog) <<
"NTRIP disconnect context:"
335 <<
"rtcm_received_first_byte=" << (saw ?
"yes" :
"no")
336 <<
"ms_since_200=" << dt
337 <<
"peer=" << _socket->peerAddress().toString() <<
":" << _socket->peerPort()
338 <<
"local=" << _socket->localAddress().toString() <<
":" << _socket->localPort();
340 }, Qt::DirectConnection);
342 _readyReadConn = QObject::connect(_socket, &QTcpSocket::readyRead,
this, &NTRIPTCPLink::_readBytes,Qt::DirectConnection);
344 auto sendHttpRequest = [
this]() {
345 if (!_mountpoint.isEmpty()) {
346 qCDebug(NTRIPLog) <<
"NTRIP Sending HTTP request";
347 const QByteArray authB64 = QString(_username +
":" + _password).toUtf8().toBase64();
349 "GET /%1 HTTP/1.1\r\n"
351 "Ntrip-Version: Ntrip/2.0\r\n"
352 "User-Agent: QGC-NTRIP\r\n"
353 "Connection: keep-alive\r\n"
355 "Authorization: Basic %3\r\n"
357 const QByteArray req = query.arg(_mountpoint).arg(_hostAddress).arg(QString::fromUtf8(authB64)).toUtf8();
358 const qint64 written = _socket->write(req);
362 const QString reqStr = QString::fromUtf8(req);
363 const int cr = reqStr.indexOf(
"\r");
364 const QString firstLine = (cr > 0) ? reqStr.left(cr).trimmed() : QString();
365 const int authIdx = reqStr.indexOf(
"Authorization: Basic ");
368 const int eol = reqStr.indexOf(
"\r\n", authIdx);
369 if (eol > authIdx) b64Len = eol - (authIdx + 21);
371 qCDebug(NTRIPLog) <<
"NTRIP HTTP request first line:" << firstLine;
372 qCDebug(NTRIPLog) <<
"NTRIP HTTP auth present:" << (authIdx >= 0) <<
"auth_b64_len:" << b64Len;
374 qCDebug(NTRIPLog) <<
"NTRIP HTTP request bytes written:" << written;
375 _state = NTRIPState::waiting_for_http_response;
378 _state = NTRIPState::waiting_for_rtcm_header;
381 qCDebug(NTRIPLog) <<
"NTRIP Socket connected"
382 <<
"local" << _socket->localAddress().toString() <<
":" << _socket->localPort()
383 <<
"-> peer" << _socket->peerAddress().toString() <<
":" << _socket->peerPort();
387 QSslSocket* sslSocket = qobject_cast<QSslSocket*>(_socket);
390 QObject::connect(sslSocket, &QSslSocket::encrypted,
this, [sendHttpRequest]() {
391 qCDebug(NTRIPLog) <<
"SPARTN TLS connection established";
393 }, Qt::DirectConnection);
395 QObject::connect(sslSocket, QOverload<
const QList<QSslError>&>::of(&QSslSocket::sslErrors),
396 this, [](
const QList<QSslError> &errors) {
397 for (
const QSslError &e : errors) {
398 qCWarning(NTRIPLog) <<
"TLS Error:" << e.errorString();
400 }, Qt::DirectConnection);
402 sslSocket->connectToHostEncrypted(_hostAddress,
static_cast<quint16
>(_port));
404 if (!sslSocket->waitForEncrypted(10000)) {
405 qCDebug(NTRIPLog) <<
"NTRIP TLS socket failed to establish encryption";
406 emit
error(_socket->errorString());
412 if (sslSocket->isEncrypted()) {
416 QTcpSocket* tcpSocket = qobject_cast<QTcpSocket*>(_socket);
419 tcpSocket->connectToHost(_hostAddress,
static_cast<quint16
>(_port));
421 if (!tcpSocket->waitForConnected(10000)) {
422 qCDebug(NTRIPLog) <<
"NTRIP Socket failed to connect";
423 emit
error(_socket->errorString());
433void NTRIPTCPLink::_parse(
const QByteArray& buffer)
435 if (_stopping.load()) {
439 static bool logged_empty_once =
false;
440 if (!logged_empty_once && _whitelist.empty()) {
441 qCDebug(NTRIPLog) <<
"NTRIP whitelist is empty at runtime; forwarding all RTCM messages.";
442 logged_empty_once =
true;
446 constexpr int kRtcmHeaderSize = 3;
448 for (
char ch : buffer) {
449 const uint8_t
byte =
static_cast<uint8_t
>(
static_cast<unsigned char>(ch));
451 if (_state == NTRIPState::waiting_for_rtcm_header) {
455 _state = NTRIPState::accumulating_rtcm_packet;
458 if (_rtcmParser->
addByte(
byte)) {
459 _state = NTRIPState::waiting_for_rtcm_header;
462 const int payload_len =
static_cast<int>(_rtcmParser->
messageLength());
463 QByteArray message(
reinterpret_cast<const char*
>(_rtcmParser->
message()),
464 kRtcmHeaderSize + payload_len);
466 const uint8_t* crc_ptr = _rtcmParser->
crcBytes();
467 const int crc_len = _rtcmParser->
crcSize();
468 message.append(
reinterpret_cast<const char*
>(crc_ptr), crc_len);
470 const uint16_t
id = _rtcmParser->
messageId();
472 if (_whitelist.empty() || _whitelist.contains(
id)) {
473 qCDebug(NTRIPLog) <<
"NTRIP RTCM packet id" <<
id <<
"len" << message.length();
475 qCDebug(NTRIPLog) <<
"NTRIP Sending" <<
id <<
"of size" << message.length();
477 qCDebug(NTRIPLog) <<
"NTRIP Ignoring" << id;
480 _rtcmParser->
reset();
485void NTRIPTCPLink::_handleSpartnData(
const QByteArray& dataIn)
487 if (dataIn.isEmpty()) {
492 _spartnBuf.append(dataIn);
494 if (_spartnNeedHeaderStrip) {
495 const bool looksHttp = _spartnBuf.startsWith(
"HTTP/") || _spartnBuf.startsWith(
"ICY ");
497 const int headerEnd = _spartnBuf.indexOf(
"\r\n\r\n");
500 if (_spartnBuf.size() > 32768) {
501 _spartnBuf = _spartnBuf.right(32768);
506 _spartnBuf.remove(0, headerEnd + 4);
508 _spartnNeedHeaderStrip =
false;
511 if (_spartnBuf.isEmpty()) {
522void NTRIPTCPLink::_readBytes()
524 if (_stopping.load()) {
529 if (_socket && _state == NTRIPState::waiting_for_spartn_data) {
530 const QByteArray bytes = _socket->readAll();
531 if (!bytes.isEmpty()) {
532 _handleSpartnData(bytes);
542 static quint64 s_totalRtcm = 0;
544 if (_state == NTRIPState::waiting_for_http_response) {
545 QByteArray responseData = _socket->readAll();
546 if (responseData.isEmpty()) {
550 QString response = QString::fromUtf8(responseData);
551 qCDebug(NTRIPLog) <<
"NTRIP HTTP response received:" << response.left(200);
554 const int hdrEnd = response.indexOf(
"\r\n\r\n");
555 const QString headers = (hdrEnd >= 0) ? response.left(hdrEnd) : response;
556 qCDebug(NTRIPLog) <<
"NTRIP HTTP response headers BEGIN >>>";
557 for (
const QString& line : headers.split(
"\r\n")) {
558 if (!line.isEmpty()) qCDebug(NTRIPLog) << line;
560 qCDebug(NTRIPLog) <<
"NTRIP HTTP response headers <<< END";
563 QStringList lines = response.split(
'\n');
564 bool foundOkResponse =
false;
565 for (
const QString& line : lines) {
566 const QString trimmed = line.trimmed();
567 if ((trimmed.startsWith(
"HTTP/") || trimmed.startsWith(
"ICY ")) &&
568 (trimmed.contains(
" 200 ") || trimmed.contains(
" 201 "))) {
569 foundOkResponse =
true;
570 qCDebug(NTRIPLog) <<
"NTRIP: Found OK response:" << trimmed;
572 }
else if ((trimmed.startsWith(
"HTTP/") || trimmed.startsWith(
"ICY ")) &&
573 (trimmed.contains(
" 4") || trimmed.contains(
" 5"))) {
574 qCWarning(NTRIPLog) <<
"NTRIP: Server error response:" << trimmed;
575 emit
error(QString(
"NTRIP HTTP error: %1").arg(trimmed));
579 QTcpSocket* sock = _socket;
581 _state = NTRIPState::uninitialised;
583 QObject::disconnect(_readyReadConn);
584 sock->disconnectFromHost();
588 if (!_stopping.load()) {
589 QTimer::singleShot(3000,
this, [
this]() {
590 if (!_stopping.load()) {
599 if (foundOkResponse) {
601 const qint64 nowMs = QDateTime::currentMSecsSinceEpoch();
602 this->setProperty(
"ntrip_postok_t0_ms", nowMs);
603 this->setProperty(
"ntrip_saw_rtcm",
false);
604 this->setProperty(
"ntrip_watchdog_fired",
false);
607 QTimer::singleShot(28000,
this, [
this]() {
608 if (this->property(
"ntrip_watchdog_fired").toBool())
return;
609 const bool saw = this->property(
"ntrip_saw_rtcm").toBool();
611 this->setProperty(
"ntrip_watchdog_fired",
true);
613 <<
"NTRIP no RTCM received 28s after 200 OK. Likely caster timeout."
614 <<
"Check: duplicate login, entitlement/region, GGA validity.";
618 _state = _useSpartn ? NTRIPState::waiting_for_spartn_data
619 : NTRIPState::waiting_for_rtcm_header;
621 qCDebug(NTRIPLog) <<
"NTRIP: HTTP handshake complete, transitioning to data state";
626 QByteArray remainingData = responseData.mid(hdrEnd + 4);
627 if (!remainingData.isEmpty()) {
628 qCDebug(NTRIPLog) <<
"NTRIP: Processing data after HTTP headers:" << remainingData.size() <<
"bytes";
630 _handleSpartnData(remainingData);
632 _parse(remainingData);
637 qCDebug(NTRIPLog) <<
"NTRIP: Waiting for complete HTTP response...";
644 QByteArray bytes = _socket->readAll();
645 if (!bytes.isEmpty()) {
647 if (!this->property(
"ntrip_saw_rtcm").toBool()) {
648 this->setProperty(
"ntrip_saw_rtcm",
true);
649 const qint64 t0 = this->property(
"ntrip_postok_t0_ms").toLongLong();
650 const qint64 dt = (t0 > 0) ? (QDateTime::currentMSecsSinceEpoch() - t0) : -1;
651 const QByteArray sample = bytes.left(48);
652 const unsigned char b0 =
static_cast<unsigned char>(sample.isEmpty() ? 0 : sample[0]);
653 qCDebug(NTRIPLog) <<
"NTRIP first RTCM bytes at" << dt <<
"ms after 200 OK";
654 qCDebug(NTRIPLog) <<
"NTRIP rx sample (hex, first 48B):" << sample.toHex(
' ');
655 qCDebug(NTRIPLog) <<
"NTRIP preamble_check first_byte=0x" << QByteArray::number(b0, 16);
658 s_totalRtcm +=
static_cast<quint64
>(bytes.size());
659 qCDebug(NTRIPLog) <<
"NTRIP rx bytes:" << bytes.size()
660 <<
"total:" << s_totalRtcm
661 <<
"bytesAvailable:" << _socket->bytesAvailable();
670 const QByteArray authB64 = QString(_username +
":" + _password).toUtf8().toBase64();
671 const QByteArray req =
672 QByteArray(
"GET / HTTP/1.1\r\n")
673 +
"Host: " + _hostAddress.toUtf8() +
"\r\n"
674 +
"User-Agent: QGC-NTRIP\r\n"
675 +
"Ntrip-Version: Ntrip/2.0\r\n"
677 +
"Authorization: Basic " + authB64 +
"\r\n"
678 +
"Connection: close\r\n\r\n";
682 if (_useSpartn || _port == 2102) {
685 sock.connectToHostEncrypted(_hostAddress,
static_cast<quint16
>(_port));
686 if (!sock.waitForEncrypted(7000)) {
687 qCWarning(NTRIPLog) <<
"SOURCETABLE TLS connect failed:" << sock.errorString();
690 if (sock.write(req) <= 0 || !sock.waitForBytesWritten(3000)) {
691 qCWarning(NTRIPLog) <<
"SOURCETABLE TLS write failed:" << sock.errorString();
694 while (sock.waitForReadyRead(3000)) {
695 all += sock.readAll();
696 if (sock.bytesAvailable() == 0 && sock.state() != QAbstractSocket::ConnectedState)
break;
702 sock.connectToHost(_hostAddress,
static_cast<quint16
>(_port));
703 if (!sock.waitForConnected(5000)) {
704 qCWarning(NTRIPLog) <<
"SOURCETABLE connect failed:" << sock.errorString();
707 if (sock.write(req) <= 0 || !sock.waitForBytesWritten(3000)) {
708 qCWarning(NTRIPLog) <<
"SOURCETABLE write failed:" << sock.errorString();
711 while (sock.waitForReadyRead(3000)) {
712 all += sock.readAll();
713 if (sock.bytesAvailable() == 0 && sock.state() != QAbstractSocket::ConnectedState)
break;
719 const int hdrEnd = all.indexOf(
"\r\n\r\n");
720 const QByteArray body = (hdrEnd >= 0) ? all.mid(hdrEnd + 4) : all;
722 qCDebug(NTRIPLog) <<
"----- NTRIP SOURCETABLE BEGIN -----";
723 for (
const QByteArray& line : body.split(
'\n')) {
724 const QByteArray l = line.trimmed();
725 if (!l.isEmpty()) qCDebug(NTRIPLog) << l;
727 qCDebug(NTRIPLog) <<
"------ NTRIP SOURCETABLE END ------";
732 if (_stopping.load()) {
735 if (!_socket || _socket->state() != QAbstractSocket::ConnectedState) {
739 QByteArray line = sentence;
742 if (line.size() >= 5 && line.at(0) ==
'$') {
743 int star = line.lastIndexOf(
'*');
747 for (
int i = 1; i < star; ++i) {
748 calc ^=
static_cast<quint8
>(line.at(i));
752 QByteArray calcCks = QByteArray::number(calc, 16)
753 .rightJustified(2,
'0')
756 bool needsRepair =
false;
757 if (star + 3 > line.size()) {
761 QByteArray txCks = line.mid(star + 1, 2).toUpper();
762 if (txCks != calcCks) {
769 line = line.left(star + 1) + calcCks;
774 for (
int i = 1; i < line.size(); ++i) {
775 calc ^=
static_cast<quint8
>(line.at(i));
777 QByteArray calcCks = QByteArray::number(calc, 16)
778 .rightJustified(2,
'0')
780 line.append(
'*').append(calcCks);
785 if (!line.endsWith(
"\r\n")) {
789 qCDebug(NTRIPLog) <<
"NTRIP Sent NMEA:" << QString::fromUtf8(line.trimmed());
791 const qint64 written = _socket->write(line);
794 qCDebug(NTRIPLog) <<
"NTRIP Socket state:" << (_socket ? _socket->state() : -1);
795 qCDebug(NTRIPLog) <<
"NTRIP Bytes written:" << written;
801 _stopping.store(
true);
804 QObject::disconnect(_readyReadConn);
805 _socket->disconnectFromHost();
818 QObject* parent =
qgcApp() ?
static_cast<QObject*
>(
qgcApp()) : static_cast<QObject*>(
qApp);
824void NTRIPManager::startNTRIP()
826 if (_startStopBusy) {
829 _startStopBusy =
true;
831 qCDebug(NTRIPLog) <<
"startNTRIP: begin";
834 _startStopBusy =
false;
838 _ntripStatus = tr(
"Connecting...");
842 NTRIPSettings* settings = SettingsManager::instance()->ntripSettings();
859 qCWarning(NTRIPLog) <<
"NTRIP settings group is null; starting with defaults";
862 qCDebug(NTRIPLog) <<
"startNTRIP: host=" << host
864 <<
" mount=" << mount
865 <<
" userIsEmpty=" << user.isEmpty();
867 _tcpThread =
new QThread(
this);
871 link->moveToThread(_tcpThread);
875 connect(_tcpThread, &QThread::finished, _tcpThread, &QObject::deleteLater);
876 connect(_tcpThread, &QThread::finished, link, &QObject::deleteLater);
881 Qt::QueuedConnection);
884 _casterStatus = CasterStatus::Connected;
887 const QString want = _useSpartn ? tr(
"Connected (SPARTN)") : tr(
"Connected");
888 if (_ntripStatus != want) {
894 if (qEnvironmentVariableIsSet(
"QGC_NTRIP_DUMP_TABLE")) {
895 static bool dumped =
false;
896 if (!dumped && _tcpLink) {
898 QMetaObject::invokeMethod(_tcpLink,
"debugFetchSourceTable", Qt::QueuedConnection);
904 if (_ggaTimer && !_ggaTimer->isActive()) {
905 _ggaTimer->setInterval(1000);
910 }, Qt::QueuedConnection);
914 static uint32_t spartn_count = 0;
915 if ((spartn_count++ % 50) == 0) {
916 qCDebug(NTRIPLog) <<
"SPARTN bytes flowing:" << data.size();
918 if (_ntripStatus != tr(
"Connected (SPARTN)")) {
919 _ntripStatus = tr(
"Connected (SPARTN)");
922 _casterStatus = CasterStatus::Connected;
926 }, Qt::QueuedConnection);
932 qCDebug(NTRIPLog) <<
"NTRIP started";
934 _startStopBusy =
false;
937void NTRIPManager::stopNTRIP()
939 if (_startStopBusy) {
942 _startStopBusy =
true;
949 QMetaObject::invokeMethod(_tcpLink,
"requestStop", Qt::QueuedConnection);
955 _tcpThread =
nullptr;
961 _ntripStatus = tr(
"Disconnected");
963 qCDebug(NTRIPLog) <<
"NTRIP stopped";
966 if (_ggaTimer && _ggaTimer->isActive()) {
970 _startStopBusy =
false;
975 qCWarning(NTRIPLog) <<
"NTRIP Server Error:" << errorMsg;
976 _ntripStatus = errorMsg;
978 _startStopBusy =
false;
981 if (errorMsg.contains(
"NO_LOCATION_PROVIDED", Qt::CaseInsensitive)) {
982 _onCasterDisconnected(errorMsg);
984 _casterStatus = CasterStatus::OtherError;
990 qgcApp()->showAppMessage(tr(
"NTRIP error: %1").arg(errorMsg));
997 qCDebug(NTRIPLog) <<
"NTRIP Forwarding RTCM to vehicle:" << data.size() <<
"bytes";
1000 if (!_rtcmMavlink &&
qgcApp()) {
1008 if (_ntripStatus != tr(
"Connected")) {
1009 _ntripStatus = tr(
"Connected");
1014 static uint32_t drop_warn_counter = 0;
1015 if ((drop_warn_counter++ % 50) == 0) {
1016 qCWarning(NTRIPLog) <<
"RTCMMavlink tool not ready; dropping RTCM bytes:" << data.size();
1020 _startStopBusy =
false;
1023static QByteArray
_makeGGA(
const QGeoCoordinate& coord,
double altitude_msl)
1026 const QTime utc = QDateTime::currentDateTimeUtc().time();
1027 const QString hhmmss = QString(
"%1%2%3")
1028 .arg(utc.hour(), 2, 10, QChar(
'0'))
1029 .arg(utc.minute(), 2, 10, QChar(
'0'))
1030 .arg(utc.second(), 2, 10, QChar(
'0'));
1034 auto dmm = [](
double deg,
bool lat) -> QString {
1035 double a = qFabs(deg);
1037 double m = (a - d) * 60.0;
1040 int m10000 = int(m * 10000.0 + 0.5);
1041 double m_rounded = m10000 / 10000.0;
1042 if (m_rounded >= 60.0) {
1048 QString mm = QString::number(m_rounded,
'f', 4);
1049 if (m_rounded < 10.0) {
1054 return QString(
"%1%2").arg(d, 2, 10, QChar(
'0')).arg(mm);
1056 return QString(
"%1%2").arg(d, 3, 10, QChar(
'0')).arg(mm);
1060 const bool latNorth = coord.latitude() >= 0.0;
1061 const bool lonEast = coord.longitude() >= 0.0;
1063 const QString latField = dmm(coord.latitude(),
true);
1064 const QString lonField = dmm(coord.longitude(),
false);
1068 const QString core = QString(
"GPGGA,%1,%2,%3,%4,%5,1,12,1.0,%6,M,0.0,M,,")
1071 .arg(latNorth ?
"N" :
"S")
1073 .arg(lonEast ?
"E" :
"W")
1074 .arg(QString::number(altitude_msl,
'f', 1));
1078 const QByteArray coreBytes = core.toUtf8();
1079 for (
char ch : coreBytes) {
1080 cksum ^=
static_cast<quint8
>(ch);
1083 const QString cks = QString(
"%1").arg(cksum, 2, 16, QChar(
'0')).toUpper();
1084 const QByteArray sentence = QByteArray(
"$") + coreBytes + QByteArray(
"*") + cks.toUtf8();
1088void NTRIPManager::_sendGGA()
1094 QGeoCoordinate coord;
1095 double alt_msl = 0.0;
1096 bool validCoord =
false;
1103 if (
Vehicle* veh = mvm->activeVehicle()) {
1107 Fact* latF = gps->getFact(QStringLiteral(
"lat"));
1108 Fact* lonF = gps->getFact(QStringLiteral(
"lon"));
1111 if (!latF) latF = gps->getFact(QStringLiteral(
"latitude"));
1112 if (!lonF) lonF = gps->getFact(QStringLiteral(
"longitude"));
1113 if (!latF) latF = gps->getFact(QStringLiteral(
"lat_deg"));
1114 if (!lonF) lonF = gps->getFact(QStringLiteral(
"lon_deg"));
1115 if (!latF) latF = gps->getFact(QStringLiteral(
"latitude_deg"));
1116 if (!lonF) lonF = gps->getFact(QStringLiteral(
"longitude_deg"));
1119 const double glat = latF->rawValue().toDouble();
1120 const double glon = lonF->rawValue().toDouble();
1123 double lat_deg = glat;
1124 double lon_deg = glon;
1127 if (qAbs(glat) > 1000.0 || qAbs(glon) > 1000.0) {
1128 lat_deg = glat * 1e-7;
1129 lon_deg = glon * 1e-7;
1132 if (qIsFinite(lat_deg) && qIsFinite(lon_deg) &&
1133 !(lat_deg == 0.0 && lon_deg == 0.0) &&
1134 qAbs(lat_deg) <= 90.0 && qAbs(lon_deg) <= 180.0) {
1136 coord = QGeoCoordinate(lat_deg, lon_deg);
1140 Fact* altF = gps->getFact(QStringLiteral(
"alt"));
1141 if (!altF) altF = gps->getFact(QStringLiteral(
"altitude"));
1143 double raw_alt = altF->rawValue().toDouble();
1145 if (qAbs(raw_alt) > 10000.0) {
1146 alt_msl = raw_alt * 1e-3;
1147 }
else if (qAbs(raw_alt) > 1000.0) {
1148 alt_msl = raw_alt * 1e-2;
1152 if (!qIsFinite(alt_msl)) alt_msl = 0.0;
1155 srcUsed = QStringLiteral(
"GPS Raw");
1156 qCDebug(NTRIPLog) <<
"NTRIP: Using raw GPS data for GGA"
1157 <<
"lat:" << lat_deg <<
"lon:" << lon_deg <<
"alt:" << alt_msl;
1162 qCDebug(NTRIPLog) <<
"NTRIP: Vehicle found but no GPS FactGroup available yet";
1166 static int no_vehicle_count = 0;
1167 if ((no_vehicle_count++ % 10) == 0) {
1168 qCDebug(NTRIPLog) <<
"NTRIP: MultiVehicleManager found but no active vehicle yet";
1173 static int no_mvm_count = 0;
1174 if ((no_mvm_count++ % 10) == 0) {
1175 qCDebug(NTRIPLog) <<
"NTRIP: MultiVehicleManager not available yet (startup)";
1183 if (
Vehicle* veh2 = mvm2->activeVehicle()) {
1184 coord = veh2->coordinate();
1185 if (coord.isValid() && !(coord.latitude() == 0.0 && coord.longitude() == 0.0)) {
1187 double a = coord.altitude();
1188 alt_msl = qIsFinite(a) ? a : 0.0;
1189 srcUsed = QStringLiteral(
"Vehicle EKF");
1190 qCDebug(NTRIPLog) <<
"NTRIP: Using vehicle EKF position for GGA" << coord;
1199 qCDebug(NTRIPLog) <<
"NTRIP: No valid position available, skipping GGA.";
1204 if (_ggaTimer && _ggaTimer->interval() != 5000) {
1205 _ggaTimer->setInterval(5000);
1206 qCDebug(NTRIPLog) <<
"NTRIP: Real position acquired, reducing GGA frequency to 5s";
1209 const QByteArray gga =
_makeGGA(coord, alt_msl);
1212 static int gga_send_count = 0;
1213 if ((gga_send_count++ % 5) == 0) {
1214 qCDebug(NTRIPLog) <<
"NTRIP: Sending GGA:" << gga;
1215 qCDebug(NTRIPLog) <<
"NTRIP: GGA coord:" << coord <<
"alt:" << alt_msl <<
"source:" << srcUsed;
1218 QMetaObject::invokeMethod(_tcpLink,
"sendNMEA",
1219 Qt::QueuedConnection,
1220 Q_ARG(QByteArray, gga));
1223 if (!srcUsed.isEmpty() && srcUsed != _ggaSource) {
1224 _ggaSource = srcUsed;
1229void NTRIPManager::_checkSettings()
1231 if (_startStopBusy) {
1232 qCDebug(NTRIPLog) <<
"NTRIP _checkSettings: busy, skipping";
1236 NTRIPSettings* settings = SettingsManager::instance()->ntripSettings();
1237 Fact* enableFact =
nullptr;
1244 if (_startupSuppress) {
1246 if (enableFact->rawValue().toBool()) {
1247 qCDebug(NTRIPLog) <<
"NTRIP Startup: coercing OFF (late restore detected)";
1248 enableFact->setRawValue(
false);
1249 _startupStableTicks = 0;
1251 _startupStableTicks++;
1254 _startupStableTicks = 0;
1258 if (_startupStableTicks >= 2) {
1259 _startupSuppress =
false;
1260 if (!_forcedOffOnce) {
1261 _forcedOffOnce =
true;
1262 if (_ntripEnableConn) {
1263 disconnect(_ntripEnableConn);
1266 qCDebug(NTRIPLog) <<
"NTRIP Startup: suppression released; honoring user setting from now on";
1272 bool shouldBeRunning =
false;
1274 shouldBeRunning = enableFact->rawValue().toBool();
1276 qCWarning(NTRIPLog) <<
"NTRIP settings missing; defaulting to disabled";
1279 bool isRunning = (_tcpLink !=
nullptr);
1281 qCDebug(NTRIPLog) <<
"NTRIP _checkSettings:"
1282 <<
" isRunning=" << isRunning
1283 <<
" shouldBeRunning=" << shouldBeRunning;
1285 if (shouldBeRunning && !isRunning) {
1286 qCDebug(NTRIPLog) <<
"NTRIP _checkSettings: calling startNTRIP()";
1288 }
else if (!shouldBeRunning && isRunning) {
1289 qCDebug(NTRIPLog) <<
"NTRIP _checkSettings: calling stopNTRIP()";
1294void NTRIPManager::_onCasterDisconnected(
const QString& reason)
1296 qWarning() <<
"NTRIP: Disconnected by server:" << reason;
1297 if (reason.contains(
"NO_LOCATION_PROVIDED", Qt::CaseInsensitive)) {
1298 _casterStatus = CasterStatus::NoLocationError;
1300 _casterStatus = CasterStatus::OtherError;
static void _ntripManagerRegisterQmlTypes()
static QByteArray _makeGGA(const QGeoCoordinate &coord, double altitude_msl)
static QObject * _ntripManagerQmlProvider(QQmlEngine *, QJSEngine *)
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Used to group Facts together into an object hierarachy.
A Fact is used to hold a single value within the system.
void rawValueChanged(const QVariant &value)
void _rtcmDataReceived(const QByteArray &data)
void casterStatusChanged(CasterStatus status)
void ntripStatusChanged()
void _tcpError(const QString &errorMsg)
Fact *ntripUseSpartn READ ntripUseSpartn CONSTANT Fact * ntripUseSpartn()
Fact *ntripUsername READ ntripUsername CONSTANT Fact * ntripUsername()
Fact *ntripServerHostAddress READ ntripServerHostAddress CONSTANT Fact * ntripServerHostAddress()
Fact *ntripServerConnectEnabled READ ntripServerConnectEnabled CONSTANT Fact * ntripServerConnectEnabled()
Fact *ntripServerPort READ ntripServerPort CONSTANT Fact * ntripServerPort()
Fact *ntripWhitelist READ ntripWhitelist CONSTANT Fact * ntripWhitelist()
Fact *ntripMountpoint READ ntripMountpoint CONSTANT Fact * ntripMountpoint()
Fact *ntripPassword READ ntripPassword CONSTANT Fact * ntripPassword()
void error(const QString &errorMsg)
void debugFetchSourceTable()
void RTCMDataUpdate(const QByteArray &message)
void SPARTNDataUpdate(const QByteArray &message)
NTRIPTCPLink(const QString &hostAddress, int port, const QString &username, const QString &password, const QString &mountpoint, const QString &whitelist, bool useSpartn, bool autoStart=true, QObject *parent=nullptr)
void sendNMEA(const QByteArray &sentence)
void RTCMDataUpdate(QByteArrayView data)
const uint8_t * crcBytes() const
bool addByte(uint8_t byte)