diff --git a/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp b/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp index 4786cc5324d..e56448ed405 100644 --- a/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp @@ -14,6 +14,8 @@ #include "ElevationMapProvider.h" #include "TerrainTileCopernicus.h" +#include "TerrainTileArduPilot.h" +#include "QGCZip.h" #include #include @@ -65,3 +67,116 @@ QByteArray CopernicusElevationProvider::serialize(const QByteArray &image) const { return TerrainTileCopernicus::serializeFromData(image); } + +/*===========================================================================*/ + +int ArduPilotTerrainElevationProvider::long2tileX(double lon, int z) const +{ + Q_UNUSED(z) + return static_cast(floor((lon + 180.0) / TerrainTileArduPilot::kTileSizeDegrees)); +} + +int ArduPilotTerrainElevationProvider::lat2tileY(double lat, int z) const +{ + Q_UNUSED(z) + return static_cast(floor((lat + 90.0) / TerrainTileArduPilot::kTileSizeDegrees)); +} + +QString ArduPilotTerrainElevationProvider::_getURL(int x, int y, int zoom) const +{ + Q_UNUSED(zoom) + + const int xForUrl = (static_cast(x) * TerrainTileArduPilot::kTileSizeDegrees) - 180.0; + const int yForUrl = (static_cast(y) * TerrainTileArduPilot::kTileSizeDegrees) - 90.0; + + if ((xForUrl < -180) || (xForUrl > 180) || (yForUrl < -180) || (yForUrl > 180)) { + qCWarning(MapProviderLog) << "Invalid x or y values for URL generation:" << x << y; + return QString(); + } + + QString formattedYLat; + if (yForUrl >= 0) { + formattedYLat = QString("N%1").arg(QString::number(yForUrl).rightJustified(2, '0')); + } else { + formattedYLat = QString("S%1").arg(QString::number(-yForUrl).rightJustified(2, '0')); + } + + QString formattedXLong; + if (xForUrl >= 0) { + formattedXLong = QString("E%1").arg(QString::number(xForUrl).rightJustified(3, '0')); + } else { + formattedXLong = QString("W%1").arg(QString::number(-xForUrl).rightJustified(3, '0')); + } + + const QString url = _mapUrl.arg(formattedYLat, formattedXLong); + return url; +} + +QGCTileSet ArduPilotTerrainElevationProvider::getTileCount(int zoom, double topleftLon, + double topleftLat, double bottomRightLon, + double bottomRightLat) const +{ + QGCTileSet set; + set.tileX0 = long2tileX(topleftLon, zoom); + set.tileY0 = lat2tileY(bottomRightLat, zoom); + set.tileX1 = long2tileX(bottomRightLon, zoom); + set.tileY1 = lat2tileY(topleftLat, zoom); + + set.tileCount = (static_cast(set.tileX1) - + static_cast(set.tileX0) + 1) * + (static_cast(set.tileY1) - + static_cast(set.tileY0) + 1); + + set.tileSize = getAverageSize() * set.tileCount; + + return set; +} + +QByteArray ArduPilotTerrainElevationProvider::serialize(const QByteArray &image) const +{ + QTemporaryFile tempFile; + tempFile.setFileTemplate(QDir::tempPath() + "/XXXXXX.zip"); + + if (!tempFile.open()) { + qCDebug(MapProviderLog) << "Could not create temporary file for zip data."; + return QByteArray(); + } + + if (tempFile.write(image) != image.size()) { + qCDebug(MapProviderLog) << "Incorrect number of bytes written."; + } + tempFile.close(); + + // QTemporaryDir + const QString outputDirectoryPath = QDir::tempPath() + "/QGC/Location/Elevation/SRTM1"; + QDir outputDirectory(outputDirectoryPath); + if (!outputDirectory.exists()) { + if (!outputDirectory.mkpath(outputDirectoryPath)) { + return QByteArray(); + } + } + + if (!QGCZip::unzipFile(tempFile.fileName(), outputDirectoryPath)) { + qCDebug(MapProviderLog) << "Unzipping failed!"; + return QByteArray(); + } + + const QStringList files = outputDirectory.entryList(QDir::Files); + if (files.isEmpty()) { + qCDebug(MapProviderLog) << "No files found in the unzipped directory!"; + return QByteArray(); + } + + const QString filename = files.constFirst(); + const QString unzippedFilePath = outputDirectoryPath + "/" + filename; + QFile file(unzippedFilePath); + if (!file.open(QIODevice::ReadOnly)) { + qCDebug(MapProviderLog) << "Could not open unzipped file for reading:" << unzippedFilePath; + return QByteArray(); + } + + const QByteArray result = file.readAll(); + file.close(); + + return TerrainTileArduPilot::serializeFromData(filename, result); +} diff --git a/src/QtLocationPlugin/Providers/ElevationMapProvider.h b/src/QtLocationPlugin/Providers/ElevationMapProvider.h index 4d83bde5bd3..6f621214a0a 100644 --- a/src/QtLocationPlugin/Providers/ElevationMapProvider.h +++ b/src/QtLocationPlugin/Providers/ElevationMapProvider.h @@ -28,6 +28,8 @@ class ElevationProvider : public MapProvider virtual QByteArray serialize(const QByteArray &image) const = 0; }; +/*===========================================================================*/ + /// https://spacedata.copernicus.eu/collections/copernicus-digital-elevation-model class CopernicusElevationProvider : public ElevationProvider { @@ -59,3 +61,36 @@ class CopernicusElevationProvider : public ElevationProvider const QString _mapUrl = QString(kProviderURL) + QStringLiteral("/api/v1/carpet?points=%1,%2,%3,%4"); }; + +/*===========================================================================*/ + +class ArduPilotTerrainElevationProvider : public ElevationProvider +{ +public: + ArduPilotTerrainElevationProvider() + : ElevationProvider( + kProviderKey, + kProviderURL, + QStringLiteral("hgt"), + kAvgElevSize, + QGeoMapType::TerrainMap) {} + + int long2tileX(double lon, int z) const final; + int lat2tileY(double lat, int z) const final; + + QGCTileSet getTileCount(int zoom, double topleftLon, + double topleftLat, double bottomRightLon, + double bottomRightLat) const final; + + QByteArray serialize(const QByteArray &image) const final; + + static constexpr const char *kProviderKey = "ArduPilot SRTM1"; + static constexpr const char *kProviderNotice = "© ArduPilot.org"; + static constexpr const char *kProviderURL = "https://terrain.ardupilot.org/SRTM1"; + static constexpr quint32 kAvgElevSize = 50000; + +private: + QString _getURL(int x, int y, int zoom) const final; + + const QString _mapUrl = QString(kProviderURL) + QStringLiteral("/%1%2.hgt.zip"); +}; diff --git a/src/QtLocationPlugin/QGCCachedTileSet.cpp b/src/QtLocationPlugin/QGCCachedTileSet.cpp index a4bba4b09a1..1e3d548e858 100644 --- a/src/QtLocationPlugin/QGCCachedTileSet.cpp +++ b/src/QtLocationPlugin/QGCCachedTileSet.cpp @@ -216,7 +216,7 @@ void QGCCachedTileSet::_networkReplyFinished() return; } - const QString type = UrlFactory::tileHashToType(hash); + const QString type = UrlFactory::tileHashToType(hash); // TODO: Type is null for elevation const SharedMapProvider mapProvider = UrlFactory::getMapProviderFromProviderType(type); Q_CHECK_PTR(mapProvider); diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index 5e14d969838..eca646879c1 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -72,7 +72,8 @@ const QList UrlFactory::_providers = { std::make_shared(), - std::make_shared() + std::make_shared(), + std::make_shared() }; QString UrlFactory::getImageFormat(int qtMapId, QByteArrayView image) diff --git a/src/Terrain/CMakeLists.txt b/src/Terrain/CMakeLists.txt index 8044bd8e85e..435ac0d3319 100644 --- a/src/Terrain/CMakeLists.txt +++ b/src/Terrain/CMakeLists.txt @@ -1,8 +1,12 @@ find_package(Qt6 REQUIRED COMPONENTS Core Location Network Positioning) qt_add_library(Terrain STATIC + Providers/TerrainQueryArduPilot.cc + Providers/TerrainQueryArduPilot.h Providers/TerrainQueryCopernicus.cc Providers/TerrainQueryCopernicus.h + Providers/TerrainTileArduPilot.cc + Providers/TerrainTileArduPilot.h Providers/TerrainTileCopernicus.cc Providers/TerrainTileCopernicus.h TerrainQuery.cc @@ -18,6 +22,7 @@ qt_add_library(Terrain STATIC target_link_libraries(Terrain PRIVATE Qt6::LocationPrivate + Compression QGCLocation Utilities PUBLIC diff --git a/src/Terrain/Providers/TerrainQueryArduPilot.cc b/src/Terrain/Providers/TerrainQueryArduPilot.cc new file mode 100644 index 00000000000..78bfc66afd6 --- /dev/null +++ b/src/Terrain/Providers/TerrainQueryArduPilot.cc @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainQueryArduPilot.h" +#include "TerrainTileArduPilot.h" +#include "ElevationMapProvider.h" +#include "QGCFileDownload.h" +#include "QGCLoggingCategory.h" +#include "QGCMapUrlEngine.h" +#include "QGCZip.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainQueryArduPilotLog, "qgc.terrain.terrainqueryardupilot") + +TerrainQueryArduPilot::TerrainQueryArduPilot(QObject *parent) + : TerrainOnlineQuery(parent) +{ + // qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << this; +} + +TerrainQueryArduPilot::~TerrainQueryArduPilot() +{ + // qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << this; +} + +void TerrainQueryArduPilot::requestCoordinateHeights(const QList &coordinates) +{ + if (coordinates.isEmpty()) { + return; + } + + _queryMode = TerrainQuery::QueryModeCoordinates; + + QSet urls; + urls.reserve(coordinates.size()); + + const SharedMapProvider provider = UrlFactory::getMapProviderFromProviderType(QString(ArduPilotTerrainElevationProvider::kProviderKey)); + for (const QGeoCoordinate &coord : coordinates) { + if (!coord.isValid()) { + continue; + } + const int x = provider->long2tileX(coord.longitude(), 1); + const int y = provider->lat2tileY(coord.latitude(), 1); + const QUrl url = provider->getTileURL(x, y, 1); + (void) urls.insert(url); + } + + const QList urlList = urls.values(); + for (const QUrl &url : urlList) { + _sendQuery(url); + } +} + +void TerrainQueryArduPilot::_sendQuery(const QUrl &url) +{ + qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << url; + + QGCFileDownload *const download = new QGCFileDownload(this); + (void) connect(download, &QGCFileDownload::downloadComplete, this, &TerrainQueryArduPilot::_parseZipFile); + (void) connect(download, &QGCFileDownload::downloadComplete, download, &QGCFileDownload::deleteLater); + if (!download->download(url.toString())) { + qCWarning(TerrainQueryArduPilotLog) << "Failed To Download File"; + _requestFailed(); + } +} + +void TerrainQueryArduPilot::_parseZipFile(const QString &remoteFile, const QString &localFile, const QString &errorMsg) +{ + if (!errorMsg.isEmpty()) { + qCWarning(TerrainQueryArduPilotLog) << "Error During Download:" << errorMsg; + _requestFailed(); + return; + } + + // TODO: verify .endsWith(".hgt") + + const QString outputDirectoryPath = QDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation)).path() + "/QGC/SRTM1"; + if (!QGCZip::unzipFile(localFile, outputDirectoryPath)) { + qCWarning(TerrainQueryArduPilotLog) << "Failed To Unzip File" << localFile; + _requestFailed(); + return; + } + + QFile file(outputDirectoryPath + "/" + localFile); + if (!file.open(QIODevice::ReadOnly)) { + qCWarning(TerrainQueryArduPilotLog) << "Failed To Unzip File" << localFile; + _requestFailed(); + return; + } + + const QByteArray hgtData = file.readAll(); + file.close(); + + qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << "success"; + switch (_queryMode) { + case TerrainQuery::QueryModeCoordinates: + _parseCoordinateData(localFile, hgtData); + break; + default: + qCWarning(TerrainQueryArduPilotLog) << Q_FUNC_INFO << "Query Mode Not Supported"; + break; + } +} + +void TerrainQueryArduPilot::_parseCoordinateData(const QString &localFile, const QByteArray &hgtData) +{ + const QList coordinates = TerrainTileArduPilot::parseCoordinateData(localFile, hgtData); + + QList heights; + heights.reserve(coordinates.size()); + for (const QGeoCoordinate &coord: coordinates) { + heights.append(coord.altitude()); + } + + emit coordinateHeightsReceived(!heights.isEmpty(), heights); +} diff --git a/src/Terrain/Providers/TerrainQueryArduPilot.h b/src/Terrain/Providers/TerrainQueryArduPilot.h new file mode 100644 index 00000000000..48b1508b17c --- /dev/null +++ b/src/Terrain/Providers/TerrainQueryArduPilot.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include "TerrainQueryInterface.h" + +Q_DECLARE_LOGGING_CATEGORY(TerrainQueryArduPilotLog) + +class QGeoCoordinate; + +class TerrainQueryArduPilot : public TerrainOnlineQuery +{ + Q_OBJECT + +public: + explicit TerrainQueryArduPilot(QObject *parent = nullptr); + ~TerrainQueryArduPilot(); + + void requestCoordinateHeights(const QList &coordinates) final; + +private: + void _sendQuery(const QUrl &path); + void _parseZipFile(const QString &remoteFile, const QString &localFile, const QString &errorMsg); + void _parseCoordinateData(const QString &localFile, const QByteArray &hgtData); +}; diff --git a/src/Terrain/Providers/TerrainTileArduPilot.cc b/src/Terrain/Providers/TerrainTileArduPilot.cc new file mode 100644 index 00000000000..93d93e7593a --- /dev/null +++ b/src/Terrain/Providers/TerrainTileArduPilot.cc @@ -0,0 +1,265 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainTileArduPilot.h" +#include "QGCLoggingCategory.h" + +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainTileArduPilotLog, "qgc.terrain.terraintileardupilot") + +TerrainTileArduPilot::TerrainTileArduPilot(const QByteArray &byteArray) + : TerrainTile(byteArray) +{ + // Additional initialization if necessary + // qCDebug(TerrainTileArduPilotLog) << Q_FUNC_INFO << this; +} + +/// Parses the filename to extract the southwest coordinate +QGeoCoordinate TerrainTileArduPilot::_parseFileName(const QString &name) +{ + static const QRegularExpression regex("^([NS])(\\d{2})([EW])(\\d{3})\\.hgt(?:\\.zip)?$"); + const QRegularExpressionMatch match = regex.match(name); + + if (!match.hasMatch()) { + qCWarning(TerrainTileArduPilotLog) << "Filename does not match expected format:" << name; + return QGeoCoordinate(); + } + + const QString hemisphereLat = match.captured(1); + const QString degreesLatStr = match.captured(2); + const QString hemisphereLon = match.captured(3); + const QString degreesLonStr = match.captured(4); + + bool okLat = false, okLon = false; + const int degreesLat = degreesLatStr.toInt(&okLat); + const int degreesLon = degreesLonStr.toInt(&okLon); + + if (!okLat || !okLon) { + qCWarning(TerrainTileArduPilotLog) << "Failed to convert degree strings to integers:" << degreesLatStr << degreesLonStr; + return QGeoCoordinate(); + } + + // Validate latitude and longitude ranges + if ((degreesLat < 0) || (degreesLat > 60)) { // Adjusted based on SRTM coverage + qCWarning(TerrainTileArduPilotLog) << "Latitude out of range:" << degreesLat; + return QGeoCoordinate(); + } + + if ((degreesLon < 0) || (degreesLon > 180)) { + qCWarning(TerrainTileArduPilotLog) << "Longitude out of range:" << degreesLon; + return QGeoCoordinate(); + } + + double latitude = static_cast(degreesLat); + if (hemisphereLat == "S") { + latitude = -latitude; + } + + double longitude = static_cast(degreesLon); + if (hemisphereLon == "W") { + longitude = -longitude; + } + + QGeoCoordinate coord(latitude, longitude); + if (!coord.isValid()) { + qCWarning(TerrainTileArduPilotLog) << "Parsed coordinates are invalid:" << coord; + } + + return coord; +} + +/// Parses raw HGT data into a vector of QGeoCoordinate with elevation as altitude +QVector TerrainTileArduPilot::parseCoordinateData(const QString &name, const QByteArray &coordinateData) +{ + QVector coordinates; + coordinates.reserve(kTotalPoints); + + // Parse the file name to get tile coordinates + const QGeoCoordinate tileCoord = _parseFileName(name); + if (!tileCoord.isValid()) { + qCWarning(TerrainTileArduPilotLog) << "Invalid tile coordinates from filename:" << name; + return coordinates; + } + + // Tile spans 1 degree, starting from tileCoord (southwest corner) + const double tileLat = tileCoord.latitude(); + const double tileLon = tileCoord.longitude(); + + // Number of points per side (SRTM1) + const int dimension = kTileDimension; + + // Each elevation value is a 16-bit signed integer (big endian) + QDataStream stream(coordinateData); + stream.setByteOrder(QDataStream::BigEndian); + stream.setFloatingPointPrecision(QDataStream::DoublePrecision); + + for (int row = 0; row < dimension; ++row) { + for (int col = 0; col < dimension; ++col) { + qint16 elevationRaw; + stream >> elevationRaw; + + if (stream.status() != QDataStream::Ok) { + qCWarning(TerrainTileArduPilotLog) << "Error reading elevation data at row" << row << "col" << col << "in file:" << name; + // Append a coordinate with NaN elevation to indicate missing data + coordinates.append(QGeoCoordinate(std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN())); + continue; // Continue processing remaining data + } + + // Calculate the latitude and longitude for this point + // Start from tileLat +1.0 (north edge) down to tileLat (south edge) + const double currentLat = tileLat + 1.0 - (row * kTileValueSpacingDegrees); + const double currentLon = tileLon + (col * kTileValueSpacingDegrees); + + // Treat invalid elevations (-32768) as NaN + const double elevation = (elevationRaw == -32768) ? std::numeric_limits::quiet_NaN() : static_cast(elevationRaw); + + // Create QGeoCoordinate with elevation as altitude + QGeoCoordinate coord(currentLat, currentLon, elevation); // altitude in meters + coordinates.append(coord); + } + } + + return coordinates; +} + +/// Serializes raw HGT data into a QByteArray compatible with TerrainTile's expectations +QByteArray TerrainTileArduPilot::serializeFromData(const QString &name, const QByteArray &hgtData) +{ + // Define constants + constexpr int kBytesPerElevation = 2; + + // Parse the file name to get base coordinates + const QGeoCoordinate baseCoord = _parseFileName(name); + if (!baseCoord.isValid()) { + qCWarning(TerrainTileArduPilotLog) << "Invalid HGT file:" << name; + return QByteArray(); + } + + // Calculate total points and validate + const int totalPoints = hgtData.size() / kBytesPerElevation; + if (totalPoints != kTotalPoints) { + qCWarning(TerrainTileArduPilotLog) << "HGT file data size does not match expected SRTM1 grid size:" << name; + return QByteArray(); + } + + const int gridSize = kTileDimension; + + // Initialize TileInfo_t + TerrainTile::TileInfo_t tileInfo{}; + tileInfo.minElevation = std::numeric_limits::max(); + tileInfo.maxElevation = std::numeric_limits::min(); + tileInfo.avgElevation = 0.0; + + tileInfo.gridSizeLat = static_cast(gridSize); + tileInfo.gridSizeLon = static_cast(gridSize); + + tileInfo.swLat = baseCoord.latitude(); + tileInfo.swLon = baseCoord.longitude(); + tileInfo.neLat = tileInfo.swLat + kTileSizeDegrees; + tileInfo.neLon = tileInfo.swLon + kTileSizeDegrees; + + QDataStream stream(hgtData); + stream.setByteOrder(QDataStream::BigEndian); + stream.setFloatingPointPrecision(QDataStream::DoublePrecision); + + QVector elevationGrid; + elevationGrid.reserve(totalPoints); + + int validPoints = 0; + double elevationSum = 0.0; + + for (int i = 0; i < totalPoints; ++i) { + qint16 elevationRaw; + stream >> elevationRaw; + + if (stream.status() != QDataStream::Ok) { + qCWarning(TerrainTileArduPilotLog) << "Error reading elevation data at point" << i << "in file:" << name; + return QByteArray(); + } + + elevationGrid.append(elevationRaw); + + if (elevationRaw != -32768) { + if (elevationRaw < tileInfo.minElevation) { + tileInfo.minElevation = elevationRaw; + } + if (elevationRaw > tileInfo.maxElevation) { + tileInfo.maxElevation = elevationRaw; + } + + elevationSum += static_cast(elevationRaw); + validPoints++; + } + } + + if (validPoints > 0) { + tileInfo.avgElevation = elevationSum / static_cast(validPoints); + } else { + tileInfo.avgElevation = std::numeric_limits::quiet_NaN(); + tileInfo.minElevation = 0; + tileInfo.maxElevation = 0; + } + + qCDebug(TerrainTileArduPilotLog) << "Serialize: TileInfo: SW Lat:" << tileInfo.swLat << "SW Lon:" << tileInfo.swLon; + qCDebug(TerrainTileArduPilotLog) << "Serialize: TileInfo: NE Lat:" << tileInfo.neLat << "NE Lon:" << tileInfo.neLon; + qCDebug(TerrainTileArduPilotLog) << "Min Elevation:" << tileInfo.minElevation << "meters"; + qCDebug(TerrainTileArduPilotLog) << "Max Elevation:" << tileInfo.maxElevation << "meters"; + qCDebug(TerrainTileArduPilotLog) << "Average Elevation:" << tileInfo.avgElevation << "meters"; + qCDebug(TerrainTileArduPilotLog) << "Grid Size (Lat x Lon):" << tileInfo.gridSizeLat << "x" << tileInfo.gridSizeLon; + + // Calculate the number of bytes for header and data + constexpr int cTileNumHeaderBytes = sizeof(TerrainTile::TileInfo_t); + const int cTileNumDataBytes = sizeof(int16_t) * tileInfo.gridSizeLat * tileInfo.gridSizeLon; + + // Ensure that elevationGrid has exactly totalPoints + if (elevationGrid.size() != static_cast(tileInfo.gridSizeLat) * tileInfo.gridSizeLon) { + qCWarning(TerrainTileArduPilotLog) << "Elevation grid size mismatch."; + return QByteArray(); + } + + // Create a QByteArray to hold TileInfo_t and elevation data + QByteArray result; + result.resize(cTileNumHeaderBytes + cTileNumDataBytes); + + // Serialize TileInfo_t fields into the QByteArray + QDataStream outStream(&result, QIODevice::WriteOnly); + outStream.setByteOrder(QDataStream::BigEndian); + outStream.setFloatingPointPrecision(QDataStream::DoublePrecision); + + outStream << tileInfo.swLat + << tileInfo.swLon + << tileInfo.neLat + << tileInfo.neLon + << tileInfo.minElevation + << tileInfo.maxElevation + << tileInfo.avgElevation + << tileInfo.gridSizeLat + << tileInfo.gridSizeLon; + + // Serialize elevation data efficiently using raw data + if (!elevationGrid.isEmpty()) { + (void) outStream.writeRawData(reinterpret_cast(elevationGrid.constData()), elevationGrid.size() * sizeof(int16_t)); + } + + // Check if serialization was successful + if (outStream.status() != QDataStream::Ok) { + qCWarning(TerrainTileArduPilotLog) << "Error during serialization of tile data:" << name; + return QByteArray(); + } + + qCDebug(TerrainTileArduPilotLog) << "Serialization complete for tile:" << name; + + return result; +} diff --git a/src/Terrain/Providers/TerrainTileArduPilot.h b/src/Terrain/Providers/TerrainTileArduPilot.h new file mode 100644 index 00000000000..373fd0bbec2 --- /dev/null +++ b/src/Terrain/Providers/TerrainTileArduPilot.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +#include "TerrainTile.h" + +Q_DECLARE_LOGGING_CATEGORY(TerrainTileArduPilotLog) + +class TerrainTileArduPilot : public TerrainTile +{ + friend class TerrainTileTest; + +public: + /// Constructor from serialized elevation data (either from file or web) + /// @param byteArray Binary data containing TileInfo_t followed by elevation data + explicit TerrainTileArduPilot(const QByteArray &byteArray); + ~TerrainTileArduPilot() override = default; + + /// Parses elevation data from a .hgt or .hgt.zip file + /// @param name Filename of the HGT file + /// @param coordinateData Raw elevation data + /// @return Vector of QGeoCoordinate with elevation as altitude + static QVector parseCoordinateData(const QString &name, const QByteArray &coordinateData); + + /// Serializes elevation data into a QByteArray compatible with TerrainTile + /// @param name Filename of the HGT file + /// @param hgtData Raw elevation data + /// @return Serialized QByteArray containing TileInfo_t and elevation data + static QByteArray serializeFromData(const QString &name, const QByteArray &hgtData); + + /// SRTM-1 (1 arc-second resolution, 30 meters): 3601 × 3601 grid of elevations (12,967,201 data points). 0-60 degrees N/S. + static constexpr double kTileSizeDegrees = 1.0; ///< Each terrain tile represents a square area of 1 degree in lat/lon + static constexpr double kTileValueSpacingDegrees = (1.0 / 3600); ///< 1 Arc-Second spacing of elevation values + static constexpr double kTileValueSpacingMeters = 30.0; ///< SRTM1 Resolution + static constexpr int kTileDimension = 3601; ///< SRTM1 Total Points per side + static constexpr int kTotalPoints = kTileDimension * kTileDimension; ///< SRTM1 Total Points + +private: + /// Parses the filename to extract the southwest coordinate + /// @param name Filename of the HGT file + /// @return QGeoCoordinate representing the southwest corner + static QGeoCoordinate _parseFileName(const QString &name); +}; diff --git a/src/Terrain/TerrainTile.cc b/src/Terrain/TerrainTile.cc index eb26e55a4ec..47e373c05f4 100644 --- a/src/Terrain/TerrainTile.cc +++ b/src/Terrain/TerrainTile.cc @@ -29,7 +29,7 @@ TerrainTile::TerrainTile(const QByteArray &byteArray) } const int cTileDataBytes = static_cast(sizeof(int16_t)) * _tileInfo.gridSizeLat * _tileInfo.gridSizeLon; - if (cTileBytesAvailable < cTileHeaderBytes + cTileDataBytes) { + if (cTileBytesAvailable < (cTileHeaderBytes + cTileDataBytes)) { qCWarning(TerrainTileLog) << "Terrain tile binary data too small for tile data"; return; } diff --git a/src/Utilities/Compression/CMakeLists.txt b/src/Utilities/Compression/CMakeLists.txt index e829fea383e..b035d83f725 100644 --- a/src/Utilities/Compression/CMakeLists.txt +++ b/src/Utilities/Compression/CMakeLists.txt @@ -28,14 +28,14 @@ set(MINIMUM_ZLIB_VERSION 1.3) if(NOT QGC_BUILD_DEPENDENCIES) set(ZLIB_USE_STATIC_LIBS ON) find_package(ZLIB ${MINIMUM_ZLIB_VERSION}) - if(ZLIB_FOUND) + if(TARGET ZLIB::ZLIB) message(STATUS "Found ZLIB ${ZLIB_VERSION_STRING}") target_link_libraries(Compression PRIVATE ZLIB::ZLIB) else() find_package(PkgConfig) if(PkgConfig_FOUND) pkg_check_modules(ZLIB IMPORTED_TARGET ZLIB>=${MINIMUM_ZLIB_VERSION}) - if(ZLIB_FOUND) + if(TARGET PkgConfig::ZLIB) message(STATUS "Found ZLIB ${ZLIB_VERSION}") target_link_libraries(Joystick PRIVATE PkgConfig::ZLIB) endif() @@ -43,7 +43,7 @@ if(NOT QGC_BUILD_DEPENDENCIES) endif() endif() -if(NOT ZLIB_FOUND) +if(NOT TARGET ZLIB::ZLIB AND NOT TARGET PkgConfig::ZLIB) set(ZLIB_BUILD_EXAMPLES OFF CACHE INTERNAL "") set(SKIP_INSTALL_FILES ON CACHE INTERNAL "") set(SKIP_INSTALL_LIBRARIES ON CACHE INTERNAL "") diff --git a/src/Vehicle/TerrainProtocolHandler.cc b/src/Vehicle/TerrainProtocolHandler.cc index 05f7aa0414f..a3fa82b953f 100644 --- a/src/Vehicle/TerrainProtocolHandler.cc +++ b/src/Vehicle/TerrainProtocolHandler.cc @@ -69,7 +69,7 @@ void TerrainProtocolHandler::_handleTerrainReport(const mavlink_message_t &messa QList altitudes; QList coordinates; QGeoCoordinate coord(static_cast(terrainReport.lat) / 1e7, static_cast(terrainReport.lon) / 1e7); - (void) coordinates.append(coord); + coordinates.append(coord); const bool altAvailable = TerrainAtCoordinateQuery::getAltitudesForCoordinates(coordinates, altitudes, error); const QString vehicleAlt = terrainReport.spacing ? QStringLiteral("%1").arg(terrainReport.terrain_height) : QStringLiteral("n/a"); QString qgcAlt; @@ -80,6 +80,7 @@ void TerrainProtocolHandler::_handleTerrainReport(const mavlink_message_t &messa } else { qgcAlt = QStringLiteral("N/A"); } + qCDebug(TerrainProtocolHandlerLog) << "TERRAIN_REPORT" << coord << QStringLiteral("Vehicle(%1) QGC(%2)").arg(vehicleAlt).arg(qgcAlt); } } diff --git a/test/Terrain/CMakeLists.txt b/test/Terrain/CMakeLists.txt index 264b3b353ce..a73c8ce8357 100644 --- a/test/Terrain/CMakeLists.txt +++ b/test/Terrain/CMakeLists.txt @@ -18,3 +18,10 @@ target_link_libraries(TerrainTest ) target_include_directories(TerrainTest PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +qt_add_resources(TerrainTest "TerrainTest" + PREFIX "/" + FILES + # N00E006.hgt + N00E006.hgt.zip +) diff --git a/test/Terrain/N00E006.hgt.zip b/test/Terrain/N00E006.hgt.zip new file mode 100644 index 00000000000..ebf0680b762 Binary files /dev/null and b/test/Terrain/N00E006.hgt.zip differ diff --git a/test/Terrain/TerrainTileTest.cc b/test/Terrain/TerrainTileTest.cc index 0ad0f690d97..118cf8d5b07 100644 --- a/test/Terrain/TerrainTileTest.cc +++ b/test/Terrain/TerrainTileTest.cc @@ -8,6 +8,61 @@ ****************************************************************************/ #include "TerrainTileTest.h" +#include "TerrainTileArduPilot.h" #include "TerrainTile.h" #include + +void TerrainTileTest::_testArduPilotParseFileName() +{ + // Valid filename + QGeoCoordinate coord = TerrainTileArduPilot::_parseFileName("S77W029.hgt"); + QCOMPARE(coord.latitude(), -77.0); + QCOMPARE(coord.longitude(), -29.0); + + // Invalid filename + coord = TerrainTileArduPilot::_parseFileName("InvalidName.hgt"); + QVERIFY(!coord.isValid()); + + // Edge cases + coord = TerrainTileArduPilot::_parseFileName("N00E000.hgt"); + QCOMPARE(coord.latitude(), 0.0); + QCOMPARE(coord.longitude(), 0.0); + + coord = TerrainTileArduPilot::_parseFileName("N84E180.hgt"); + QCOMPARE(coord.latitude(), 84.0); + QCOMPARE(coord.longitude(), 180.0); + + // Out of range + coord = TerrainTileArduPilot::_parseFileName("N85E180.hgt"); + QVERIFY(!coord.isValid()); + + coord = TerrainTileArduPilot::_parseFileName("S85E180.hgt"); + QVERIFY(!coord.isValid()); +} + +void TerrainTileTest::_testArduPilotParseCoordinateData() +{ + // Sample elevation data (big endian) + QByteArray sampleData; + QDataStream stream(&sampleData, QIODevice::WriteOnly); + stream.setByteOrder(QDataStream::BigEndian); + // Populate with 4 elevation points for a 2x2 grid + qint16 elevations[] = { 100, -32768, 200, 300 }; + for (int i = 0; i < 4; ++i) { + stream << elevations[i]; + } + + QVector coords = TerrainTileArduPilot::parseCoordinateData("N00E000.hgt", sampleData); + QVERIFY(coords.size() == 4); + + QCOMPARE(coords[0].latitude(), 1.0); // tileLat +1.0 - 0*0.00027778 + QCOMPARE(coords[0].longitude(), 0.0); // tileLon +0*0.00027778 + QCOMPARE(coords[0].altitude(), 100.0); + + QCOMPARE(coords[1].latitude(), 1.0); + QCOMPARE(coords[1].longitude(), 0.00027778); // tileLon +1*0.00027778 + QCOMPARE(coords[1].altitude(), 0.0); // -32768 treated as 0.0 + + QCOMPARE(coords[2].latitude(), 0.99972222); // tileLat +1.0 -1*0.00027778 +} diff --git a/test/Terrain/TerrainTileTest.h b/test/Terrain/TerrainTileTest.h index 83938754222..9b31bae3e5a 100644 --- a/test/Terrain/TerrainTileTest.h +++ b/test/Terrain/TerrainTileTest.h @@ -16,4 +16,6 @@ class TerrainTileTest : public UnitTest Q_OBJECT private slots: + void _testArduPilotParseFileName(); + void _testArduPilotParseCoordinateData(); };