diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index a9e7c8e9853bbfba734cb44aa4b0b2795410f3a2..fcdf43b5342f9ec4ea6c9ab54cea42a571d7552a 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -27,7 +27,6 @@ src/FlightDisplay/FlightDisplayViewUVC.qml src/AutoPilotPlugins/PX4/FlightModesComponentSummary.qml src/ui/preferences/GeneralSettings.qml - src/MissionEditor/GeoFenceEditor.qml src/AnalyzeView/GeoTagPage.qml src/VehicleSetup/JoystickConfig.qml src/ui/preferences/LinkSettings.qml @@ -57,6 +56,8 @@ src/QmlControls/FlightModeDropdown.qml src/QmlControls/FlightModeMenu.qml src/MissionEditor/FWLandingPatternMapVisual.qml + src/MissionEditor/GeoFenceEditor.qml + src/MissionEditor/GeoFenceMapVisuals.qml src/QmlControls/GuidedBar.qml src/QmlControls/IndicatorButton.qml src/QmlControls/JoystickThumbPad.qml diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index f707bc32da80097fa4b6d816bb059dc63baac057..be7540e1af620b4b8723e23852c5a01e5ff4a3ec 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -13,6 +13,8 @@ #include "MAVLinkProtocol.h" #include "QGCApplication.h" #include "ParameterManager.h" +#include "QmlObjectListModel.h" +#include "QGCQGeoCoordinate.h" const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION"; @@ -44,7 +46,7 @@ APMGeoFenceManager::~APMGeoFenceManager() } -void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const QList& polygon) +void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) { if (_vehicle->isOfflineEditingVehicle()) { return; @@ -61,21 +63,20 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const // Validate int validatedPolygonCount = 0; - if (polygonEnabled()) { - if (polygon.count() >= 3) { - validatedPolygonCount = polygon.count(); - } - if (polygon.count() > std::numeric_limits::max()) { - _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count())); - validatedPolygonCount = 0; - } + if (polygon.count() >= 3) { + validatedPolygonCount = polygon.count(); + } + if (polygon.count() > std::numeric_limits::max()) { + _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count())); + validatedPolygonCount = 0; } _breachReturnPoint = breachReturn; + _polygon.clear(); if (validatedPolygonCount) { - _polygon = polygon; - } else { - _polygon.clear(); + for (int i=0; i(i)->coordinate()); + } } // Total point count, +1 polygon close in last index, +1 for breach in index 0 @@ -306,7 +307,7 @@ void APMGeoFenceManager::_parametersReady(void) float APMGeoFenceManager::circleRadius(void) const { - if (_circleEnabled) { + if (_circleRadiusFact) { return _circleRadiusFact->rawValue().toFloat(); } else { return 0.0; @@ -328,3 +329,10 @@ QString APMGeoFenceManager::editorQml(void) const return QStringLiteral("qrc:/FirmwarePlugin/NoGeoFenceEditor.qml"); } } + +void APMGeoFenceManager::removeAll(void) +{ + QmlObjectListModel emptyPolygon; + + sendToVehicle(_breachReturnPoint, emptyPolygon); +} diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h index 0540c3e68ec8a40e03180b873f72dedf940180ab..ccdd198b387e80312c4581e7212a9df6c4e8b2ea 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.h +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.h @@ -14,6 +14,8 @@ #include "QGCMAVLink.h" #include "FactSystem.h" +class QmlObjectListModel; + class APMGeoFenceManager : public GeoFenceManager { Q_OBJECT @@ -25,7 +27,7 @@ public: // Overrides from GeoFenceManager bool inProgress (void) const final; void loadFromVehicle (void) final; - void sendToVehicle (const QGeoCoordinate& breachReturn, const QList& polygon) final; + void sendToVehicle (const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) final; bool circleEnabled (void) const final { return _circleEnabled; } bool polygonEnabled (void) const final { return _polygonEnabled; } bool breachReturnEnabled (void) const final { return _breachReturnEnabled; } @@ -33,6 +35,7 @@ public: QVariantList params (void) const final { return _params; } QStringList paramLabels (void) const final { return _paramLabels; } QString editorQml (void) const final; + void removeAll (void) final; private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc index f25aaf3cc0362e7dff4b06c00f2eb6442da4cfdd..0e44314d93e138fca67f63c3478036143c4dd476 100644 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.cc @@ -149,3 +149,10 @@ bool APMRallyPointManager::rallyPointsSupported(void) const { return _vehicle->parameterManager()->parameterExists(_vehicle->defaultComponentId(), _rallyTotalParam); } + +void APMRallyPointManager::removeAll(void) +{ + QList noRallyPoints; + + sendToVehicle(noRallyPoints); +} diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.h b/src/FirmwarePlugin/APM/APMRallyPointManager.h index 4a5ac20672ef02bff5180a817632dbe98d8c5f35..74fc7012c02bb07fbae1c19eac576177d48d2e01 100644 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.h +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.h @@ -27,6 +27,8 @@ public: void sendToVehicle (const QList& rgPoints) final; bool rallyPointsSupported (void) const final; + void removeAll (void); + QString editorQml(void) const final { return QStringLiteral("qrc:/FirmwarePlugin/APM/APMRallyPointEditor.qml"); } private slots: diff --git a/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml b/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml index a07ead19404e0ed84e41baf7b073ca80a0d34e20..e55535a81ab169bd0aefefe0f1192a46bc62d39d 100644 --- a/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml +++ b/src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml @@ -24,7 +24,6 @@ Column { anchors.right: parent.right wrapMode: Text.WordWrap text: qsTr("Click in map to set breach return point.") - visible: geoFenceController.breachReturnSupported } QGCLabel { text: qsTr("Fence Settings:") } @@ -73,14 +72,15 @@ Column { } } - QGCMapPolygonControls { - anchors.left: parent.left - anchors.right: parent.right - flightMap: editorMap - polygon: geoFenceController.polygon - sectionLabel: qsTr("Fence Polygon:") - visible: geoFenceController.polygonSupported + QGCButton { + text: qsTr("Add fence") + visible: geoFenceController.mapPolygon.count < 3 + onClicked: geoFenceController.addFence() + } - onPolygonEditCompleted: geoFenceController.validateBreachReturn() + QGCButton { + text: qsTr("Remove fence") + visible: geoFenceController.mapPolygon.count >= 3 + onClicked: geoFenceController.removeFence() } } diff --git a/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml b/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml index d3a954b9a610ff1e74d75aec279d62ff73711298..5449a2868460eb5078914b3245a700405107b078 100644 --- a/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml +++ b/src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml @@ -196,13 +196,15 @@ Column { FENCE_RET_RALLY - if set to 1 the aircraft will head to the nearest Rally Point rather than the fence return point when the fence is breached. Note that the loiter altitude of the Rally Point is used as the return altitude. */ - QGCMapPolygonControls { - anchors.left: parent.left - anchors.right: parent.right - flightMap: editorMap - polygon: geoFenceController.polygon - sectionLabel: qsTr("Fence Polygon:") + QGCButton { + text: qsTr("Add fence") + visible: geoFenceController.mapPolygon.count < 3 + onClicked: geoFenceController.addFence() + } - onPolygonEditCompleted: geoFenceController.validateBreachReturn() + QGCButton { + text: qsTr("Remove fence") + visible: geoFenceController.mapPolygon.count >= 3 + onClicked: geoFenceController.removeFence() } } diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc index 063b428baaaf33a04d69096cf051923dc123491b..c1be41589ddec4903d14fa4c735d0ce0da3dbae5 100644 --- a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc +++ b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc @@ -80,3 +80,7 @@ void PX4GeoFenceManager::_circleRadiusRawValueChanged(QVariant value) emit circleRadiusChanged(value.toFloat()); } +void PX4GeoFenceManager::removeAll(void) +{ + // Only params so nothing to do +} diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h index b29cf1038f759d7f6aa6b45ba482efae1e2bda5c..edfe955e6ad33a550bbf7c6f375763f86e6aa426 100644 --- a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h +++ b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h @@ -28,6 +28,7 @@ public: QVariantList params (void) const final { return _params; } QStringList paramLabels (void) const final { return _paramLabels; } QString editorQml (void) const final { return QStringLiteral("qrc:/FirmwarePlugin/PX4/PX4GeoFenceEditor.qml"); } + void removeAll (void) final; private slots: void _circleRadiusRawValueChanged(QVariant value); diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index be541ec65424cf10079c3d589962ce3593dd710e..6b9588cb0e85414daa077d95322e2b18e954979b 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -148,6 +148,7 @@ QGCView { anchors.fill: parent flightWidgets: flightDisplayViewWidgets rightPanelWidth: ScreenTools.defaultFontPixelHeight * 9 + qgcView: root } } diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index aaf840381fcb9666b4885d7947edf00f234a251f..36428c2ba94d7f6c0a285158662305e2168585f1 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -8,10 +8,11 @@ ****************************************************************************/ -import QtQuick 2.3 -import QtQuick.Controls 1.2 -import QtLocation 5.3 -import QtPositioning 5.3 +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 +import QtQuick.Dialogs 1.2 import QGroundControl 1.0 import QGroundControl.FlightDisplay 1.0 @@ -34,6 +35,7 @@ FlightMap { property alias missionController: missionController property var flightWidgets property var rightPanelWidth + property var qgcView ///< QGCView control which contains this map property bool _followVehicle: true property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle @@ -77,6 +79,49 @@ FlightMap { Component.onCompleted: start(false /* editMode */) } + // The following code is used to track vehicle states such that we prompt to remove mission from vehicle when mission completes + + property bool vehicleArmed: _activeVehicle ? _activeVehicle.armed : false + property bool vehicleWasArmed: false + property bool vehicleInMissionFlightMode: _activeVehicle ? (_activeVehicle.flightMode === _activeVehicle.missionFlightMode) : false + property bool promptForMissionRemove: false + + onVehicleArmedChanged: { + if (vehicleArmed) { + if (!promptForMissionRemove) { + promptForMissionRemove = vehicleInMissionFlightMode + vehicleWasArmed = true + } + } else { + if (promptForMissionRemove && (missionController.containsItems || geoFenceController.containsItems || rallyPointController.containsItems)) { + qgcView.showDialog(removeMissionDialogComponent, qsTr("Flight complete"), showDialogDefaultWidth, StandardButton.No | StandardButton.Yes) + } + promptForMissionRemove = false + } + } + + onVehicleInMissionFlightModeChanged: { + if (!promptForMissionRemove && vehicleArmed) { + promptForMissionRemove = true + } + } + + Component { + id: removeMissionDialogComponent + + QGCViewMessage { + message: qsTr("Do you want to remove the mission from the vehicle?") + + function accept() { + missionController.removeAllFromVehicle() + geoFenceController.removeAllFromVehicle() + rallyPointController.removeAllFromVehicle() + hideDialog() + + } + } + } + ExclusiveGroup { id: _mapTypeButtonsExclusiveGroup } @@ -224,32 +269,11 @@ FlightMap { model: _mainIsMap ? missionController.waypointLines : 0 } - // GeoFence polygon - MapPolygon { - border.color: "#80FF0000" - border.width: 3 - path: geoFenceController.polygon.path - visible: geoFenceController.polygonEnabled - } - - // GeoFence circle - MapCircle { - border.color: "#80FF0000" - border.width: 3 - center: missionController.plannedHomePosition - radius: geoFenceController.circleRadius - z: QGroundControl.zOrderMapItems - visible: geoFenceController.circleEnabled - } - - // GeoFence breach return point - MapQuickItem { - anchorPoint.x: sourceItem.anchorPointX - anchorPoint.y: sourceItem.anchorPointY - coordinate: geoFenceController.breachReturnPoint - visible: geoFenceController.breachReturnEnabled - sourceItem: MissionItemIndexLabel { label: "F" } - z: QGroundControl.zOrderMapItems + GeoFenceMapVisuals { + map: flightMap + myGeoFenceController: geoFenceController + interactive: false + homePosition: _activeVehicle && _activeVehicle.homePositionAvailable ? _activeVehicle.homePosition : undefined } // Rally points on map diff --git a/src/FlightMap/Widgets/MapFitFunctions.qml b/src/FlightMap/Widgets/MapFitFunctions.qml index d2398a3803d38c983da5d4eaf2fcc6058365640e..676df4503fdfc8fe119b1e17140b4734f14f9a86 100644 --- a/src/FlightMap/Widgets/MapFitFunctions.qml +++ b/src/FlightMap/Widgets/MapFitFunctions.qml @@ -113,9 +113,9 @@ Item { coordList.push(edgeCoordinate) } } - if (mapGeoFenceController.polygonEnabled && mapGeoFenceController.polygon.count() > 2) { - for (var i=0; i 2) { + for (var i=0; i #include @@ -308,3 +309,30 @@ QString JsonHelper::_jsonValueTypeToString(QJsonValue::Type type) return QObject::tr("Unknown type: %1").arg(type); } + +bool JsonHelper::loadPolygon(const QJsonArray& polygonArray, QmlObjectListModel& list, QObject* parent, QString& errorString) +{ + for (int i=0; i(i)->coordinate(); + + QJsonValue jsonValue; + JsonHelper::saveGeoCoordinate(vertex, false /* writeAltitude */, jsonValue); + polygonArray.append(jsonValue); + } +} diff --git a/src/JsonHelper.h b/src/JsonHelper.h index c008df2fd9cd299de8ca1fa2f04a41b4deef4923..c57bfed6e5bbfa4bc91cd7b5b89154e737c0d5b4 100644 --- a/src/JsonHelper.h +++ b/src/JsonHelper.h @@ -14,6 +14,8 @@ #include #include +class QmlObjectListModel; + class JsonHelper { public: @@ -52,6 +54,12 @@ public: QGeoCoordinate& coordinate, ///< returned QGeoCordinate QString& errorString); ///< returned error string if load failure + /// Loads a polygon from an array + static bool loadPolygon(const QJsonArray& polygonArray, ///< Array of coordinates + QmlObjectListModel& list, ///< Empty list to add vertices to + QObject* parent, ///< parent for newly allocated QGCQGeoCoordinates + QString& errorString); ///< returned error string if load failure + /// Saves a QGeoCoordinate static void saveGeoCoordinate(const QGeoCoordinate& coordinate, ///< QGeoCoordinate to save bool writeAltitude, ///< true: write altitude to json @@ -76,6 +84,10 @@ public: bool writeAltitude, ///< true: write altitide value QJsonValue& jsonValue); ///< json value to save to + /// Saves a polygon to a json array + static void savePolygon(QmlObjectListModel& list, ///< List which contains vertices + QJsonArray& polygonArray); ///< Array to save into + static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString); diff --git a/src/MissionEditor/GeoFenceEditor.qml b/src/MissionEditor/GeoFenceEditor.qml index 19aec35e5d2a40e6064b7cc019b47b0a75ed71f3..9a65116c3bfec3906c62dea42a6b45367bb6475f 100644 --- a/src/MissionEditor/GeoFenceEditor.qml +++ b/src/MissionEditor/GeoFenceEditor.qml @@ -12,11 +12,16 @@ QGCFlickable { contentHeight: geoFenceEditorRect.height clip: true + property real availableWidth + property real availableHeight + property var myGeoFenceController + property var flightMap + readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 15) readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2 - property var polygon: geoFenceController.polygon + property var polygon: myGeoFenceController.polygon Rectangle { id: geoFenceEditorRect @@ -60,9 +65,11 @@ QGCFlickable { anchors.margins: _margin anchors.top: geoLabel.bottom anchors.left: parent.left - source: geoFenceController.editorQml + source: myGeoFenceController.editorQml - property real availableWidth: parent.width - (_margin * 2) + property real availableWidth: parent.width - (_margin * 2) + property var geoFenceController: myGeoFenceController + property var myFlightMap: flightMap } } } diff --git a/src/MissionEditor/GeoFenceMapVisuals.qml b/src/MissionEditor/GeoFenceMapVisuals.qml new file mode 100644 index 0000000000000000000000000000000000000000..584d528523d924a728173b5c9e6d2e68589676ff --- /dev/null +++ b/src/MissionEditor/GeoFenceMapVisuals.qml @@ -0,0 +1,314 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FlightMap 1.0 + +/// Survey Complex Mission Item visuals +Item { + z: QGroundControl.zOrderMapItems + + property var map + property var myGeoFenceController + property bool interactive: false ///< true: user can interact with items + property bool planView: false ///< true: visuals showing in plan view + property var homePosition + + property var _polygonComponent + property var _dragHandles + property var _splitHandles + property var _breachReturnPointComponent + property var _mouseAreaComponent + property var _circleComponent + property var _mapPolygon: myGeoFenceController.mapPolygon + + function _addVisualElements() { + _polygonComponent = polygonComponent.createObject(map) + map.addMapItem(_polygonComponent) + _circleComponent = circleComponent.createObject(map) + map.addMapItem(_circleComponent) + _breachReturnPointComponent = breachReturnPointComponent.createObject(map) + map.addMapItem(_breachReturnPointComponent) + _mouseAreaComponent = mouseAreaComponent.createObject(map) + } + + function _destroyVisualElements() { + _polygonComponent.destroy() + _circleComponent.destroy() + _breachReturnPointComponent.destroy() + _mouseAreaComponent.destroy() + } + + function _addDragHandles() { + if (!_dragHandles) { + _dragHandles = dragHandlesComponent.createObject(map) + } + if (!_splitHandles) { + _splitHandles = splitHandlesComponent.createObject(map) + } + } + + function _destroyDragHandles() { + if (_dragHandles) { + _dragHandles.destroy() + _dragHandles = undefined + } + if (_splitHandles) { + _splitHandles.destroy() + _splitHandles = undefined + } + } + + /// Add an initial 4 sided polygon if there is none + function _addInitialPolygon() { + // Initial polygon is inset to take 2/3rds space + var rect = map.centerViewport + rect.x += (rect.width * 0.25) / 2 + rect.y += (rect.height * 0.25) / 2 + rect.width *= 0.75 + rect.height *= 0.75 + var topLeft = Qt.point(rect.x, rect.y) + var topRight = Qt.point(rect.x + rect.width, rect.y) + var bottomLeft = Qt.point(rect.x, rect.y + rect.height) + var bottomRight = Qt.point(rect.x + rect.width, rect.y + rect.height) + _mapPolygon.clear() + _mapPolygon.appendVertex(map.toCoordinate(topLeft, false /* clipToViewPort */)) + _mapPolygon.appendVertex(map.toCoordinate(topRight, false /* clipToViewPort */)) + _mapPolygon.appendVertex(map.toCoordinate(bottomRight, false /* clipToViewPort */)) + _mapPolygon.appendVertex(map.toCoordinate(bottomLeft, false /* clipToViewPort */)) + } + + Component.onCompleted: { + _addVisualElements() + _addDragHandles() + } + + Component.onDestruction: { + _destroyVisualElements() + _destroyDragHandles() + } + + Connections { + target: myGeoFenceController + + onAddFencePolygon: { + _addInitialPolygon() + } + } + + + // Mouse area to capture breach return point coordinate + Component { + id: mouseAreaComponent + + MouseArea { + anchors.fill: map + visible: interactive + onClicked: myGeoFenceController.breachReturnPoint = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) + } + } + + // GeoFence polygon + Component { + id: polygonComponent + + MapPolygon { + z: QGroundControl.zOrderMapItems + border.width: 2 + border.color: "orange" + color: "transparent" + opacity: 0.5 + path: _mapPolygon.path + visible: planView || geoFenceController.polygonEnabled + } + } + + // GeoFence circle + Component { + id: circleComponent + + MapCircle { + z: QGroundControl.zOrderMapItems + border.width: 2 + border.color: "orange" + color: "transparent" + center: homePosition ? homePosition : undefined + radius: myGeoFenceController.circleRadius + visible: planView || geoFenceController.circleEnabled + } + } + + Component { + id: splitHandleComponent + + MapQuickItem { + id: mapQuickItem + anchorPoint.x: dragHandle.width / 2 + anchorPoint.y: dragHandle.height / 2 + z: QGroundControl.zOrderMapItems + 1 + visible: interactive + + property int vertexIndex + + sourceItem: Rectangle { + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width / 2 + color: "white" + opacity: .50 + + QGCLabel { + anchors.horizontalCenter: parent.horizontalCenter + anchors.verticalCenter: parent.verticalCenter + text: "+" + } + + QGCMouseArea { + fillItem: parent + onClicked: _mapPolygon.splitPolygonSegment(mapQuickItem.vertexIndex) + } + } + } + } + + Component { + id: splitHandlesComponent + + Repeater { + model: _mapPolygon.path.length > 2 ? _mapPolygon.path : 0 + + delegate: Item { + property var _splitHandle + property var _vertices: _mapPolygon.path + + function _setHandlePosition() { + var nextIndex = index + 1 + if (nextIndex > _vertices.length - 1) { + nextIndex = 0 + } + var distance = _vertices[index].distanceTo(_vertices[nextIndex]) + var azimuth = _vertices[index].azimuthTo(_vertices[nextIndex]) + _splitHandle.coordinate = _vertices[index].atDistanceAndAzimuth(distance / 2, azimuth) + } + + Component.onCompleted: { + _splitHandle = splitHandleComponent.createObject(map) + _splitHandle.vertexIndex = index + _setHandlePosition() + map.addMapItem(_splitHandle) + } + + Component.onDestruction: { + _splitHandle.destroy() + } + } + } + } + + // Control which is used to drag polygon vertices + Component { + id: dragAreaComponent + + MissionItemIndicatorDrag { + id: dragArea + visible: interactive + + property int polygonVertex + + property bool _creationComplete: false + + Component.onCompleted: _creationComplete = true + + onItemCoordinateChanged: { + if (_creationComplete) { + // During component creation some bad coordinate values got through which screws up polygon draw + _mapPolygon.adjustVertex(polygonVertex, itemCoordinate) + } + } + + onClicked: _mapPolygon.removePolygonVertex(polygonVertex) + } + } + + Component { + id: dragHandleComponent + + MapQuickItem { + id: mapQuickItem + anchorPoint.x: dragHandle.width / 2 + anchorPoint.y: dragHandle.height / 2 + z: QGroundControl.zOrderMapItems + 2 + visible: interactive + + sourceItem: Rectangle { + id: dragHandle + width: ScreenTools.defaultFontPixelHeight * 1.5 + height: width + radius: width / 2 + color: "white" + opacity: .90 + } + } + } + + // Add all polygon vertex drag handles to the map + Component { + id: dragHandlesComponent + + Repeater { + model: _mapPolygon.pathModel + + delegate: Item { + property var _visuals: [ ] + + Component.onCompleted: { + var dragHandle = dragHandleComponent.createObject(map) + dragHandle.coordinate = Qt.binding(function() { return object.coordinate }) + map.addMapItem(dragHandle) + var dragArea = dragAreaComponent.createObject(map, { "itemIndicator": dragHandle, "itemCoordinate": object.coordinate }) + dragArea.polygonVertex = Qt.binding(function() { return index }) + _visuals.push(dragHandle) + _visuals.push(dragArea) + } + + Component.onDestruction: { + for (var i=0; i<_visuals.length; i++) { + _visuals[i].destroy() + } + _visuals = [ ] + } + } + } + } + + // Breach return point + Component { + id: breachReturnPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: myGeoFenceController.breachReturnPoint + + sourceItem: MissionItemIndexLabel { + label: "B" + } + } + } +} diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index d4ff77ae1f5d26e0418b6d4cbd071b7cc59ee2ea..c58689d457f50b1062d7d99991131381cf887e44 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -129,9 +129,7 @@ QGCView { mapFitFunctions.fitMapViewportToMissionItems() } - onVisualItemsChanged: { - itemDragger.clearItem() - } + onVisualItemsChanged: itemDragger.clearItem() onNewItemsFromVehicle: { if (_visualItems && _visualItems.count != 1) { @@ -165,17 +163,6 @@ QGCView { } } - function validateBreachReturn() { - if (geoFenceController.polygon.path.length > 0) { - if (!geoFenceController.polygon.containsCoordinate(geoFenceController.breachReturnPoint)) { - geoFenceController.breachReturnPoint = geoFenceController.polygon.center() - } - if (!geoFenceController.polygon.containsCoordinate(geoFenceController.breachReturnPoint)) { - geoFenceController.breachReturnPoint = geoFenceController.polygon.path[0] - } - } - } - function fitViewportToItems() { mapFitFunctions.fitMapViewportToFenceItems() } @@ -393,12 +380,6 @@ QGCView { insertSimpleMissionItem(coordinate, missionController.visualItems.count) } break - case _layerGeoFence: - if (geoFenceController.breachReturnEnabled) { - geoFenceController.breachReturnPoint = coordinate - geoFenceController.validateBreachReturn() - } - break case _layerRallyPoints: if (rallyPointController.rallyPointsSupported) { rallyPointController.addPoint(coordinate) @@ -492,6 +473,7 @@ QGCView { anchors.top: parent.top anchors.leftMargin: parent.width - _rightPanelWidth anchors.left: parent.left + z: QGroundControl.zOrderWidgets spacing: _horizontalMargin visible: QGroundControl.corePlugin.options.enablePlanViewSelector @@ -613,40 +595,20 @@ QGCView { anchors.top: planElementSelectorRow.bottom anchors.right: parent.right opacity: _rightPanelOpacity - z: QGroundControl.zOrderTopMost - source: _editingLayer == _layerGeoFence ? "qrc:/qml/GeoFenceEditor.qml" : "" - - property real availableWidth: _rightPanelWidth - property real availableHeight: ScreenTools.availableHeight - } - - // GeoFence polygon - MapPolygon { - border.color: "#80FF0000" - border.width: 3 - path: geoFenceController.polygon.path - z: QGroundControl.zOrderMapItems - visible: geoFenceController.polygonEnabled - } + z: QGroundControl.zOrderWidgets + sourceComponent: _editingLayer == _layerGeoFence ? geoFenceEditorComponent : undefined - // GeoFence circle - MapCircle { - border.color: "#80FF0000" - border.width: 3 - center: missionController.plannedHomePosition - radius: geoFenceController.circleRadius - z: QGroundControl.zOrderMapItems - visible: geoFenceController.circleEnabled + property real availableWidth: _rightPanelWidth + property real availableHeight: ScreenTools.availableHeight + property var myGeoFenceController: geoFenceController } - // GeoFence breach return point - MapQuickItem { - anchorPoint.x: sourceItem.anchorPointX - anchorPoint.y: sourceItem.anchorPointY - coordinate: geoFenceController.breachReturnPoint - visible: geoFenceController.breachReturnEnabled - sourceItem: MissionItemIndexLabel { label: "F" } - z: QGroundControl.zOrderMapItems + GeoFenceMapVisuals { + map: editorMap + myGeoFenceController: geoFenceController + interactive: _editingLayer == _layerGeoFence + homePosition: missionController.plannedHomePosition + planView: true } // Rally Point Editor @@ -997,4 +959,15 @@ QGCView { } } // Column } + + Component { + id: geoFenceEditorComponent + + GeoFenceEditor { + availableWidth: _rightPanelWidth + availableHeight: ScreenTools.availableHeight + myGeoFenceController: geoFenceController + flightMap: editorMap + } + } } // QGCVIew diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 5a00533dcfc404826d85460af52b8b29344b1828..64b11cf91044d63a5325bc7ad225f13fcf964ba4 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -18,6 +18,7 @@ #include "QGCApplication.h" #include "ParameterManager.h" #include "JsonHelper.h" +#include "QGCQGeoCoordinate.h" #ifndef __mobile__ #include "MainWindow.h" @@ -25,6 +26,7 @@ #endif #include +#include QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog") @@ -34,8 +36,10 @@ const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn"; GeoFenceController::GeoFenceController(QObject* parent) : PlanElementController(parent) , _dirty(false) + , _mapPolygon(this) { - + connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); + connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); } GeoFenceController::~GeoFenceController() @@ -61,7 +65,7 @@ void GeoFenceController::startStaticActiveVehicle(Vehicle* vehicle) void GeoFenceController::_init(void) { - connect(&_polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); + } void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint) @@ -139,13 +143,11 @@ bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorStr } if (polygonEnabled()) { - if (!_polygon.loadFromJson(json, false /* reauired */, errorString)) { + if (!_mapPolygon.loadFromJson(json, true, errorString)) { return false; } - } else { - _polygon.clear(); } - _polygon.setDirty(false); + _mapPolygon.setDirty(false); return true; } @@ -289,7 +291,7 @@ void GeoFenceController::saveToFile(const QString& filename) } if (polygonEnabled()) { - _polygon.saveToJson(fenceFileObject); + _mapPolygon.saveToJson(fenceFileObject); } QJsonDocument saveDoc(fenceFileObject); @@ -314,7 +316,7 @@ void GeoFenceController::saveToFilePicker(void) void GeoFenceController::removeAll(void) { setBreachReturnPoint(QGeoCoordinate()); - _polygon.clear(); + _mapPolygon.clear(); } void GeoFenceController::loadFromVehicle(void) @@ -329,8 +331,8 @@ void GeoFenceController::loadFromVehicle(void) void GeoFenceController::sendToVehicle(void) { if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { - _activeVehicle->geoFenceManager()->sendToVehicle(_breachReturnPoint, _polygon.coordinateList()); - _polygon.setDirty(false); + _activeVehicle->geoFenceManager()->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel()); + _mapPolygon.setDirty(false); setDirty(false); } else { qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); @@ -344,7 +346,7 @@ bool GeoFenceController::syncInProgress(void) const bool GeoFenceController::dirty(void) const { - return _dirty | _polygon.dirty(); + return _dirty; } @@ -353,7 +355,7 @@ void GeoFenceController::setDirty(bool dirty) if (dirty != _dirty) { _dirty = dirty; if (!dirty) { - _polygon.setDirty(dirty); + _mapPolygon.setDirty(dirty); } emit dirtyChanged(dirty); } @@ -388,9 +390,11 @@ void GeoFenceController::_setDirty(void) void GeoFenceController::_setPolygonFromManager(const QList& polygon) { - _polygon.setPath(polygon); - _polygon.setDirty(false); - emit polygonPathChanged(_polygon.path()); + _mapPolygon.clear(); + for (int i=0; i 2; +} + +void GeoFenceController::_updateContainsItems(void) +{ + emit containsItemsChanged(containsItems()); +} + +void GeoFenceController::removeAllFromVehicle(void) +{ + _activeVehicle->geoFenceManager()->removeAll(); +} + +void GeoFenceController::addFence(void) +{ + // GeoFenceMapVisuals control is resposible for this + emit addFencePolygon(); +} + +void GeoFenceController::removeFence(void) +{ + _mapPolygon.clear(); +} + diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index aff9018179b48ab1952aed4a7e875559c8e38b06..db76eafdd3264e976cb17857d5eaeb4632f05610 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -28,27 +28,22 @@ class GeoFenceController : public PlanElementController public: GeoFenceController(QObject* parent = NULL); ~GeoFenceController(); - - Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged) - Q_PROPERTY(float circleRadius READ circleRadius NOTIFY circleRadiusChanged) - Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged) - Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT) + Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged) + Q_PROPERTY(float circleRadius READ circleRadius NOTIFY circleRadiusChanged) - Q_PROPERTY(bool breachReturnEnabled READ breachReturnEnabled NOTIFY breachReturnEnabledChanged) - Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged) + Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT) - Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged) - Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged) - Q_PROPERTY(QString editorQml READ editorQml NOTIFY editorQmlChanged) + Q_PROPERTY(bool breachReturnEnabled READ breachReturnEnabled NOTIFY breachReturnEnabledChanged) + Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) -#if 0 - Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged) - Q_PROPERTY(bool fenceEnabled READ fenceEnabled NOTIFY fenceEnabledChanged) - Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged) - Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged) - Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged) -#endif + Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged) + Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged) + Q_PROPERTY(QString editorQml READ editorQml NOTIFY editorQmlChanged) + + Q_INVOKABLE void addFence(void); + Q_INVOKABLE void removeFence(void); void start (bool editMode) final; void startStaticActiveVehicle (Vehicle* vehicle) final; @@ -59,9 +54,11 @@ public: void saveToFilePicker (void) final; void saveToFile (const QString& filename) final; void removeAll (void) final; + void removeAllFromVehicle (void) final; bool syncInProgress (void) const final; bool dirty (void) const final; void setDirty (bool dirty) final; + bool containsItems (void) const final; QString fileExtension(void) const final; @@ -69,7 +66,7 @@ public: bool polygonEnabled (void) const; bool breachReturnEnabled (void) const; float circleRadius (void) const; - QGCMapPolygon* polygon (void) { return &_polygon; } + QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; } QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; } QVariantList params (void) const; QStringList paramLabels (void) const; @@ -78,11 +75,11 @@ public: void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); signals: + void addFencePolygon (void); void circleEnabledChanged (bool circleEnabled); void polygonEnabledChanged (bool polygonEnabled); void breachReturnEnabledChanged (bool breachReturnEnabled); void circleRadiusChanged (float circleRadius); - void polygonPathChanged (const QVariantList& polygonPath); void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); void paramsChanged (QVariantList params); void paramLabelsChanged (QStringList paramLabels); @@ -95,6 +92,7 @@ private slots: void _setPolygonFromManager(const QList& polygon); void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint); void _loadComplete(const QGeoCoordinate& breachReturn, const QList& polygon); + void _updateContainsItems(void); private: void _init(void); @@ -105,7 +103,7 @@ private: void _activeVehicleSet(void) final; bool _dirty; - QGCMapPolygon _polygon; + QGCMapPolygon _mapPolygon; QGeoCoordinate _breachReturnPoint; QVariantList _params; diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 331cfe5eb237a0ba553edf3e8d0ff83883f101ae..a2e8f14bb058814f4e3a1686cc63ca3b81a8f2fc 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -9,6 +9,7 @@ #include "GeoFenceManager.h" #include "Vehicle.h" +#include "QmlObjectListModel.h" QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog") @@ -37,7 +38,7 @@ void GeoFenceManager::loadFromVehicle(void) emit loadComplete(QGeoCoordinate(), QList()); } -void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, const QList& polygon) +void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) { Q_UNUSED(breachReturn); Q_UNUSED(polygon); diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index f676e42010af54acbb9cda2c9d5e6da521aa751b..f63b3402d13ee7e3143dbc33786e0d7a509224c5 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -16,6 +16,7 @@ #include "QGCLoggingCategory.h" class Vehicle; +class QmlObjectListModel; Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog) @@ -36,7 +37,9 @@ public: virtual void loadFromVehicle(void); /// Send the current settings to the vehicle - virtual void sendToVehicle(const QGeoCoordinate& breachReturn, const QList& polygon); + virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon); + + virtual void removeAll(void) { }; virtual bool circleEnabled (void) const { return false; } virtual bool polygonEnabled (void) const { return false; } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index fba71fa76bcb5d19335cc462b3c46d7d0f97b6c1..10a28f8003fd6ff9f3317d94e23b9086d961d07a 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -95,27 +95,41 @@ void MissionController::_init(void) } // Called when new mission items have completed downloading from Vehicle -void MissionController::_newMissionItemsAvailableFromVehicle(void) +void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllRequested) { qCDebug(MissionControllerLog) << "_newMissionItemsAvailableFromVehicle"; - if (!_editMode || _missionItemsRequested || _visualItems->count() == 1) { - // Fly Mode: + if (!_editMode || removeAllRequested || _missionItemsRequested || _visualItems->count() == 1) { + // Fly Mode (accept if): // - Always accepts new items from the vehicle so Fly view is kept up to date - // Edit Mode: + // Edit Mode (accept if): // - Either a load from vehicle was manually requested or // - The initial automatic load from a vehicle completed and the current editor is empty + // - Remove all way requested from Fly view (clear mission on flight end) QmlObjectListModel* newControllerMissionItems = new QmlObjectListModel(this); const QList& newMissionItems = _activeVehicle->missionManager()->missionItems(); + qCDebug(MissionControllerLog) << "loading from vehicle: count"<< newMissionItems.count(); + + int i = 0; + if (_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() && newMissionItems.count() != 0) { + // First item is fake home position + _addMissionSettings(_activeVehicle, newControllerMissionItems, false /* addToCenter */); + MissionSettingsComplexItem* settingsItem = newControllerMissionItems->value(0); + if (!settingsItem) { + qWarning() << "First item is not settings item"; + return; + } + settingsItem->setCoordinate(newMissionItems[0]->coordinate()); + i = 1; + } - qCDebug(MissionControllerLog) << "loading from vehicle: count"<< _visualItems->count(); - foreach(const MissionItem* missionItem, newMissionItems) { + for (; iappend(new SimpleMissionItem(_activeVehicle, *missionItem, this)); } _deinitAllVisualItems(); - _visualItems->deleteLater(); _visualItems = newControllerMissionItems; @@ -1112,9 +1126,10 @@ void MissionController::_initAllVisualItems(void) _recalcAll(); - emit visualItemsChanged(); - connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); + connect(_visualItems, &QmlObjectListModel::countChanged, this, &MissionController::_updateContainsItems); + + emit visualItemsChanged(); _visualItems->setDirty(false); } @@ -1126,6 +1141,7 @@ void MissionController::_deinitAllVisualItems(void) } disconnect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); + disconnect(_visualItems, &QmlObjectListModel::countChanged, this, &MissionController::_updateContainsItems); } void MissionController::_initVisualItem(VisualMissionItem* visualItem) @@ -1494,3 +1510,19 @@ void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualIte scanIndex++; } } + +void MissionController::_updateContainsItems(void) +{ + emit containsItemsChanged(containsItems()); +} + +bool MissionController::containsItems(void) const +{ + return _visualItems ? _visualItems->count() > 1 : false; +} + +void MissionController::removeAllFromVehicle(void) +{ + _missionItemsRequested = true; + _activeVehicle->missionManager()->removeAll(); +} diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 16f1e3ff504dea95c289b5a9930f1f99e399d34f..c5d3e2d87bc177516b92b384c2d9c918f63bfead 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -82,9 +82,11 @@ public: void saveToFilePicker (void) final; void saveToFile (const QString& filename) final; void removeAll (void) final; + void removeAllFromVehicle (void) final; bool syncInProgress (void) const final; bool dirty (void) const final; void setDirty (bool dirty) final; + bool containsItems (void) const final; QString fileExtension(void) const final; @@ -122,7 +124,7 @@ signals: void hoverSpeedChanged(double hoverSpeed); private slots: - void _newMissionItemsAvailableFromVehicle(); + void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); void _itemCommandChanged(void); void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable); void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition); @@ -131,6 +133,7 @@ private slots: void _recalcWaypointLines(void); void _recalcAltitudeRangeBearing(void); void _homeCoordinateChanged(void); + void _updateContainsItems(void); private: void _init(void); diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index acac452c2b724f20c90b34ecf486b315f3d62eb6..7e9b7195188b5b9e36ae07e05a22ec9824df71bc 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -73,7 +73,7 @@ void MissionManager::writeMissionItems(const QList& missionItems) } } } - emit newMissionItemsAvailable(); + emit newMissionItemsAvailable(missionItems.count() == 0); qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); @@ -306,7 +306,7 @@ void MissionManager::_readTransactionComplete(void) _vehicle->sendMessageOnLink(_dedicatedLink, message); _finishTransaction(true); - emit newMissionItemsAvailable(); + emit newMissionItemsAvailable(false); } void MissionManager::_handleMissionCount(const mavlink_message_t& message) @@ -737,7 +737,7 @@ void MissionManager::_finishTransaction(bool success) if (!success && _readTransactionInProgress) { // Read from vehicle failed, clear partial list _missionItems.clear(); - emit newMissionItemsAvailable(); + emit newMissionItemsAvailable(false); } _itemIndicesToRead.clear(); @@ -767,3 +767,10 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) emit currentItemChanged(_currentMissionItem); } } + +void MissionManager::removeAll(void) +{ + QList emptyList; + + writeMissionItems(emptyList); +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index c898d3b0a4455bb7d0bbede8396348acb394a50a..583d8f858e9c5650e1f8fde12c6f79da79320b62 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -49,6 +49,9 @@ public: /// @param altChangeOnly true: only altitude change, false: lat/lon/alt change void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly); + /// Removes all mission items from vehicle + void removeAll(void); + /// Error codes returned in error signal typedef enum { InternalError, @@ -66,7 +69,7 @@ public: static const int _maxRetryCount = 5; signals: - void newMissionItemsAvailable(void); + void newMissionItemsAvailable(bool removeAllRequested); void inProgressChanged(bool inProgress); void error(int errorCode, const QString& errorMsg); void currentItemChanged(int currentItem); diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index a498d07d6c2fe0aaf5320dfae9cde2db70f3a193..189b32b2c4e9c07268499cf69e230a57a150e34f 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -25,17 +25,14 @@ public: PlanElementController(QObject* parent = NULL); ~PlanElementController(); - /// true: information is currently being saved/sent, false: no active save/send in progress - Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) + Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty + Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send + Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) ///< Returns the file extention for plan element file type. + Q_PROPERTY(Vehicle* vehicle READ vehicle NOTIFY vehicleChanged) - /// true: unsaved/sent changes are present, false: no changes since last save/send - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) - - /// Returns the file extention for plan element file type. - Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) virtual QString fileExtension(void) const = 0; - Q_PROPERTY(Vehicle* vehicle READ vehicle NOTIFY vehicleChanged) /// Should be called immediately upon Component.onCompleted. /// @param editMode true: controller being used in Plan view, false: controller being used in Fly view @@ -51,8 +48,10 @@ public: Q_INVOKABLE virtual void loadFromFile(const QString& filename) = 0; Q_INVOKABLE virtual void saveToFilePicker(void) = 0; Q_INVOKABLE virtual void saveToFile(const QString& filename) = 0; - Q_INVOKABLE virtual void removeAll(void) = 0; + Q_INVOKABLE virtual void removeAll(void) = 0; ///< Removes all from controller only, synce required to remove from vehicle + Q_INVOKABLE virtual void removeAllFromVehicle(void) = 0; ///< Removes all from vehicle and controller + virtual bool containsItems (void) const = 0; virtual bool syncInProgress (void) const = 0; virtual bool dirty (void) const = 0; virtual void setDirty (bool dirty) = 0; @@ -60,6 +59,7 @@ public: Vehicle* vehicle(void) { return _activeVehicle; } signals: + void containsItemsChanged (bool containsItems); void syncInProgressChanged (bool syncInProgress); void dirtyChanged (bool dirty); void vehicleChanged (Vehicle* vehicle); diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index 1667fad9ddb105442e6820cf9efd55c40f5fb181..cdc31d2917bc213332d0c91210093d915e6a2f70 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -10,6 +10,7 @@ #include "QGCMapPolygon.h" #include "QGCGeo.h" #include "JsonHelper.h" +#include "QGCQGeoCoordinate.h" #include #include @@ -17,21 +18,13 @@ const char* QGCMapPolygon::_jsonPolygonKey = "polygon"; -QGCMapPolygon::QGCMapPolygon(QObject* parent) +QGCMapPolygon::QGCMapPolygon(QObject* newCoordParent, QObject* parent) : QObject(parent) + , _newCoordParent(newCoordParent) , _dirty(false) { - -} - -const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other) -{ - _polygonPath = other._polygonPath; - setDirty(true); - - emit pathChanged(); - - return *this; + connect(&_polygonModel, &QmlObjectListModel::dirtyChanged, this, &QGCMapPolygon::_polygonModelDirtyChanged); + connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_polygonModelCountChanged); } void QGCMapPolygon::clear(void) @@ -48,13 +41,18 @@ void QGCMapPolygon::clear(void) // will cause the polygon to go away. _polygonPath.clear(); + _polygonModel.clearAndDeleteContents(); + setDirty(true); } -void QGCMapPolygon::adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate) +void QGCMapPolygon::adjustVertex(int vertexIndex, const QGeoCoordinate coordinate) { _polygonPath[vertexIndex] = QVariant::fromValue(coordinate); emit pathChanged(); + + _polygonModel.value(vertexIndex)->setCoordinate(coordinate); + setDirty(true); } @@ -126,9 +124,12 @@ QGeoCoordinate QGCMapPolygon::center(void) const void QGCMapPolygon::setPath(const QList& path) { _polygonPath.clear(); + _polygonModel.clearAndDeleteContents(); foreach(const QGeoCoordinate& coord, path) { - _polygonPath << QVariant::fromValue(coord); + _polygonPath.append(QVariant::fromValue(coord)); + _polygonModel.append(new QGCQGeoCoordinate(coord, _newCoordParent)); } + setDirty(true); emit pathChanged(); } @@ -136,6 +137,12 @@ void QGCMapPolygon::setPath(const QList& path) void QGCMapPolygon::setPath(const QVariantList& path) { _polygonPath = path; + + _polygonModel.clearAndDeleteContents(); + for (int i=0; i<_polygonPath.count(); i++) { + _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value(), _newCoordParent)); + } + setDirty(true); emit pathChanged(); } @@ -175,9 +182,70 @@ QList QGCMapPolygon::coordinateList(void) const { QList coords; - for (int i=0; i()); } return coords; } + +void QGCMapPolygon::splitPolygonSegment(int vertexIndex) +{ + int nextIndex = vertexIndex + 1; + if (nextIndex > _polygonPath.length() - 1) { + nextIndex = 0; + } + + QGeoCoordinate firstVertex = _polygonPath[vertexIndex].value(); + QGeoCoordinate nextVertex = _polygonPath[nextIndex].value(); + + double distance = firstVertex.distanceTo(nextVertex); + double azimuth = firstVertex.azimuthTo(nextVertex); + QGeoCoordinate newVertex = firstVertex.atDistanceAndAzimuth(distance / 2, azimuth); + + if (nextIndex == 0) { + appendVertex(newVertex); + } else { + _polygonModel.insert(nextIndex, new QGCQGeoCoordinate(newVertex, this)); + _polygonPath.insert(nextIndex, QVariant::fromValue(newVertex)); + emit pathChanged(); + } +} + +void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate) +{ + _polygonPath.append(QVariant::fromValue(coordinate)); + _polygonModel.append(new QGCQGeoCoordinate(coordinate, _newCoordParent)); + emit pathChanged(); +} + +void QGCMapPolygon::_polygonModelDirtyChanged(bool dirty) +{ + if (dirty) { + setDirty(true); + } +} + +void QGCMapPolygon::removeVertex(int vertexIndex) +{ + if (vertexIndex < 0 && vertexIndex > _polygonPath.length() - 1) { + qWarning() << "Call to removePolygonCoordinate with bad vertexIndex:count" << vertexIndex << _polygonPath.length(); + return; + } + + if (_polygonPath.length() <= 3) { + // Don't allow the user to trash the polygon + return; + } + + QObject* coordObj = _polygonModel.removeAt(vertexIndex); + coordObj->deleteLater(); + + _polygonPath.removeAt(vertexIndex); + emit pathChanged(); +} + +void QGCMapPolygon::_polygonModelCountChanged(int count) +{ + emit countChanged(count); +} diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h index 599fd98b090928360ea918a4e92d656fb62aee14..4e534f01848bfcf24f8580e09a84439d2d674a0b 100644 --- a/src/MissionManager/QGCMapPolygon.h +++ b/src/MissionManager/QGCMapPolygon.h @@ -15,33 +15,33 @@ #include #include -/// The QGCMapPolygon class provides a polygon which can be displayed on a map using a MapPolygon control. -/// It works in conjunction with the QGCMapPolygonControls control which provides the UI for drawing and -/// editing map polygons. +#include "QmlObjectListModel.h" + +/// The QGCMapPolygon class provides a polygon which can be displayed on a map using a map visuals control. +/// It maintains a representation of the polygon on QVariantList and QmlObjectListModel format. class QGCMapPolygon : public QObject { Q_OBJECT public: - QGCMapPolygon(QObject* parent = NULL); - - const QGCMapPolygon& operator=(const QGCMapPolygon& other); - - QGeoCoordinate operator[](int index) const { return _polygonPath[index].value(); } - - /// The polygon path to be bound to the MapPolygon.path property - Q_PROPERTY(QVariantList path READ path WRITE setPath NOTIFY pathChanged) + QGCMapPolygon(QObject* newCoordParent, QObject* parent = NULL); - /// true: Polygon has changed since last time dirty was false - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + Q_PROPERTY(int count READ count NOTIFY countChanged) + Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged) + Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT) + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) - /// Remove all points from polygon Q_INVOKABLE void clear(void); + Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate); + Q_INVOKABLE void removeVertex(int vertexIndex); /// Adjust the value for the specified coordinate /// @param vertexIndex Polygon point index to modify (0-based) /// @param coordinate New coordinate for point - Q_INVOKABLE void adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate); + Q_INVOKABLE void adjustVertex(int vertexIndex, const QGeoCoordinate coordinate); + + /// Splits the segment comprised of vertextIndex -> vertexIndex + 1 + Q_INVOKABLE void splitPolygonSegment(int vertexIndex); /// Returns the center point coordinate for the polygon Q_INVOKABLE QGeoCoordinate center(void) const; @@ -49,9 +49,6 @@ public: /// Returns true if the specified coordinate is within the polygon Q_INVOKABLE bool containsCoordinate(const QGeoCoordinate& coordinate) const; - /// Returns the number of points in the polygon - Q_INVOKABLE int count(void) const { return _polygonPath.count(); } - /// Returns the path in a list of QGeoCoordinate's format QList coordinateList(void) const; @@ -68,24 +65,35 @@ public: // Property methods - bool dirty(void) const { return _dirty; } - void setDirty(bool dirty); + int count (void) const { return _polygonPath.count(); } + bool dirty (void) const { return _dirty; } + void setDirty(bool dirty); + + QVariantList path (void) const { return _polygonPath; } + QmlObjectListModel* qmlPathModel(void) { return &_polygonModel; } + QmlObjectListModel& pathModel (void) { return _polygonModel; } - QVariantList path(void) const { return _polygonPath; } void setPath(const QList& path); void setPath(const QVariantList& path); signals: + void countChanged(int count); void pathChanged(void); void dirtyChanged(bool dirty); +private slots: + void _polygonModelCountChanged(int count); + void _polygonModelDirtyChanged(bool dirty); + private: QPolygonF _toPolygonF(void) const; QGeoCoordinate _coordFromPointF(const QPointF& point) const; QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const; - QVariantList _polygonPath; - bool _dirty; + QObject* _newCoordParent; + QVariantList _polygonPath; + QmlObjectListModel _polygonModel; + bool _dirty; static const char* _jsonPolygonKey; }; diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index adc43f62fb958dbca3a72df5167c2e6af5e8d859..a14ef9f29e29ac4dba6fa2342657ff1ad7e69953 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -40,7 +40,7 @@ RallyPointController::RallyPointController(QObject* parent) , _dirty(false) , _currentRallyPoint(NULL) { - + connect(&_points, &QmlObjectListModel::countChanged, this, &RallyPointController::_updateContainsItems); } RallyPointController::~RallyPointController() @@ -313,3 +313,18 @@ void RallyPointController::_setFirstPointCurrent(void) { setCurrentRallyPoint(_points.count() ? _points[0] : NULL); } + +bool RallyPointController::containsItems(void) const +{ + return _points.count() > 0; +} + +void RallyPointController::_updateContainsItems(void) +{ + emit containsItemsChanged(containsItems()); +} + +void RallyPointController::removeAllFromVehicle(void) +{ + _activeVehicle->rallyPointManager()->removeAll(); +} diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index 1c040a689efdc1fcf127481fb3c7bae41ee7b904..a910a039038c60ed5ac20a34316989db411aae95 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -37,16 +37,18 @@ public: Q_INVOKABLE void addPoint(QGeoCoordinate point); Q_INVOKABLE void removePoint(QObject* rallyPoint); - void loadFromVehicle (void) final; - void sendToVehicle (void) final; - void loadFromFilePicker (void) final; - void loadFromFile (const QString& filename) final; - void saveToFilePicker (void) final; - void saveToFile (const QString& filename) final; - void removeAll (void) final; - bool syncInProgress (void) const final; - bool dirty (void) const final { return _dirty; } - void setDirty (bool dirty) final; + void loadFromVehicle (void) final; + void sendToVehicle (void) final; + void loadFromFilePicker (void) final; + void loadFromFile (const QString& filename) final; + void saveToFilePicker (void) final; + void saveToFile (const QString& filename) final; + void removeAll (void) final; + void removeAllFromVehicle (void) final; + bool syncInProgress (void) const final; + bool dirty (void) const final { return _dirty; } + void setDirty (bool dirty) final; + bool containsItems (void) const final; QString fileExtension(void) const final; @@ -65,6 +67,7 @@ signals: private slots: void _loadComplete(const QList rgPoints); void _setFirstPointCurrent(void); + void _updateContainsItems(void); private: bool _loadJsonFile(QJsonDocument& jsonDoc, QString& errorString); diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h index 92132b5a680e19e3fb984ac236b5f66882f2a235..ca391edaaba635598b231122e85973423fcc12ff 100644 --- a/src/MissionManager/RallyPointManager.h +++ b/src/MissionManager/RallyPointManager.h @@ -38,6 +38,8 @@ public: /// Send the current settings to the vehicle virtual void sendToVehicle(const QList& rgPoints); + virtual void removeAll(void) { }; + virtual bool rallyPointsSupported(void) const { return false; } QList points(void) const { return _rgPoints; } diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index e9f68eb85424d7ab411880261a324aa2cd4271d1..74b7f869800e88a00e6b5606a47a2a183c15b76e 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -335,17 +335,8 @@ void SurveyMissionItem::save(QJsonArray& missionItems) } // Polygon shape - QJsonArray polygonArray; - - for (int i=0; i<_polygonPath.count(); i++) { - const QVariant& polyVar = _polygonPath[i]; - - QJsonValue jsonValue; - JsonHelper::saveGeoCoordinate(polyVar.value(), false /* writeAltitude */, jsonValue); - polygonArray += jsonValue; - } - + JsonHelper::savePolygon(_polygonModel, polygonArray); saveObject[_jsonPolygonObjectKey] = polygonArray; missionItems.append(saveObject); @@ -494,16 +485,12 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumbe // Polygon shape QJsonArray polygonArray(v2Object[_jsonPolygonObjectKey].toArray()); - for (int i=0; i(i)->coordinate()); } _generateGrid(); diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 1359b5e55bf7dd80bb2bd0ff76b2c7afd3a200af..120de6833b938e860f7fd606579a69ae53ab6486 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -10,6 +10,8 @@ ExclusiveGroupItem 1.0 ExclusiveGroupItem.qml FactSliderPanel 1.0 FactSliderPanel.qml FlightModeDropdown 1.0 FlightModeDropdown.qml FlightModeMenu 1.0 FlightModeMenu.qml +GeoFenceEditor 1.0 GeoFenceEditor.qml +GeoFenceMapVisuals 1.0 GeoFenceMapVisuals.qml GuidedBar 1.0 GuidedBar.qml IndicatorButton 1.0 IndicatorButton.qml JoystickThumbPad 1.0 JoystickThumbPad.qml diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 2515d8de0de4c02ac56728a510ff58f03d2b2f8b..78a202dd0db9054179ea3e83aa217552a6f76def 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -33,7 +33,7 @@ public: int count(void) const; - bool dirty(void) { return _dirty; } + bool dirty(void) const { return _dirty; } void setDirty(bool dirty); void append(QObject* object); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a068eb571829c0af113e46751308933e94f08619..3c0581a4fa5370111bb49aa213362ec67dcf9401 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -166,6 +166,7 @@ Vehicle::Vehicle(LinkInterface* link, connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection); connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_handleFlightModeChanged); connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged); + _uas = new UAS(_mavlink, this, _firmwarePluginManager); connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady); @@ -317,14 +318,16 @@ void Vehicle::_commonInit(void) _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType); _missionManager = new MissionManager(this); - connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError); + connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError); + connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable); _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); // GeoFenceManager needs to access ParameterManager so make sure to create after _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); - connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); + connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); + connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_newGeoFenceAvailable); _rallyPointManager = _firmwarePlugin->newRallyPointManager(this); connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);