diff --git a/README.md b/README.md index b74050dd6632a86a24abee5c2109c46c9cbc94cc..715b3d872f54fdba6a20fe91fa7a57db5e693b61 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ You **need to install Qt as described below** instead of using pre-built package * Windows: Make sure to install VS 2015 32 bit package. ###### Install additional packages: -* Ubuntu: sudo apt-get install speech-dispatcher libudev-dev libsdl2-dev libgstreamer1.0-0 gstreamer1.0-plugins-base libgstreamer-plugins-base1.0-dev gstreamer1.0* +* Ubuntu: sudo apt-get install speech-dispatcher libudev-dev libsdl2-dev * Fedora: sudo dnf install speech-dispatcher SDL2-devel SDL2 systemd-devel * Arch Linux: pacman -Sy speech-dispatcher * Windows: [USB Driver](http://www.pixhawk.org/firmware/downloads) to connect to Pixhawk/PX4Flow/3DR Radio diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index fee54686ae51139fd53dfeed5fc56270f9ac6736..912e1a8aeaa8083a1b8b3894a3775aebf8e9e1e1 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -346,6 +346,7 @@ INCLUDEPATH += \ src/QtLocationPlugin \ src/QtLocationPlugin/QMLControl \ src/Settings \ + src/Terrain \ src/VehicleSetup \ src/ViewWidgets \ src/Audio \ @@ -586,7 +587,7 @@ HEADERS += \ src/Settings/SettingsManager.h \ src/Settings/UnitsSettings.h \ src/Settings/VideoSettings.h \ - src/Terrain.h \ + src/Terrain/TerrainQuery.h \ src/TerrainTile.h \ src/Vehicle/MAVLinkLogManager.h \ src/VehicleSetup/JoystickConfigController.h \ @@ -779,7 +780,7 @@ SOURCES += \ src/Settings/SettingsManager.cc \ src/Settings/UnitsSettings.cc \ src/Settings/VideoSettings.cc \ - src/Terrain.cc \ + src/Terrain/TerrainQuery.cc \ src/TerrainTile.cc\ src/Vehicle/MAVLinkLogManager.cc \ src/VehicleSetup/JoystickConfigController.cc \ diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 2a93b928bd2eb8bc704f2aa1cd7979e0281fce4c..174c6f570edcca25fa3e33c4f5c11847894a9a03 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -23,7 +23,7 @@ QGC_LOGGING_CATEGORY(CorridorScanComplexItemLog, "CorridorScanComplexItemLog") const char* CorridorScanComplexItem::settingsGroup = "CorridorScan"; const char* CorridorScanComplexItem::corridorWidthName = "CorridorWidth"; -const char* CorridorScanComplexItem::_entryPointName = "EntryPoint"; +const char* CorridorScanComplexItem::_jsonEntryPointKey = "EntryPoint"; const char* CorridorScanComplexItem::jsonComplexItemTypeValue = "CorridorScan"; @@ -77,16 +77,17 @@ int CorridorScanComplexItem::lastSequenceNumber(void) const return _sequenceNumber + itemCount - 1; } -void CorridorScanComplexItem::save(QJsonArray& missionItems) +void CorridorScanComplexItem::save(QJsonArray& planItems) { QJsonObject saveObject; - saveObject[JsonHelper::jsonVersionKey] = 1; + _save(saveObject); + + saveObject[JsonHelper::jsonVersionKey] = 2; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; saveObject[corridorWidthName] = _corridorWidthFact.rawValue().toDouble(); - saveObject[turnAroundDistanceName] = _turnAroundDistanceFact.rawValue().toDouble(); - saveObject[_entryPointName] = _entryPoint; + saveObject[_jsonEntryPointKey] = _entryPoint; QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); @@ -94,28 +95,29 @@ void CorridorScanComplexItem::save(QJsonArray& missionItems) _corridorPolyline.saveToJson(saveObject); - _save(saveObject); - - missionItems.append(saveObject); + planItems.append(saveObject); } bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString) { + // We don't recalc while loading since all the information we need is specified in the file + _ignoreRecalc = true; + QList keyInfoList = { { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { VisualMissionItem::jsonTypeKey, QJsonValue::String, true }, { ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true }, { corridorWidthName, QJsonValue::Double, true }, - { turnAroundDistanceName, QJsonValue::Double, true }, - { _entryPointName, QJsonValue::Double, true }, + { _jsonEntryPointKey, QJsonValue::Double, true }, { QGCMapPolyline::jsonPolylineKey, QJsonValue::Array, true }, - { _jsonCameraCalcKey, QJsonValue::Object, true }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + _ignoreRecalc = false; return false; } if (!_corridorPolyline.loadFromJson(complexObject, true, errorString)) { + _ignoreRecalc = false; return false; } @@ -123,27 +125,32 @@ bool CorridorScanComplexItem::load(const QJsonObject& complexObject, int sequenc QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType); + _ignoreRecalc = false; return false; } int version = complexObject[JsonHelper::jsonVersionKey].toInt(); - if (version != 1) { + if (version != 2) { errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); + _ignoreRecalc = false; return false; } setSequenceNumber(sequenceNumber); if (!_load(complexObject, errorString)) { + _ignoreRecalc = false; return false; } _corridorWidthFact.setRawValue (complexObject[corridorWidthName].toDouble()); - _entryPoint = complexObject[_entryPointName].toInt(); + _entryPoint = complexObject[_jsonEntryPointKey].toInt(); _rebuildCorridor(); + _ignoreRecalc = false; + return true; } @@ -159,64 +166,75 @@ int CorridorScanComplexItem::_transectCount(void) const return fullWidth > 0.0 ? qCeil(fullWidth / transectSpacing) : 1; } -void CorridorScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +void CorridorScanComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) { - int seqNum = _sequenceNumber; - int pointIndex = 0; - bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - - while (pointIndex < _transectPoints.count()) { - if (_hasTurnaround()) { - QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - if (imagesEverywhere && pointIndex == 1) { - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance - 0, // shutter integration (ignore) - 1, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - } + qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems"; + + int seqNum = _sequenceNumber; + + foreach (const MissionItem* loadedMissionItem, _loadedMissionItems) { + MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); + item->setSequenceNumber(seqNum++); + items.append(item); + } +} + +void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) +{ + qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems"; + + // First adjust for terrain (this will set altitudes into _transectionPoints in all cases + _adjustTransectPointsForTerrain(); + + // Now build the mission items from the transect points + + MissionItem* item; + int seqNum = _sequenceNumber; + bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); + bool addTriggerAtBeginning = imagesEverywhere; + bool firstPoint = true; + bool entryPoint = true; + + MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; + + foreach (const QVariant& transectPointVar, _transectPoints) { + QGeoCoordinate transectPoint = transectPointVar.value(); + + item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + mavFrame, + 0, // No hold time + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + transectPoint.latitude(), + transectPoint.longitude(), + qAbs(transectPoint.altitude()), // qAbs since negative value indicates survey edge + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); - bool addTrigger = imagesEverywhere ? false : true; - for (int i=0; i<_corridorPolyline.count(); i++) { - QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); + if (firstPoint && addTriggerAtBeginning) { + // Start triggering + addTriggerAtBeginning = false; + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance + 0, // shutter integration (ignore) + 1, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); items.append(item); + } + firstPoint = false; - if (addTrigger) { - addTrigger = false; + if (transectPoint.altitude() < 0 && !imagesEverywhere) { + if (entryPoint) { + // Start triggering item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -228,43 +246,26 @@ void CorridorScanComplexItem::appendMissionItems(QList& items, QOb false, // isCurrentItem missionItemParent); items.append(item); + } else { + // Stop triggering + item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0, // stop triggering + 0, // shutter integration (ignore) + 0, // trigger immediately when starting + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); } - } - - if (!imagesEverywhere) { - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // stop triggering - 0, // shutter integration (ignore) - 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - - if (_hasTurnaround()) { - QGeoCoordinate vertexCoord = _transectPoints[pointIndex++].value(); - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - _cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - vertexCoord.latitude(), - vertexCoord.longitude(), - _cameraCalc.distanceToSurface()->rawValue().toDouble(), // Altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + entryPoint = !entryPoint; } } if (imagesEverywhere) { + // Stop triggering MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -279,6 +280,17 @@ void CorridorScanComplexItem::appendMissionItems(QList& items, QOb } } +void CorridorScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + if (_loadedMissionItems.count()) { + // We have mission items from the loaded plan, use those + _appendLoadedMissionItems(items, missionItemParent); + } else { + // Build the mission items on the fly + _buildAndAppendMissionItems(items, missionItemParent); + } +} + void CorridorScanComplexItem::applyNewAltitude(double newAltitude) { _cameraCalc.valueSetIsDistance()->setRawValue(true); @@ -324,10 +336,21 @@ void CorridorScanComplexItem::_rebuildCorridorPolygon(void) } } -void CorridorScanComplexItem::_rebuildTransects(void) +void CorridorScanComplexItem::_rebuildTransectsPhase1(void) { + if (_ignoreRecalc) { + return; + } + + // If the transects are getting rebuilt then any previsouly loaded mission items are now invalid + if (_loadedMissionItemsParent) { + _loadedMissionItems.clear(); + _loadedMissionItemsParent->deleteLater(); + _loadedMissionItemsParent = NULL; + } + _transectPoints.clear(); - _cameraShots = 0; + _transectsPathHeightInfo.clear(); double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); double fullWidth = _corridorWidthFact.rawValue().toDouble(); @@ -336,13 +359,9 @@ void CorridorScanComplexItem::_rebuildTransects(void) double normalizedTransectPosition = transectSpacing / 2.0; if (_corridorPolyline.count() >= 2) { - int singleTransectImageCount = qCeil(_corridorPolyline.length() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); - // First build up the transects all going the same direction QList> transects; for (int i=0; i transect = _corridorPolyline.offsetPolyline(offsetDistance); + transect[0].setAltitude(_surveyEdgeIndicator); + transect[1].setAltitude(_surveyEdgeIndicator); if (_hasTurnaround()) { QGeoCoordinate extensionCoord; // Extend the transect ends for turnaround double azimuth = transect[0].azimuthTo(transect[1]); extensionCoord = transect[0].atDistanceAndAzimuth(-_turnAroundDistanceFact.rawValue().toDouble(), azimuth); + extensionCoord.setAltitude(qQNaN()); transect.prepend(extensionCoord); azimuth = transect.last().azimuthTo(transect[transect.count() - 2]); extensionCoord = transect.last().atDistanceAndAzimuth(-_turnAroundDistanceFact.rawValue().toDouble(), azimuth); + extensionCoord.setAltitude(qQNaN()); transect.append(extensionCoord); } @@ -415,8 +438,6 @@ void CorridorScanComplexItem::_rebuildTransects(void) // Convert the list of transects to grid points reverseVertices = false; for (int i=0; i transectVertices = transects[i]; if (reverseVertices) { @@ -437,14 +458,22 @@ void CorridorScanComplexItem::_rebuildTransects(void) } } + _queryTransectsPathHeightInfo(); +} + +void CorridorScanComplexItem::_rebuildTransectsPhase2(void) +{ // Calculate distance flown for complex item _complexDistance = 0; for (int i=0; i<_transectPoints.count() - 2; i++) { _complexDistance += _transectPoints[i].value().distanceTo(_transectPoints[i+1].value()); } - if (_cameraTriggerInTurnAroundFact.rawValue().toDouble()) { + if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { _cameraShots = qCeil(_complexDistance / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + } else { + int singleTransectImageCount = qCeil(_corridorPolyline.length() / _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble()); + _cameraShots = singleTransectImageCount * _transectCount(); } _coordinate = _transectPoints.count() ? _transectPoints.first().value() : QGeoCoordinate(); @@ -460,5 +489,11 @@ void CorridorScanComplexItem::_rebuildTransects(void) void CorridorScanComplexItem::_rebuildCorridor(void) { _rebuildCorridorPolygon(); - _rebuildTransects(); + _rebuildTransectsPhase1(); + _rebuildTransectsPhase2(); +} + +bool CorridorScanComplexItem::readyForSave(void) const +{ + return TransectStyleComplexItem::readyForSave(); } diff --git a/src/MissionManager/CorridorScanComplexItem.h b/src/MissionManager/CorridorScanComplexItem.h index 01c1983ff05f465ee1dbb8e084a62570cafb03b9..495d9361477bbab4a07662297c18a2d043488473 100644 --- a/src/MissionManager/CorridorScanComplexItem.h +++ b/src/MissionManager/CorridorScanComplexItem.h @@ -36,17 +36,18 @@ public: Q_INVOKABLE void rotateEntryPoint(void); // Overrides from ComplexMissionItem - - int lastSequenceNumber (void) const final; - bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; - QString mapVisualQML (void) const final { return QStringLiteral("CorridorScanMapVisual.qml"); } + int lastSequenceNumber (void) const final; + bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; + QString mapVisualQML (void) const final { return QStringLiteral("CorridorScanMapVisual.qml"); } // Overrides from TransectStyleComplexItem + void save (QJsonArray& planItems) final; + bool specifiesCoordinate (void) const final; + void appendMissionItems (QList& items, QObject* missionItemParent) final; + void applyNewAltitude (double newAltitude) final; - void save (QJsonArray& missionItems) final; - bool specifiesCoordinate (void) const final; - void appendMissionItems (QList& items, QObject* missionItemParent) final; - void applyNewAltitude (double newAltitude) final; + // Overrides from VisualMissionionItem + bool readyForSave (void) const; static const char* jsonComplexItemTypeValue; @@ -54,17 +55,19 @@ public: static const char* corridorWidthName; private slots: - void _polylineDirtyChanged (bool dirty); - void _polylineCountChanged (int count); - void _rebuildCorridor (void); + void _polylineDirtyChanged (bool dirty); + void _polylineCountChanged (int count); + void _rebuildCorridor (void); // Overrides from TransectStyleComplexItem - virtual void _rebuildTransects (void) final; + void _rebuildTransectsPhase1 (void) final; + void _rebuildTransectsPhase2 (void) final; private: - int _transectCount (void) const; - void _rebuildCorridorPolygon(void); - + int _transectCount (void) const; + void _rebuildCorridorPolygon (void); + void _buildAndAppendMissionItems(QList& items, QObject* missionItemParent); + void _appendLoadedMissionItems (QList& items, QObject* missionItemParent); QGCMapPolyline _corridorPolyline; QList> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points @@ -75,5 +78,5 @@ private: QMap _metaDataMap; SettingsFact _corridorWidthFact; - static const char* _entryPointName; + static const char* _jsonEntryPointKey; }; diff --git a/src/MissionManager/CorridorScanComplexItemTest.cc b/src/MissionManager/CorridorScanComplexItemTest.cc index bcbd9cf3fd56484e9bfa9f6517c98c333fe746b9..30abd256b0912f017c0a6c622aca7872ea4ba2cc 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.cc +++ b/src/MissionManager/CorridorScanComplexItemTest.cc @@ -134,13 +134,26 @@ void CorridorScanComplexItemTest::_testItemCount(void) { QList items; - _corridorItem->turnAroundDistance()->setRawValue(20); + _corridorItem->turnAroundDistance()->setRawValue(0); _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); _corridorItem->appendMissionItems(items, this); QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); items.clear(); + _corridorItem->turnAroundDistance()->setRawValue(0); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + + _corridorItem->turnAroundDistance()->setRawValue(20); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + + _corridorItem->turnAroundDistance()->setRawValue(20); _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); _corridorItem->appendMissionItems(items, this); QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index db1347ba08742b1eafc4dac866b3252ee74852c5..f9ffdc18546eda2909dca3f0493f80bfdaa660d4 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -277,7 +277,7 @@ void MissionController::convertToKMLDocument(QDomDocument& document) return; } - float altitude = missionJson[_jsonPlannedHomePositionKey].toArray()[2].toDouble(); + float homeAltitude = missionJson[_jsonPlannedHomePositionKey].toArray()[2].toDouble(); QString coord; QStringList coords; @@ -292,11 +292,12 @@ void MissionController::convertToKMLDocument(QDomDocument& document) qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command()); if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) { + double amslAltitude = item->param7() + (item->frame() == MAV_FRAME_GLOBAL ? 0 : homeAltitude); coord = QString::number(item->param6(),'f',7) \ + "," \ + QString::number(item->param5(),'f',7) \ + "," \ - + QString::number(item->param7() + altitude,'f',2); + + QString::number(amslAltitude,'f',2); coords.append(coord); } } diff --git a/src/MissionManager/TransectStyle.SettingsGroup.json b/src/MissionManager/TransectStyle.SettingsGroup.json index d949f6ba24ec433e4aa073df3692bd887c904882..f1bdf254f0bde0e4c7a6e3fc624e7ce1422b18b9 100644 --- a/src/MissionManager/TransectStyle.SettingsGroup.json +++ b/src/MissionManager/TransectStyle.SettingsGroup.json @@ -34,5 +34,32 @@ "shortDescription": "Refly the pattern at a 90 degree angle", "type": "bool", "defaultValue": false +}, +{ + "name": "TerrainAdjustTolerance", + "shortDescription": "TerrainAdjustTolerance", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m", + "defaultValue": 10 +}, +{ + "name": "TerrainAdjustMaxClimbRate", + "shortDescription": "TerrainAdjustMaxClimbRate", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m/s", + "defaultValue": 0 +}, +{ + "name": "TerrainAdjustMaxDescentRate", + "shortDescription": "TerrainAdjustMaxDescentRate", + "type": "double", + "decimalPlaces": 2, + "min": 0, + "units": "m/s", + "defaultValue": 0 } ] diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 27c1f54018ff3df37873a45df5a86ffbe5fd83b7..4fb1cdaff054e4a1f57b21a050efa3234e3e2b52 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -26,28 +26,52 @@ const char* TransectStyleComplexItem::turnAroundDistanceMultiRotorName = "Tur const char* TransectStyleComplexItem::cameraTriggerInTurnAroundName = "CameraTriggerInTurnAround"; const char* TransectStyleComplexItem::hoverAndCaptureName = "HoverAndCapture"; const char* TransectStyleComplexItem::refly90DegreesName = "Refly90Degrees"; +const char* TransectStyleComplexItem::terrainAdjustToleranceName = "TerrainAdjustTolerance"; +const char* TransectStyleComplexItem::terrainAdjustMaxClimbRateName = "TerrainAdjustMaxClimbRate"; +const char* TransectStyleComplexItem::terrainAdjustMaxDescentRateName = "TerrainAdjustMaxDescentRate"; -const char* TransectStyleComplexItem::_jsonCameraCalcKey = "CameraCalc"; +const char* TransectStyleComplexItem::_jsonTransectStyleComplexItemKey = "TransectStyleComplexItem"; +const char* TransectStyleComplexItem::_jsonCameraCalcKey = "CameraCalc"; +const char* TransectStyleComplexItem::_jsonTransectPointsKey = "TransectPoints"; +const char* TransectStyleComplexItem::_jsonItemsKey = "Items"; +const char* TransectStyleComplexItem::_jsonFollowTerrainKey = "FollowTerrain"; + +const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 500; +const double TransectStyleComplexItem::_surveyEdgeIndicator = -10; TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString settingsGroup, QObject* parent) - : ComplexMissionItem (vehicle, parent) - , _settingsGroup (settingsGroup) - , _sequenceNumber (0) - , _dirty (false) - , _ignoreRecalc (false) - , _complexDistance (0) - , _cameraShots (0) - , _cameraMinTriggerInterval (0) - , _cameraCalc (vehicle) - , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/TransectStyle.SettingsGroup.json"), this)) - , _turnAroundDistanceFact (_settingsGroup, _metaDataMap[_vehicle->multiRotor() ? turnAroundDistanceMultiRotorName : turnAroundDistanceName]) - , _cameraTriggerInTurnAroundFact(_settingsGroup, _metaDataMap[cameraTriggerInTurnAroundName]) - , _hoverAndCaptureFact (_settingsGroup, _metaDataMap[hoverAndCaptureName]) - , _refly90DegreesFact (_settingsGroup, _metaDataMap[refly90DegreesName]) + : ComplexMissionItem (vehicle, parent) + , _settingsGroup (settingsGroup) + , _sequenceNumber (0) + , _dirty (false) + , _terrainPolyPathQuery (NULL) + , _ignoreRecalc (false) + , _complexDistance (0) + , _cameraShots (0) + , _cameraMinTriggerInterval (0) + , _cameraCalc (vehicle) + , _followTerrain (false) + , _loadedMissionItemsParent (NULL) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/TransectStyle.SettingsGroup.json"), this)) + , _turnAroundDistanceFact (_settingsGroup, _metaDataMap[_vehicle->multiRotor() ? turnAroundDistanceMultiRotorName : turnAroundDistanceName]) + , _cameraTriggerInTurnAroundFact (_settingsGroup, _metaDataMap[cameraTriggerInTurnAroundName]) + , _hoverAndCaptureFact (_settingsGroup, _metaDataMap[hoverAndCaptureName]) + , _refly90DegreesFact (_settingsGroup, _metaDataMap[refly90DegreesName]) + , _terrainAdjustToleranceFact (_settingsGroup, _metaDataMap[terrainAdjustToleranceName]) + , _terrainAdjustMaxClimbRateFact (_settingsGroup, _metaDataMap[terrainAdjustMaxClimbRateName]) + , _terrainAdjustMaxDescentRateFact (_settingsGroup, _metaDataMap[terrainAdjustMaxDescentRateName]) { + _terrainQueryTimer.setInterval(_terrainQueryTimeoutMsecs); + _terrainQueryTimer.setSingleShot(true); + connect(&_terrainQueryTimer, &QTimer::timeout, this, &TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo); + connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_rebuildTransects); @@ -60,6 +84,10 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); connect(_cameraCalc.adjustedFootprintSide(), &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); connect(_cameraCalc.adjustedFootprintFrontal(), &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_signalLastSequenceNumberChanged); connect(&_turnAroundDistanceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::complexDistanceChanged); @@ -75,6 +103,10 @@ TransectStyleComplexItem::TransectStyleComplexItem(Vehicle* vehicle, QString set connect(&_cameraTriggerInTurnAroundFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_refly90DegreesFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(this, &TransectStyleComplexItem::followTerrainChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustMaxClimbRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustMaxDescentRateFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); + connect(&_terrainAdjustToleranceFact, &Fact::valueChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this, &TransectStyleComplexItem::_setDirty); connect(&_surveyAreaPolygon, &QGCMapPolygon::dirtyChanged, this, &TransectStyleComplexItem::_setIfDirty); @@ -108,14 +140,45 @@ void TransectStyleComplexItem::setDirty(bool dirty) void TransectStyleComplexItem::_save(QJsonObject& complexObject) { - complexObject[turnAroundDistanceName] = _turnAroundDistanceFact.rawValue().toDouble(); - complexObject[cameraTriggerInTurnAroundName] = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - complexObject[hoverAndCaptureName] = _hoverAndCaptureFact.rawValue().toBool(); - complexObject[refly90DegreesName] = _refly90DegreesFact.rawValue().toBool(); + QJsonObject innerObject; + + innerObject[JsonHelper::jsonVersionKey] = 1; + innerObject[turnAroundDistanceName] = _turnAroundDistanceFact.rawValue().toDouble(); + innerObject[cameraTriggerInTurnAroundName] = _cameraTriggerInTurnAroundFact.rawValue().toBool(); + innerObject[hoverAndCaptureName] = _hoverAndCaptureFact.rawValue().toBool(); + innerObject[refly90DegreesName] = _refly90DegreesFact.rawValue().toBool(); + innerObject[_jsonFollowTerrainKey] = _followTerrain; + + if (_followTerrain) { + innerObject[terrainAdjustToleranceName] = _terrainAdjustToleranceFact.rawValue().toDouble(); + innerObject[terrainAdjustMaxClimbRateName] = _terrainAdjustMaxClimbRateFact.rawValue().toDouble(); + innerObject[terrainAdjustMaxDescentRateName] = _terrainAdjustMaxDescentRateFact.rawValue().toDouble(); + } QJsonObject cameraCalcObject; _cameraCalc.save(cameraCalcObject); - complexObject[_jsonCameraCalcKey] = cameraCalcObject; + innerObject[_jsonCameraCalcKey] = cameraCalcObject; + + QJsonValue transectPointsJson; + + // Save transects polyline + JsonHelper::saveGeoCoordinateArray(_transectPoints, false /* writeAltitude */, transectPointsJson); + innerObject[_jsonTransectPointsKey] = transectPointsJson; + + // Save the interal mission items + QJsonArray missionItemsJsonArray; + QObject* missionItemParent = new QObject(); + QList missionItems; + appendMissionItems(missionItems, missionItemParent); + foreach (const MissionItem* missionItem, missionItems) { + QJsonObject missionItemJsonObject; + missionItem->save(missionItemJsonObject); + missionItemsJsonArray.append(missionItemJsonObject); + } + missionItemParent->deleteLater(); + innerObject[_jsonItemsKey] = missionItemsJsonArray; + + complexObject[_jsonTransectStyleComplexItemKey] = innerObject; } void TransectStyleComplexItem::setSequenceNumber(int sequenceNumber) @@ -129,25 +192,84 @@ void TransectStyleComplexItem::setSequenceNumber(int sequenceNumber) bool TransectStyleComplexItem::_load(const QJsonObject& complexObject, QString& errorString) { + QList keyInfoList = { + { _jsonTransectStyleComplexItemKey, QJsonValue::Object, true }, + }; + if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + return false; + } + + // The TransectStyleComplexItem is a sub-object of the main complex item object + QJsonObject innerObject = complexObject[_jsonTransectStyleComplexItemKey].toObject(); + + if (innerObject.contains(JsonHelper::jsonVersionKey)) { + int version = innerObject[JsonHelper::jsonVersionKey].toInt(); + if (version != 1) { + errorString = tr("TransectStyleComplexItem version %2 not supported").arg(version); + return false; + } + } + + QList innerKeyInfoList = { + { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { turnAroundDistanceName, QJsonValue::Double, true }, { cameraTriggerInTurnAroundName, QJsonValue::Bool, true }, { hoverAndCaptureName, QJsonValue::Bool, true }, { refly90DegreesName, QJsonValue::Bool, true }, { _jsonCameraCalcKey, QJsonValue::Object, true }, + { _jsonTransectPointsKey, QJsonValue::Array, true }, + { _jsonItemsKey, QJsonValue::Array, true }, + { _jsonFollowTerrainKey, QJsonValue::Bool, true }, }; - if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { + if (!JsonHelper::validateKeys(innerObject, innerKeyInfoList, errorString)) { return false; } - if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) { + // Load transect points + if (!JsonHelper::loadGeoCoordinateArray(innerObject[_jsonTransectPointsKey], false /* altitudeRequired */, _transectPoints, errorString)) { return false; } - _turnAroundDistanceFact.setRawValue (complexObject[turnAroundDistanceName].toDouble()); - _cameraTriggerInTurnAroundFact.setRawValue (complexObject[cameraTriggerInTurnAroundName].toBool()); - _hoverAndCaptureFact.setRawValue (complexObject[hoverAndCaptureName].toBool()); - _hoverAndCaptureFact.setRawValue (complexObject[refly90DegreesName].toBool()); + // Load generated mission items + _loadedMissionItemsParent = new QObject(this); + QJsonArray missionItemsJsonArray = innerObject[_jsonItemsKey].toArray(); + foreach (const QJsonValue& missionItemJson, missionItemsJsonArray) { + MissionItem* missionItem = new MissionItem(_loadedMissionItemsParent); + if (!missionItem->load(missionItemJson.toObject(), 0 /* sequenceNumber */, errorString)) { + _loadedMissionItemsParent->deleteLater(); + _loadedMissionItemsParent = NULL; + return false; + } + _loadedMissionItems.append(missionItem); + } + + // Load CameraCalc data + if (!_cameraCalc.load(innerObject[_jsonCameraCalcKey].toObject(), errorString)) { + return false; + } + + // Load TransectStyleComplexItem individual values + _turnAroundDistanceFact.setRawValue (innerObject[turnAroundDistanceName].toDouble()); + _cameraTriggerInTurnAroundFact.setRawValue (innerObject[cameraTriggerInTurnAroundName].toBool()); + _hoverAndCaptureFact.setRawValue (innerObject[hoverAndCaptureName].toBool()); + _refly90DegreesFact.setRawValue (innerObject[refly90DegreesName].toBool()); + _followTerrain = innerObject[_jsonFollowTerrainKey].toBool(); + + if (_followTerrain) { + QList followTerrainKeyInfoList = { + { terrainAdjustToleranceName, QJsonValue::Double, true }, + { terrainAdjustMaxClimbRateName, QJsonValue::Double, true }, + { terrainAdjustMaxDescentRateName, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(innerObject, followTerrainKeyInfoList, errorString)) { + return false; + } + + _terrainAdjustToleranceFact.setRawValue (innerObject[terrainAdjustToleranceName].toDouble()); + _terrainAdjustMaxClimbRateFact.setRawValue (innerObject[terrainAdjustMaxClimbRateName].toDouble()); + _terrainAdjustMaxDescentRateFact.setRawValue (innerObject[terrainAdjustMaxDescentRateName].toDouble()); + } return true; } @@ -230,3 +352,192 @@ bool TransectStyleComplexItem::hoverAndCaptureAllowed(void) const return _vehicle->multiRotor() || _vehicle->vtol(); } +void TransectStyleComplexItem::_rebuildTransects(void) +{ + _rebuildTransectsPhase1(); + _rebuildTransectsPhase2(); +} + +void TransectStyleComplexItem::_queryTransectsPathHeightInfo(void) +{ + _transectsPathHeightInfo.clear(); + if (_terrainPolyPathQuery) { + // Toss previous query + _terrainPolyPathQuery->deleteLater(); + _terrainPolyPathQuery = NULL; + } + + if (_transectPoints.count() > 1) { + // We don't actually send the query until this timer times out. This way we only send + // the laset request if we get a bunch in a row. + _terrainQueryTimer.start(); + } +} + +void TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo(void) +{ + if (_transectPoints.count() > 1) { + _terrainPolyPathQuery = new TerrainPolyPathQuery(this); + connect(_terrainPolyPathQuery, &TerrainPolyPathQuery::terrainData, this, &TransectStyleComplexItem::_polyPathTerrainData); + _terrainPolyPathQuery->requestData(_transectPoints); + } +} + +void TransectStyleComplexItem::_polyPathTerrainData(bool success, const QList& rgPathHeightInfo) +{ + if (success) { + _transectsPathHeightInfo = rgPathHeightInfo; + } else { + _transectsPathHeightInfo.clear(); + } +} + +bool TransectStyleComplexItem::readyForSave(void) const +{ + // Make sure we have the terrain data we need + return _transectPoints.count() > 1 ? _transectsPathHeightInfo.count() : false; +} + +/// Add altitude values to the standard transect points (whether following terrain or not) +void TransectStyleComplexItem::_adjustTransectPointsForTerrain(void) +{ + if (_followTerrain && !readyForSave()) { + qCWarning(TransectStyleComplexItemLog) << "_adjustTransectPointsForTerrain called when terrain data not ready"; + qgcApp()->showMessage(tr("INTERNAL ERROR: TransectStyleComplexItem::_adjustTransectPointsForTerrain called when terrain data not ready. Plan will be incorrect.")); + return; + } + + double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + + qDebug() << _transectPoints.count() << _transectsPathHeightInfo.count(); + for (int i=0; i<_transectPoints.count() - 1; i++) { + QGeoCoordinate transectPoint = _transectPoints[i].value(); + + bool surveyEdgeIndicator = transectPoint.altitude() == _surveyEdgeIndicator; + if (_followTerrain) { + transectPoint.setAltitude(_transectsPathHeightInfo[i].heights[0] + requestedAltitude); + } else { + transectPoint.setAltitude(requestedAltitude); + } + if (surveyEdgeIndicator) { + // Use to indicate survey edge + transectPoint.setAltitude(-transectPoint.altitude()); + } + + _transectPoints[i] = QVariant::fromValue(transectPoint); + } + + // Take care of last point + QGeoCoordinate transectPoint = _transectPoints.last().value(); + bool surveyEdgeIndicator = transectPoint.altitude() == _surveyEdgeIndicator; + if (_followTerrain){ + transectPoint.setAltitude(_transectsPathHeightInfo.last().heights.last() + requestedAltitude); + } else { + transectPoint.setAltitude(requestedAltitude); + } + if (surveyEdgeIndicator) { + // Use to indicate survey edge + transectPoint.setAltitude(-transectPoint.altitude()); + } + _transectPoints[_transectPoints.count() - 1] = QVariant::fromValue(transectPoint); + + _addInterstitialTransectsForTerrain(); +} + +/// Returns the altitude in between the two points on a line. +/// @param precentTowardsTo Example: .25 = twenty five percent along the distance of from to to +double TransectStyleComplexItem::_altitudeBetweenCoords(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo) +{ + double fromAlt = qAbs(fromCoord.altitude()); + double toAlt = qAbs(toCoord.altitude()); + double altDiff = toAlt - fromAlt; + return fromAlt + (altDiff * percentTowardsTo); +} + +/// Determine the maximum height within the PathHeightInfo +/// @param fromIndex First height index to consider +/// @param fromIndex Last height index to consider +/// @param[out] maxHeight Maximum height +/// @return index within PathHeightInfo_t.heights which contains maximum height, -1 no height data in between from and to indices +int TransectStyleComplexItem::_maxPathHeight(const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight) +{ + maxHeight = qQNaN(); + + if (toIndex - fromIndex < 2) { + return -1; + } + + fromIndex++; + toIndex--; + + int maxIndex = fromIndex; + maxHeight = pathHeightInfo.heights[fromIndex]; + + for (int i=fromIndex; i<=toIndex; i++) { + if (pathHeightInfo.heights[i] > maxHeight) { + maxIndex = i; + maxHeight = pathHeightInfo.heights[i]; + } + } + + return maxIndex; +} + +/// Add points in between existing points to account for terrain +void TransectStyleComplexItem::_addInterstitialTransectsForTerrain(void) +{ + if (!_followTerrain) { + return; + } + + double requestedAltitude = _cameraCalc.distanceToSurface()->rawValue().toDouble(); + double tolerance = _terrainAdjustToleranceFact.rawValue().toDouble(); + QList terrainAdjustedTransectPoints; + + // Check for needed terrain adjust in between the transect points + for (int i=0; i<_transectPoints.count() - 1; i++) { + terrainAdjustedTransectPoints.append(_transectPoints[i]); + + QGeoCoordinate fromCoord = _transectPoints[i].value(); + QGeoCoordinate toCoord = _transectPoints[i+1].value(); + + const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo = _transectsPathHeightInfo[i]; + int cHeights = pathHeightInfo.heights.count(); + int lastAddedHeightIndex = 0; + //qDebug() << "cHeights" << cHeights << pathHeightInfo.heights; + + for (int pathHeightIndex=1; pathHeightIndex tolerance) { + // We need to add a new point to adjust for terrain + double azimuth = fromCoord.azimuthTo(toCoord); + double distance = fromCoord.distanceTo(toCoord); + QGeoCoordinate interstitialCoord = fromCoord.atDistanceAndAzimuth(distance * percentTowardsTo, azimuth); + interstitialCoord.setAltitude(interstitialTerrainHeight + requestedAltitude); + terrainAdjustedTransectPoints.append(QVariant::fromValue(interstitialCoord)); + fromCoord = interstitialCoord; + lastAddedHeightIndex = pathHeightIndex; + //qDebug() << "Added index" << terrainAdjustedTransectPoints.count() - 1; + } + } + } + terrainAdjustedTransectPoints.append(_transectPoints.last()); + + _transectPoints = terrainAdjustedTransectPoints; +} + +void TransectStyleComplexItem::setFollowTerrain(bool followTerrain) +{ + if (followTerrain != _followTerrain) { + _followTerrain = followTerrain; + emit followTerrainChanged(followTerrain); + } +} diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index d624557eb4509d5736600147aa364a7b1253bb29..e4201efea9391c2b6e074babb81aff81a9299980 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -16,6 +16,7 @@ #include "QGCMapPolyline.h" #include "QGCMapPolygon.h" #include "CameraCalc.h" +#include "TerrainQuery.h" Q_DECLARE_LOGGING_CATEGORY(TransectStyleComplexItemLog) @@ -26,34 +27,45 @@ class TransectStyleComplexItem : public ComplexMissionItem public: TransectStyleComplexItem(Vehicle* vehicle, QString settignsGroup, QObject* parent = NULL); - Q_PROPERTY(QGCMapPolygon* surveyAreaPolygon READ surveyAreaPolygon CONSTANT) - Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) - Q_PROPERTY(Fact* turnAroundDistance READ turnAroundDistance CONSTANT) - Q_PROPERTY(Fact* cameraTriggerInTurnAround READ cameraTriggerInTurnAround CONSTANT) - Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT) - Q_PROPERTY(Fact* refly90Degrees READ refly90Degrees CONSTANT) - - Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) - Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) - Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) - Q_PROPERTY(double cameraMinTriggerInterval READ cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) - Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT) - Q_PROPERTY(QVariantList transectPoints READ transectPoints NOTIFY transectPointsChanged) + Q_PROPERTY(QGCMapPolygon* surveyAreaPolygon READ surveyAreaPolygon CONSTANT) + Q_PROPERTY(CameraCalc* cameraCalc READ cameraCalc CONSTANT) + Q_PROPERTY(Fact* turnAroundDistance READ turnAroundDistance CONSTANT) + Q_PROPERTY(Fact* cameraTriggerInTurnAround READ cameraTriggerInTurnAround CONSTANT) + Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT) + Q_PROPERTY(Fact* refly90Degrees READ refly90Degrees CONSTANT) + + Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) + Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) + Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) + Q_PROPERTY(double cameraMinTriggerInterval READ cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged) + Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT) + Q_PROPERTY(QVariantList transectPoints READ transectPoints NOTIFY transectPointsChanged) + + Q_PROPERTY(bool followTerrain READ followTerrain WRITE setFollowTerrain NOTIFY followTerrainChanged) + Q_PROPERTY(Fact* terrainAdjustTolerance READ terrainAdjustTolerance CONSTANT) + Q_PROPERTY(Fact* terrainAdjustMaxDescentRate READ terrainAdjustMaxDescentRate CONSTANT) + Q_PROPERTY(Fact* terrainAdjustMaxClimbRate READ terrainAdjustMaxClimbRate CONSTANT) QGCMapPolygon* surveyAreaPolygon (void) { return &_surveyAreaPolygon; } CameraCalc* cameraCalc (void) { return &_cameraCalc; } QVariantList transectPoints (void) { return _transectPoints; } - Fact* turnAroundDistance (void) { return &_turnAroundDistanceFact; } - Fact* cameraTriggerInTurnAround (void) { return &_cameraTriggerInTurnAroundFact; } - Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; } - Fact* refly90Degrees (void) { return &_refly90DegreesFact; } + Fact* turnAroundDistance (void) { return &_turnAroundDistanceFact; } + Fact* cameraTriggerInTurnAround (void) { return &_cameraTriggerInTurnAroundFact; } + Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; } + Fact* refly90Degrees (void) { return &_refly90DegreesFact; } + Fact* terrainAdjustTolerance (void) { return &_terrainAdjustToleranceFact; } + Fact* terrainAdjustMaxDescentRate (void) { return &_terrainAdjustMaxClimbRateFact; } + Fact* terrainAdjustMaxClimbRate (void) { return &_terrainAdjustMaxDescentRateFact; } int cameraShots (void) const { return _cameraShots; } double timeBetweenShots (void); double coveredArea (void) const; double cameraMinTriggerInterval(void) const { return _cameraMinTriggerInterval; } bool hoverAndCaptureAllowed (void) const; + bool followTerrain (void) const { return _followTerrain; } + + void setFollowTerrain(bool followTerrain); // Overrides from ComplexMissionItem @@ -66,7 +78,7 @@ public: // Overrides from VisualMissionItem - void save (QJsonArray& missionItems) override = 0; + void save (QJsonArray& planItems) override = 0; bool specifiesCoordinate (void) const override = 0; void appendMissionItems (QList& items, QObject* missionItemParent) override = 0; void applyNewAltitude (double newAltitude) override = 0; @@ -85,6 +97,7 @@ public: double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; + bool readyForSave (void) const override; bool coordinateHasRelativeAltitude (void) const final { return true /*_altitudeRelative*/; } bool exitCoordinateHasRelativeAltitude (void) const final { return true /*_altitudeRelative*/; } @@ -99,6 +112,9 @@ public: static const char* cameraTriggerInTurnAroundName; static const char* hoverAndCaptureName; static const char* refly90DegreesName; + static const char* terrainAdjustToleranceName; + static const char* terrainAdjustMaxClimbRateName; + static const char* terrainAdjustMaxDescentRateName; signals: void cameraShotsChanged (void); @@ -106,33 +122,41 @@ signals: void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval); void transectPointsChanged (void); void coveredAreaChanged (void); + void followTerrainChanged (bool followTerrain); protected slots: - virtual void _rebuildTransects (void) = 0; + virtual void _rebuildTransectsPhase1 (void) = 0; + virtual void _rebuildTransectsPhase2 (void) = 0; void _setDirty (void); void _setIfDirty (bool dirty); void _updateCoordinateAltitudes (void); void _signalLastSequenceNumberChanged (void); + void _polyPathTerrainData (bool success, const QList& rgPathHeightInfo); protected: - void _save (QJsonObject& saveObject); - bool _load (const QJsonObject& complexObject, QString& errorString); - void _setExitCoordinate (const QGeoCoordinate& coordinate); - void _setCameraShots (int cameraShots); - double _triggerDistance (void) const; - int _transectCount (void) const; - bool _hasTurnaround (void) const; - double _turnaroundDistance (void) const; + void _save (QJsonObject& saveObject); + bool _load (const QJsonObject& complexObject, QString& errorString); + void _setExitCoordinate (const QGeoCoordinate& coordinate); + void _setCameraShots (int cameraShots); + double _triggerDistance (void) const; + bool _hasTurnaround (void) const; + double _turnaroundDistance (void) const; + void _queryTransectsPathHeightInfo (void); + void _adjustTransectPointsForTerrain (void); QString _settingsGroup; int _sequenceNumber; bool _dirty; QGeoCoordinate _coordinate; QGeoCoordinate _exitCoordinate; - QVariantList _transectPoints; QGCMapPolygon _surveyAreaPolygon; + QVariantList _transectPoints; + QList _transectsPathHeightInfo; + TerrainPolyPathQuery* _terrainPolyPathQuery; + QTimer _terrainQueryTimer; + bool _ignoreRecalc; double _complexDistance; int _cameraShots; @@ -140,6 +164,10 @@ protected: double _cameraMinTriggerInterval; double _cruiseSpeed; CameraCalc _cameraCalc; + bool _followTerrain; + + QObject* _loadedMissionItemsParent; ///< Parent for all items in _loadedMissionItems for simpler delete + QList _loadedMissionItems; ///< Mission items loaded from plan file QMap _metaDataMap; @@ -147,6 +175,25 @@ protected: SettingsFact _cameraTriggerInTurnAroundFact; SettingsFact _hoverAndCaptureFact; SettingsFact _refly90DegreesFact; + SettingsFact _terrainAdjustToleranceFact; + SettingsFact _terrainAdjustMaxClimbRateFact; + SettingsFact _terrainAdjustMaxDescentRateFact; static const char* _jsonCameraCalcKey; + static const char* _jsonTransectStyleComplexItemKey; + static const char* _jsonTransectPointsKey; + static const char* _jsonItemsKey; + static const char* _jsonFollowTerrainKey; + + static const int _terrainQueryTimeoutMsecs; + static const double _surveyEdgeIndicator; ///< Altitude value in _transectPoints which indicates survey entry + +private slots: + void _rebuildTransects (void); + void _reallyQueryTransectsPathHeightInfo (void); + +private: + void _addInterstitialTransectsForTerrain (void); + double _altitudeBetweenCoords (const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord, double percentTowardsTo); + int _maxPathHeight (const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo, int fromIndex, int toIndex, double& maxHeight); }; diff --git a/src/MissionManager/TransectStyleComplexItemTest.cc b/src/MissionManager/TransectStyleComplexItemTest.cc index a9f2372af32e32225d8bfdbf1612ce063f09f790..2ca3b0eef3147122c25ea1a3c948192cd77e8b1a 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.cc +++ b/src/MissionManager/TransectStyleComplexItemTest.cc @@ -174,7 +174,12 @@ TransectStyleItem::TransectStyleItem(Vehicle* vehicle, QObject* parent) } -void TransectStyleItem::_rebuildTransects(void) +void TransectStyleItem::_rebuildTransectsPhase1(void) { rebuildTransectsCalled = true; } + +void TransectStyleItem::_rebuildTransectsPhase2(void) +{ + +} diff --git a/src/MissionManager/TransectStyleComplexItemTest.h b/src/MissionManager/TransectStyleComplexItemTest.h index d3aeea3202af69898f5d74f5098f99b84182756f..b6648fdb083c034a6a0810e25522cafe35e6065f 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.h +++ b/src/MissionManager/TransectStyleComplexItemTest.h @@ -101,5 +101,6 @@ public: private slots: // Overrides from TransectStyleComplexItem - void _rebuildTransects (void) final; + void _rebuildTransectsPhase1(void) final; + void _rebuildTransectsPhase2(void) final; }; diff --git a/src/MissionManager/VisualMissionItem.cc b/src/MissionManager/VisualMissionItem.cc index ba309b72f6ac54b434f2ca2d5aca0f85eab7d591..24da25b2713515dc1f689378e28a1a20e8c2d023 100644 --- a/src/MissionManager/VisualMissionItem.cc +++ b/src/MissionManager/VisualMissionItem.cc @@ -15,7 +15,7 @@ #include "FirmwarePluginManager.h" #include "QGCApplication.h" #include "JsonHelper.h" -#include "Terrain.h" +#include "TerrainQuery.h" const char* VisualMissionItem::jsonTypeKey = "type"; const char* VisualMissionItem::jsonTypeSimpleItemValue = "SimpleItem"; @@ -172,18 +172,18 @@ void VisualMissionItem::_reallyUpdateTerrainAltitude(void) if (coord.isValid() && (qIsNaN(_terrainAltitude) || !qFuzzyCompare(_lastLatTerrainQuery, coord.latitude()) || qFuzzyCompare(_lastLonTerrainQuery, coord.longitude()))) { _lastLatTerrainQuery = coord.latitude(); _lastLonTerrainQuery = coord.longitude(); - ElevationProvider* terrain = new ElevationProvider(this); - connect(terrain, &ElevationProvider::terrainData, this, &VisualMissionItem::_terrainDataReceived); + TerrainAtCoordinateQuery* terrain = new TerrainAtCoordinateQuery(this); + connect(terrain, &TerrainAtCoordinateQuery::terrainData, this, &VisualMissionItem::_terrainDataReceived); QList rgCoord; rgCoord.append(coordinate()); - terrain->queryTerrainData(rgCoord); + terrain->requestData(rgCoord); } } -void VisualMissionItem::_terrainDataReceived(bool success, QList altitudes) +void VisualMissionItem::_terrainDataReceived(bool success, QList heights) { if (success) { - _terrainAltitude = altitudes[0]; + _terrainAltitude = heights[0]; emit terrainAltitudeChanged(_terrainAltitude); sender()->deleteLater(); } diff --git a/src/MissionManager/VisualMissionItem.h b/src/MissionManager/VisualMissionItem.h index eb1232cf7093015836a4a0479c77cdbcb6bf9875..9324c8cf6346859da4f1587ba9ba35db4d88f10d 100644 --- a/src/MissionManager/VisualMissionItem.h +++ b/src/MissionManager/VisualMissionItem.h @@ -211,7 +211,7 @@ protected: private slots: void _updateTerrainAltitude (void); void _reallyUpdateTerrainAltitude (void); - void _terrainDataReceived (bool success, QList altitudes); + void _terrainDataReceived (bool success, QList heights); private: QTimer _updateTerrainTimer; diff --git a/src/PlanView/CorridorScanEditor.qml b/src/PlanView/CorridorScanEditor.qml index 72cd5d3d80a7bb1f5672f00b8f6373b85ead7873..bf3bb139b754bc33010538e4a98b05ef253f12d4 100644 --- a/src/PlanView/CorridorScanEditor.qml +++ b/src/PlanView/CorridorScanEditor.qml @@ -56,6 +56,11 @@ Rectangle { anchors.right: parent.right spacing: _margin + QGCLabel { + text: "WIP: Careful!" + color: qgcPal.warningText + } + QGCLabel { anchors.left: parent.left anchors.right: parent.right @@ -126,6 +131,59 @@ Rectangle { onClicked: missionItem.rotateEntryPoint() } + SectionHeader { + id: terrainHeader + text: qsTr("Terrain") + checked: false + } + + ColumnLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: terrainHeader.checked + + QGCCheckBox { + id: followsTerrainCheckBox + text: qsTr("Vehicle follows terrain") + checked: missionItem.followTerrain + onClicked: missionItem.followTerrain = checked + } + + GridLayout { + anchors.left: parent.left + anchors.right: parent.right + columnSpacing: _margin + rowSpacing: _margin + columns: 2 + visible: followsTerrainCheckBox.checked + + QGCLabel { + text: "WIP: Careful!" + color: qgcPal.warningText + Layout.columnSpan: 2 + } + + QGCLabel { text: qsTr("Tolerance") } + FactTextField { + fact: missionItem.terrainAdjustTolerance + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Max Climb Rate") } + FactTextField { + fact: missionItem.terrainAdjustMaxClimbRate + Layout.fillWidth: true + } + + QGCLabel { text: qsTr("Max Descent Rate") } + FactTextField { + fact: missionItem.terrainAdjustMaxDescentRate + Layout.fillWidth: true + } + } + } + SectionHeader { id: statsHeader text: qsTr("Statistics") diff --git a/src/Terrain.cc b/src/Terrain.cc deleted file mode 100644 index 55dd81556929fb8e2d25a12e338f9b4454f84af7..0000000000000000000000000000000000000000 --- a/src/Terrain.cc +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "Terrain.h" -#include "QGCMapEngine.h" -#include "QGeoMapReplyQGC.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -QGC_LOGGING_CATEGORY(ElevationProviderLog, "ElevationProviderLog") - -Q_GLOBAL_STATIC(TerrainBatchManager, _terrainBatchManager) - -TerrainBatchManager::TerrainBatchManager(void) -{ -} - -void TerrainBatchManager::addQuery(ElevationProvider* elevationProvider, const QList& coordinates) -{ - if (coordinates.length() > 0) { - QList altitudes; - - if (!_getAltitudesForCoordinates(coordinates, altitudes)) { - QueuedRequestInfo_t queuedRequestInfo = { elevationProvider, coordinates }; - _requestQueue.append(queuedRequestInfo); - return; - } - - qCDebug(ElevationProviderLog) << "All altitudes taken from cached data"; - elevationProvider->_signalTerrainData(coordinates.count() == altitudes.count(), altitudes); - } -} - -bool TerrainBatchManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes) -{ - foreach (const QGeoCoordinate& coordinate, coordinates) { - QString tileHash = _getTileHash(coordinate); - _tilesMutex.lock(); - if (!_tiles.contains(tileHash)) { - qCDebug(ElevationProviderLog) << "Need to download tile " << tileHash; - - // Schedule the fetch task - if (_state != State::Downloading) { - QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); - QGeoTileSpec spec; - spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); - spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); - spec.setZoom(1); - spec.setMapId(UrlFactory::AirmapElevation); - QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); - connect(reply, &QGeoTiledMapReplyQGC::finished, this, &TerrainBatchManager::_fetchedTile); - connect(reply, &QGeoTiledMapReplyQGC::aborted, this, &TerrainBatchManager::_fetchedTile); - _state = State::Downloading; - } - _tilesMutex.unlock(); - - return false; - } else { - if (_tiles[tileHash].isIn(coordinate)) { - altitudes.push_back(_tiles[tileHash].elevation(coordinate)); - } else { - qCDebug(ElevationProviderLog) << "Error: coordinate not in tile region"; - altitudes.push_back(-1.0); - } - } - _tilesMutex.unlock(); - } - return true; -} - -void TerrainBatchManager::_tileFailed(void) -{ - QList noAltitudes; - - foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { - requestInfo.elevationProvider->_signalTerrainData(false, noAltitudes); - } - _requestQueue.clear(); -} - -void TerrainBatchManager::_fetchedTile() -{ - QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); - _state = State::Idle; - - if (!reply) { - qCDebug(ElevationProviderLog) << "Elevation tile fetched but invalid reply data type."; - return; - } - - // remove from download queue - QGeoTileSpec spec = reply->tileSpec(); - QString hash = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, spec.x(), spec.y(), spec.zoom()); - - // handle potential errors - if (reply->error() != QGeoTiledMapReply::NoError) { - if (reply->error() == QGeoTiledMapReply::CommunicationError) { - qCDebug(ElevationProviderLog) << "Elevation tile fetching returned communication error. " << reply->errorString(); - } else { - qCDebug(ElevationProviderLog) << "Elevation tile fetching returned error. " << reply->errorString(); - } - _tileFailed(); - reply->deleteLater(); - return; - } - if (!reply->isFinished()) { - qCDebug(ElevationProviderLog) << "Error in fetching elevation tile. Not finished. " << reply->errorString(); - _tileFailed(); - reply->deleteLater(); - return; - } - - // parse received data and insert into hash table - QByteArray responseBytes = reply->mapImageData(); - - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - if (parseError.error != QJsonParseError::NoError) { - qCDebug(ElevationProviderLog) << "Could not parse terrain tile " << parseError.errorString(); - qCDebug(ElevationProviderLog) << responseBytes; - _tileFailed(); - reply->deleteLater(); - return; - } - - TerrainTile* terrainTile = new TerrainTile(responseJson); - if (terrainTile->isValid()) { - _tilesMutex.lock(); - if (!_tiles.contains(hash)) { - _tiles.insert(hash, *terrainTile); - } else { - delete terrainTile; - } - _tilesMutex.unlock(); - } else { - qCDebug(ElevationProviderLog) << "Received invalid tile"; - } - reply->deleteLater(); - - // now try to query the data again - for (int i = _requestQueue.count() - 1; i >= 0; i--) { - QList altitudes; - if (_getAltitudesForCoordinates(_requestQueue[i].coordinates, altitudes)) { - _requestQueue[i].elevationProvider->_signalTerrainData(_requestQueue[i].coordinates.count() == altitudes.count(), altitudes); - _requestQueue.removeAt(i); - } - } -} - -QString TerrainBatchManager::_getTileHash(const QGeoCoordinate& coordinate) -{ - QString ret = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1); - qCDebug(ElevationProviderLog) << "Computing unique tile hash for " << coordinate << ret; - - return ret; -} - -ElevationProvider::ElevationProvider(QObject* parent) - : QObject(parent) -{ - -} - -bool ElevationProvider::queryTerrainData(const QList& coordinates) -{ - if (coordinates.length() == 0) { - return false; - } - - _terrainBatchManager->addQuery(this, coordinates); - - return false; -} - -void ElevationProvider::_signalTerrainData(bool success, QList& altitudes) -{ - emit terrainData(success, altitudes); -} diff --git a/src/Terrain.h b/src/Terrain.h deleted file mode 100644 index 18bc55f22b4243f7191004aa48e4234a857dc39e..0000000000000000000000000000000000000000 --- a/src/Terrain.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * (c) 2017 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 "TerrainTile.h" -#include "QGCMapEngineData.h" -#include "QGCLoggingCategory.h" - -#include -#include -#include -#include -#include -#include - -Q_DECLARE_LOGGING_CATEGORY(ElevationProviderLog) - -class ElevationProvider; - -/// Used internally by ElevationProvider to batch requests together -class TerrainBatchManager : public QObject { - Q_OBJECT - -public: - TerrainBatchManager(void); - - void addQuery(ElevationProvider* elevationProvider, const QList& coordinates); - -private slots: - void _fetchedTile (void); /// slot to handle fetched elevation tiles - -private: - typedef struct { - ElevationProvider* elevationProvider; - QList coordinates; - } QueuedRequestInfo_t; - - enum class State { - Idle, - Downloading, - }; - - void _tileFailed(void); - bool _getAltitudesForCoordinates(const QList& coordinates, QList& altitudes); - QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile - - QList _requestQueue; - State _state = State::Idle; - QNetworkAccessManager _networkManager; - - QMutex _tilesMutex; - QHash _tiles; -}; - -class ElevationProvider : public QObject -{ - Q_OBJECT -public: - ElevationProvider(QObject* parent = NULL); - - /** - * Async elevation query for a list of lon,lat coordinates. When the query is done, the terrainData() signal - * is emitted. This call caches local elevation tables for faster lookup in the future. - * @param coordinates - * @return true on success - */ - bool queryTerrainData(const QList& coordinates); - - /// Internal method - void _signalTerrainData(bool success, QList& altitudes); - -signals: - /// signal returning requested elevation data - void terrainData(bool success, QList altitudes); -}; diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc new file mode 100644 index 0000000000000000000000000000000000000000..829718ae61bce213d34029d6fecffe7a7ec2d351 --- /dev/null +++ b/src/Terrain/TerrainQuery.cc @@ -0,0 +1,631 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainQuery.h" +#include "QGCMapEngine.h" +#include "QGeoMapReplyQGC.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainQueryLog, "TerrainQueryLog") + +Q_GLOBAL_STATIC(TerrainAtCoordinateBatchManager, _TerrainAtCoordinateBatchManager) +Q_GLOBAL_STATIC(TerrainTileManager, _terrainTileManager) + +TerrainAirMapQuery::TerrainAirMapQuery(QObject* parent) + : TerrainQueryInterface(parent) +{ + +} + +void TerrainAirMapQuery::requestCoordinateHeights(const QList& coordinates) +{ + QString points; + foreach (const QGeoCoordinate& coord, coordinates) { + points += QString::number(coord.latitude(), 'f', 10) + "," + + QString::number(coord.longitude(), 'f', 10) + ","; + } + points = points.mid(0, points.length() - 1); // remove the last ',' from string + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = QueryModeCoordinates; + _sendQuery(QString() /* path */, query); +} + +void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) +{ + QString points; + points += QString::number(fromCoord.latitude(), 'f', 10) + "," + + QString::number(fromCoord.longitude(), 'f', 10) + ","; + points += QString::number(toCoord.latitude(), 'f', 10) + "," + + QString::number(toCoord.longitude(), 'f', 10); + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = QueryModePath; + _sendQuery(QStringLiteral("/path"), query); +} + +void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) +{ + QString points; + points += QString::number(swCoord.latitude(), 'f', 10) + "," + + QString::number(swCoord.longitude(), 'f', 10) + ","; + points += QString::number(neCoord.latitude(), 'f', 10) + "," + + QString::number(neCoord.longitude(), 'f', 10); + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = QueryModeCarpet; + _carpetStatsOnly = statsOnly; + + _sendQuery(QStringLiteral("/carpet"), query); +} + +void TerrainAirMapQuery::_sendQuery(const QString& path, const QUrlQuery& urlQuery) +{ + QUrl url(QStringLiteral("https://api.airmap.com/elevation/v1/ele") + path); + url.setQuery(urlQuery); + qCDebug(TerrainQueryLog) << "_sendQuery" << url; + + QNetworkRequest request(url); + + QNetworkProxy tProxy; + tProxy.setType(QNetworkProxy::DefaultProxy); + _networkManager.setProxy(tProxy); + + QNetworkReply* networkReply = _networkManager.get(request); + if (!networkReply) { + qCDebug(TerrainQueryLog) << "QNetworkManager::Get did not return QNetworkReply"; + _requestFailed(); + return; + } + + connect(networkReply, &QNetworkReply::finished, this, &TerrainAirMapQuery::_requestFinished); +} + +void TerrainAirMapQuery::_requestFinished(void) +{ + QNetworkReply* reply = qobject_cast(QObject::sender()); + + if (reply->error() != QNetworkReply::NoError) { + qCDebug(TerrainQueryLog) << "_requestFinished error:" << reply->error(); + reply->deleteLater(); + _requestFailed(); + return; + } + + QByteArray responseBytes = reply->readAll(); + reply->deleteLater(); + + // Convert the response to Json + QJsonParseError parseError; + QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); + if (parseError.error != QJsonParseError::NoError) { + qCDebug(TerrainQueryLog) << "_requestFinished unable to parse json:" << parseError.errorString(); + _requestFailed(); + return; + } + + // Check airmap reponse status + QJsonObject rootObject = responseJson.object(); + QString status = rootObject["status"].toString(); + if (status != "success") { + qCDebug(TerrainQueryLog) << "_requestFinished status != success:" << status; + _requestFailed(); + return; + } + + // Send back data + const QJsonValue& jsonData = rootObject["data"]; + qCDebug(TerrainQueryLog) << "_requestFinished" << jsonData; + switch (_queryMode) { + case QueryModeCoordinates: + emit _parseCoordinateData(jsonData); + break; + case QueryModePath: + emit _parsePathData(jsonData); + break; + case QueryModeCarpet: + emit _parseCarpetData(jsonData); + break; + } +} + +void TerrainAirMapQuery::_requestFailed(void) +{ + switch (_queryMode) { + case QueryModeCoordinates: + emit coordinateHeights(false /* success */, QList() /* heights */); + break; + case QueryModePath: + emit pathHeights(false /* success */, qQNaN() /* latStep */, qQNaN() /* lonStep */, QList() /* heights */); + break; + case QueryModeCarpet: + emit carpetHeights(false /* success */, qQNaN() /* minHeight */, qQNaN() /* maxHeight */, QList>() /* carpet */); + break; + } +} + +void TerrainAirMapQuery::_parseCoordinateData(const QJsonValue& coordinateJson) +{ + QList heights; + const QJsonArray& dataArray = coordinateJson.toArray(); + for (int i = 0; i < dataArray.count(); i++) { + heights.append(dataArray[i].toDouble()); + } + + emit coordinateHeights(true /* success */, heights); +} + +void TerrainAirMapQuery::_parsePathData(const QJsonValue& pathJson) +{ + QJsonObject jsonObject = pathJson.toArray()[0].toObject(); + QJsonArray stepArray = jsonObject["step"].toArray(); + QJsonArray profileArray = jsonObject["profile"].toArray(); + + double latStep = stepArray[0].toDouble(); + double lonStep = stepArray[1].toDouble(); + + QList heights; + foreach (const QJsonValue& profileValue, profileArray) { + heights.append(profileValue.toDouble()); + } + + emit pathHeights(true /* success */, latStep, lonStep, heights); +} + +void TerrainAirMapQuery::_parseCarpetData(const QJsonValue& carpetJson) +{ + QJsonObject jsonObject = carpetJson.toArray()[0].toObject(); + + QJsonObject statsObject = jsonObject["stats"].toObject(); + double minHeight = statsObject["min"].toDouble(); + double maxHeight = statsObject["min"].toDouble(); + + QList> carpet; + if (!_carpetStatsOnly) { + QJsonArray carpetArray = jsonObject["carpet"].toArray(); + + for (int i=0; i()); + + for (int j=0; j& coordinates) +{ + if (coordinates.length() == 0) { + return false; + } + + _tileBatchManager->addQuery(this, coordinates); + + return false; +} + +void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) +{ + // TODO +} + +void TerrainOfflineAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) +{ + // TODO +} + +void TerrainOfflineAirMapQuery::_signalCoordinateHeights(bool success, QList heights) +{ + emit coordinateHeights(success, heights) +} + +void TerrainOfflineAirMapQuery::_signalPathHeights(bool success, double latStep, double lonStep, const QList& heights) +{ + // TODO +} + +void TerrainOfflineAirMapQuery::_signalCarpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet) +{ + // TODO +} + +TerrainTileManager::TerrainTileManager(void) +{ + +} + +void TerrainTileManager::addQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates) +{ + if (coordinates.length() > 0) { + QList altitudes; + + if (!_getAltitudesForCoordinates(coordinates, altitudes)) { + QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, coordinates, QueryMode::QueryModeCoordinates }; + _requestQueue.append(queuedRequestInfo); + return; + } + + qCDebug(TerrainQueryLog) << "All altitudes taken from cached data"; + terrainQueryInterface->_signalTerrainData(coordinates.count() == altitudes.count(), altitudes); + } +} + +bool TerrainTileManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes) +{ + foreach (const QGeoCoordinate& coordinate, coordinates) { + QString tileHash = _getTileHash(coordinate); + _tilesMutex.lock(); + if (!_tiles.contains(tileHash)) { + qCDebug(TerrainQueryLog) << "Need to download tile " << tileHash; + + // Schedule the fetch task + if (_state != State::Downloading) { + QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); + QGeoTileSpec spec; + spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); + spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); + spec.setZoom(1); + spec.setMapId(UrlFactory::AirmapElevation); + QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); + connect(reply, &QGeoTiledMapReplyQGC::finished, this, &TerrainTileManager::_fetchedTile); + connect(reply, &QGeoTiledMapReplyQGC::aborted, this, &TerrainTileManager::_fetchedTile); + _state = State::Downloading; + } + _tilesMutex.unlock(); + + return false; + } else { + if (_tiles[tileHash].isIn(coordinate)) { + altitudes.push_back(_tiles[tileHash].elevation(coordinate)); + } else { + qCDebug(TerrainQueryLog) << "Error: coordinate not in tile region"; + altitudes.push_back(-1.0); + } + } + _tilesMutex.unlock(); + } + return true; +} + +void TerrainTileManager::_tileFailed(void) +{ + QList noAltitudes; + + foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { + if (requestInfo.queryMode == QueryMode::QueryModeCoordinates) { + requestInfo.terrainQueryInterface->_signalCoordinateHeights(false, noAltitudes); + } + } + _requestQueue.clear(); +} + +void TerrainTileManager::_fetchedTile() +{ + QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); + _state = State::Idle; + + if (!reply) { + qCDebug(TerrainQueryLog) << "Elevation tile fetched but invalid reply data type."; + return; + } + + // remove from download queue + QGeoTileSpec spec = reply->tileSpec(); + QString hash = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, spec.x(), spec.y(), spec.zoom()); + + // handle potential errors + if (reply->error() != QGeoTiledMapReply::NoError) { + if (reply->error() == QGeoTiledMapReply::CommunicationError) { + qCDebug(TerrainQueryLog) << "Elevation tile fetching returned communication error. " << reply->errorString(); + } else { + qCDebug(TerrainQueryLog) << "Elevation tile fetching returned error. " << reply->errorString(); + } + _tileFailed(); + reply->deleteLater(); + return; + } + if (!reply->isFinished()) { + qCDebug(TerrainQueryLog) << "Error in fetching elevation tile. Not finished. " << reply->errorString(); + _tileFailed(); + reply->deleteLater(); + return; + } + + // parse received data and insert into hash table + QByteArray responseBytes = reply->mapImageData(); + + QJsonParseError parseError; + QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); + if (parseError.error != QJsonParseError::NoError) { + qCDebug(TerrainQueryLog) << "Could not parse terrain tile " << parseError.errorString(); + qCDebug(TerrainQueryLog) << responseBytes; + _tileFailed(); + reply->deleteLater(); + return; + } + + TerrainTile* terrainTile = new TerrainTile(responseJson); + if (terrainTile->isValid()) { + _tilesMutex.lock(); + if (!_tiles.contains(hash)) { + _tiles.insert(hash, *terrainTile); + } else { + delete terrainTile; + } + _tilesMutex.unlock(); + } else { + qCDebug(TerrainQueryLog) << "Received invalid tile"; + } + reply->deleteLater(); + + // now try to query the data again + for (int i = _requestQueue.count() - 1; i >= 0; i--) { + QList altitudes; + if (_getAltitudesForCoordinates(_requestQueue[i].coordinates, altitudes)) { + if (_requestQueue[i].queryMode == QueryMode::QueryModeCoordinates) { + _requestQueue[i].terrainQueryInterface->_signalCoordinateHeights(_requestQueue[i].coordinates.count() == altitudes.count(), altitudes); + } + _requestQueue.removeAt(i); + } + } +} + +QString TerrainTileManager::_getTileHash(const QGeoCoordinate& coordinate) +{ + QString ret = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1); + qCDebug(TerrainQueryLog) << "Computing unique tile hash for " << coordinate << ret; + + return ret; +} + +TerrainAtCoordinateBatchManager::TerrainAtCoordinateBatchManager(void) +{ + _batchTimer.setSingleShot(true); + _batchTimer.setInterval(_batchTimeout); + connect(&_batchTimer, &QTimer::timeout, this, &TerrainAtCoordinateBatchManager::_sendNextBatch); + connect(&_terrainQuery, &TerrainQueryInterface::coordinateHeights, this, &TerrainAtCoordinateBatchManager::_coordinateHeights); +} + +void TerrainAtCoordinateBatchManager::addQuery(TerrainAtCoordinateQuery* terrainAtCoordinateQuery, const QList& coordinates) +{ + if (coordinates.length() > 0) { + qCDebug(TerrainQueryLog) << "addQuery: TerrainAtCoordinateQuery:coordinates.count" << terrainAtCoordinateQuery << coordinates.count(); + connect(terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed); + QueuedRequestInfo_t queuedRequestInfo = { terrainAtCoordinateQuery, coordinates }; + _requestQueue.append(queuedRequestInfo); + if (!_batchTimer.isActive()) { + _batchTimer.start(); + } + } +} + +void TerrainAtCoordinateBatchManager::_sendNextBatch(void) +{ + qCDebug(TerrainQueryLog) << "_sendNextBatch _state:_requestQueue.count:_sentRequests.count" << _stateToString(_state) << _requestQueue.count() << _sentRequests.count(); + + if (_state != State::Idle) { + // Waiting for last download the complete, wait some more + _batchTimer.start(); + return; + } + + if (_requestQueue.count() == 0) { + return; + } + + _sentRequests.clear(); + + // Convert coordinates to point strings for json query + QList coords; + foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { + SentRequestInfo_t sentRequestInfo = { requestInfo.terrainAtCoordinateQuery, false, requestInfo.coordinates.count() }; + qCDebug(TerrainQueryLog) << "Building request: coordinate count" << requestInfo.coordinates.count(); + _sentRequests.append(sentRequestInfo); + + coords += requestInfo.coordinates; + } + _requestQueue.clear(); + + _terrainQuery.requestCoordinateHeights(coords); + _state = State::Downloading; +} + +void TerrainAtCoordinateBatchManager::_batchFailed(void) +{ + QList noHeights; + + foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { + if (!sentRequestInfo.queryObjectDestroyed) { + disconnect(sentRequestInfo.terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed); + sentRequestInfo.terrainAtCoordinateQuery->_signalTerrainData(false, noHeights); + } + } + _sentRequests.clear(); +} + +void TerrainAtCoordinateBatchManager::_queryObjectDestroyed(QObject* terrainAtCoordinateQuery) +{ + // Remove/Mark deleted objects queries from queues + + qCDebug(TerrainQueryLog) << "_TerrainAtCoordinateQueryDestroyed TerrainAtCoordinateQuery" << terrainAtCoordinateQuery; + + int i = 0; + while (i < _requestQueue.count()) { + const QueuedRequestInfo_t& requestInfo = _requestQueue[i]; + if (requestInfo.terrainAtCoordinateQuery == terrainAtCoordinateQuery) { + qCDebug(TerrainQueryLog) << "Removing deleted provider from _requestQueue index:terrainAtCoordinateQuery" << i << requestInfo.terrainAtCoordinateQuery; + _requestQueue.removeAt(i); + } else { + i++; + } + } + + for (int i=0; i<_sentRequests.count(); i++) { + SentRequestInfo_t& sentRequestInfo = _sentRequests[i]; + if (sentRequestInfo.terrainAtCoordinateQuery == terrainAtCoordinateQuery) { + qCDebug(TerrainQueryLog) << "Zombieing deleted provider from _sentRequests index:terrainAtCoordinateQuery" << sentRequestInfo.terrainAtCoordinateQuery; + sentRequestInfo.queryObjectDestroyed = true; + } + } +} + +QString TerrainAtCoordinateBatchManager::_stateToString(State state) +{ + switch (state) { + case State::Idle: + return QStringLiteral("Idle"); + case State::Downloading: + return QStringLiteral("Downloading"); + } + + return QStringLiteral("State unknown"); +} + +void TerrainAtCoordinateBatchManager::_coordinateHeights(bool success, QList heights) +{ + _state = State::Idle; + + if (!success) { + _batchFailed(); + return; + } + + int currentIndex = 0; + foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { + if (!sentRequestInfo.queryObjectDestroyed) { + disconnect(sentRequestInfo.terrainAtCoordinateQuery, &TerrainAtCoordinateQuery::destroyed, this, &TerrainAtCoordinateBatchManager::_queryObjectDestroyed); + QList requestAltitudes = heights.mid(currentIndex, sentRequestInfo.cCoord); + sentRequestInfo.terrainAtCoordinateQuery->_signalTerrainData(true, requestAltitudes); + currentIndex += sentRequestInfo.cCoord; + } + } + _sentRequests.clear(); +} + +TerrainAtCoordinateQuery::TerrainAtCoordinateQuery(QObject* parent) + : QObject(parent) +{ + +} +void TerrainAtCoordinateQuery::requestData(const QList& coordinates) +{ + if (coordinates.length() == 0) { + return; + } + + _TerrainAtCoordinateBatchManager->addQuery(this, coordinates); +} + +void TerrainAtCoordinateQuery::_signalTerrainData(bool success, QList& heights) +{ + emit terrainData(success, heights); +} + +TerrainPathQuery::TerrainPathQuery(QObject* parent) + : QObject(parent) +{ + qRegisterMetaType(); + connect(&_terrainQuery, &TerrainQueryInterface::pathHeights, this, &TerrainPathQuery::_pathHeights); +} + +void TerrainPathQuery::requestData(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) +{ + _terrainQuery.requestPathHeights(fromCoord, toCoord); +} + +void TerrainPathQuery::_pathHeights(bool success, double latStep, double lonStep, const QList& heights) +{ + PathHeightInfo_t pathHeightInfo; + pathHeightInfo.latStep = latStep; + pathHeightInfo.lonStep = lonStep; + pathHeightInfo.heights = heights; + emit terrainData(success, pathHeightInfo); +} + +TerrainPolyPathQuery::TerrainPolyPathQuery(QObject* parent) + : QObject (parent) + , _curIndex (0) +{ + connect(&_pathQuery, &TerrainPathQuery::terrainData, this, &TerrainPolyPathQuery::_terrainDataReceived); +} + +void TerrainPolyPathQuery::requestData(const QVariantList& polyPath) +{ + QList path; + + foreach (const QVariant& geoVar, polyPath) { + path.append(geoVar.value()); + } + + requestData(path); +} + +void TerrainPolyPathQuery::requestData(const QList& polyPath) +{ + // Kick off first request + _rgCoords = polyPath; + _curIndex = 0; + _pathQuery.requestData(_rgCoords[0], _rgCoords[1]); +} + +void TerrainPolyPathQuery::_terrainDataReceived(bool success, const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo) +{ + if (!success) { + _rgPathHeightInfo.clear(); + emit terrainData(false /* success */, _rgPathHeightInfo); + return; + } + + _rgPathHeightInfo.append(pathHeightInfo); + + if (++_curIndex >= _rgCoords.count() - 1) { + // We've finished all requests + emit terrainData(true /* success */, _rgPathHeightInfo); + } else { + _pathQuery.requestData(_rgCoords[_curIndex], _rgCoords[_curIndex+1]); + } +} + +TerrainCarpetQuery::TerrainCarpetQuery(QObject* parent) + : QObject(parent) +{ + connect(&_terrainQuery, &TerrainQueryInterface::carpetHeights, this, &TerrainCarpetQuery::terrainData); +} + +void TerrainCarpetQuery::requestData(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) +{ + _terrainQuery.requestCarpetHeights(swCoord, neCoord, statsOnly); +} diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h new file mode 100644 index 0000000000000000000000000000000000000000..31cc01de30353c5d37ec59fc2c4a65ddcec5a147 --- /dev/null +++ b/src/Terrain/TerrainQuery.h @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * (c) 2017 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 "TerrainTile.h" +#include "QGCMapEngineData.h" +#include "QGCLoggingCategory.h" + +#include +#include +#include +#include +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(TerrainQueryLog) + +class TerrainAtCoordinateQuery; + +/// Base class for offline/online terrain queries +class TerrainQueryInterface : public QObject +{ + Q_OBJECT + +public: + TerrainQueryInterface(QObject* parent) : QObject(parent) { } + + /// Request terrain heights for specified coodinates. + /// Signals: coordinateHeights when data is available + virtual void requestCoordinateHeights(const QList& coordinates) = 0; + + /// Requests terrain heights along the path specified by the two coordinates. + /// Signals: pathHeights + /// @param coordinates to query + virtual void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) = 0; + + /// Request terrain heights for the rectangular area specified. + /// Signals: carpetHeights when data is available + /// @param swCoord South-West bound of rectangular area to query + /// @param neCoord North-East bound of rectangular area to query + /// @param statsOnly true: Return only stats, no carpet data + virtual void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) = 0; + +signals: + void coordinateHeights(bool success, QList heights); + void pathHeights(bool success, double latStep, double lonStep, const QList& heights); + void carpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet); +}; + +/// AirMap online implementation of terrain queries +class TerrainAirMapQuery : public TerrainQueryInterface { + Q_OBJECT + +public: + TerrainAirMapQuery(QObject* parent = NULL); + + // Overrides from TerrainQueryInterface + void requestCoordinateHeights(const QList& coordinates) final; + void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) final; + void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) final; + +private slots: + void _requestFinished(void); + +private: + void _sendQuery (const QString& path, const QUrlQuery& urlQuery); + void _requestFailed (void); + void _parseCoordinateData (const QJsonValue& coordinateJson); + void _parsePathData (const QJsonValue& pathJson); + void _parseCarpetData (const QJsonValue& carpetJson); + + enum QueryMode { + QueryModeCoordinates, + QueryModePath, + QueryModeCarpet + }; + + QNetworkAccessManager _networkManager; + QueryMode _queryMode; + bool _carpetStatsOnly; +}; + +/// AirMap online implementation of terrain queries +class TerrainOfflineAirMapQuery : public TerrainQueryInterface { + Q_OBJECT + +public: + TerrainOfflineAirMapQuery(QObject* parent = NULL); + + // Overrides from TerrainQueryInterface + void requestCoordinateHeights(const QList& coordinates) final; + void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) final; + void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) final; + + // Internal method + void _signalCoordinateHeights(bool success, QList heights); + void _signalPathHeights(bool success, double latStep, double lonStep, const QList& heights); + void _signalCarpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet); + + bool _carpetStatsOnly; +}; + +/// Used internally by TerrainOfflineAirMapQuery to manage terrain tiles +class TerrainTileManager : public QObject { + Q_OBJECT + +public: + TerrainTileManager(void); + + void addQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); + +private slots: + void _fetchedTile (void); /// slot to handle fetched elevation tiles + +private: + typedef struct { + TerrainOfflineAirMapQuery* terrainQueryInterface; + QList coordinates; + QueryMode queryMode; + } QueuedRequestInfo_t; + + enum class State { + Idle, + Downloading, + }; + + enum QueryMode { + QueryModeCoordinates, + QueryModePath, + QueryModeCarpet + }; + + void _tileFailed(void); + bool _getAltitudesForCoordinates(const QList& coordinates, QList& altitudes); + QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile + + QList _requestQueue; + State _state = State::Idle; + QNetworkAccessManager _networkManager; + + QMutex _tilesMutex; + QHash _tiles; +}; + +/// Used internally by TerrainAtCoordinateQuery to batch coordinate requests together +class TerrainAtCoordinateBatchManager : public QObject { + Q_OBJECT + +public: + TerrainAtCoordinateBatchManager(void); + + void addQuery(TerrainAtCoordinateQuery* terrainAtCoordinateQuery, const QList& coordinates); + +private slots: + void _sendNextBatch (void); + void _queryObjectDestroyed (QObject* elevationProvider); + void _coordinateHeights (bool success, QList heights); + +private: + typedef struct { + TerrainAtCoordinateQuery* terrainAtCoordinateQuery; + QList coordinates; + } QueuedRequestInfo_t; + + typedef struct { + TerrainAtCoordinateQuery* terrainAtCoordinateQuery; + bool queryObjectDestroyed; + int cCoord; + } SentRequestInfo_t; + + + enum class State { + Idle, + Downloading, + }; + + void _batchFailed(void); + QString _stateToString(State state); + + QList _requestQueue; + QList _sentRequests; + State _state = State::Idle; + const int _batchTimeout = 500; + QTimer _batchTimer; + TerrainAirMapQuery _terrainQuery; +}; + +/// NOTE: TerrainAtCoordinateQuery is not thread safe. All instances/calls to ElevationProvider must be on main thread. +class TerrainAtCoordinateQuery : public QObject +{ + Q_OBJECT +public: + TerrainAtCoordinateQuery(QObject* parent = NULL); + + /// Async terrain query for a list of lon,lat coordinates. When the query is done, the terrainData() signal + /// is emitted. + /// @param coordinates to query + void requestData(const QList& coordinates); + + // Internal method + void _signalTerrainData(bool success, QList& heights); + +signals: + void terrainData(bool success, QList heights); +}; + +class TerrainPathQuery : public QObject +{ + Q_OBJECT + +public: + TerrainPathQuery(QObject* parent = NULL); + + /// Async terrain query for terrain heights between two lat/lon coordinates. When the query is done, the terrainData() signal + /// is emitted. + /// @param coordinates to query + void requestData(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord); + + typedef struct { + double latStep; ///< Amount of latitudinal distance between each returned height + double lonStep; ///< Amount of longitudinal distance between each returned height + QList heights; ///< Terrain heights along path + } PathHeightInfo_t; + +signals: + /// Signalled when terrain data comes back from server + void terrainData(bool success, const PathHeightInfo_t& pathHeightInfo); + +private slots: + void _pathHeights(bool success, double latStep, double lonStep, const QList& heights); + +private: + TerrainAirMapQuery _terrainQuery; +}; + +Q_DECLARE_METATYPE(TerrainPathQuery::PathHeightInfo_t) + +class TerrainPolyPathQuery : public QObject +{ + Q_OBJECT + +public: + TerrainPolyPathQuery(QObject* parent = NULL); + + /// Async terrain query for terrain heights for the paths between each specified QGeoCoordinate. + /// When the query is done, the terrainData() signal is emitted. + /// @param polyPath List of QGeoCoordinate + void requestData(const QVariantList& polyPath); + void requestData(const QList& polyPath); + +signals: + /// Signalled when terrain data comes back from server + void terrainData(bool success, const QList& rgPathHeightInfo); + +private slots: + void _terrainDataReceived(bool success, const TerrainPathQuery::PathHeightInfo_t& pathHeightInfo); + +private: + int _curIndex; + QList _rgCoords; + QList _rgPathHeightInfo; + TerrainPathQuery _pathQuery; +}; + + +class TerrainCarpetQuery : public QObject +{ + Q_OBJECT + +public: + TerrainCarpetQuery(QObject* parent = NULL); + + /// Async terrain query for terrain information bounded by the specifed corners. + /// When the query is done, the terrainData() signal is emitted. + /// @param swCoord South-West bound of rectangular area to query + /// @param neCoord North-East bound of rectangular area to query + /// @param statsOnly true: Return only stats, no carpet data + void requestData(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly); + +signals: + /// Signalled when terrain data comes back from server + void terrainData(bool success, double minHeight, double maxHeight, const QList>& carpet); + +private: + TerrainAirMapQuery _terrainQuery; +}; + diff --git a/src/VideoStreaming/README.md b/src/VideoStreaming/README.md index 585cb9cf8805fa004d62073531028acf94961d8f..eb8d8f45c6936ee13c23a4d38b762d94401e98d2 100644 --- a/src/VideoStreaming/README.md +++ b/src/VideoStreaming/README.md @@ -40,6 +40,9 @@ list=$(apt-cache --names-only search ^gstreamer1.0-* | awk '{ print $1 }' | grep ``` sudo apt-get install $list ``` +``` +sudo apt-get install libgstreamer-plugins-base1.0-dev +``` The build system is setup to use pkgconfig and it will find the necessary headers and libraries automatically. diff --git a/src/VideoStreaming/VideoStreaming.pri b/src/VideoStreaming/VideoStreaming.pri index b6b4324f3471fbbec7ca0601729022ae8c865d42..9474c54baa128b267ff61a95a779148c3223e989 100644 --- a/src/VideoStreaming/VideoStreaming.pri +++ b/src/VideoStreaming/VideoStreaming.pri @@ -181,24 +181,7 @@ VideoEnabled { } else { LinuxBuild|MacBuild|iOSBuild|WindowsBuild|AndroidBuild { message("Skipping support for video streaming (GStreamer libraries not installed)") - MacBuild { - message(" You can download it from http://gstreamer.freedesktop.org/data/pkg/osx/") - message(" Select the devel package and install it (gstreamer-1.0-devel-1.x.x-x86_64.pkg)") - message(" It will be installed in /Libraries/Frameworks") - } - LinuxBuild { - message(" You can install it using apt-get") - message(" sudo apt-get install gstreamer1.0*") - } - WindowsBuild { - message(" You can download it from http://gstreamer.freedesktop.org/data/pkg/windows/") - message(" Select the devel AND runtime packages and install them (x86, not the 64-Bit)") - message(" It will be installed in C:/gstreamer. You need to update you PATH to point to the bin directory.") - } - AndroidBuild { - message(" You can download it from http://gstreamer.freedesktop.org/data/pkg/android/") - message(" Uncompress the archive into the qgc root source directory (same directory where qgroundcontrol.pro is found.") - } + message("Installation instructions here: https://github.com/mavlink/qgroundcontrol/blob/master/src/VideoStreaming/README.md") } else { message("Skipping support for video streaming (Unsupported platform)") } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9ad225d6a9e48914c7d531b5aaf8cf026421bdc5..53a381b81ebe55050b5bea5472e820a18408419c 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -35,9 +35,15 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle // testing of a gazebo vehicle and a mocklink vehicle -double MockLink::_defaultVehicleLatitude = 47.397f; -double MockLink::_defaultVehicleLongitude = 8.5455f; -double MockLink::_defaultVehicleAltitude = 488.056f; +#if 1 +double MockLink::_defaultVehicleLatitude = 47.397; +double MockLink::_defaultVehicleLongitude = 8.5455; +double MockLink::_defaultVehicleAltitude = 488.056; +#else +double MockLink::_defaultVehicleLatitude = 47.6333022928789; +double MockLink::_defaultVehicleLongitude = -122.08833157994995; +double MockLink::_defaultVehicleAltitude = 19.0; +#endif int MockLink::_nextVehicleSystemId = 128; const char* MockLink::_failParam = "COM_FLTMODE6";