diff --git a/src/core/positioning/egenioussreceiver.cpp b/src/core/positioning/egenioussreceiver.cpp index 1a0409da40..a61d41ee83 100644 --- a/src/core/positioning/egenioussreceiver.cpp +++ b/src/core/positioning/egenioussreceiver.cpp @@ -23,6 +23,8 @@ #include #include +#include + QLatin1String EgenioussReceiver::identifier = QLatin1String( "egeniouss" ); EgenioussReceiver::EgenioussReceiver( const QString &address, const int port, QObject *parent ) @@ -43,52 +45,36 @@ EgenioussReceiver::~EgenioussReceiver() void EgenioussReceiver::handleConnectDevice() { - QNetworkRequest request( QString( "http://%1:%2/app/start" ).arg( mAddress.toString() ).arg( mPort ) ); + QNetworkRequest request( QString( "http://%1:%2/app/getStatus" ).arg( mAddress.toString() ).arg( mPort ) ); QNetworkReply *reply = QgsNetworkAccessManager::instance()->get( request ); connect( reply, &QNetworkReply::finished, this, [this, reply]() { if ( reply->error() != QNetworkReply::NoError ) { - handleErrorMessage( QString( "HTTP request failed: %1" ).arg( reply->errorString() ) ); + const QString errorMessage = tr( "Is the app running? HTTP request failed: %1" ).arg( reply->errorString() ); + qInfo() << QStringLiteral( "EgenioussReceiver: %1" ).arg( errorMessage ); + handleErrorMessage( errorMessage ); + reply->deleteLater(); return; } const QJsonObject jsonObject = QJsonDocument::fromJson( reply->readAll() ).object(); - const bool success = jsonObject.value( "success" ).toBool(); + const bool isRunning = jsonObject.value( "estimation_status" ).toBool(); - if ( success ) - { - mTcpSocket->connectToHost( mAddress, mPort + 1, QTcpSocket::ReadOnly ); - } - else + if ( !isRunning ) { - handleErrorMessage( tr( "Failed to start egeniouss server." ) ); + const QString errorMessage = tr( "Egeniouss service is not ready." ); + qInfo() << QStringLiteral( "EgenioussReceiver: %1" ).arg( errorMessage ); + handleErrorMessage( errorMessage ); + reply->deleteLater(); } + + mTcpSocket->connectToHost( mAddress, mPort + 1, QTcpSocket::ReadOnly ); reply->deleteLater(); } ); } void EgenioussReceiver::handleDisconnectDevice() { - QNetworkRequest request( QString( "http://%1:%2/app/stop" ).arg( mAddress.toString() ).arg( mPort ) ); - QNetworkReply *reply = QgsNetworkAccessManager::instance()->get( request ); - connect( reply, &QNetworkReply::finished, this, [this, reply]() { - if ( reply->error() != QNetworkReply::NoError ) - { - handleErrorMessage( QString( "HTTP request failed: %1" ).arg( reply->errorString() ) ); - return; - } - const QJsonObject jsonObject = QJsonDocument::fromJson( reply->readAll() ).object(); - const bool success = jsonObject.value( "success" ).toBool(); - - if ( success ) - { - mTcpSocket->disconnectFromHost(); - } - else - { - handleErrorMessage( tr( "Failed to stop egeniouss server." ) ); - } - reply->deleteLater(); - } ); + mTcpSocket->disconnectFromHost(); } void EgenioussReceiver::handleErrorMessage( const QString &errorMessage ) @@ -106,7 +92,29 @@ GnssPositionDetails EgenioussReceiver::details() const return detailsList; } - detailsList.append( "q", mPayload.value( "q" ).toDouble() ); + const qint64 timeMs = static_cast( mPayload.value( "time" ).toDouble() * 1000.0 ); + const QDateTime timestamp = QDateTime::fromMSecsSinceEpoch( timeMs, QTimeZone( QTimeZone::Initialization::UTC ) ); + detailsList.append( tr( "Last fix" ), timestamp.toLocalTime().toString( QStringLiteral( "hh:mm:ss.zzz" ) ) ); + + const double heading = mPayload.value( "kappa" ).toDouble( std::numeric_limits::quiet_NaN() ); + if ( !std::isnan( heading ) ) + { + detailsList.append( tr( "Heading" ), QStringLiteral( "%1°" ).arg( heading, 0, 'f', 1 ) ); + } + + const double roll = mPayload.value( "omega" ).toDouble( std::numeric_limits::quiet_NaN() ); + const double pitch = mPayload.value( "phi" ).toDouble( std::numeric_limits::quiet_NaN() ); + if ( !std::isnan( roll ) && !std::isnan( pitch ) ) + { + detailsList.append( tr( "Roll" ), QStringLiteral( "%1°" ).arg( roll, 0, 'f', 1 ) ); + detailsList.append( tr( "Pitch" ), QStringLiteral( "%1°" ).arg( pitch, 0, 'f', 1 ) ); + } + + const double sigKappa = mPayload.value( "sig_kappa" ).toDouble( std::numeric_limits::quiet_NaN() ); + if ( !std::isnan( sigKappa ) ) + { + detailsList.append( tr( "Heading sigma" ), QStringLiteral( "%1°" ).arg( sigKappa, 0, 'f', 2 ) ); + } return detailsList; } @@ -145,9 +153,20 @@ void EgenioussReceiver::onReadyRead() return; } mPayload = jsonDoc.object(); + qDebug() << "EgenioussReceiver payload:" << QJsonDocument( mPayload ).toJson( QJsonDocument::Compact ); const double latitude = mPayload.value( "lat" ).toDouble() == 0 ? std::numeric_limits::quiet_NaN() : mPayload.value( "lat" ).toDouble(); const double longitude = mPayload.value( "lon" ).toDouble() == 0 ? std::numeric_limits::quiet_NaN() : mPayload.value( "lon" ).toDouble(); const double elevation = mPayload.value( "alt" ).toDouble() == 0 ? std::numeric_limits::quiet_NaN() : mPayload.value( "alt" ).toDouble(); + // omega=roll, phi=pitch, kappa=yaw/heading — Egeniouss naming convention + const double roll = mPayload.value( "omega" ).toDouble( std::numeric_limits::quiet_NaN() ); + const double pitch = mPayload.value( "phi" ).toDouble( std::numeric_limits::quiet_NaN() ); + const double heading = mPayload.value( "kappa" ).toDouble( std::numeric_limits::quiet_NaN() ); + // 1-sigma position accuracy in metres + const double sigLat = mPayload.value( "sig_lat" ).toDouble( std::numeric_limits::quiet_NaN() ); + const double sigLon = mPayload.value( "sig_lon" ).toDouble( std::numeric_limits::quiet_NaN() ); + const double sigAlt = mPayload.value( "sig_alt" ).toDouble( std::numeric_limits::quiet_NaN() ); + // horizontal accuracy: RSS of sig_lat and sig_lon + const double hacc = ( std::isnan( sigLat ) || std::isnan( sigLon ) ) ? std::numeric_limits::quiet_NaN() : std::sqrt( sigLat * sigLat + sigLon * sigLon ); mLastGnssPositionInformation = GnssPositionInformation( latitude, longitude, @@ -158,12 +177,24 @@ void EgenioussReceiver::onReadyRead() 0, 0, 0, + hacc, + sigAlt, + QDateTime::fromMSecsSinceEpoch( static_cast( mPayload.value( "time" ).toDouble() * 1000.0 ), QTimeZone( QTimeZone::Initialization::UTC ) ), + QChar(), + 0, + 1, + 0, + QChar(), + QList(), + false, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - QDateTime::fromMSecsSinceEpoch( mPayload.value( "time" ).toDouble() / 1e6, QTimeZone( QTimeZone::Initialization::UTC ) ), - QChar(), 0, - 1 ); + QString(), + true, + roll, + pitch, + heading ); emit lastGnssPositionInformationChanged( mLastGnssPositionInformation ); } diff --git a/src/core/positioning/positioningsource.cpp b/src/core/positioning/positioningsource.cpp index e4bc94884f..30a2166a05 100644 --- a/src/core/positioning/positioningsource.cpp +++ b/src/core/positioning/positioningsource.cpp @@ -396,7 +396,7 @@ void PositioningSource::lastGnssPositionInformationChanged( const GnssPositionIn lastGnssPositionInformation.imuPitch(), lastGnssPositionInformation.imuHeading(), lastGnssPositionInformation.imuSteering(), - mOrientation ); + lastGnssPositionInformation.imuHeadingValid() ? lastGnssPositionInformation.imuHeading() : mOrientation ); mPositionInformation = positionInformation; if ( !mBackgroundMode )