QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
NTRIP.cc
Go to the documentation of this file.
1#include "NTRIP.h"
2#include "NTRIPSettings.h"
3#include "Fact.h"
4#include "FactGroup.h"
5#include "QGCApplication.h"
7#include "RTCMMavlink.h"
8#include "SettingsManager.h"
9#include <QtNetwork/QSslSocket>
10#include <QtNetwork/QSslError>
11
12#include "MultiVehicleManager.h"
13#include "Vehicle.h"
14#include "PositionManager.h"
15#include <QtPositioning/QGeoCoordinate>
16#include <QtCore/QDateTime>
17
18#include <QtCore/QCoreApplication>
19#include <QtCore/QDebug>
20#include <QtCore/QtGlobal>
21
22#include <QtQml/QQmlEngine>
23#include <QtQml/QJSEngine>
24
25QGC_LOGGING_CATEGORY(NTRIPLog, "qgc.ntrip")
26
27// Register the QML type without constructing the singleton up-front.
28// This avoids creating NTRIPManager during a temporary Q(Core)Application
29// used for command-line parsing, which could lead to it being destroyed
30// early and leaving a dangling static pointer.
31static QObject* _ntripManagerQmlProvider(QQmlEngine*, QJSEngine*)
32{
33 return NTRIPManager::instance();
34}
35
37{
38 qmlRegisterSingletonType<NTRIPManager>("QGroundControl.NTRIP", 1, 0, "NTRIPManager", _ntripManagerQmlProvider);
39}
40Q_COREAPP_STARTUP_FUNCTION(_ntripManagerRegisterQmlTypes)
41
42NTRIPManager* NTRIPManager::_instance = nullptr;
43
44// constructor
45NTRIPManager::NTRIPManager(QObject* parent)
46 : QObject(parent)
47 , _ntripStatus(tr("Disconnected"))
48{
49 qCDebug(NTRIPLog) << "NTRIPManager created";
50 _startupTimer.start();
51
52 _rtcmMavlink = qgcApp() ? qgcApp()->findChild<RTCMMavlink*>() : nullptr;
53 if (!_rtcmMavlink) {
54 QObject* parentObj = qgcApp() ? static_cast<QObject*>(qgcApp()) : static_cast<QObject*>(this);
55 // Ensure an RTCMMavlink helper exists for forwarding RTCM messages
56 _rtcmMavlink = new RTCMMavlink(parentObj);
57 _rtcmMavlink->setObjectName(QStringLiteral("RTCMMavlink"));
58 qCDebug(NTRIPLog) << "NTRIP Created RTCMMavlink helper";
59 }
60
61 // Force NTRIP checkbox OFF during app startup and keep it OFF until first settings tick.
62 {
63 NTRIPSettings* settings = SettingsManager::instance()->ntripSettings();
64 if (settings && settings->ntripServerConnectEnabled()) {
65 Fact* fact = settings->ntripServerConnectEnabled();
66
67 // Force OFF immediately
68 fact->setRawValue(false);
69
70 // While !_forcedOffOnce, if something tries to flip it ON, flip it back OFF.
71 _ntripEnableConn = connect(fact, &Fact::rawValueChanged,
72 this, [this, fact]() {
73 if (!_forcedOffOnce) {
74 const bool wantOn = fact->rawValue().toBool();
75 if (wantOn) {
76 qCDebug(NTRIPLog) << "NTRIP Startup: coercing ntripServerConnectEnabled back to false";
77 fact->setRawValue(false);
78 }
79 }
80 });
81 }
82 }
83
84 // Check settings periodically
85 _settingsCheckTimer = new QTimer(this);
86 connect(_settingsCheckTimer, &QTimer::timeout, this, &NTRIPManager::_checkSettings);
87 _settingsCheckTimer->start(1000); // Check every second
88
89 _ggaTimer = new QTimer(this);
90 _ggaTimer->setInterval(5000);
91 connect(_ggaTimer, &QTimer::timeout, this, &NTRIPManager::_sendGGA);
92
93 connect(qApp, &QCoreApplication::aboutToQuit, this, &NTRIPManager::stopNTRIP, Qt::QueuedConnection);
94
95 _checkSettings();
96}
97
98// destructor
99NTRIPManager::~NTRIPManager()
100{
101 qCDebug(NTRIPLog) << "NTRIPManager destroyed";
102 stopNTRIP();
103 // Clear singleton pointer in case we were destroyed (e.g., if created under
104 // a short-lived Q(Core)Application).
105 if (_instance == this) {
106 _instance = nullptr;
107 }
108}
109
111{
112 reset();
113}
114
116{
117 _state = WaitingForPreamble;
118 _messageLength = 0;
119 _bytesRead = 0;
120 _lengthBytesRead = 0;
121 _crcBytesRead = 0;
122}
123
124bool RTCMParser::addByte(uint8_t byte)
125{
126 switch (_state) {
127 case WaitingForPreamble:
128 if (byte == RTCM3_PREAMBLE) {
129 _buffer[0] = byte;
130 _bytesRead = 1;
131 _state = ReadingLength;
132 _lengthBytesRead = 0;
133 }
134 break;
135
136 case ReadingLength:
137 _lengthBytes[_lengthBytesRead++] = byte;
138 _buffer[_bytesRead++] = byte;
139 if (_lengthBytesRead == 2) {
140 // Extract 10-bit length from bytes (6 reserved bits + 10 length bits)
141 _messageLength = ((_lengthBytes[0] & 0x03) << 8) | _lengthBytes[1];
142 if (_messageLength > 0 && _messageLength < 1021) { // Valid RTCM3 length
143 _state = ReadingMessage;
144 } else {
145 reset(); // Invalid length, restart
146 }
147 }
148 break;
149
150 case ReadingMessage:
151 _buffer[_bytesRead++] = byte;
152 if (_bytesRead >= _messageLength + 3) { // +3 for header
153 _state = ReadingCRC;
154 _crcBytesRead = 0;
155 }
156 break;
157
158 case ReadingCRC:
159 _crcBytes[_crcBytesRead++] = byte;
160 if (_crcBytesRead == 3) {
161 // Message complete - for simplicity, we'll skip CRC validation
162 return true;
163 }
164 break;
165 }
166 return false;
167}
168
170{
171 if (_messageLength >= 2) {
172 // Message ID is in bits 14-25 of the message (after the 24-bit header)
173 return ((_buffer[3] << 4) | (_buffer[4] >> 4)) & 0xFFF;
174 }
175 return 0;
176}
177
178NTRIPTCPLink::NTRIPTCPLink(const QString& hostAddress,
179 int port,
180 const QString& username,
181 const QString& password,
182 const QString& mountpoint,
183 const QString& whitelist,
184 bool useSpartn,
185 bool autoStart,
186 QObject* parent)
187 : QObject(parent)
188 , _hostAddress(hostAddress)
189 , _port(port)
190 , _username(username)
191 , _password(password)
192 , _mountpoint(mountpoint)
193 , _useSpartn(useSpartn)
194{
195
196
197 if (_useSpartn) {
198 // SPARTN path does not use RTCM whitelist or parser
199 if (!whitelist.isEmpty()) {
200 qCDebug(NTRIPLog) << "NTRIP SPARTN enabled; ignoring RTCM whitelist:" << whitelist;
201 }
202 // Initialize SPARTN header-strip guard
203 _spartnBuf.clear();
204 _spartnNeedHeaderStrip = true;
205
206 // Helpful hint if user left the default RTCM port
207 if (_port != 2102) {
208 qCWarning(NTRIPLog) << "NTRIP SPARTN is enabled but port is" << _port << "(expected 2102 for TLS)";
209 }
210 } else {
211 // RTCM path: build whitelist and parser as before
212 for (const auto& msg : whitelist.split(',')) {
213 int msg_int = msg.toInt();
214 if (msg_int)
215 _whitelist.append(msg_int);
216 }
217 qCDebug(NTRIPLog) << "NTRIP whitelist:" << _whitelist;
218 if (_whitelist.empty()) {
219 qCDebug(NTRIPLog) << "NTRIP whitelist is empty; all RTCM message IDs will be forwarded.";
220 }
221 if (!_rtcmParser) {
222 _rtcmParser = new RTCMParser();
223 }
224 _rtcmParser->reset();
225 }
226
227 _state = NTRIPState::uninitialised;
228
229 // Start only when requested so unit tests can exercise parser behavior without sockets.
230 if (autoStart) {
231 start();
232 }
233}
234
236{
237 _stopping.store(true);
238
239 if (_socket) {
240 QObject::disconnect(_readyReadConn);
241 _socket->disconnectFromHost();
242 _socket->close();
243 delete _socket;
244 _socket = nullptr;
245 }
246
247 if (_rtcmParser) {
248 delete _rtcmParser;
249 _rtcmParser = nullptr;
250 }
251}
252
254{
255 // This runs in the worker thread after QThread::started is emitted
256 _hardwareConnect();
257}
258
259void NTRIPTCPLink::_hardwareConnect()
260{
261 if (_stopping.load()) {
262 return;
263 }
264
265 qCDebug(NTRIPLog) << "NTRIP connectToHost" << _hostAddress << ":" << _port << " mount=" << _mountpoint
266 << " tls=" << (_useSpartn ? "yes" : "no");
267
268 // allocate the appropriate socket type
269 // SPARTN selection (TLS on 2102 or when explicitly enabled)
270 if (_useSpartn) {
271 QSslSocket* sslSocket = new QSslSocket(this);
272 _socket = sslSocket;
273 } else {
274 _socket = new QTcpSocket(this);
275 }
276
277 _socket->setSocketOption(QAbstractSocket::KeepAliveOption, 1);
278 _socket->setSocketOption(QAbstractSocket::LowDelayOption, 1);
279 _socket->setReadBufferSize(0);
280
281 QObject::connect(_socket, &QTcpSocket::errorOccurred, this, [this](QAbstractSocket::SocketError code) {
282 // Suppress errors generated during intentional shutdown
283 if (_stopping.load() || !_socket) {
284 qCDebug(NTRIPLog) << "NTRIP suppressing socket error during intentional stop:" << int(code);
285 return;
286 }
287
288 QString msg = _socket->errorString();
289
290 // If the peer closes before we receive any HTTP headers, try to give a helpful hint.
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());
296 } else {
297 if (_port == 2102) {
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)";
301 }
302 }
303 }
304
305 qCWarning(NTRIPLog) << "NTRIP socket error code:" << int(code)
306 << " msg:" << msg
307 << " state:" << int(_socket->state())
308 << " peer:" << _socket->peerAddress().toString() << ":" << _socket->peerPort()
309 << " local:" << _socket->localAddress().toString() << ":" << _socket->localPort();
310 emit error(msg);
311 }, Qt::DirectConnection);
312
313 // If the server cleanly disconnects, capture reason and emit error so NTRIPManager updates status
314 QObject::connect(_socket, &QTcpSocket::disconnected, this, [this]() {
315 // Suppress disconnect noise during intentional shutdown
316 if (_stopping.load() || !_socket) {
317 qCDebug(NTRIPLog) << "NTRIP suppressing disconnect signal during intentional stop";
318 return;
319 }
320
321 const QByteArray trailing = _socket->readAll();
322 QString reason;
323 if (!trailing.isEmpty()) {
324 reason = QString::fromUtf8(trailing).trimmed();
325 qCWarning(NTRIPLog) << "NTRIP disconnected; trailing bytes:" << trailing;
326 } else {
327 reason = "Server disconnected";
328 qCWarning(NTRIPLog) << "NTRIP disconnected cleanly by server";
329 }
330 // Emit richer context on disconnect
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();
339 emit error(reason); // Will call NTRIPManager::_tcpError()
340 }, Qt::DirectConnection);
341
342 _readyReadConn = QObject::connect(_socket, &QTcpSocket::readyRead,this, &NTRIPTCPLink::_readBytes,Qt::DirectConnection);
343
344 auto sendHttpRequest = [this]() {
345 if (!_mountpoint.isEmpty()) {
346 qCDebug(NTRIPLog) << "NTRIP Sending HTTP request";
347 const QByteArray authB64 = QString(_username + ":" + _password).toUtf8().toBase64();
348 QString query =
349 "GET /%1 HTTP/1.1\r\n"
350 "Host: %2\r\n"
351 "Ntrip-Version: Ntrip/2.0\r\n"
352 "User-Agent: QGC-NTRIP\r\n"
353 "Connection: keep-alive\r\n"
354 "Accept: */*\r\n"
355 "Authorization: Basic %3\r\n"
356 "\r\n";
357 const QByteArray req = query.arg(_mountpoint).arg(_hostAddress).arg(QString::fromUtf8(authB64)).toUtf8();
358 const qint64 written = _socket->write(req);
359 _socket->flush();
360
361 // Extra diagnostics: request line and auth presence (length only)
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 ");
366 int b64Len = -1;
367 if (authIdx >= 0) {
368 const int eol = reqStr.indexOf("\r\n", authIdx);
369 if (eol > authIdx) b64Len = eol - (authIdx + 21);
370 }
371 qCDebug(NTRIPLog) << "NTRIP HTTP request first line:" << firstLine;
372 qCDebug(NTRIPLog) << "NTRIP HTTP auth present:" << (authIdx >= 0) << "auth_b64_len:" << b64Len;
373
374 qCDebug(NTRIPLog) << "NTRIP HTTP request bytes written:" << written;
375 _state = NTRIPState::waiting_for_http_response;
376 }
377 else {
378 _state = NTRIPState::waiting_for_rtcm_header;
379 emit connected();
380 }
381 qCDebug(NTRIPLog) << "NTRIP Socket connected"
382 << "local" << _socket->localAddress().toString() << ":" << _socket->localPort()
383 << "-> peer" << _socket->peerAddress().toString() << ":" << _socket->peerPort();
384 };
385
386 if (_useSpartn) {
387 QSslSocket* sslSocket = qobject_cast<QSslSocket*>(_socket);
388 Q_ASSERT(sslSocket);
389
390 QObject::connect(sslSocket, &QSslSocket::encrypted, this, [sendHttpRequest]() {
391 qCDebug(NTRIPLog) << "SPARTN TLS connection established";
392 sendHttpRequest();
393 }, Qt::DirectConnection);
394
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();
399 }
400 }, Qt::DirectConnection);
401
402 sslSocket->connectToHostEncrypted(_hostAddress, static_cast<quint16>(_port));
403
404 if (!sslSocket->waitForEncrypted(10000)) {
405 qCDebug(NTRIPLog) << "NTRIP TLS socket failed to establish encryption";
406 emit error(_socket->errorString());
407 delete _socket;
408 _socket = nullptr;
409 return;
410 }
411
412 if (sslSocket->isEncrypted()) {
413 sendHttpRequest();
414 }
415 } else {
416 QTcpSocket* tcpSocket = qobject_cast<QTcpSocket*>(_socket);
417 Q_ASSERT(tcpSocket);
418
419 tcpSocket->connectToHost(_hostAddress, static_cast<quint16>(_port));
420
421 if (!tcpSocket->waitForConnected(10000)) {
422 qCDebug(NTRIPLog) << "NTRIP Socket failed to connect";
423 emit error(_socket->errorString());
424 delete _socket;
425 _socket = nullptr;
426 return;
427 }
428
429 sendHttpRequest();
430 }
431}
432
433void NTRIPTCPLink::_parse(const QByteArray& buffer)
434{
435 if (_stopping.load()) {
436 return;
437 }
438
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;
443 }
444
445 // RTCM v3 transport sizes
446 constexpr int kRtcmHeaderSize = 3; // preamble (1) + length (2)
447
448 for (char ch : buffer) {
449 const uint8_t byte = static_cast<uint8_t>(static_cast<unsigned char>(ch));
450
451 if (_state == NTRIPState::waiting_for_rtcm_header) {
452 if (byte != RTCM3_PREAMBLE) {
453 continue;
454 }
455 _state = NTRIPState::accumulating_rtcm_packet;
456 }
457
458 if (_rtcmParser->addByte(byte)) {
459 _state = NTRIPState::waiting_for_rtcm_header;
460
461 // Build exact on-wire frame: [D3 | len(2) | payload | CRC(3)]
462 const int payload_len = static_cast<int>(_rtcmParser->messageLength());
463 QByteArray message(reinterpret_cast<const char*>(_rtcmParser->message()),
464 kRtcmHeaderSize + payload_len);
465
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);
469
470 const uint16_t id = _rtcmParser->messageId();
471
472 if (_whitelist.empty() || _whitelist.contains(id)) {
473 qCDebug(NTRIPLog) << "NTRIP RTCM packet id" << id << "len" << message.length();
474 emit RTCMDataUpdate(message);
475 qCDebug(NTRIPLog) << "NTRIP Sending" << id << "of size" << message.length();
476 } else {
477 qCDebug(NTRIPLog) << "NTRIP Ignoring" << id;
478 }
479
480 _rtcmParser->reset();
481 }
482 }
483}
484
485void NTRIPTCPLink::_handleSpartnData(const QByteArray& dataIn)
486{
487 if (dataIn.isEmpty()) {
488 return;
489 }
490
491 // Accumulate so we can remove a response header exactly once if it ever appears here.
492 _spartnBuf.append(dataIn);
493
494 if (_spartnNeedHeaderStrip) {
495 const bool looksHttp = _spartnBuf.startsWith("HTTP/") || _spartnBuf.startsWith("ICY ");
496 if (looksHttp) {
497 const int headerEnd = _spartnBuf.indexOf("\r\n\r\n");
498 if (headerEnd < 0) {
499 // Header incomplete; cap buffer to avoid growth on a bad stream.
500 if (_spartnBuf.size() > 32768) {
501 _spartnBuf = _spartnBuf.right(32768);
502 }
503 return;
504 }
505 // Drop the header and keep the payload.
506 _spartnBuf.remove(0, headerEnd + 4);
507 }
508 _spartnNeedHeaderStrip = false;
509 }
510
511 if (_spartnBuf.isEmpty()) {
512 return;
513 }
514
515 // Deliver raw SPARTN bytes to whoever is consuming them (e.g., a GNSS forwarder).
516 emit SPARTNDataUpdate(_spartnBuf);
517
518 // Clear after handing off.
519 _spartnBuf.clear();
520}
521
522void NTRIPTCPLink::_readBytes()
523{
524 if (_stopping.load()) {
525 return;
526 }
527
528 // SPARTN path: after HTTP 200 OK, feed raw bytes through the SPARTN handler
529 if (_socket && _state == NTRIPState::waiting_for_spartn_data) {
530 const QByteArray bytes = _socket->readAll();
531 if (!bytes.isEmpty()) {
532 _handleSpartnData(bytes);
533 }
534 return;
535 }
536
537 if (!_socket) {
538 return;
539 }
540
541 // Static counters and flags scoped to this function (no header changes needed)
542 static quint64 s_totalRtcm = 0;
543
544 if (_state == NTRIPState::waiting_for_http_response) {
545 QByteArray responseData = _socket->readAll();
546 if (responseData.isEmpty()) {
547 return;
548 }
549
550 QString response = QString::fromUtf8(responseData);
551 qCDebug(NTRIPLog) << "NTRIP HTTP response received:" << response.left(200);
552
553 // Dump full headers once for diagnostics
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;
559 }
560 qCDebug(NTRIPLog) << "NTRIP HTTP response headers <<< END";
561
562 // Parse status line
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;
571 break;
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));
576
577 // Null out _socket before disconnectFromHost() so the
578 // synchronous 'disconnected' handler bails via !_socket guard.
579 QTcpSocket* sock = _socket;
580 _socket = nullptr;
581 _state = NTRIPState::uninitialised;
582
583 QObject::disconnect(_readyReadConn);
584 sock->disconnectFromHost();
585 sock->close();
586 sock->deleteLater();
587
588 if (!_stopping.load()) {
589 QTimer::singleShot(3000, this, [this]() {
590 if (!_stopping.load()) {
591 _hardwareConnect();
592 }
593 });
594 }
595 return;
596 }
597 }
598
599 if (foundOkResponse) {
600 // Mark the 200 OK time and reset first-RTCM flag using QObject properties.
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);
605
606 // Fire a one-shot watchdog that only logs if no RTCM arrives by ~28s.
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();
610 if (!saw) {
611 this->setProperty("ntrip_watchdog_fired", true);
612 qCWarning(NTRIPLog)
613 << "NTRIP no RTCM received 28s after 200 OK. Likely caster timeout."
614 << "Check: duplicate login, entitlement/region, GGA validity.";
615 }
616 });
617
618 _state = _useSpartn ? NTRIPState::waiting_for_spartn_data
619 : NTRIPState::waiting_for_rtcm_header;
620
621 qCDebug(NTRIPLog) << "NTRIP: HTTP handshake complete, transitioning to data state";
622 emit connected();
623
624 // Process any remaining data after the headers
625 if (hdrEnd >= 0) {
626 QByteArray remainingData = responseData.mid(hdrEnd + 4);
627 if (!remainingData.isEmpty()) {
628 qCDebug(NTRIPLog) << "NTRIP: Processing data after HTTP headers:" << remainingData.size() << "bytes";
629 if (_useSpartn) {
630 _handleSpartnData(remainingData);
631 } else {
632 _parse(remainingData);
633 }
634 }
635 }
636 } else {
637 qCDebug(NTRIPLog) << "NTRIP: Waiting for complete HTTP response...";
638 }
639
640 return; // done with HTTP response handling
641 }
642
643 // Data after handshake (RTCM path)
644 QByteArray bytes = _socket->readAll();
645 if (!bytes.isEmpty()) {
646 // Mark first RTCM arrival and log timing/sample
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);
656 }
657
658 s_totalRtcm += static_cast<quint64>(bytes.size());
659 qCDebug(NTRIPLog) << "NTRIP rx bytes:" << bytes.size()
660 << "total:" << s_totalRtcm
661 << "bytesAvailable:" << _socket->bytesAvailable();
662
663 _parse(bytes);
664 }
665}
666
668{
669 // Build request (include Authorization so the caster can tailor the table to your creds)
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"
676 + "Accept: */*\r\n"
677 + "Authorization: Basic " + authB64 + "\r\n"
678 + "Connection: close\r\n\r\n";
679
680 QByteArray all;
681
682 if (_useSpartn || _port == 2102) {
683 // TLS (e.g., SPARTN/2102)
684 QSslSocket sock;
685 sock.connectToHostEncrypted(_hostAddress, static_cast<quint16>(_port));
686 if (!sock.waitForEncrypted(7000)) {
687 qCWarning(NTRIPLog) << "SOURCETABLE TLS connect failed:" << sock.errorString();
688 return;
689 }
690 if (sock.write(req) <= 0 || !sock.waitForBytesWritten(3000)) {
691 qCWarning(NTRIPLog) << "SOURCETABLE TLS write failed:" << sock.errorString();
692 return;
693 }
694 while (sock.waitForReadyRead(3000)) {
695 all += sock.readAll();
696 if (sock.bytesAvailable() == 0 && sock.state() != QAbstractSocket::ConnectedState) break;
697 }
698 sock.close();
699 } else {
700 // Plain TCP (e.g., RTCM/2101)
701 QTcpSocket sock;
702 sock.connectToHost(_hostAddress, static_cast<quint16>(_port));
703 if (!sock.waitForConnected(5000)) {
704 qCWarning(NTRIPLog) << "SOURCETABLE connect failed:" << sock.errorString();
705 return;
706 }
707 if (sock.write(req) <= 0 || !sock.waitForBytesWritten(3000)) {
708 qCWarning(NTRIPLog) << "SOURCETABLE write failed:" << sock.errorString();
709 return;
710 }
711 while (sock.waitForReadyRead(3000)) {
712 all += sock.readAll();
713 if (sock.bytesAvailable() == 0 && sock.state() != QAbstractSocket::ConnectedState) break;
714 }
715 sock.close();
716 }
717
718 // Split headers/body and dump the body (the sourcetable)
719 const int hdrEnd = all.indexOf("\r\n\r\n");
720 const QByteArray body = (hdrEnd >= 0) ? all.mid(hdrEnd + 4) : all;
721
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;
726 }
727 qCDebug(NTRIPLog) << "------ NTRIP SOURCETABLE END ------";
728}
729
730void NTRIPTCPLink::sendNMEA(const QByteArray& sentence)
731{
732 if (_stopping.load()) {
733 return;
734 }
735 if (!_socket || _socket->state() != QAbstractSocket::ConnectedState) {
736 return;
737 }
738
739 QByteArray line = sentence;
740
741 // Validate or repair checksum in the form $CORE*XX
742 if (line.size() >= 5 && line.at(0) == '$') {
743 int star = line.lastIndexOf('*');
744 if (star > 1) {
745 // Calculate checksum over bytes between '$' and '*'
746 quint8 calc = 0;
747 for (int i = 1; i < star; ++i) {
748 calc ^= static_cast<quint8>(line.at(i));
749 }
750
751 // Format calculated checksum as two uppercase hex digits
752 QByteArray calcCks = QByteArray::number(calc, 16)
753 .rightJustified(2, '0')
754 .toUpper();
755
756 bool needsRepair = false;
757 if (star + 3 > line.size()) {
758 // Not enough chars after '*', definitely needs repair
759 needsRepair = true;
760 } else {
761 QByteArray txCks = line.mid(star + 1, 2).toUpper();
762 if (txCks != calcCks) {
763 needsRepair = true;
764 }
765 }
766
767 if (needsRepair) {
768 // Remove any existing checksum and replace with correct one
769 line = line.left(star + 1) + calcCks;
770 }
771 } else {
772 // No '*' found, append one and correct checksum
773 quint8 calc = 0;
774 for (int i = 1; i < line.size(); ++i) {
775 calc ^= static_cast<quint8>(line.at(i));
776 }
777 QByteArray calcCks = QByteArray::number(calc, 16)
778 .rightJustified(2, '0')
779 .toUpper();
780 line.append('*').append(calcCks);
781 }
782 }
783
784 // Ensure CRLF termination
785 if (!line.endsWith("\r\n")) {
786 line.append("\r\n");
787 }
788
789 qCDebug(NTRIPLog) << "NTRIP Sent NMEA:" << QString::fromUtf8(line.trimmed());
790
791 const qint64 written = _socket->write(line);
792 _socket->flush();
793
794 qCDebug(NTRIPLog) << "NTRIP Socket state:" << (_socket ? _socket->state() : -1);
795 qCDebug(NTRIPLog) << "NTRIP Bytes written:" << written;
796}
797
798
800{
801 _stopping.store(true);
802
803 if (_socket) {
804 QObject::disconnect(_readyReadConn);
805 _socket->disconnectFromHost();
806 _socket->close();
807 delete _socket;
808 _socket = nullptr;
809 }
810
811 emit finished(); // let NTRIPManager's QThread quit in response
812}
813
814NTRIPManager* NTRIPManager::instance()
815{
816 if (!_instance) {
817 // Prefer QGCApplication as parent if available, otherwise fall back to qApp
818 QObject* parent = qgcApp() ? static_cast<QObject*>(qgcApp()) : static_cast<QObject*>(qApp);
819 _instance = new NTRIPManager(parent);
820 }
821 return _instance;
822}
823
824void NTRIPManager::startNTRIP()
825{
826 if (_startStopBusy) {
827 return;
828 }
829 _startStopBusy = true; // guard begin
830
831 qCDebug(NTRIPLog) << "startNTRIP: begin";
832
833 if (_tcpLink) {
834 _startStopBusy = false; // already started; release guard
835 return;
836 }
837
838 _ntripStatus = tr("Connecting...");
839 emit ntripStatusChanged();
840
841
842 NTRIPSettings* settings = SettingsManager::instance()->ntripSettings();
843 QString host;
844 int port = 2101;
845 QString user;
846 QString pass;
847 QString mount;
848 QString whitelist;
849
850 if (settings) {
851 if (settings->ntripServerHostAddress()) host = settings->ntripServerHostAddress()->rawValue().toString();
852 if (settings->ntripServerPort()) port = settings->ntripServerPort()->rawValue().toInt();
853 if (settings->ntripUsername()) user = settings->ntripUsername()->rawValue().toString();
854 if (settings->ntripPassword()) pass = settings->ntripPassword()->rawValue().toString();
855 if (settings->ntripMountpoint()) mount = settings->ntripMountpoint()->rawValue().toString();
856 if (settings->ntripWhitelist()) whitelist = settings->ntripWhitelist()->rawValue().toString();
857 if (settings->ntripUseSpartn()) _useSpartn = settings->ntripUseSpartn()->rawValue().toBool();
858 } else {
859 qCWarning(NTRIPLog) << "NTRIP settings group is null; starting with defaults";
860 }
861
862 qCDebug(NTRIPLog) << "startNTRIP: host=" << host
863 << " port=" << port
864 << " mount=" << mount
865 << " userIsEmpty=" << user.isEmpty();
866
867 _tcpThread = new QThread(this);
868 NTRIPTCPLink* link = new NTRIPTCPLink(host, port, user, pass, mount, whitelist, _useSpartn);
869 _tcpLink = link;
870
871 link->moveToThread(_tcpThread);
872
873 connect(_tcpThread, &QThread::started, link, &NTRIPTCPLink::start);
874 connect(link, &NTRIPTCPLink::finished, _tcpThread, &QThread::quit);
875 connect(_tcpThread, &QThread::finished, _tcpThread, &QObject::deleteLater);
876 connect(_tcpThread, &QThread::finished, link, &QObject::deleteLater);
877
878 // Surface errors (including server disconnect reasons) to status/UI
879 connect(_tcpLink, &NTRIPTCPLink::error,
881 Qt::QueuedConnection);
882
883 connect(_tcpLink, &NTRIPTCPLink::connected, this, [this]() {
884 _casterStatus = CasterStatus::Connected;
885 emit casterStatusChanged(_casterStatus);
886
887 const QString want = _useSpartn ? tr("Connected (SPARTN)") : tr("Connected");
888 if (_ntripStatus != want) {
889 _ntripStatus = want;
890 emit ntripStatusChanged();
891 }
892
893 // >>> ONE-SHOT SOURCETABLE DUMP (enable with env var QGC_NTRIP_DUMP_TABLE=1)
894 if (qEnvironmentVariableIsSet("QGC_NTRIP_DUMP_TABLE")) {
895 static bool dumped = false;
896 if (!dumped && _tcpLink) {
897 dumped = true;
898 QMetaObject::invokeMethod(_tcpLink, "debugFetchSourceTable", Qt::QueuedConnection);
899 }
900 }
901 // <<<
902
903 // Start rapid 1 Hz GGA until we get the first valid coord
904 if (_ggaTimer && !_ggaTimer->isActive()) {
905 _ggaTimer->setInterval(1000); // 1 Hz while waiting for fix
906 _sendGGA(); // try immediately
907 _ggaTimer->start();
908 }
909
910 }, Qt::QueuedConnection);
911
912 if (_useSpartn) {
913 connect(_tcpLink, &NTRIPTCPLink::SPARTNDataUpdate, this, [this](const QByteArray& data){
914 static uint32_t spartn_count = 0;
915 if ((spartn_count++ % 50) == 0) {
916 qCDebug(NTRIPLog) << "SPARTN bytes flowing:" << data.size();
917 }
918 if (_ntripStatus != tr("Connected (SPARTN)")) {
919 _ntripStatus = tr("Connected (SPARTN)");
920 emit ntripStatusChanged();
921 }
922 _casterStatus = CasterStatus::Connected;
923 emit casterStatusChanged(_casterStatus);
924
925 _rtcmDataReceived(data);
926 }, Qt::QueuedConnection);
927 } else {
928 connect(_tcpLink, &NTRIPTCPLink::RTCMDataUpdate, this, &NTRIPManager::_rtcmDataReceived, Qt::QueuedConnection);
929 }
930
931 _tcpThread->start();
932 qCDebug(NTRIPLog) << "NTRIP started";
933
934 _startStopBusy = false; // guard end
935}
936
937void NTRIPManager::stopNTRIP()
938{
939 if (_startStopBusy) {
940 return;
941 }
942 _startStopBusy = true; // guard begin
943
944 if (_tcpLink) {
945 // Prevent spurious "NTRIP error: ..." toasts during user-initiated shutdown
946 disconnect(_tcpLink, &NTRIPTCPLink::error, this, &NTRIPManager::_tcpError);
947
948 // Ask the worker to stop in its own thread
949 QMetaObject::invokeMethod(_tcpLink, "requestStop", Qt::QueuedConnection);
950
951 // If we still own the thread, stop and wait for it
952 if (_tcpThread) {
953 _tcpThread->quit(); // or just rely on 'finished' from requestStop()
954 _tcpThread->wait();
955 _tcpThread = nullptr;
956 }
957
958 // _tcpLink will be deleted via deleteLater() when the thread finishes
959 _tcpLink = nullptr;
960
961 _ntripStatus = tr("Disconnected");
962 emit ntripStatusChanged();
963 qCDebug(NTRIPLog) << "NTRIP stopped";
964 }
965
966 if (_ggaTimer && _ggaTimer->isActive()) {
967 _ggaTimer->stop();
968 }
969
970 _startStopBusy = false; // guard end
971}
972
973void NTRIPManager::_tcpError(const QString& errorMsg)
974{
975 qCWarning(NTRIPLog) << "NTRIP Server Error:" << errorMsg;
976 _ntripStatus = errorMsg;
977 emit ntripStatusChanged();
978 _startStopBusy = false; // make sure guard clears on error
979
980 // Check for specific "no location provided" disconnect reason
981 if (errorMsg.contains("NO_LOCATION_PROVIDED", Qt::CaseInsensitive)) {
982 _onCasterDisconnected(errorMsg);
983 } else {
984 _casterStatus = CasterStatus::OtherError;
985 emit casterStatusChanged(_casterStatus);
986 }
987
988 // Show error to user via QGC notification (toast)
989 if (qgcApp()) {
990 qgcApp()->showAppMessage(tr("NTRIP error: %1").arg(errorMsg));
991 }
992}
993
994void NTRIPManager::_rtcmDataReceived(const QByteArray& data)
995{
996
997 qCDebug(NTRIPLog) << "NTRIP Forwarding RTCM to vehicle:" << data.size() << "bytes";
998
999 // Lazily resolve the tool if we don't have it yet
1000 if (!_rtcmMavlink && qgcApp()) {
1001 _rtcmMavlink = qgcApp()->findChild<RTCMMavlink*>();
1002 }
1003
1004 if (_rtcmMavlink) {
1005 // Forward raw RTCM bytes to the autopilot via MAVLink GPS_RTCM_DATA
1006 _rtcmMavlink->RTCMDataUpdate(data);
1007
1008 if (_ntripStatus != tr("Connected")) {
1009 _ntripStatus = tr("Connected");
1010 emit ntripStatusChanged();
1011 }
1012 } else {
1013 // Tool not constructed yet — avoid crash, keep status, and don't spam
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();
1017 }
1018 }
1019
1020 _startStopBusy = false; // guard clears once bytes flow (or we tried)
1021}
1022
1023static QByteArray _makeGGA(const QGeoCoordinate& coord, double altitude_msl)
1024{
1025 // UTC time hhmmss format
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'));
1031
1032 // Convert decimal degrees to NMEA DMM with zero-padded minutes:
1033 // lat: ddmm.mmmm, lon: dddmm.mmmm
1034 auto dmm = [](double deg, bool lat) -> QString {
1035 double a = qFabs(deg);
1036 int d = int(a);
1037 double m = (a - d) * 60.0;
1038
1039 // Round to 4 decimals and handle 59.99995 -> 60.0000 carry into degrees
1040 int m10000 = int(m * 10000.0 + 0.5);
1041 double m_rounded = m10000 / 10000.0;
1042 if (m_rounded >= 60.0) {
1043 m_rounded -= 60.0;
1044 d += 1;
1045 }
1046
1047 // Ensure minutes have two digits before the decimal (e.g. 08.1234)
1048 QString mm = QString::number(m_rounded, 'f', 4);
1049 if (m_rounded < 10.0) {
1050 mm.prepend("0");
1051 }
1052
1053 if (lat) {
1054 return QString("%1%2").arg(d, 2, 10, QChar('0')).arg(mm);
1055 } else {
1056 return QString("%1%2").arg(d, 3, 10, QChar('0')).arg(mm);
1057 }
1058 };
1059
1060 const bool latNorth = coord.latitude() >= 0.0;
1061 const bool lonEast = coord.longitude() >= 0.0;
1062
1063 const QString latField = dmm(coord.latitude(), true);
1064 const QString lonField = dmm(coord.longitude(), false);
1065
1066 // IMPORTANT: include geoid separation as 0.0 and unit 'M'
1067 // Fields: time,lat,N,lon,E,fix,nsat,hdop,alt,M,geoid,M,age,station
1068 const QString core = QString("GPGGA,%1,%2,%3,%4,%5,1,12,1.0,%6,M,0.0,M,,")
1069 .arg(hhmmss)
1070 .arg(latField)
1071 .arg(latNorth ? "N" : "S")
1072 .arg(lonField)
1073 .arg(lonEast ? "E" : "W")
1074 .arg(QString::number(altitude_msl, 'f', 1));
1075
1076 // NMEA checksum over bytes between '$' and '*'
1077 quint8 cksum = 0;
1078 const QByteArray coreBytes = core.toUtf8();
1079 for (char ch : coreBytes) {
1080 cksum ^= static_cast<quint8>(ch);
1081 }
1082
1083 const QString cks = QString("%1").arg(cksum, 2, 16, QChar('0')).toUpper();
1084 const QByteArray sentence = QByteArray("$") + coreBytes + QByteArray("*") + cks.toUtf8();
1085 return sentence;
1086}
1087
1088void NTRIPManager::_sendGGA()
1089{
1090 if (!_tcpLink) {
1091 return;
1092 }
1093
1094 QGeoCoordinate coord;
1095 double alt_msl = 0.0;
1096 bool validCoord = false;
1097 QString srcUsed;
1098
1099 if (qgcApp()) {
1100 // PRIORITY 1: Raw GPS data from vehicle GPS facts (most important for RTK)
1101 MultiVehicleManager* mvm = MultiVehicleManager::instance();
1102 if (mvm) {
1103 if (Vehicle* veh = mvm->activeVehicle()) {
1104 FactGroup* gps = veh->gpsFactGroup();
1105 if (gps) {
1106 // Try common GPS fact names
1107 Fact* latF = gps->getFact(QStringLiteral("lat"));
1108 Fact* lonF = gps->getFact(QStringLiteral("lon"));
1109
1110 // If "lat"/"lon" don't work, try alternative names
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"));
1117
1118 if (latF && lonF) {
1119 const double glat = latF->rawValue().toDouble();
1120 const double glon = lonF->rawValue().toDouble();
1121
1122 // GPS coordinates might be in 1e-7 degrees format (int32 scaled)
1123 double lat_deg = glat;
1124 double lon_deg = glon;
1125
1126 // Check if values look like scaled integers (> 1000 suggests 1e-7 format)
1127 if (qAbs(glat) > 1000.0 || qAbs(glon) > 1000.0) {
1128 lat_deg = glat * 1e-7;
1129 lon_deg = glon * 1e-7;
1130 }
1131
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) {
1135
1136 coord = QGeoCoordinate(lat_deg, lon_deg);
1137 validCoord = true;
1138
1139 // Get altitude from GPS if available
1140 Fact* altF = gps->getFact(QStringLiteral("alt"));
1141 if (!altF) altF = gps->getFact(QStringLiteral("altitude"));
1142 if (altF) {
1143 double raw_alt = altF->rawValue().toDouble();
1144 // Altitude might be in mm or cm, convert to meters
1145 if (qAbs(raw_alt) > 10000.0) { // > 10km suggests mm format
1146 alt_msl = raw_alt * 1e-3; // mm to meters
1147 } else if (qAbs(raw_alt) > 1000.0) { // > 1km suggests cm format
1148 alt_msl = raw_alt * 1e-2; // cm to meters
1149 } else {
1150 alt_msl = raw_alt; // already in meters
1151 }
1152 if (!qIsFinite(alt_msl)) alt_msl = 0.0;
1153 }
1154
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;
1158 }
1159 }
1160 } else {
1161 // Vehicle exists but no GPS FactGroup yet
1162 qCDebug(NTRIPLog) << "NTRIP: Vehicle found but no GPS FactGroup available yet";
1163 }
1164 } else {
1165 // MultiVehicleManager exists but no active vehicle yet
1166 static int no_vehicle_count = 0;
1167 if ((no_vehicle_count++ % 10) == 0) { // Log every 10th time to avoid spam
1168 qCDebug(NTRIPLog) << "NTRIP: MultiVehicleManager found but no active vehicle yet";
1169 }
1170 }
1171 } else {
1172 // MultiVehicleManager not found yet - this is normal during early startup
1173 static int no_mvm_count = 0;
1174 if ((no_mvm_count++ % 10) == 0) { // Log every 10th time to avoid spam
1175 qCDebug(NTRIPLog) << "NTRIP: MultiVehicleManager not available yet (startup)";
1176 }
1177 }
1178
1179 // PRIORITY 2: Vehicle global position estimate (EKF output)
1180 if (!validCoord) {
1181 MultiVehicleManager* mvm2 = MultiVehicleManager::instance();
1182 if (mvm2) {
1183 if (Vehicle* veh2 = mvm2->activeVehicle()) {
1184 coord = veh2->coordinate();
1185 if (coord.isValid() && !(coord.latitude() == 0.0 && coord.longitude() == 0.0)) {
1186 validCoord = true;
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;
1191 }
1192 }
1193 }
1194 }
1195
1196 }
1197
1198 if (!validCoord) {
1199 qCDebug(NTRIPLog) << "NTRIP: No valid position available, skipping GGA.";
1200 return;
1201 }
1202
1203 // Once we have a real valid coord, slow down GGA to 5 seconds
1204 if (_ggaTimer && _ggaTimer->interval() != 5000) {
1205 _ggaTimer->setInterval(5000);
1206 qCDebug(NTRIPLog) << "NTRIP: Real position acquired, reducing GGA frequency to 5s";
1207 }
1208
1209 const QByteArray gga = _makeGGA(coord, alt_msl);
1210
1211 // Debug: Log the GGA sentence being sent (but reduce spam)
1212 static int gga_send_count = 0;
1213 if ((gga_send_count++ % 5) == 0) { // Log every 5th GGA to reduce spam
1214 qCDebug(NTRIPLog) << "NTRIP: Sending GGA:" << gga;
1215 qCDebug(NTRIPLog) << "NTRIP: GGA coord:" << coord << "alt:" << alt_msl << "source:" << srcUsed;
1216 }
1217
1218 QMetaObject::invokeMethod(_tcpLink, "sendNMEA",
1219 Qt::QueuedConnection,
1220 Q_ARG(QByteArray, gga));
1221
1222 // Update GGA source for UI display
1223 if (!srcUsed.isEmpty() && srcUsed != _ggaSource) {
1224 _ggaSource = srcUsed;
1225 emit ggaSourceChanged();
1226 }
1227}
1228
1229void NTRIPManager::_checkSettings()
1230{
1231 if (_startStopBusy) {
1232 qCDebug(NTRIPLog) << "NTRIP _checkSettings: busy, skipping";
1233 return;
1234 }
1235
1236 NTRIPSettings* settings = SettingsManager::instance()->ntripSettings();
1237 Fact* enableFact = nullptr;
1238 if (settings && settings->ntripServerConnectEnabled()) {
1239 enableFact = settings->ntripServerConnectEnabled();
1240 }
1241
1242 // During startup, keep forcing OFF until we observe the Fact stable at false
1243 // for 2 consecutive ticks. This defeats late restores from QML/settings.
1244 if (_startupSuppress) {
1245 if (enableFact) {
1246 if (enableFact->rawValue().toBool()) {
1247 qCDebug(NTRIPLog) << "NTRIP Startup: coercing OFF (late restore detected)";
1248 enableFact->setRawValue(false);
1249 _startupStableTicks = 0; // reset stability counter on any true
1250 } else {
1251 _startupStableTicks++;
1252 }
1253 } else {
1254 _startupStableTicks = 0; // no fact yet, not stable
1255 }
1256
1257 // Release once we have seen false for 2 ticks
1258 if (_startupStableTicks >= 2) {
1259 _startupSuppress = false;
1260 if (!_forcedOffOnce) {
1261 _forcedOffOnce = true;
1262 if (_ntripEnableConn) {
1263 disconnect(_ntripEnableConn);
1264 }
1265 }
1266 qCDebug(NTRIPLog) << "NTRIP Startup: suppression released; honoring user setting from now on";
1267 } else {
1268 return; // keep suppressing this tick
1269 }
1270 }
1271
1272 bool shouldBeRunning = false;
1273 if (enableFact) {
1274 shouldBeRunning = enableFact->rawValue().toBool();
1275 } else {
1276 qCWarning(NTRIPLog) << "NTRIP settings missing; defaulting to disabled";
1277 }
1278
1279 bool isRunning = (_tcpLink != nullptr);
1280
1281 qCDebug(NTRIPLog) << "NTRIP _checkSettings:"
1282 << " isRunning=" << isRunning
1283 << " shouldBeRunning=" << shouldBeRunning;
1284
1285 if (shouldBeRunning && !isRunning) {
1286 qCDebug(NTRIPLog) << "NTRIP _checkSettings: calling startNTRIP()";
1287 startNTRIP();
1288 } else if (!shouldBeRunning && isRunning) {
1289 qCDebug(NTRIPLog) << "NTRIP _checkSettings: calling stopNTRIP()";
1290 stopNTRIP();
1291 }
1292}
1293
1294void NTRIPManager::_onCasterDisconnected(const QString& reason)
1295{
1296 qWarning() << "NTRIP: Disconnected by server:" << reason;
1297 if (reason.contains("NO_LOCATION_PROVIDED", Qt::CaseInsensitive)) {
1298 _casterStatus = CasterStatus::NoLocationError;
1299 } else {
1300 _casterStatus = CasterStatus::OtherError;
1301 }
1302 emit casterStatusChanged(_casterStatus);
1303}
static void _ntripManagerRegisterQmlTypes()
Definition NTRIP.cc:36
static QByteArray _makeGGA(const QGeoCoordinate &coord, double altitude_msl)
Definition NTRIP.cc:1023
static QObject * _ntripManagerQmlProvider(QQmlEngine *, QJSEngine *)
Definition NTRIP.cc:31
#define RTCM3_PREAMBLE
Definition NTRIP.h:21
#define qgcApp()
#define qApp
Error error
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Used to group Facts together into an object hierarachy.
Definition FactGroup.h:19
A Fact is used to hold a single value within the system.
Definition Fact.h:19
void rawValueChanged(const QVariant &value)
void _rtcmDataReceived(const QByteArray &data)
Definition NTRIP.cc:994
void casterStatusChanged(CasterStatus status)
void ntripStatusChanged()
void ggaSourceChanged()
void _tcpError(const QString &errorMsg)
Definition NTRIP.cc:973
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 reset()
Definition NTRIP.cc:115
int crcSize() const
Definition NTRIP.h:33
uint16_t messageId()
Definition NTRIP.cc:169
RTCMParser()
Definition NTRIP.cc:110
uint16_t messageLength()
Definition NTRIP.h:30
uint8_t * message()
Definition NTRIP.h:29
const uint8_t * crcBytes() const
Definition NTRIP.h:32
bool addByte(uint8_t byte)
Definition NTRIP.cc:124