17#include <QtPositioning/QGeoCoordinate>
18#include <QtCore/QDateTime>
20#include <QtCore/QApplicationStatic>
21#include <QtCore/QCoreApplication>
22#include <QtCore/QDebug>
23#include <QtCore/QtGlobal>
31 return _ntripManagerInstance();
37 qCDebug(NTRIPManagerLog) <<
"NTRIPManager created";
41 QObject* parentObj =
qgcApp() ?
static_cast<QObject*
>(
qgcApp()) :
static_cast<QObject*
>(
this);
43 _rtcmMavlink->setObjectName(QStringLiteral(
"RTCMMavlink"));
48 const quint16 port =
static_cast<quint16
>(
49 settings->rtcmUdpInputPort()->rawValue().toUInt());
55 auto applyUdpInputSettings = [
this, settings]() {
56 const quint16 uin_port =
static_cast<quint16
>(
57 settings->rtcmUdpInputPort()->rawValue().toUInt());
58 _rtcmUdpInput->
setPort(uin_port);
59 if (settings->rtcmUdpInputEnabled()->rawValue().toBool()) {
60 _rtcmUdpInput->
start();
62 _rtcmUdpInput->
stop();
66 this, applyUdpInputSettings);
68 this, applyUdpInputSettings);
70 auto connectSetting = [
this](
Fact* fact) {
76 connectSetting(settings->ntripServerConnectEnabled());
77 connectSetting(settings->ntripServerHostAddress());
78 connectSetting(settings->ntripServerPort());
79 connectSetting(settings->ntripUsername());
80 connectSetting(settings->ntripPassword());
81 connectSetting(settings->ntripMountpoint());
82 connectSetting(settings->ntripWhitelist());
83 connectSetting(settings->ntripUseTls());
84 connectSetting(settings->ntripUdpForwardEnabled());
85 connectSetting(settings->ntripUdpTargetAddress());
86 connectSetting(settings->ntripUdpTargetPort());
87 applyUdpInputSettings();
91 QTimer::singleShot(0,
this, &NTRIPManager::_onSettingChanged);
93 _ggaTimer =
new QTimer(
this);
94 _ggaTimer->setInterval(5000);
95 connect(_ggaTimer, &QTimer::timeout,
this, &NTRIPManager::_sendGGA);
97 _dataRateTimer =
new QTimer(
this);
98 _dataRateTimer->setInterval(1000);
99 connect(_dataRateTimer, &QTimer::timeout,
this, [
this]() {
100 const double rate =
static_cast<double>(_bytesReceived - _dataRatePrevBytes);
101 _dataRatePrevBytes = _bytesReceived;
102 if (!qFuzzyCompare(_dataRateBytesPerSec + 1.0, rate + 1.0)) {
103 _dataRateBytesPerSec = rate;
115 qCDebug(NTRIPManagerLog) <<
"NTRIPManager destroyed";
119void NTRIPManager::_setStatus(ConnectionStatus status,
const QString& msg)
121 bool changed =
false;
123 if (_connectionStatus != status) {
124 _connectionStatus = status;
129 if (_statusMessage != msg) {
130 _statusMessage = msg;
136 qCDebug(NTRIPManagerLog) <<
"NTRIP status:" <<
static_cast<int>(status) << msg;
148 if (settings->ntripServerHostAddress()) config.
host = settings->ntripServerHostAddress()->rawValue().toString();
149 if (settings->ntripServerPort()) config.
port = settings->ntripServerPort()->rawValue().toInt();
150 if (settings->ntripUsername()) config.
username = settings->ntripUsername()->rawValue().toString();
151 if (settings->ntripPassword()) config.
password = settings->ntripPassword()->rawValue().toString();
152 if (settings->ntripMountpoint()) config.
mountpoint = settings->ntripMountpoint()->rawValue().toString();
153 if (settings->ntripWhitelist()) config.
whitelist = settings->ntripWhitelist()->rawValue().toString();
154 if (settings->ntripUseTls()) config.
useTls = settings->ntripUseTls()->rawValue().toBool();
161 if (_startStopBusy || _transport) {
162 if (_startStopBusy) {
163 qCWarning(NTRIPManagerLog) <<
"startNTRIP called while start/stop in progress";
167 _startStopBusy =
true;
169 if (_reconnectTimer) {
170 _reconnectTimer->stop();
176 if (settings && settings->ntripUdpForwardEnabled()) {
177 _udpForwardEnabled = settings->ntripUdpForwardEnabled()->rawValue().toBool();
180 if (_udpForwardEnabled && settings) {
181 const QString udpAddr = settings->ntripUdpTargetAddress()->rawValue().toString();
182 const quint16 udpPort =
static_cast<quint16
>(settings->ntripUdpTargetPort()->rawValue().toUInt());
183 const QHostAddress parsedAddr(udpAddr);
184 if (!udpAddr.isEmpty() && !parsedAddr.isNull() && udpPort > 0) {
185 _udpTargetAddress = parsedAddr;
186 _udpTargetPort = udpPort;
188 _udpSocket =
new QUdpSocket(
this);
190 qCDebug(NTRIPManagerLog) <<
"NTRIP UDP forwarding enabled:" << udpAddr <<
":" << udpPort;
192 qCWarning(NTRIPManagerLog) <<
"NTRIP UDP forwarding enabled but address/port invalid";
193 _udpForwardEnabled =
false;
197 if (config.
host.isEmpty()) {
198 qCWarning(NTRIPManagerLog) <<
"NTRIP host address is empty";
200 _startStopBusy =
false;
204 if (config.
port <= 0 || config.
port > 65535) {
205 qCWarning(NTRIPManagerLog) <<
"NTRIP port is invalid:" << config.
port;
207 _startStopBusy =
false;
211 qCDebug(NTRIPManagerLog) <<
"startNTRIP: host=" << config.
host <<
" port=" << config.
port
217 _messagesReceived = 0;
218 _dataRateBytesPerSec = 0.0;
219 _dataRatePrevBytes = 0;
224 _runningConfig = config;
225 _runningUdpForward = _udpForwardEnabled;
226 _runningUdpAddr = _udpForwardEnabled ? _udpTargetAddress.toString() : QString();
227 _runningUdpPort = _udpForwardEnabled ? _udpTargetPort : 0;
232 this, &NTRIPManager::_tcpError);
235 _reconnectAttempts = 0;
241 if (_ggaTimer && !_ggaTimer->isActive()) {
242 _ggaTimer->setInterval(1000);
246 _dataRateTimer->start();
252 qCDebug(NTRIPManagerLog) <<
"NTRIP started";
254 _startStopBusy =
false;
259 if (_startStopBusy) {
260 qCWarning(NTRIPManagerLog) <<
"stopNTRIP called while start/stop in progress";
263 _startStopBusy =
true;
265 if (_reconnectTimer) {
266 _reconnectTimer->stop();
272 _transport->deleteLater();
273 _transport =
nullptr;
276 _runningUdpForward =
false;
277 _runningUdpAddr.clear();
281 qCDebug(NTRIPManagerLog) <<
"NTRIP stopped";
284 if (_ggaTimer && _ggaTimer->isActive()) {
287 _dataRateTimer->stop();
288 if (_dataRateBytesPerSec != 0.0) {
289 _dataRateBytesPerSec = 0.0;
290 _dataRatePrevBytes = 0;
297 _udpSocket =
nullptr;
299 _udpForwardEnabled =
false;
301 _startStopBusy =
false;
304void NTRIPManager::_tcpError(
const QString& errorMsg)
306 qCWarning(NTRIPManagerLog) <<
"NTRIP error:" << errorMsg;
309 if (errorMsg.contains(
"NO_LOCATION_PROVIDED", Qt::CaseInsensitive)) {
316 _scheduleReconnect();
319void NTRIPManager::_scheduleReconnect()
322 bool shouldRun = settings && settings->ntripServerConnectEnabled() &&
323 settings->ntripServerConnectEnabled()->rawValue().toBool();
333 qCDebug(NTRIPManagerLog) <<
"NTRIP reconnecting in" << backoffMs <<
"ms (attempt" << _reconnectAttempts <<
")";
337 if (!_reconnectTimer) {
338 _reconnectTimer =
new QTimer(
this);
339 _reconnectTimer->setSingleShot(
true);
340 connect(_reconnectTimer, &QTimer::timeout,
this, [
this]() {
347 _reconnectTimer->start(backoffMs);
350void NTRIPManager::_rtcmDataReceived(
const QByteArray& data)
352 _bytesReceived +=
static_cast<quint64
>(data.size());
355 qCDebug(NTRIPManagerLog) <<
"NTRIP Forwarding RTCM to vehicle:" << data.size() <<
"bytes";
357 if (!_rtcmMavlink &&
qgcApp()) {
368 qCWarning(NTRIPManagerLog) <<
"RTCMMavlink not ready; dropping" << data.size() <<
"bytes";
371 if (_udpForwardEnabled && _udpSocket && _udpTargetPort > 0) {
372 const qint64 sent = _udpSocket->writeDatagram(data, _udpTargetAddress, _udpTargetPort);
374 qCWarning(NTRIPManagerLog) <<
"NTRIP UDP forward failed:" << _udpSocket->errorString();
381 const QTime utc = QDateTime::currentDateTimeUtc().time();
382 const QString hhmmss = QString(
"%1%2%3")
383 .arg(utc.hour(), 2, 10, QChar(
'0'))
384 .arg(utc.minute(), 2, 10, QChar(
'0'))
385 .arg(utc.second(), 2, 10, QChar(
'0'));
387 auto dmm = [](
double deg,
bool lat) -> QString {
388 double a = qFabs(deg);
390 double m = (a - d) * 60.0;
392 int m10000 = int(m * 10000.0 + 0.5);
393 double m_rounded = m10000 / 10000.0;
394 if (m_rounded >= 60.0) {
399 QString mm = QString::number(m_rounded,
'f', 4);
400 if (m_rounded < 10.0) {
405 return QString(
"%1%2").arg(d, 2, 10, QChar(
'0')).arg(mm);
407 return QString(
"%1%2").arg(d, 3, 10, QChar(
'0')).arg(mm);
411 const bool latNorth = coord.latitude() >= 0.0;
412 const bool lonEast = coord.longitude() >= 0.0;
414 const QString latField = dmm(coord.latitude(),
true);
415 const QString lonField = dmm(coord.longitude(),
false);
417 const QString core = QString(
"GPGGA,%1,%2,%3,%4,%5,1,12,1.0,%6,M,0.0,M,,")
420 .arg(latNorth ?
"N" :
"S")
422 .arg(lonEast ?
"E" :
"W")
423 .arg(QString::number(altitude_msl,
'f', 1));
426 const QByteArray coreBytes = core.toUtf8();
427 for (
char ch : coreBytes) {
428 cksum ^=
static_cast<quint8
>(ch);
431 const QString cks = QString(
"%1").arg(cksum, 2, 16, QChar(
'0')).toUpper();
432 const QByteArray sentence = QByteArray(
"$") + coreBytes + QByteArray(
"*") + cks.toUtf8();
436QPair<QGeoCoordinate, QString> NTRIPManager::_getBestPosition()
const
447 const double lat = latF->
rawValue().toDouble();
448 const double lon = lonF->
rawValue().toDouble();
450 if (qIsFinite(lat) && qIsFinite(lon) &&
451 !(lat == 0.0 && lon == 0.0) &&
452 qAbs(lat) <= 90.0 && qAbs(lon) <= 180.0) {
454 return {QGeoCoordinate(lat, lon, veh->coordinate().altitude()),
455 QStringLiteral(
"GPS Raw")};
460 QGeoCoordinate coord = veh->coordinate();
461 if (coord.isValid() && !(coord.latitude() == 0.0 && coord.longitude() == 0.0)) {
462 return {coord, QStringLiteral(
"Vehicle EKF")};
470 if (coord.isValid() && !(coord.latitude() == 0.0 && coord.longitude() == 0.0)) {
471 return {coord, QStringLiteral(
"GCS Position")};
475 return {QGeoCoordinate(), QString()};
478void NTRIPManager::_sendGGA()
484 const auto [coord, srcUsed] = _getBestPosition();
486 if (!coord.isValid()) {
487 qCDebug(NTRIPManagerLog) <<
"NTRIP: No valid position, skipping GGA";
491 if (_ggaTimer && _ggaTimer->interval() != 5000) {
492 _ggaTimer->setInterval(5000);
493 qCDebug(NTRIPManagerLog) <<
"NTRIP: Position acquired, reducing GGA to 5s";
496 double alt_msl = coord.altitude();
497 if (!qIsFinite(alt_msl)) {
501 const QByteArray gga =
makeGGA(coord, alt_msl);
504 if (!srcUsed.isEmpty() && srcUsed != _ggaSource) {
505 _ggaSource = srcUsed;
510void NTRIPManager::_onSettingChanged()
512 if (_startStopBusy) {
517 bool shouldRun = settings && settings->ntripServerConnectEnabled() &&
518 settings->ntripServerConnectEnabled()->rawValue().toBool();
519 bool isRunning = (_transport !=
nullptr);
521 if (shouldRun && isRunning && settings) {
523 const bool udpFwd = settings->ntripUdpForwardEnabled()->rawValue().toBool();
524 const QString udpAddr = settings->ntripUdpTargetAddress()->rawValue().toString();
525 const quint16 udpPort =
static_cast<quint16
>(settings->ntripUdpTargetPort()->rawValue().toUInt());
527 if (config != _runningConfig ||
528 udpFwd != _runningUdpForward || udpAddr != _runningUdpAddr || udpPort != _runningUdpPort) {
529 qCDebug(NTRIPManagerLog) <<
"NTRIP settings changed while running, restarting";
536 const bool reconnectPending = _reconnectTimer && _reconnectTimer->isActive();
538 if (shouldRun && !isRunning && !reconnectPending) {
540 }
else if (!shouldRun) {
541 if (_reconnectTimer) {
542 _reconnectTimer->stop();
546 }
else if (reconnectPending) {
547 _reconnectAttempts = 0;
555 if (_sourceTableModel) {
567 if (_sourceTableModel && _sourceTableModel->
count() > 0 && _sourceTableFetchedAtMs > 0) {
568 const qint64 age = QDateTime::currentMSecsSinceEpoch() - _sourceTableFetchedAtMs;
570 qCDebug(NTRIPManagerLog) <<
"Source table cache hit, age:" << age <<
"ms";
582 const QString host = settings->ntripServerHostAddress()->rawValue().toString();
583 const int port = settings->ntripServerPort()->rawValue().toInt();
584 const QString user = settings->ntripUsername()->rawValue().toString();
585 const QString pass = settings->ntripPassword()->rawValue().toString();
586 const bool useTls = settings->ntripUseTls() ? settings->ntripUseTls()->rawValue().toBool() :
false;
588 if (host.isEmpty()) {
589 _mountpointFetchError = tr(
"Host address is empty");
596 if (!_sourceTableModel) {
601 if (_sourceTableFetcher) {
602 _sourceTableFetcher->deleteLater();
603 _sourceTableFetcher =
nullptr;
607 _mountpointFetchError.clear();
615 _sourceTableFetchedAtMs = QDateTime::currentMSecsSinceEpoch();
620 QGeoCoordinate coord = veh->coordinate();
621 if (coord.isValid()) {
629 qCDebug(NTRIPManagerLog) <<
"NTRIP source table fetched:" << _sourceTableModel->
count() <<
"mountpoints";
633 _mountpointFetchError = errorMsg;
637 qCWarning(NTRIPManagerLog) <<
"NTRIP source table fetch error:" << errorMsg;
641 _sourceTableFetcher->deleteLater();
642 _sourceTableFetcher =
nullptr;
645 _sourceTableFetcher->
fetch();
651 if (settings && settings->ntripMountpoint()) {
652 settings->ntripMountpoint()->setRawValue(mountpoint);
653 qCDebug(NTRIPManagerLog) <<
"NTRIP mountpoint selected:" << mountpoint;
Q_APPLICATION_STATIC(NTRIPManager, _ntripManagerInstance)
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Used to group Facts together into an object hierarachy.
Q_INVOKABLE Fact * getFact(const QString &name) const
A Fact is used to hold a single value within the system.
void rawValueChanged(const QVariant &value)
QVariant rawValue() const
Value after translation.
static MultiVehicleManager * instance()
Vehicle * activeVehicle() const
void error(const QString &errorMsg)
void sendNMEA(const QByteArray &nmea)
void RTCMDataUpdate(const QByteArray &message)
static constexpr int kMaxReconnectMs
static constexpr int kMaxReconnectAttempts
void statusMessageChanged()
void messagesReceivedChanged()
void bytesReceivedChanged()
void mountpointFetchStatusChanged()
void casterStatusChanged(CasterStatus status)
QmlObjectListModel * mountpointModel() const
Q_INVOKABLE void fetchMountpoints()
Q_INVOKABLE void selectMountpoint(const QString &mountpoint)
NTRIPManager(QObject *parent=nullptr)
static constexpr int kMinReconnectMs
static constexpr int kSourceTableCacheTtlMs
void connectionStatusChanged()
void mountpointFetchErrorChanged()
static QByteArray makeGGA(const QGeoCoordinate &coord, double altitude_msl)
void mountpointModelChanged()
void error(const QString &errorMsg)
void sourceTableReceived(const QString &table)
void updateDistances(const QGeoCoordinate &from)
void parseSourceTable(const QString &raw)
QmlObjectListModel * mountpoints() const
static QGCPositionManager * instance()
QGeoCoordinate gcsPosition() const
void RTCMDataUpdate(QByteArrayView data)
static SettingsManager * instance()
NTRIPSettings * ntripSettings() const