diff --git a/ChangeLog.md b/ChangeLog.md index f035c450f1531fe8c8002bb04fe38dd1778c8a92..3a49d9eda786bdb4cf7cf09bf966e04c665791f1 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -10,6 +10,8 @@ Note: This file only contains high level features or important fixes. ### 4.0.6 - Not yet released +* Plan: Much better conversion of missions to KML for 3d visualization/verification of missions + ### 4.0.5 - Stable * Solo: Fix mission upload failures diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index eeb4682cbedd09c9da808d7a3a9de04891f06621..2f2eab3c2a1333dc03f485f72d38919c2b599b52 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -587,7 +587,7 @@ HEADERS += \ src/MissionManager/FixedWingLandingComplexItem.h \ src/MissionManager/GeoFenceController.h \ src/MissionManager/GeoFenceManager.h \ - src/MissionManager/KML.h \ + src/MissionManager/KMLPlanDomDocument.h \ src/MissionManager/MissionCommandList.h \ src/MissionManager/MissionCommandTree.h \ src/MissionManager/MissionCommandUIInfo.h \ @@ -795,7 +795,7 @@ SOURCES += \ src/MissionManager/FixedWingLandingComplexItem.cc \ src/MissionManager/GeoFenceController.cc \ src/MissionManager/GeoFenceManager.cc \ - src/MissionManager/KML.cc \ + src/MissionManager/KMLPlanDomDocument.cc \ src/MissionManager/MissionCommandList.cc \ src/MissionManager/MissionCommandTree.cc \ src/MissionManager/MissionCommandUIInfo.cc \ diff --git a/src/MissionManager/KML.cc b/src/MissionManager/KML.cc deleted file mode 100644 index ed2ffaab119b99dbdc3a4a35d317e13faac861ac..0000000000000000000000000000000000000000 --- a/src/MissionManager/KML.cc +++ /dev/null @@ -1,128 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "KML.h" - -#include -#include - -const QString Kml::_version("version=\"1.0\""); -const QString Kml::_encoding("encoding=\"UTF-8\""); -const QString Kml::_opengis("http://www.opengis.net/kml/2.2"); -const QString Kml::_qgckml("QGC KML"); - -Kml::Kml() -{ - //create header - createHeader(); - //name - createTextElement(_docEle, "name", _qgckml); - //open - createTextElement(_docEle, "open", "1"); - //create style - createStyles(); -} - -void Kml::points(const QStringList& points) -{ - //create placemark - QDomElement placemark = _domDocument.createElement("Placemark"); - _docEle.appendChild(placemark); - createTextElement(placemark, "styleUrl", "yellowLineGreenPoly"); - createTextElement(placemark, "name", "Absolute"); - createTextElement(placemark, "visibility", "0"); - createTextElement(placemark, "description", "Transparent purple line"); - - QStringList latLonAlt = points[0].split(","); - QStringList lookAtList({latLonAlt[0], latLonAlt[1], "0" \ - , "-100", "45", "2500"}); - createLookAt(placemark, lookAtList); - - //Add linestring - QDomElement lineString = _domDocument.createElement("LineString"); - placemark.appendChild(lineString); - - //extruder - createTextElement(lineString, "extruder", "1"); - createTextElement(lineString, "tessellate", "1"); - createTextElement(lineString, "altitudeMode", "absolute"); - QString coordinates; - for(const auto& point : points) { - coordinates += point + "\n"; - } - createTextElement(lineString, "coordinates", coordinates); -} - -void Kml::save(QDomDocument& document) -{ - document = _domDocument; -} - -void Kml::createHeader() -{ - QDomProcessingInstruction header = _domDocument.createProcessingInstruction("xml", _version + " " + _encoding); - _domDocument.appendChild(header); - QDomElement kml = _domDocument.createElement("kml"); - kml.setAttribute("xmlns", _opengis); - _docEle = _domDocument.createElement("Document"); - kml.appendChild(_docEle); - _domDocument.appendChild(kml); -} - -void Kml::createStyles() -{ - QDomElement style = _domDocument.createElement("Style"); - style.setAttribute("id", "yellowLineGreenPoly"); - createStyleLine(style, "7f00ffff", "4", "7f00ff00"); - _docEle.appendChild(style); -} - -void Kml::createLookAt(QDomElement& placemark, const QStringList &lookAtList) -{ - QDomElement lookAt = _domDocument.createElement("LookAt"); - placemark.appendChild(lookAt); - createTextElement(lookAt, "longitude", lookAtList[0]); - createTextElement(lookAt, "latitude", lookAtList[1]); - createTextElement(lookAt, "altitude", lookAtList[2]); - createTextElement(lookAt, "heading", lookAtList[3]); - createTextElement(lookAt, "tilt", lookAtList[4]); - createTextElement(lookAt, "range", lookAtList[5]); -} - -void Kml::createTextElement(QDomElement& domEle, const QString& elementName, const QString& textElement) -{ - // textElement - auto element = _domDocument.createElement(elementName); - element.appendChild(_domDocument.createTextNode(textElement)); - domEle.appendChild(element); -} - -void Kml::createStyleLine(QDomElement& domEle, const QString& lineColor, const QString& lineWidth, const QString& polyColor) -{ - /* - - 7f00ffff - 4 - - - 7f00ff00 - - */ - auto lineStyle = _domDocument.createElement("LineStyle"); - auto polyStyle = _domDocument.createElement("PolyStyle"); - domEle.appendChild(lineStyle); - domEle.appendChild(polyStyle); - createTextElement(lineStyle, "color", lineColor); - createTextElement(lineStyle, "width", lineWidth); - createTextElement(polyStyle, "color", polyColor); -} - -Kml::~Kml() -{ -} diff --git a/src/MissionManager/KML.h b/src/MissionManager/KML.h deleted file mode 100644 index 88e08a3fad292613b6404a9f61f56a2aa755d48e..0000000000000000000000000000000000000000 --- a/src/MissionManager/KML.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#ifndef KML_H -#define KML_H - -#include -#include - -class Kml -{ - -public: - Kml(); - ~Kml(); - - void points(const QStringList& points); - void polygon(const QStringList& points); - void save(QDomDocument& document); - -private: - void createHeader(); - void createLookAt(QDomElement& placemark, const QStringList &lookAtList); - void createStyles(); - void createStyleLine(QDomElement& domEle, const QString& lineColor, const QString& lineWidth, const QString& polyColor); - void createTextElement(QDomElement& domEle, const QString& elementName, const QString& textElement); - - QDomDocument _domDocument; - QDomElement _docEle; - static const QString _encoding; - static const QString _opengis; - static const QString _qgckml; - static const QString _version; -}; - -#endif diff --git a/src/MissionManager/KMLPlanDomDocument.cc b/src/MissionManager/KMLPlanDomDocument.cc new file mode 100644 index 0000000000000000000000000000000000000000..25e3715101f7193e1d707b86f757f544c1971195 --- /dev/null +++ b/src/MissionManager/KMLPlanDomDocument.cc @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "KMLPlanDomDocument.h" +#include "QGCPalette.h" +#include "QGCApplication.h" +#include "MissionCommandTree.h" +#include "MissionCommandUIInfo.h" +#include "FactMetaData.h" + +#include +#include + +const char* KMLPlanDomDocument::_missionLineStyleName = "MissionLineStyle"; +const char* KMLPlanDomDocument::_ballonStyleName = "BalloonStyle"; + +KMLPlanDomDocument::KMLPlanDomDocument() +{ + QDomProcessingInstruction header = createProcessingInstruction(QStringLiteral("xml"), QStringLiteral("version=\"1.0\" encoding=\"UTF-8\"")); + appendChild(header); + + QDomElement kmlElement = createElement(QStringLiteral("kml")); + kmlElement.setAttribute(QStringLiteral("xmlns"), "http://www.opengis.net/kml/2.2"); + + _documentElement = createElement(QStringLiteral("Document")); + kmlElement.appendChild(_documentElement); + appendChild(kmlElement); + + _addTextElement(_documentElement, "name", QStringLiteral("%1 Plan KML").arg(qgcApp()->applicationName())); + _addTextElement(_documentElement, "open", "1"); + + _addStyles(); +} + +QString KMLPlanDomDocument::_kmlCoordString(const QGeoCoordinate& coord) +{ + return QStringLiteral("%1,%2,%3").arg(QString::number(coord.longitude(), 'f', 7)).arg(QString::number(coord.latitude(), 'f', 7)).arg(QString::number(coord.altitude(), 'f', 2)); +} + +void KMLPlanDomDocument::addMissionItems(Vehicle* vehicle, QList rgMissionItems) +{ + if (rgMissionItems.count() == 0) { + return; + } + + QDomElement itemFolderElement = createElement("Folder"); + _documentElement.appendChild(itemFolderElement); + + _addTextElement(itemFolderElement, "name", "Items"); + + QDomElement flightPathElement = createElement("Placemark"); + _documentElement.appendChild(flightPathElement); + + _addTextElement(flightPathElement, "styleUrl", QStringLiteral("#%1").arg(_missionLineStyleName)); + _addTextElement(flightPathElement, "name", "Flight Path"); + _addTextElement(flightPathElement, "visibility", "1"); + _addLookAt(flightPathElement, rgMissionItems[0]->coordinate()); + + // Build up the mission trajectory line coords + QList rgFlightCoords; + QGeoCoordinate homeCoord = rgMissionItems[0]->coordinate(); + for (const MissionItem* item : rgMissionItems) { + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(vehicle, item->command()); + if (uiInfo) { + double altAdjustment = item->frame() == MAV_FRAME_GLOBAL ? 0 : homeCoord.altitude(); // Used to convert to amsl + if (uiInfo->isTakeoffCommand() && !vehicle->fixedWing()) { + // These takeoff items go straight up from home position to specified altitude + QGeoCoordinate coord = homeCoord; + coord.setAltitude(item->param7() + altAdjustment); + rgFlightCoords += coord; + } + if (uiInfo->specifiesCoordinate()) { + QGeoCoordinate coord = item->coordinate(); + coord.setAltitude(coord.altitude() + altAdjustment); // convert to amsl + + if (!uiInfo->isStandaloneCoordinate()) { + // Flight path goes through this item + rgFlightCoords += coord; + } + + // Add a place mark for each WP + + QDomElement wpPlacemarkElement = createElement("Placemark"); + _addTextElement(wpPlacemarkElement, "name", QStringLiteral("%1 %2").arg(QString::number(item->sequenceNumber())).arg(item->command() == MAV_CMD_NAV_WAYPOINT ? "" : uiInfo->friendlyName())); + _addTextElement(wpPlacemarkElement, "styleUrl", QStringLiteral("#%1").arg(_ballonStyleName)); + + QDomElement wpPointElement = createElement("Point"); + _addTextElement(wpPointElement, "altitudeMode", "absolute"); + _addTextElement(wpPointElement, "coordinates", _kmlCoordString(coord)); + _addTextElement(wpPointElement, "extrude", "1"); + + QDomElement descriptionElement = createElement("description"); + QString htmlString; + htmlString += QStringLiteral("Index: %1\n").arg(item->sequenceNumber()); + htmlString += uiInfo->friendlyName() + "\n"; + htmlString += QStringLiteral("Alt AMSL: %1 %2\n").arg(QString::number(FactMetaData::metersToAppSettingsDistanceUnits(coord.altitude()).toDouble(), 'f', 2)).arg(FactMetaData::appSettingsDistanceUnitsString()); + htmlString += QStringLiteral("Alt Rel: %1 %2\n").arg(QString::number(FactMetaData::metersToAppSettingsDistanceUnits(coord.altitude() - homeCoord.altitude()).toDouble(), 'f', 2)).arg(FactMetaData::appSettingsDistanceUnitsString()); + htmlString += QStringLiteral("Lat: %1\n").arg(QString::number(coord.latitude(), 'f', 7)); + htmlString += QStringLiteral("Lon: %1\n").arg(QString::number(coord.longitude(), 'f', 7)); + QDomCDATASection cdataSection = createCDATASection(htmlString); + descriptionElement.appendChild(cdataSection); + + wpPlacemarkElement.appendChild(descriptionElement); + wpPlacemarkElement.appendChild(wpPointElement); + itemFolderElement.appendChild(wpPlacemarkElement); + } + } + } + + // Create a LineString element from the coords + + QDomElement lineStringElement = createElement("LineString"); + flightPathElement.appendChild(lineStringElement); + + _addTextElement(lineStringElement, "extruder", "1"); + _addTextElement(lineStringElement, "tessellate", "1"); + _addTextElement(lineStringElement, "altitudeMode", "absolute"); + + QString coordString; + for (const QGeoCoordinate& coord : rgFlightCoords) { + coordString += QStringLiteral("%1\n").arg(_kmlCoordString(coord)); + } + _addTextElement(lineStringElement, "coordinates", coordString); +} + +QString KMLPlanDomDocument::_kmlColorString (const QColor& color) +{ + return QStringLiteral("ff%1%2%3").arg(color.blue(), 2, 16, QChar('0')).arg(color.green(), 2, 16, QChar('0')).arg(color.red(), 2, 16, QChar('0')); +} + +void KMLPlanDomDocument::_addStyles(void) +{ + QGCPalette palette; + + QDomElement styleElement1 = createElement("Style"); + styleElement1.setAttribute("id", _missionLineStyleName); + QDomElement lineStyleElement = createElement("LineStyle"); + _addTextElement(lineStyleElement, "color", _kmlColorString(palette.mapMissionTrajectory())); + _addTextElement(lineStyleElement, "width", "4"); + styleElement1.appendChild(lineStyleElement); + + QDomElement styleElement2 = createElement("Style"); + styleElement2.setAttribute("id", _ballonStyleName); + QDomElement balloonStyleElement = createElement("BalloonStyle"); + _addTextElement(balloonStyleElement, "text", "$[description]"); + styleElement2.appendChild(balloonStyleElement); + + _documentElement.appendChild(styleElement1); + _documentElement.appendChild(styleElement2); +} + +void KMLPlanDomDocument::_addTextElement(QDomElement &element, const QString &name, const QString &value) +{ + QDomElement textElement = createElement(name); + textElement.appendChild(createTextNode(value)); + element.appendChild(textElement); +} + +void KMLPlanDomDocument::_addLookAt(QDomElement& element, const QGeoCoordinate& coord) +{ + QDomElement lookAtElement = createElement("LookAt"); + _addTextElement(lookAtElement, "latitude", QString::number(coord.latitude(), 'f', 7)); + _addTextElement(lookAtElement, "longitude", QString::number(coord.longitude(), 'f', 7)); + _addTextElement(lookAtElement, "altitude", QString::number(coord.longitude(), 'f', 2)); + _addTextElement(lookAtElement, "heading", "-100"); + _addTextElement(lookAtElement, "tilt", "45"); + _addTextElement(lookAtElement, "range", "2500"); + element.appendChild(lookAtElement); +} diff --git a/src/MissionManager/KMLPlanDomDocument.h b/src/MissionManager/KMLPlanDomDocument.h new file mode 100644 index 0000000000000000000000000000000000000000..2afaf1cd9439dd250f476a14d95795455edd1bd7 --- /dev/null +++ b/src/MissionManager/KMLPlanDomDocument.h @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +class MissionItem; +class Vehicle; + +/// Used to convert a Plan to a KML document +class KMLPlanDomDocument : public QDomDocument +{ + +public: + KMLPlanDomDocument(); + + void addMissionItems(Vehicle* vehicle, QList rgMissionItems); + +private: + void _addStyles (void); + QString _kmlColorString (const QColor& color); + void _addTextElement (QDomElement& element, const QString& name, const QString& value); + QString _kmlCoordString (const QGeoCoordinate& coord); + void _addLookAt(QDomElement& element, const QGeoCoordinate& coord); + + QDomElement _documentElement; + + static const char* _missionLineStyleName; + static const char* _ballonStyleName; +}; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 27182c2cf7669770d5f1bbe0534f538a055dbe26..2b8e1fc9c466d90420613b4aafda61d761e1c12a 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -28,7 +28,7 @@ #include "MissionSettingsItem.h" #include "QGCQGeoCoordinate.h" #include "PlanMasterController.h" -#include "KML.h" +#include "KMLPlanDomDocument.h" #include "QGCCorePlugin.h" #include "TakeoffMissionItem.h" #include "PlanViewSettings.h" @@ -273,46 +273,14 @@ bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMission return endActionSet; } -void MissionController::convertToKMLDocument(QDomDocument& document) +void MissionController::addMissionToKML(KMLPlanDomDocument& planKML) { QObject* deleteParent = new QObject(); QList rgMissionItems; _convertToMissionItems(_visualItems, rgMissionItems, deleteParent); - if (rgMissionItems.count() == 0) { - return; - } - - const double homePositionAltitude = _settingsItem->coordinate().altitude(); - - QString coord; - QStringList coords; - // Drop home position - bool dropPoint = true; - for(const auto& item : rgMissionItems) { - if(dropPoint) { - dropPoint = false; - continue; - } - const MissionCommandUIInfo* uiInfo = \ - qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command()); - - if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) { - double amslAltitude = item->param7() + (item->frame() == MAV_FRAME_GLOBAL ? 0 : homePositionAltitude); - coord = QString::number(item->param6(),'f',7) \ - + "," \ - + QString::number(item->param5(),'f',7) \ - + "," \ - + QString::number(amslAltitude,'f',2); - coords.append(coord); - } - } - + planKML.addMissionItems(_controllerVehicle, rgMissionItems); deleteParent->deleteLater(); - - Kml kml; - kml.points(coords); - kml.save(document); } void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems) diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 2274408d6f3c9f86e38e9a09acc1a7625f5bd0c2..c85e726b0079f860ee4bb93b6d0eeba11ba3a85d 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -13,7 +13,7 @@ #include "QmlObjectListModel.h" #include "Vehicle.h" #include "QGCLoggingCategory.h" - +#include "KMLPlanDomDocument.h" #include "QGCGeoBoundingCube.h" #include @@ -195,7 +195,7 @@ public: bool showPlanFromManagerVehicle (void) final; // Create KML file - void convertToKMLDocument(QDomDocument& document); + void addMissionToKML(KMLPlanDomDocument& planKML); // Property accessors diff --git a/src/MissionManager/MissionSettingsTest.cc b/src/MissionManager/MissionSettingsTest.cc index 40e45be9db6595d099035777deca364b9fd36456..994d8b9eb5b3874cb99187830b3c2fa4c374615c 100644 --- a/src/MissionManager/MissionSettingsTest.cc +++ b/src/MissionManager/MissionSettingsTest.cc @@ -9,7 +9,6 @@ #include "MissionSettingsTest.h" #include "QGCApplication.h" -#include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" MissionSettingsTest::MissionSettingsTest(void) diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index 953e7f46778042dc8256bdf790a9c202413036e5..0bebc7424dea49bac90bcfcd52860615f4f5f576 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -15,7 +15,7 @@ #include "AppSettings.h" #include "JsonHelper.h" #include "MissionManager.h" -#include "KML.h" +#include "KMLPlanDomDocument.h" #include "SurveyPlanCreator.h" #include "StructureScanPlanCreator.h" #include "CorridorScanPlanCreator.h" @@ -492,10 +492,10 @@ void PlanMasterController::saveToKml(const QString& filename) if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { qgcApp()->showMessage(tr("KML save error %1 : %2").arg(filename).arg(file.errorString())); } else { - QDomDocument domDocument; - _missionController.convertToKMLDocument(domDocument); + KMLPlanDomDocument planKML; + _missionController.addMissionToKML(planKML); QTextStream stream(&file); - stream << domDocument.toString(); + stream << planKML.toString(); file.close(); } }