From added25a3e0c24744aff687280754091ed0fdd24 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Sun, 28 May 2023 08:31:17 +0200 Subject: [PATCH 01/11] DO_SET_HOME command support: This commit adds support for setting home clicking on map. It is shown as another action in the submenu when we click over the map with an active vehicle online. As per mavlink specs, this command must use AMSL altitude, so we need to first query terrain altitude, and when received send the command. Several checks have been implemented, and in case terrain altitude is not shown at the selected location a popup will appear indicating there are no terrain data for the specified location --- src/FlightDisplay/FlyViewMap.qml | 14 ++++- src/FlightDisplay/GuidedActionsController.qml | 12 ++++ src/Vehicle/Vehicle.cc | 57 +++++++++++++++++++ src/Vehicle/Vehicle.h | 11 ++++ 4 files changed, 93 insertions(+), 1 deletion(-) diff --git a/src/FlightDisplay/FlyViewMap.qml b/src/FlightDisplay/FlyViewMap.qml index 5d60f5e9ce3..c5a627ddfb6 100644 --- a/src/FlightDisplay/FlyViewMap.qml +++ b/src/FlightDisplay/FlyViewMap.qml @@ -588,11 +588,23 @@ FlightMap { globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionROI, clickMenu.coord, roiLocationItem) } } + + QGCButton { + Layout.fillWidth: true + text: "Set home here" + visible: globals.guidedControllerFlyView.showSetHome + onClicked: { + if (clickMenu.opened) { + clickMenu.close() + } + globals.guidedControllerFlyView.confirmAction(globals.guidedControllerFlyView.actionSetHome, clickMenu.coord) + } + } } } onClicked: { - if (!globals.guidedControllerFlyView.guidedUIVisible && (globals.guidedControllerFlyView.showGotoLocation || globals.guidedControllerFlyView.showOrbit || globals.guidedControllerFlyView.showROI)) { + if (!globals.guidedControllerFlyView.guidedUIVisible && (globals.guidedControllerFlyView.showGotoLocation || globals.guidedControllerFlyView.showOrbit || globals.guidedControllerFlyView.showROI || globals.guidedControllerFlyView.showSetHome)) { orbitMapCircle.hide() gotoLocationItem.hide() var clickCoord = _root.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 541e4b3ddc3..005fc287d6e 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -55,6 +55,7 @@ Item { readonly property string gotoTitle: qsTr("Go To Location") readonly property string vtolTransitionTitle: qsTr("VTOL Transition") readonly property string roiTitle: qsTr("ROI") + readonly property string setHomeTitle: qsTr("Set Home") readonly property string actionListTitle: qsTr("Action") readonly property string armMessage: qsTr("Arm the vehicle.") @@ -79,6 +80,7 @@ Item { readonly property string vtolTransitionFwdMessage: qsTr("Transition VTOL to fixed wing flight.") readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.") readonly property string roiMessage: qsTr("Make the specified location a Region Of Interest.") + readonly property string setHomeMessage: qsTr("Set vehicle home as the specified location. This will affect Return to Home position") readonly property int actionRTL: 1 readonly property int actionLand: 2 @@ -105,6 +107,7 @@ Item { readonly property int actionActionList: 23 readonly property int actionForceArm: 24 readonly property int actionChangeSpeed: 25 + readonly property int actionSetHome: 26 property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property bool _useChecklist: QGroundControl.settingsManager.appSettings.useChecklist.rawValue && QGroundControl.corePlugin.options.preFlightChecklistUrl.toString().length @@ -130,6 +133,7 @@ Item { property bool showROI: _guidedActionsEnabled && _vehicleFlying && __roiSupported && !_missionActive property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _fixedWingOnApproach property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying + property bool showSetHome: _guidedActionsEnabled property bool showActionList: _guidedActionsEnabled && (showStartMission || showResumeMission || showChangeAlt || showLandAbort || actionList.hasCustomActions) property string changeSpeedTitle: _fixedWing ? changeAirspeedTitle : changeCruiseSpeedTitle property string changeSpeedMessage: _fixedWing ? changeAirspeedMessage : changeCruiseSpeedMessage @@ -496,6 +500,11 @@ Item { confirmDialog.message = changeSpeedMessage guidedValueSlider.visible = true break + case actionSetHome: + confirmDialog.title = setHomeTitle + confirmDialog.message = setHomeMessage + confirmDialog.hideTrigger = Qt.binding(function() { return !showSetHome }) + break default: console.warn("Unknown actionCode", actionCode) return @@ -585,6 +594,9 @@ Item { } } break + case actionSetHome: + _activeVehicle.doSetHome(actionData) + break default: console.warn(qsTr("Internal error: unknown actionCode"), actionCode) break diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index de37158c94e..862441dbdbf 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -66,6 +66,8 @@ QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") #define UPDATE_TIMER 50 #define DEFAULT_LAT 38.965767f #define DEFAULT_LON -120.083923f +#define SET_HOME_TERRAIN_ALT_MAX 10000 +#define SET_HOME_TERRAIN_ALT_MIN -500 const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode not supported by Vehicle."); @@ -1514,6 +1516,7 @@ void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) { if (homeCoord != _homePosition) { _homePosition = homeCoord; + qCDebug(VehicleLog) << "new home location set at coordinate: " << homeCoord; emit homePositionChanged(_homePosition); } } @@ -4074,6 +4077,60 @@ void Vehicle::flashBootloader() } #endif +void Vehicle::doSetHome(const QGeoCoordinate& coord) +{ + if (coord.isValid()) { + // If for some reason we already did a query and it hasn't arrived yet, disconnect signals and unset current query. TerrainQuery system will + // automatically delete that forgotten query. This could happen if we send 2 do_set_home commands one after another, so usually the latest one + // is the one we would want to arrive to the vehicle, so it is fine to forget about the previous ones in case it could happen. + if (_currentDoSetHomeTerrainAtCoordinateQuery) { + disconnect(_currentDoSetHomeTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_doSetHomeTerrainReceived); + _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; + } + // Save the coord for using when our terrain data arrives. If there was a pending terrain query paired with an older coordinate it is safe to + // Override now, as we just disconnected the signal that would trigger the command sending + _doSetHomeCoordinate = coord; + // Now setup and trigger the new terrain query + _currentDoSetHomeTerrainAtCoordinateQuery = new TerrainAtCoordinateQuery(true /* autoDelet */); + connect(_currentDoSetHomeTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &Vehicle::_doSetHomeTerrainReceived); + QList rgCoord; + rgCoord.append(coord); + _currentDoSetHomeTerrainAtCoordinateQuery->requestData(rgCoord); + } +} + +// This will be called after our query started in doSetHome arrives +void Vehicle::_doSetHomeTerrainReceived(bool success, QList heights) +{ + if (success) { + double terrainAltitude = heights[0]; + // Coord and terrain alt sanity check + if (_doSetHomeCoordinate.isValid() && terrainAltitude <= SET_HOME_TERRAIN_ALT_MAX && terrainAltitude >= SET_HOME_TERRAIN_ALT_MIN) { + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_SET_HOME, + true, // show error if fails + 0, + 0, + 0, + 0, + _doSetHomeCoordinate.latitude(), + _doSetHomeCoordinate.longitude(), + terrainAltitude); + + } else if (_doSetHomeCoordinate.isValid()) { + qCDebug(VehicleLog) << "_doSetHomeTerrainReceived: internal error, elevation data out of limits "; + } else { + qCDebug(VehicleLog) << "_doSetHomeTerrainReceived: internal error, cached home coordinate is not valid"; + } + } else { + qgcApp()->showAppMessage(tr("Set Home failed, terrain data not available for selected coordinate")); + } + // Clean up + _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; + _doSetHomeCoordinate = QGeoCoordinate(); // So isValid() will no longer return true, for extra safety +} + void Vehicle::gimbalControlValue(double pitch, double yaw) { if (apmFirmware()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 05c11fd5aac..2cd160efdf3 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -47,6 +47,7 @@ #include "FTPManager.h" #include "ImageProtocolManager.h" #include "HealthAndArmingCheckReport.h" +#include "TerrainQuery.h" class Actuators; class EventHandler; @@ -462,6 +463,8 @@ class Vehicle : public FactGroup #if !defined(NO_ARDUPILOT_DIALECT) Q_INVOKABLE void flashBootloader(); #endif + /// Set home from flight map coordinate + Q_INVOKABLE void doSetHome(const QGeoCoordinate& coord); bool isInitialConnectComplete() const; bool guidedModeSupported () const; @@ -1083,6 +1086,9 @@ private slots: static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, MavCmdResultFailureCode_t failureCode); + // This is called after we get terrain data triggered from a doSetHome() + void _doSetHomeTerrainReceived (bool success, QList heights); + int _id; ///< Mavlink system id int _defaultComponentId; bool _offlineEditingVehicle = false; ///< true: This Vehicle is a "disconnected" vehicle for ui use while offline editing @@ -1445,6 +1451,11 @@ private slots: // Settings keys static const char* _settingsGroup; static const char* _joystickEnabledSettingsKey; + + // Terrain query members, used to get terrain altitude for doSetHome() + QTimer _updateDoSetHomeTerrainTimer; + TerrainAtCoordinateQuery* _currentDoSetHomeTerrainAtCoordinateQuery = nullptr; + QGeoCoordinate _doSetHomeCoordinate; }; Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t) From 6b1817cda010ec93f7b0c89716064e7df202b650 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 14 Jun 2023 03:20:02 +0200 Subject: [PATCH 02/11] FlightMapSettings: add elevation map provider setting --- src/Settings/FlightMap.SettingsGroup.json | 6 ++++++ src/Settings/FlightMapSettings.cc | 1 + src/Settings/FlightMapSettings.h | 1 + 3 files changed, 8 insertions(+) diff --git a/src/Settings/FlightMap.SettingsGroup.json b/src/Settings/FlightMap.SettingsGroup.json index 1c0746207aa..403c850d957 100644 --- a/src/Settings/FlightMap.SettingsGroup.json +++ b/src/Settings/FlightMap.SettingsGroup.json @@ -14,6 +14,12 @@ "shortDesc": "Currently selected map type for flight maps", "type": "string", "default": "Hybrid" +}, +{ + "name": "elevationMapProvider", + "shortDesc": "Currently selected elevation map provider", + "type": "string", + "default": "Airmap Elevation" } ] } diff --git a/src/Settings/FlightMapSettings.cc b/src/Settings/FlightMapSettings.cc index dd8a0b58d5a..bd3a3d48e24 100644 --- a/src/Settings/FlightMapSettings.cc +++ b/src/Settings/FlightMapSettings.cc @@ -23,3 +23,4 @@ DECLARE_SETTINGGROUP(FlightMap, "FlightMap") DECLARE_SETTINGSFACT(FlightMapSettings, mapProvider) DECLARE_SETTINGSFACT(FlightMapSettings, mapType) +DECLARE_SETTINGSFACT(FlightMapSettings, elevationMapProvider) diff --git a/src/Settings/FlightMapSettings.h b/src/Settings/FlightMapSettings.h index 6627b3fe2d7..528a42fb576 100644 --- a/src/Settings/FlightMapSettings.h +++ b/src/Settings/FlightMapSettings.h @@ -22,5 +22,6 @@ class FlightMapSettings : public SettingsGroup DEFINE_SETTING_NAME_GROUP() DEFINE_SETTINGFACT(mapProvider) DEFINE_SETTINGFACT(mapType) + DEFINE_SETTINGFACT(elevationMapProvider) }; From ed2dff43a680223a9a57c584f65146a8af04982e Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 14 Jun 2023 04:04:40 +0200 Subject: [PATCH 03/11] GeneralSettings.qml, QGCMapEngineManager: UI to choose elevation provider setting --- .../QMLControl/QGCMapEngineManager.cc | 15 ++++++++ .../QMLControl/QGCMapEngineManager.h | 36 ++++++++++--------- src/ui/preferences/GeneralSettings.qml | 20 +++++++++++ 3 files changed, 54 insertions(+), 17 deletions(-) diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index d1ed4d5eb58..c1203d6cf47 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -226,6 +226,21 @@ QGCMapEngineManager::mapProviderList() mapList.removeDuplicates(); return mapList; } +//----------------------------------------------------------------------------- +QStringList +QGCMapEngineManager::elevationProviderList() +{ + QStringList mapList = getQGCMapEngine()->getMapNameList(); + QStringList elevationMapList; + + for (const QString& providerString : mapList) { + int mapid = getQGCMapEngine()->urlFactory()->getIdFromType(providerString); + if (getQGCMapEngine()->urlFactory()->isElevation(mapid)) { + elevationMapList.append(providerString); + } + } + return elevationMapList; +} //----------------------------------------------------------------------------- QStringList diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h index b7defc145e8..a0ff7918523 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h @@ -37,26 +37,27 @@ class QGCMapEngineManager : public QGCTool }; Q_ENUM(ImportAction) - Q_PROPERTY(quint64 tileCount READ tileCount NOTIFY tileCountChanged) - Q_PROPERTY(QString tileCountStr READ tileCountStr NOTIFY tileCountChanged) - Q_PROPERTY(quint64 tileSize READ tileSize NOTIFY tileSizeChanged) - Q_PROPERTY(QString tileSizeStr READ tileSizeStr NOTIFY tileSizeChanged) - Q_PROPERTY(QmlObjectListModel* tileSets READ tileSets NOTIFY tileSetsChanged) - Q_PROPERTY(QStringList mapList READ mapList CONSTANT) - Q_PROPERTY(QStringList mapProviderList READ mapProviderList CONSTANT) - Q_PROPERTY(quint32 maxMemCache READ maxMemCache WRITE setMaxMemCache NOTIFY maxMemCacheChanged) - Q_PROPERTY(quint32 maxDiskCache READ maxDiskCache WRITE setMaxDiskCache NOTIFY maxDiskCacheChanged) - Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) - Q_PROPERTY(bool fetchElevation READ fetchElevation WRITE setFetchElevation NOTIFY fetchElevationChanged) + Q_PROPERTY(quint64 tileCount READ tileCount NOTIFY tileCountChanged) + Q_PROPERTY(QString tileCountStr READ tileCountStr NOTIFY tileCountChanged) + Q_PROPERTY(quint64 tileSize READ tileSize NOTIFY tileSizeChanged) + Q_PROPERTY(QString tileSizeStr READ tileSizeStr NOTIFY tileSizeChanged) + Q_PROPERTY(QmlObjectListModel* tileSets READ tileSets NOTIFY tileSetsChanged) + Q_PROPERTY(QStringList mapList READ mapList CONSTANT) + Q_PROPERTY(QStringList mapProviderList READ mapProviderList CONSTANT) + Q_PROPERTY(QStringList elevationProviderList READ elevationProviderList CONSTANT) + Q_PROPERTY(quint32 maxMemCache READ maxMemCache WRITE setMaxMemCache NOTIFY maxMemCacheChanged) + Q_PROPERTY(quint32 maxDiskCache READ maxDiskCache WRITE setMaxDiskCache NOTIFY maxDiskCacheChanged) + Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) + Q_PROPERTY(bool fetchElevation READ fetchElevation WRITE setFetchElevation NOTIFY fetchElevationChanged) //-- Disk Space in MB - Q_PROPERTY(quint32 freeDiskSpace READ freeDiskSpace NOTIFY freeDiskSpaceChanged) - Q_PROPERTY(quint32 diskSpace READ diskSpace CONSTANT) + Q_PROPERTY(quint32 freeDiskSpace READ freeDiskSpace NOTIFY freeDiskSpaceChanged) + Q_PROPERTY(quint32 diskSpace READ diskSpace CONSTANT) //-- Tile set export - Q_PROPERTY(int selectedCount READ selectedCount NOTIFY selectedCountChanged) - Q_PROPERTY(int actionProgress READ actionProgress NOTIFY actionProgressChanged) - Q_PROPERTY(ImportAction importAction READ importAction WRITE setImportAction NOTIFY importActionChanged) + Q_PROPERTY(int selectedCount READ selectedCount NOTIFY selectedCountChanged) + Q_PROPERTY(int actionProgress READ actionProgress NOTIFY actionProgressChanged) + Q_PROPERTY(ImportAction importAction READ importAction WRITE setImportAction NOTIFY importActionChanged) - Q_PROPERTY(bool importReplace READ importReplace WRITE setImportReplace NOTIFY importReplaceChanged) + Q_PROPERTY(bool importReplace READ importReplace WRITE setImportReplace NOTIFY importReplaceChanged) Q_INVOKABLE void loadTileSets (); Q_INVOKABLE void updateForCurrentView (double lon0, double lat0, double lon1, double lat1, int minZoom, int maxZoom, const QString& mapName); @@ -79,6 +80,7 @@ class QGCMapEngineManager : public QGCTool QString tileSizeStr () const; QStringList mapList (); QStringList mapProviderList (); + QStringList elevationProviderList (); Q_INVOKABLE QStringList mapTypeList (QString provider); QmlObjectListModel* tileSets () { return &_tileSets; } quint32 maxMemCache (); diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index a3a27b21d1e..cb2891059a3 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -42,6 +42,7 @@ Rectangle { property real _valueFieldWidth: ScreenTools.defaultFontPixelWidth * 10 property string _mapProvider: QGroundControl.settingsManager.flightMapSettings.mapProvider.value property string _mapType: QGroundControl.settingsManager.flightMapSettings.mapType.value + property string _elevationMapProvider: QGroundControl.settingsManager.flightMapSettings.elevationMapProvider.value property Fact _followTarget: QGroundControl.settingsManager.appSettings.followTarget property real _panelWidth: _root.width * _internalWidthRatio property real _margins: ScreenTools.defaultFontPixelWidth @@ -596,6 +597,25 @@ Rectangle { } } + QGCLabel { + text: qsTr("Elevation Map Provider") + width: _labelWidth + } + QGCComboBox { + id: elevationMapProviderCombo + model: QGroundControl.mapEngineManager.elevationProviderList + Layout.preferredWidth: _comboFieldWidth + onActivated: { + _elevationMapProvider = textAt(index) + QGroundControl.settingsManager.flightMapSettings.elevationMapProvider.value=textAt(index) + } + Component.onCompleted: { + var index = elevationMapProviderCombo.find(_elevationMapProvider) + if(index < 0) index = 0 + elevationMapProviderCombo.currentIndex = index + } + } + QGCLabel { text: qsTr("Stream GCS Position") visible: _followTarget.visible From 9ccf3499215cc963727f7ad63e13d9ff2ef3ee65 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Fri, 9 Jun 2023 17:43:44 +0200 Subject: [PATCH 04/11] ElevationMapProvider: unlink Airmap referrer from ElevationProvider baseclass: in order to make ElevationMapProvider be able to be base class of anything besides Airmap. referrer was hardcoded to the base class --- src/QtLocationPlugin/ElevationMapProvider.cpp | 4 ++-- src/QtLocationPlugin/ElevationMapProvider.h | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/QtLocationPlugin/ElevationMapProvider.cpp b/src/QtLocationPlugin/ElevationMapProvider.cpp index c362464b503..61960214a64 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/ElevationMapProvider.cpp @@ -6,8 +6,8 @@ #include "QGCMapEngine.h" #include "TerrainTile.h" -ElevationProvider::ElevationProvider(const QString& imageFormat, quint32 averageSize, QGeoMapType::MapStyle mapType, QObject* parent) - : MapProvider(QStringLiteral("https://api.airmap.com/"), imageFormat, averageSize, mapType, parent) {} +ElevationProvider::ElevationProvider(const QString& imageFormat, quint32 averageSize, QGeoMapType::MapStyle mapType, const QString &referrer, QObject* parent) + : MapProvider(referrer, imageFormat, averageSize, mapType, parent) {} //----------------------------------------------------------------------------- int AirmapElevationProvider::long2tileX(const double lon, const int z) const { diff --git a/src/QtLocationPlugin/ElevationMapProvider.h b/src/QtLocationPlugin/ElevationMapProvider.h index ce2cbea7345..138a8ab257f 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.h +++ b/src/QtLocationPlugin/ElevationMapProvider.h @@ -16,7 +16,8 @@ class ElevationProvider : public MapProvider { Q_OBJECT public: ElevationProvider(const QString& imageFormat, quint32 averageSize, - QGeoMapType::MapStyle mapType, QObject* parent = nullptr); + QGeoMapType::MapStyle mapType, const QString &referrer, + QObject* parent = nullptr); virtual bool _isElevationProvider() const override { return true; } }; @@ -29,7 +30,7 @@ class AirmapElevationProvider : public ElevationProvider { public: AirmapElevationProvider(QObject* parent = nullptr) : ElevationProvider(QStringLiteral("bin"), AVERAGE_AIRMAP_ELEV_SIZE, - QGeoMapType::StreetMap, parent) {} + QGeoMapType::StreetMap, QStringLiteral("https://api.airmap.com/"), parent) {} int long2tileX(const double lon, const int z) const override; From 28797a8c626576daa803545e4f01350f07f03842 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 14 Jun 2023 04:45:35 +0200 Subject: [PATCH 05/11] FlightMap.SettingsGroup.json: provisionally require reboot for change in elevation map provider: This is to be on the safe side in case we have something in the qeue for downloading, and the responses are somehow messed up if we change provider, in case we are basing the parsing of the response on terrain provider settings. Maybe it is all right and it can be done correctly or even it is already fine, but until super sure of that this is safer --- src/Settings/FlightMap.SettingsGroup.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Settings/FlightMap.SettingsGroup.json b/src/Settings/FlightMap.SettingsGroup.json index 403c850d957..861f71c944b 100644 --- a/src/Settings/FlightMap.SettingsGroup.json +++ b/src/Settings/FlightMap.SettingsGroup.json @@ -19,7 +19,8 @@ "name": "elevationMapProvider", "shortDesc": "Currently selected elevation map provider", "type": "string", - "default": "Airmap Elevation" + "default": "Airmap Elevation", + "qgcRebootRequired": true } ] } From 749ede8d62696b9d4dbb3340531e3e31b00dea23 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 14 Jun 2023 04:34:58 +0200 Subject: [PATCH 06/11] QGCMapEngineManager: decouple elevation from Airmap: We needed to decuple here in 2 scenarios: 1 - on updateForCurrentView: when _fetchElevation true, we use retrieve the elevation map provider selected from settingsManager->flightMapSettings-> elevationMapProvider() 2 - on startDownload: We need to check here if the set selected corresponds to a terrain map provider, in case the fetch terrain data box is ticked, so we don't download twice. It used to be hardcoded to airmap elevation string --- .../QMLControl/QGCMapEngineManager.cc | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index c1203d6cf47..3c52335b266 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -19,6 +19,7 @@ #include "QGCApplication.h" #include "QGCMapTileSet.h" #include "QGCMapUrlEngine.h" +#include "SettingsManager.h" #include #include @@ -84,7 +85,8 @@ QGCMapEngineManager::updateForCurrentView(double lon0, double lat0, double lon1, _imageSet += set; } if (_fetchElevation) { - QGCTileSet set = QGCMapEngine::getTileCount(1, lon0, lat0, lon1, lat1, "Airmap Elevation"); + QString elevationProviderName = _toolbox->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); + QGCTileSet set = QGCMapEngine::getTileCount(1, lon0, lat0, lon1, lat1, elevationProviderName); _elevationSet += set; } @@ -159,9 +161,14 @@ QGCMapEngineManager::startDownload(const QString& name, const QString& mapType) } else { qWarning() << "QGCMapEngineManager::startDownload() No Tiles to save"; } - if (mapType != "Airmap Elevation" && _fetchElevation) { + // Fetch elevation: + // If _fetchElevation is true the user ticked "fetch elevation data". But if the user + // Selected in map provider an elevation provider we avoid here to download twice. + int mapid = getQGCMapEngine()->urlFactory()->getIdFromType(mapType); + if (_fetchElevation && !getQGCMapEngine()->urlFactory()->isElevation(mapid)) { QGCCachedTileSet* set = new QGCCachedTileSet(name + " Elevation"); - set->setMapTypeStr("Airmap Elevation"); + QString elevationProviderName = qgcApp()->toolbox()->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); + set->setMapTypeStr(elevationProviderName); set->setTopleftLat(_topleftLat); set->setTopleftLon(_topleftLon); set->setBottomRightLat(_bottomRightLat); @@ -170,7 +177,7 @@ QGCMapEngineManager::startDownload(const QString& name, const QString& mapType) set->setMaxZoom(1); set->setTotalTileSize(_elevationSet.tileSize); set->setTotalTileCount(static_cast(_elevationSet.tileCount)); - set->setType("Airmap Elevation"); + set->setType(elevationProviderName); QGCCreateTileSetTask* task = new QGCCreateTileSetTask(set); //-- Create Tile Set (it will also create a list of tiles to download) connect(task, &QGCCreateTileSetTask::tileSetSaved, this, &QGCMapEngineManager::_tileSetSaved); From fe3433a321585054ef1cda57e118b61aa77c270d Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 14 Jun 2023 20:29:43 +0200 Subject: [PATCH 07/11] TerrainQuery: remove use of Airmap Elevation string, prepare for other providers: - getAltitudesForCoordinates: we get the name of the provider from settings for making the query, so the QGeoTileSpec of the query uses whatever map provider is set on settings. - _terrainDone: this is the response to what we did above. Here we get the string for the map provider from the QGeoTileSpec itself, this way we are sure we are handling it with the proper hash, corresponding to the proper elevation provider. - _getTileHash: we use here the map provider string from settings. This is what is called when getAltitudesForCoordinates is called. So this way we make sure it uses the provider indicated in settings. NOTE: there is still work to do to make all this TerrainQuery file provider agnostic, this only addresses the parts where we were hardcoding the string Airmap Elevation. --- src/Terrain/TerrainQuery.cc | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 8e32d005fd6..41fec6782ed 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -11,6 +11,7 @@ #include "QGCMapEngine.h" #include "QGeoMapReplyQGC.h" #include "QGCApplication.h" +#include "SettingsManager.h" #include #include @@ -442,13 +443,14 @@ bool TerrainTileManager::getAltitudesForCoordinates(const QList& altitudes.push_back(elevation); } else { if (_state != State::Downloading) { - QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL("Airmap Elevation", getQGCMapEngine()->urlFactory()->long2tileX("Airmap Elevation",coordinate.longitude(), 1), getQGCMapEngine()->urlFactory()->lat2tileY("Airmap Elevation", coordinate.latitude(), 1), 1, &_networkManager); + QString elevationProviderName = qgcApp()->toolbox()->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); + QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(elevationProviderName, getQGCMapEngine()->urlFactory()->long2tileX(elevationProviderName,coordinate.longitude(), 1), getQGCMapEngine()->urlFactory()->lat2tileY(elevationProviderName, coordinate.latitude(), 1), 1, &_networkManager); qCDebug(TerrainQueryLog) << "TerrainTileManager::getAltitudesForCoordinates query from database" << request.url(); QGeoTileSpec spec; - spec.setX(getQGCMapEngine()->urlFactory()->long2tileX("Airmap Elevation", coordinate.longitude(), 1)); - spec.setY(getQGCMapEngine()->urlFactory()->lat2tileY("Airmap Elevation", coordinate.latitude(), 1)); + spec.setX(getQGCMapEngine()->urlFactory()->long2tileX(elevationProviderName, coordinate.longitude(), 1)); + spec.setY(getQGCMapEngine()->urlFactory()->lat2tileY(elevationProviderName, coordinate.latitude(), 1)); spec.setZoom(1); - spec.setMapId(getQGCMapEngine()->urlFactory()->getIdFromType("Airmap Elevation")); + spec.setMapId(getQGCMapEngine()->urlFactory()->getIdFromType(elevationProviderName)); QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); connect(reply, &QGeoTiledMapReplyQGC::terrainDone, this, &TerrainTileManager::_terrainDone); _state = State::Downloading; @@ -489,7 +491,7 @@ void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::N // remove from download queue QGeoTileSpec spec = reply->tileSpec(); - QString hash = QGCMapEngine::getTileHash("Airmap Elevation", spec.x(), spec.y(), spec.zoom()); + QString hash = QGCMapEngine::getTileHash(getQGCMapEngine()->urlFactory()->getTypeFromId(spec.mapId()), spec.x(), spec.y(), spec.zoom()); // handle potential errors if (error != QNetworkReply::NoError) { @@ -555,10 +557,11 @@ void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::N QString TerrainTileManager::_getTileHash(const QGeoCoordinate& coordinate) { + QString elevationProviderName = qgcApp()->toolbox()->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); QString ret = QGCMapEngine::getTileHash( - "Airmap Elevation", - getQGCMapEngine()->urlFactory()->long2tileX("Airmap Elevation", coordinate.longitude(), 1), - getQGCMapEngine()->urlFactory()->lat2tileY("Airmap Elevation", coordinate.latitude(), 1), + elevationProviderName, + getQGCMapEngine()->urlFactory()->long2tileX(elevationProviderName, coordinate.longitude(), 1), + getQGCMapEngine()->urlFactory()->lat2tileY(elevationProviderName, coordinate.latitude(), 1), 1); qCDebug(TerrainQueryVerboseLog) << "Computing unique tile hash for " << coordinate << ret; From 43a70980f392da915bd65d3fea201b629e30e7f1 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 14 Jun 2023 21:24:40 +0200 Subject: [PATCH 08/11] Move serializeFromAirMapJson to map providers, from QGCMapTileSet: We used to have in QGCMapTileSet::_networkReplyFinished() this serializeFromAirMapJson hardcoded to the airmap elevation string. Instead we added functions in urlFactory and map providers so we can understand from the map provider hash if such map provider needs its tiles to be serialized, and if so, we call the same provider to perform such serialization. This way the base MapProvider class returns by default that no serialization is needed, and the method to serialize just returns the same QByteArray ( we should never use this, it is just a sanity check ) Then in Airmap elevation map provider we override this, and we implement our own serialization method, which calls the TerrainTile method that was originally called from QGCMapTileSet. This way it is also more obvious when developing support for new elevation map providers, as the relevant methods are contained within the elevation map providers definition files --- src/QtLocationPlugin/ElevationMapProvider.cpp | 4 ++++ src/QtLocationPlugin/ElevationMapProvider.h | 5 +++++ src/QtLocationPlugin/MapProvider.h | 3 +++ src/QtLocationPlugin/QGCMapTileSet.cpp | 5 +++-- src/QtLocationPlugin/QGCMapUrlEngine.cpp | 12 ++++++++++++ src/QtLocationPlugin/QGCMapUrlEngine.h | 3 +++ 6 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/QtLocationPlugin/ElevationMapProvider.cpp b/src/QtLocationPlugin/ElevationMapProvider.cpp index 61960214a64..b373446a5a6 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/ElevationMapProvider.cpp @@ -49,3 +49,7 @@ QGCTileSet AirmapElevationProvider::getTileCount(const int zoom, const double to return set; } + +QByteArray AirmapElevationProvider::serializeTile(QByteArray image) { + return TerrainTile::serializeFromAirMapJson(image); +} \ No newline at end of file diff --git a/src/QtLocationPlugin/ElevationMapProvider.h b/src/QtLocationPlugin/ElevationMapProvider.h index 138a8ab257f..12e920669c7 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.h +++ b/src/QtLocationPlugin/ElevationMapProvider.h @@ -40,6 +40,11 @@ class AirmapElevationProvider : public ElevationProvider { const double topleftLat, const double bottomRightLon, const double bottomRightLat) const override; + // Airmap needs to serialize the tiles, because they are received in json format. This way we can work with + // them in the map tiles database + bool serializeTilesNeeded() override { return true; } + QByteArray serializeTile(QByteArray image) override; + protected: QString _getURL(const int x, const int y, const int zoom, QNetworkAccessManager* networkManager) override; }; diff --git a/src/QtLocationPlugin/MapProvider.h b/src/QtLocationPlugin/MapProvider.h index d61a023d8fa..032f359a766 100644 --- a/src/QtLocationPlugin/MapProvider.h +++ b/src/QtLocationPlugin/MapProvider.h @@ -52,6 +52,9 @@ class MapProvider : public QObject { const double topleftLat, const double bottomRightLon, const double bottomRightLat) const; + virtual bool serializeTilesNeeded() { return false; } + virtual QByteArray serializeTile(QByteArray image) { return image; } + protected: QString _tileXYToQuadKey(const int tileX, const int tileY, const int levelOfDetail) const; int _getServerNum(const int x, const int y, const int max) const; diff --git a/src/QtLocationPlugin/QGCMapTileSet.cpp b/src/QtLocationPlugin/QGCMapTileSet.cpp index e8ad05d1afb..5efa7aec992 100644 --- a/src/QtLocationPlugin/QGCMapTileSet.cpp +++ b/src/QtLocationPlugin/QGCMapTileSet.cpp @@ -287,8 +287,9 @@ QGCCachedTileSet::_networkReplyFinished() qCDebug(QGCCachedTileSetLog) << "Tile fetched" << hash; QByteArray image = reply->readAll(); QString type = getQGCMapEngine()->hashToType(hash); - if (type == "Airmap Elevation" ) { - image = TerrainTile::serializeFromAirMapJson(image); + // Some providers need the images to be serialized because the response is not directly a image based tile + if (getQGCMapEngine()->urlFactory()->needsSerializingTiles(type)) { + image = getQGCMapEngine()->urlFactory()->serializeTileForId(image, type); } QString format = getQGCMapEngine()->urlFactory()->getImageFormat(type, image); if(!format.isEmpty()) { diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index 24edbda93fc..e68d740b179 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -207,3 +207,15 @@ UrlFactory::getTileCount(int zoom, double topleftLon, double topleftLat, double bool UrlFactory::isElevation(int mapId){ return _providersTable[getTypeFromId(mapId)]->_isElevationProvider(); } + +bool UrlFactory::isElevation(QString mapType){ + return _providersTable[mapType]->_isElevationProvider(); +} + +bool UrlFactory::needsSerializingTiles(QString mapType){ + return _providersTable[mapType]->serializeTilesNeeded(); +} + +QByteArray UrlFactory::serializeTileForId(QByteArray image, QString mapType){ + return _providersTable[mapType]->serializeTile(image); +} \ No newline at end of file diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.h b/src/QtLocationPlugin/QGCMapUrlEngine.h index 898cf474704..5071ff929d0 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.h +++ b/src/QtLocationPlugin/QGCMapUrlEngine.h @@ -54,7 +54,10 @@ class UrlFactory : public QObject { double bottomRightLon, double bottomRightLat, QString mapType); + bool isElevation(QString mapType); bool isElevation(int mapId); + bool needsSerializingTiles(QString mapType); + QByteArray serializeTileForId(QByteArray image, QString mapType); private: int _timeout; From 5f4cb317de065e82250e62e040f9007031a4c1c2 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 12 Jun 2023 12:47:47 +0200 Subject: [PATCH 09/11] ElevationMapProvider, QGCMapUrlEngine: WIP commit, provisionally include srtm1 ardupilot terrain servers --- src/QtLocationPlugin/ElevationMapProvider.cpp | 60 ++++++++++++++++++- src/QtLocationPlugin/ElevationMapProvider.h | 23 +++++++ src/QtLocationPlugin/QGCMapUrlEngine.cpp | 1 + 3 files changed, 83 insertions(+), 1 deletion(-) diff --git a/src/QtLocationPlugin/ElevationMapProvider.cpp b/src/QtLocationPlugin/ElevationMapProvider.cpp index b373446a5a6..de0822d8888 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/ElevationMapProvider.cpp @@ -52,4 +52,62 @@ QGCTileSet AirmapElevationProvider::getTileCount(const int zoom, const double to QByteArray AirmapElevationProvider::serializeTile(QByteArray image) { return TerrainTile::serializeFromAirMapJson(image); -} \ No newline at end of file +} +// Ardupilot STR1 elevation provider +//----------------------------------------------------------------------------- +int ApStr1ElevationProvider::long2tileX(const double lon, const int z) const { + Q_UNUSED(z) + return static_cast(floor((lon + 180.0))); +} + +//----------------------------------------------------------------------------- +int ApStr1ElevationProvider::lat2tileY(const double lat, const int z) const { + Q_UNUSED(z) + return static_cast(floor((lat + 90.0))); +} + +//----------------------------------------------------------------------------- +QString ApStr1ElevationProvider::_getURL(const int x, const int y, const int zoom, QNetworkAccessManager* networkManager) { + Q_UNUSED(networkManager) + Q_UNUSED(zoom) + + QString formattedStringYLat; + QString formattedStringXLong; + + // For saving them internally we do 0-360 and 0-180 to avoid signs. Need to redo that to obtain proper format for call + int xForUrl = x - 180; + int yForUrl = y - 90; + + formattedStringYLat = ( yForUrl > 0 ) ? QString("N%1").arg(QString::number(yForUrl).rightJustified(2, '0')) : + QString("S%1").arg(QString::number(-yForUrl).rightJustified(2, '0')); + + formattedStringXLong = ( xForUrl > 0 ) ? QString("E%1").arg(QString::number(xForUrl).rightJustified(3, '0')) : + QString("W%1").arg(QString::number(-xForUrl).rightJustified(3, '0')); + + QString urlString = QString("https://terrain.ardupilot.org/SRTM1/%1%2.hgt.zip") + .arg(QString(formattedStringYLat)) + .arg(QString(formattedStringXLong)) + ; + + return urlString; +} + +//-------------------------------------------------------------------------------- +QGCTileSet ApStr1ElevationProvider::getTileCount(const int zoom, const double topleftLon, + const double topleftLat, const double bottomRightLon, + const 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; +} diff --git a/src/QtLocationPlugin/ElevationMapProvider.h b/src/QtLocationPlugin/ElevationMapProvider.h index 12e920669c7..53967184fcc 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.h +++ b/src/QtLocationPlugin/ElevationMapProvider.h @@ -11,6 +11,7 @@ #include static const quint32 AVERAGE_AIRMAP_ELEV_SIZE = 2786; +static const quint32 AVERAGE_APSTR3_ELEV_SIZE = 100000; // Around 1 MB? class ElevationProvider : public MapProvider { Q_OBJECT @@ -49,3 +50,25 @@ class AirmapElevationProvider : public ElevationProvider { QString _getURL(const int x, const int y, const int zoom, QNetworkAccessManager* networkManager) override; }; +// ----------------------------------------------------------- +// Ardupilot Terrain Server Elevation SRTM1 +// https://terrain.ardupilot.org/SRTM1/ + +class ApStr1ElevationProvider : public ElevationProvider { + Q_OBJECT + public: + ApStr1ElevationProvider(QObject* parent = nullptr) + : ElevationProvider(QStringLiteral("bin"), AVERAGE_APSTR3_ELEV_SIZE, + QGeoMapType::StreetMap, QStringLiteral("https://terrain.ardupilot.org/SRTM1/"), parent) {} + + int long2tileX(const double lon, const int z) const override; + + int lat2tileY(const double lat, const int z) const override; + + QGCTileSet getTileCount(const int zoom, const double topleftLon, + const double topleftLat, const double bottomRightLon, + const double bottomRightLat) const override; + + protected: + QString _getURL(const int x, const int y, const int zoom, QNetworkAccessManager* networkManager) override; +}; diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index e68d740b179..cfe101bb839 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -76,6 +76,7 @@ UrlFactory::UrlFactory() : _timeout(5 * 1000) { _providersTable["VWorld Satellite Map"] = new VWorldSatMapProvider(this); _providersTable["Airmap Elevation"] = new AirmapElevationProvider(this); + _providersTable["Ardupilot-SRTM1 Elevation"] = new ApStr1ElevationProvider(this); _providersTable["Japan-GSI Contour"] = new JapanStdMapProvider(this); _providersTable["Japan-GSI Seamless"] = new JapanSeamlessMapProvider(this); From cdf746b964e470c3b5ff11a6adf01f62c8133717 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 19 Jun 2023 03:56:50 +0200 Subject: [PATCH 10/11] Manage map providers fetching zipped files: We implement again a method on QGCMapUrlEngine, mapProvider and overriden in AP ElevationMapProvider to check if tiles received need to be unzipped, and also a method to unzip them --- src/QtLocationPlugin/ElevationMapProvider.cpp | 7 +++++++ src/QtLocationPlugin/ElevationMapProvider.h | 3 +++ src/QtLocationPlugin/MapProvider.h | 2 ++ src/QtLocationPlugin/QGCMapUrlEngine.cpp | 8 ++++++++ src/QtLocationPlugin/QGCMapUrlEngine.h | 2 ++ 5 files changed, 22 insertions(+) diff --git a/src/QtLocationPlugin/ElevationMapProvider.cpp b/src/QtLocationPlugin/ElevationMapProvider.cpp index de0822d8888..35a21ad901f 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/ElevationMapProvider.cpp @@ -88,6 +88,8 @@ QString ApStr1ElevationProvider::_getURL(const int x, const int y, const int zoo .arg(QString(formattedStringYLat)) .arg(QString(formattedStringXLong)) ; + + qDebug() << "Ardupilot string: " << urlString; return urlString; } @@ -111,3 +113,8 @@ QGCTileSet ApStr1ElevationProvider::getTileCount(const int zoom, const double to return set; } + +QByteArray ApStr1ElevationProvider::unzipTile(QByteArray response) { + + return response; +} \ No newline at end of file diff --git a/src/QtLocationPlugin/ElevationMapProvider.h b/src/QtLocationPlugin/ElevationMapProvider.h index 53967184fcc..8b2889fe694 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.h +++ b/src/QtLocationPlugin/ElevationMapProvider.h @@ -69,6 +69,9 @@ class ApStr1ElevationProvider : public ElevationProvider { const double topleftLat, const double bottomRightLon, const double bottomRightLat) const override; + bool unzippingTilesNeeded() override { return true; } + QByteArray unzipTile(QByteArray response) override; + protected: QString _getURL(const int x, const int y, const int zoom, QNetworkAccessManager* networkManager) override; }; diff --git a/src/QtLocationPlugin/MapProvider.h b/src/QtLocationPlugin/MapProvider.h index 032f359a766..cbc7a0d19f3 100644 --- a/src/QtLocationPlugin/MapProvider.h +++ b/src/QtLocationPlugin/MapProvider.h @@ -54,6 +54,8 @@ class MapProvider : public QObject { virtual bool serializeTilesNeeded() { return false; } virtual QByteArray serializeTile(QByteArray image) { return image; } + virtual bool unzippingTilesNeeded() { return false; } + virtual QByteArray unzipTile(QByteArray response) { return response; } protected: QString _tileXYToQuadKey(const int tileX, const int tileY, const int levelOfDetail) const; diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index cfe101bb839..fc8c98f1921 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -219,4 +219,12 @@ bool UrlFactory::needsSerializingTiles(QString mapType){ QByteArray UrlFactory::serializeTileForId(QByteArray image, QString mapType){ return _providersTable[mapType]->serializeTile(image); +} + +bool UrlFactory::needsUnzippingTiles(QString mapType){ + return _providersTable[mapType]->unzippingTilesNeeded(); +} + +QByteArray UrlFactory::unzipTileForType(QByteArray response, QString mapType) { + return _providersTable[mapType]->unzipTile(response); } \ No newline at end of file diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.h b/src/QtLocationPlugin/QGCMapUrlEngine.h index 5071ff929d0..dc196aacb49 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.h +++ b/src/QtLocationPlugin/QGCMapUrlEngine.h @@ -58,6 +58,8 @@ class UrlFactory : public QObject { bool isElevation(int mapId); bool needsSerializingTiles(QString mapType); QByteArray serializeTileForId(QByteArray image, QString mapType); + bool needsUnzippingTiles(QString mapType); + QByteArray unzipTileForType(QByteArray response, QString mapType); private: int _timeout; From e556c4581eab371d7962e910e11c982043da6e3c Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 19 Jun 2023 05:10:28 +0200 Subject: [PATCH 11/11] WIP, current progress unzipping AP server elevation tiles --- src/QtLocationPlugin/ElevationMapProvider.cpp | 12 ++++++ src/QtLocationPlugin/ElevationMapProvider.h | 2 +- src/QtLocationPlugin/QGeoMapReplyQGC.cpp | 15 ++++++- src/Terrain/TerrainQuery.cc | 40 ++++++++++++++++++- src/TerrainTile.cc | 1 + 5 files changed, 67 insertions(+), 3 deletions(-) diff --git a/src/QtLocationPlugin/ElevationMapProvider.cpp b/src/QtLocationPlugin/ElevationMapProvider.cpp index 35a21ad901f..eed0852df84 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/ElevationMapProvider.cpp @@ -115,6 +115,18 @@ QGCTileSet ApStr1ElevationProvider::getTileCount(const int zoom, const double to } QByteArray ApStr1ElevationProvider::unzipTile(QByteArray response) { + + // This is not working, we need to use a proper zip library like zlib + + int decompressedSize = 25000000; // Uncompressed AP terrain files STR1 are aprox 25 Mb. + response.prepend((unsigned char)((decompressedSize >> 24) & 0xFF)); + response.prepend((unsigned char)((decompressedSize >> 16) & 0xFF)); + response.prepend((unsigned char)((decompressedSize >> 8) & 0xFF)); + response.prepend((unsigned char)((decompressedSize >> 0) & 0xFF)); + + qDebug() << "response before: " << response.size(); + response = qUncompress(response); + qDebug() << "response aftery: " << response.size(); return response; } \ No newline at end of file diff --git a/src/QtLocationPlugin/ElevationMapProvider.h b/src/QtLocationPlugin/ElevationMapProvider.h index 8b2889fe694..770cae669f8 100644 --- a/src/QtLocationPlugin/ElevationMapProvider.h +++ b/src/QtLocationPlugin/ElevationMapProvider.h @@ -58,7 +58,7 @@ class ApStr1ElevationProvider : public ElevationProvider { Q_OBJECT public: ApStr1ElevationProvider(QObject* parent = nullptr) - : ElevationProvider(QStringLiteral("bin"), AVERAGE_APSTR3_ELEV_SIZE, + : ElevationProvider(QStringLiteral("hgt"), AVERAGE_APSTR3_ELEV_SIZE, QGeoMapType::StreetMap, QStringLiteral("https://terrain.ardupilot.org/SRTM1/"), parent) {} int long2tileX(const double lon, const int z) const override; diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp index 36c148cdd97..309a8cffbaf 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp @@ -132,8 +132,17 @@ QGeoTiledMapReplyQGC::networkReplyFinished() UrlFactory* urlFactory = getQGCMapEngine()->urlFactory(); QString format = urlFactory->getImageFormat(tileSpec().mapId(), a); //-- Test for a specialized, elevation data (not map tile) + qDebug() << "control point 0"; if( getQGCMapEngine()->urlFactory()->isElevation(tileSpec().mapId())){ - a = TerrainTile::serializeFromAirMapJson(a); + QString type = getQGCMapEngine()->urlFactory()->getTypeFromId(tileSpec().mapId()); + // Some providers need the images to be serialized because the response is not directly a image based tile + if (getQGCMapEngine()->urlFactory()->needsSerializingTiles(type)) { + a = getQGCMapEngine()->urlFactory()->serializeTileForId(a, type); + } + // Some providers return the tiles zipped, like Ardupilot terrain servers + if (getQGCMapEngine()->urlFactory()->needsUnzippingTiles(type)) { + a = getQGCMapEngine()->urlFactory()->unzipTileForType(a, type); + } //-- Cache it if valid if(!a.isEmpty()) { getQGCMapEngine()->cacheTile( @@ -141,6 +150,7 @@ QGeoTiledMapReplyQGC::networkReplyFinished() tileSpec().mapId()), tileSpec().x(), tileSpec().y(), tileSpec().zoom(), a, format); } + qDebug() << "control point 5"; emit terrainDone(a, QNetworkReply::NoError); } else { MapProvider* mapProvider = urlFactory->getMapProviderFromId(tileSpec().mapId()); @@ -173,6 +183,7 @@ QGeoTiledMapReplyQGC::networkReplyError(QNetworkReply::NetworkError error) } //-- Test for a specialized, elevation data (not map tile) if( getQGCMapEngine()->urlFactory()->isElevation(tileSpec().mapId())){ + qDebug() << "control point 1"; emit terrainDone(QByteArray(), error); } else { //-- Regular map tile @@ -191,6 +202,7 @@ QGeoTiledMapReplyQGC::cacheError(QGCMapTask::TaskType type, QString /*errorStrin { if(!getQGCMapEngine()->isInternetActive()) { if( getQGCMapEngine()->urlFactory()->isElevation(tileSpec().mapId())){ + qDebug() << "control point 2"; emit terrainDone(QByteArray(), QNetworkReply::NetworkSessionFailedError); } else { setError(QGeoTiledMapReply::CommunicationError, "Network not available"); @@ -228,6 +240,7 @@ QGeoTiledMapReplyQGC::cacheReply(QGCCacheTile* tile) { //-- Test for a specialized, elevation data (not map tile) if( getQGCMapEngine()->urlFactory()->isElevation(tileSpec().mapId())){ + qDebug() << "control point 3"; emit terrainDone(tile->img(), QNetworkReply::NoError); } else { //-- Regular map tile diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 41fec6782ed..d100013c580 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -500,6 +500,43 @@ void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::N reply->deleteLater(); return; } + + // This is Mattew's code + + // QString providerName = getQGCMapEngine()->urlFactory()->getTypeFromId(spec.mapId()); + // // Some providers like Ardupilot SRTM servers send files zipped. check if it is needed + // if (getQGCMapEngine()->urlFactory()->needsUnzippingTiles(providerName)) { + // // responseBytes = getQGCMapEngine()->urlFactory()->unzipTileForType(responseBytes, providerName); + + // // Split out filename from path + // QString remoteFileName = QFileInfo(reply->url().toString()).fileName(); + // if (remoteFileName.isEmpty()) { + // qWarning() << "Unabled to parse filename from remote url" << reply->url().toString(); + // remoteFileName = "DownloadedFile"; + // } + + // // Strip out http parameters from remote filename + // int parameterIndex = remoteFileName.indexOf("?"); + // if (parameterIndex != -1) { + // remoteFileName = remoteFileName.left(parameterIndex); + // } + // QFileInfo fi(remoteFileName); + // QString ext = fi.completeSuffix(); + // //need to save ardupilot terrain data zip for extraction + // if (ext == "hgt.zip"){ + // // Determine location to download file to + // QString downloadFilename = QStandardPaths::writableLocation(QStandardPaths::DownloadLocation); + // downloadFilename += "/" + remoteFileName; + // if (!downloadFilename.isEmpty()) { + // // Store downloaded file in download location + // QFile file(downloadFilename); + // if (file.open(QIODevice::WriteOnly | QIODevice::Truncate)) { + // file.write(image); + // file.close(); + // } + // } + // } + // } if (responseBytes.isEmpty()) { qCWarning(TerrainQueryLog) << "Error in fetching elevation tile. Empty response."; _tileFailed(); @@ -508,7 +545,8 @@ void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::N } qCDebug(TerrainQueryLog) << "Received some bytes of terrain data: " << responseBytes.size(); - + qDebug() << "Received some bytes of terrain data: " << responseBytes.size(); + return; TerrainTile* terrainTile = new TerrainTile(responseBytes); if (terrainTile->isValid()) { _tilesMutex.lock(); diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 654847fc58b..ff0ebdd4480 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -92,6 +92,7 @@ TerrainTile::TerrainTile(QByteArray byteArray) } _data = new int16_t*[_gridSizeLat]; + // Here we have trouble crashing, obviously, we need to manage this whole terrain tile thing for other providers for (int k = 0; k < _gridSizeLat; k++) { _data[k] = new int16_t[_gridSizeLon]; }