From c39e9c7f359ebf0e0bb2e48f4c4b88d3825e69d9 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 29 Aug 2016 19:21:19 -0700 Subject: [PATCH] Initial GeoFence implementation --- qgroundcontrol.pro | 8 + qgroundcontrol.qrc | 2 + src/FlightDisplay/FlightDisplayViewMap.qml | 19 ++ src/FlightMap/FlightMap.qml | 32 +-- src/FlightMap/qmldir | 3 + src/MissionEditor/GeoFenceEditor.qml | 122 +++++++++ src/MissionEditor/MissionEditor.qml | 289 ++++++++++++-------- src/MissionEditor/QGCMapPolygonControls.qml | 73 +++++ src/MissionEditor/SurveyItemEditor.qml | 27 +- src/MissionManager/GeoFenceController.cc | 213 +++++++++++++++ src/MissionManager/GeoFenceController.h | 89 ++++++ src/MissionManager/GeoFenceManager.cc | 249 +++++++++++++++++ src/MissionManager/GeoFenceManager.h | 99 +++++++ src/MissionManager/MissionController.cc | 158 ++++------- src/MissionManager/MissionController.h | 48 ++-- src/MissionManager/MissionControllerTest.cc | 3 - src/MissionManager/MissionManager.h | 1 - src/MissionManager/PlanElementController.cc | 48 ++++ src/MissionManager/PlanElementController.h | 71 +++++ src/MissionManager/QGCMapPolygon.cc | 108 ++++++++ src/MissionManager/QGCMapPolygon.h | 53 ++++ src/QGCApplication.cc | 20 +- src/Vehicle/Vehicle.cc | 38 ++- src/Vehicle/Vehicle.h | 15 + 24 files changed, 1499 insertions(+), 289 deletions(-) create mode 100644 src/MissionEditor/GeoFenceEditor.qml create mode 100644 src/MissionEditor/QGCMapPolygonControls.qml create mode 100644 src/MissionManager/GeoFenceController.cc create mode 100644 src/MissionManager/GeoFenceController.h create mode 100644 src/MissionManager/GeoFenceManager.cc create mode 100644 src/MissionManager/GeoFenceManager.h create mode 100644 src/MissionManager/PlanElementController.cc create mode 100644 src/MissionManager/PlanElementController.h create mode 100644 src/MissionManager/QGCMapPolygon.cc create mode 100644 src/MissionManager/QGCMapPolygon.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 320cc7595..2af4b9eb8 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -273,12 +273,16 @@ HEADERS += \ src/LogCompressor.h \ src/MG.h \ src/MissionManager/ComplexMissionItem.h \ + src/MissionManager/GeoFenceController.h \ + src/MissionManager/GeoFenceManager.h \ + src/MissionManager/QGCMapPolygon.h \ src/MissionManager/MissionCommandList.h \ src/MissionManager/MissionCommandTree.h \ src/MissionManager/MissionCommandUIInfo.h \ src/MissionManager/MissionController.h \ src/MissionManager/MissionItem.h \ src/MissionManager/MissionManager.h \ + src/MissionManager/PlanElementController.h \ src/MissionManager/SimpleMissionItem.h \ src/MissionManager/SurveyMissionItem.h \ src/MissionManager/VisualMissionItem.h \ @@ -434,12 +438,16 @@ SOURCES += \ src/LogCompressor.cc \ src/main.cc \ src/MissionManager/ComplexMissionItem.cc \ + src/MissionManager/GeoFenceController.cc \ + src/MissionManager/GeoFenceManager.cc \ + src/MissionManager/QGCMapPolygon.cc \ src/MissionManager/MissionCommandList.cc \ src/MissionManager/MissionCommandTree.cc \ src/MissionManager/MissionCommandUIInfo.cc \ src/MissionManager/MissionController.cc \ src/MissionManager/MissionItem.cc \ src/MissionManager/MissionManager.cc \ + src/MissionManager/PlanElementController.cc \ src/MissionManager/SimpleMissionItem.cc \ src/MissionManager/SurveyMissionItem.cc \ src/MissionManager/VisualMissionItem.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 8f9bbf04e..de4277fce 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -92,6 +92,7 @@ src/ViewWidgets/ViewWidget.qml src/MissionEditor/SimpleItemEditor.qml src/MissionEditor/SurveyItemEditor.qml + src/MissionEditor/GeoFenceEditor.qml src/FactSystem/FactControls/FactBitmask.qml src/FactSystem/FactControls/FactCheckBox.qml src/FactSystem/FactControls/FactComboBox.qml @@ -107,6 +108,7 @@ src/FlightDisplay/VirtualJoystick.qml src/FlightMap/qmldir src/FlightMap/FlightMap.qml + src/MissionEditor/QGCMapPolygonControls.qml src/FlightMap/MapScale.qml src/FlightMap/MapItems/MissionItemIndicator.qml src/FlightMap/MapItems/MissionItemView.qml diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index a80689968..0f0eaba7d 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -58,6 +58,11 @@ FlightMap { Component.onCompleted: start(false /* editMode */) } + GeoFenceController { + id: _geoFenceController + Component.onCompleted: start(false /* editMode */) + } + // Add trajectory points to the map MapItemView { model: _mainIsMap ? _activeVehicle ? _activeVehicle.trajectoryPoints : 0 : 0 @@ -96,6 +101,20 @@ FlightMap { model: _mainIsMap ? _missionController.waypointLines : 0 } + // GeoFence polygon + MapPolygon { + border.color: "#80FF0000" + border.width: 3 + path: _geoFenceController.polygon.path + } + + // GeoFence breach return point + MapQuickItem { + anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) + coordinate: _geoFenceController.breachReturnPoint + sourceItem: MissionItemIndexLabel { label: "F" } + } + // GoTo here waypoint MapQuickItem { coordinate: _gotoHereCoordinate diff --git a/src/FlightMap/FlightMap.qml b/src/FlightMap/FlightMap.qml index c510232df..3b72803c4 100644 --- a/src/FlightMap/FlightMap.qml +++ b/src/FlightMap/FlightMap.qml @@ -198,30 +198,17 @@ Map { property bool adjustingPolygon: false property bool polygonReady: polygonDrawerPolygon.path.length > 3 ///< true: enough points have been captured to create a closed polygon - /// New polygon capture has started - signal polygonCaptureStarted - - /// Polygon capture is complete - /// @param coordinates Map coordinates for the polygon points - signal polygonCaptureFinished(var coordinates) - - /// Polygon adjustment has begun - signal polygonAdjustStarted - - /// Polygon Vertex coordinate has been adjusted - signal polygonAdjustVertex(int vertexIndex, var vertexCoordinate) - - /// Polygon adjustment finished - signal polygonAdjustFinished + property var _callbackObject property var _vertexDragList: [] /// Begin capturing a new polygon /// polygonCaptureStarted will be signalled - function startCapturePolygon() { + function startCapturePolygon(callback) { + polygonDrawer._callbackObject = callback polygonDrawer.drawingPolygon = true polygonDrawer._clearPolygon() - polygonDrawer.polygonCaptureStarted() + polygonDrawer._callbackObject.polygonCaptureStarted() } /// Finish capturing the polygon @@ -236,11 +223,12 @@ Map { polygonPath.pop() // get rid of drag coordinate polygonDrawer._clearPolygon() polygonDrawer.drawingPolygon = false - polygonDrawer.polygonCaptureFinished(polygonPath) + polygonDrawer._callbackObject.polygonCaptureFinished(polygonPath) return true } - function startAdjustPolygon(vertexCoordinates) { + function startAdjustPolygon(callback, vertexCoordinates) { + polygonDraw._callbackObject = callback polygonDrawer.adjustingPolygon = true for (var i=0; i 2) { + geoFenceController.breachReturnPoint = geoFenceController.polygon.center() + } + } + } + + Rectangle { + id: geoFenceEditorRect + width: parent.width + height: geoFenceItems.y + geoFenceItems.height + (_margin * 2) + radius: _radius + color: qgcPal.buttonHighlight + + QGCLabel { + id: geoFenceLabel + anchors.margins: _margin + anchors.left: parent.left + anchors.top: parent.top + text: qsTr("Geo-Fence (WIP careful!)") + color: "black" + } + + Rectangle { + id: geoFenceItems + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: geoFenceLabel.bottom + height: editorColumn.height + (_margin * 2) + color: qgcPal.windowShadeDark + radius: _radius + + Column { + id: editorColumn + anchors.margins: _margin + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + text: qsTr("Click in map to set breach return point.") + } + + QGCLabel { text: qsTr("Fence Settings:") } + + Rectangle { + anchors.left: parent.left + anchors.right: parent.right + height: 1 + color: qgcPal.text + } + + QGCLabel { + text: qsTr("Must be connected to Vehicle to change fence settings.") + visible: !QGroundControl.multiVehicleManager.activeVehicle + } + + Repeater { + model: geoFenceController.params + + Item { + width: editorColumn.width + height: textField.height + + QGCLabel { + id: textFieldLabel + anchors.baseline: textField.baseline + text: modelData.name + } + + FactTextField { + id: textField + anchors.right: parent.right + width: _editFieldWidth + showUnits: true + fact: modelData + } + } + } + + QGCMapPolygonControls { + anchors.left: parent.left + anchors.right: parent.right + flightMap: editorMap + polygon: root.polygon + sectionLabel: qsTr("Fence Polygon:") + } + } + } + } +} diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 4716a9b71..cb6cd177f 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -40,20 +40,23 @@ QGCView { readonly property real _rightPanelWidth: Math.min(parent.width / 3, ScreenTools.defaultFontPixelWidth * 30) readonly property real _rightPanelOpacity: 0.8 readonly property int _toolButtonCount: 6 - readonly property string _autoSyncKey: "AutoSync" readonly property real _toolButtonTopMargin: parent.height - ScreenTools.availableHeight + (ScreenTools.defaultFontPixelHeight / 2) - readonly property int _addMissionItemsButtonAutoOffTimeout: 10000 readonly property var _defaultVehicleCoordinate: QtPositioning.coordinate(37.803784, -122.462276) - - property bool _syncNeeded: controller.visualItems.dirty // Unsaved changes, visible to parent container - property var _visualItems: controller.visualItems + property var _visualItems: missionController.visualItems property var _currentMissionItem property int _currentMissionIndex: 0 property bool _firstVehiclePosition: true property var activeVehiclePosition: _activeVehicle ? _activeVehicle.coordinate : QtPositioning.coordinate() property bool _lightWidgetBorders: editorMap.isSatelliteMap + /// The controller which should be called for load/save, send to/from vehicle calls + property var _syncDropDownController: missionController + + readonly property int _layerMission: 1 + readonly property int _layerGeoFence: 2 + property int _editingLayer: _layerMission + onActiveVehiclePositionChanged: updateMapToVehiclePosition() Connections { @@ -73,28 +76,6 @@ QGCView { } } - function loadFromVehicle() { - controller.getMissionItems() - } - - function loadFromFile() { - if (ScreenTools.isMobile) { - _root.showDialog(mobileFilePicker, qsTr("Select Mission File"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) - } else { - controller.loadMissionFromFilePicker() - fitViewportToMissionItems() - _currentMissionItem = _visualItems.get(0) - } - } - - function saveToFile() { - if (ScreenTools.isMobile) { - _root.showDialog(mobileFileSaver, qsTr("Save Mission File"), _root.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) - } else { - controller.saveMissionToFilePicker() - } - } - function normalizeLat(lat) { // Normalize latitude to range: 0 to 180, S to N return lat + 90.0 @@ -134,23 +115,45 @@ QGCView { } MissionController { - id: controller + id: missionController Component.onCompleted: { start(true /* editMode */) setCurrentItem(0) } - /* - FIXME: autoSync is temporarily disconnected since it's still buggy + function loadFromSelectedFile() { + if (ScreenTools.isMobile) { + _root.showDialog(mobileFilePicker, qsTr("Select Mission File"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) + } else { + missionController.loadFromFilePicker() + fitViewportToMissionItems() + _currentMissionItem = _visualItems.get(0) + } + } + + function saveToSelectedFile() { + if (ScreenTools.isMobile) { + _root.showDialog(mobileFileSaver, qsTr("Save Mission File"), _root.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) + } else { + missionController.saveToFilePicker() + } + } + + onVisualItemsChanged: { + itemDragger.clearItem() + } - autoSync: QGroundControl.flightMapSettings.loadMapSetting(editorMap.mapName, _autoSyncKey, true) + onNewItemsFromVehicle: { + fitViewportToMissionItems() + _currentMissionItem = _visualItems.get(0) + } + } - onAutoSyncChanged: QGroundControl.flightMapSettings.saveMapSetting(editorMap.mapName, _autoSyncKey, autoSync) -*/ + GeoFenceController { + id: geoFenceController - onVisualItemsChanged: itemDragger.clearItem() - onNewItemsFromVehicle: fitViewportToMissionItems() + Component.onCompleted: start(true /* editMode */) } QGCPalette { id: qgcPal; colorGroupEnabled: enabled } @@ -183,14 +186,9 @@ QGCView { id: mobileFilePicker QGCMobileFileDialog { - openDialog: true - fileExtension: QGroundControl.missionFileExtension - - onFilenameReturned: { - controller.loadMissionFromFile(filename) - fitViewportToMissionItems() - _currentMissionItem = _visualItems.get(0) - } + openDialog: true + fileExtension: QGroundControl.missionFileExtension + onFilenameReturned: _syncDropDownController.loadFromfile(filename) } } @@ -198,12 +196,9 @@ QGCView { id: mobileFileSaver QGCMobileFileDialog { - openDialog: false - fileExtension: QGroundControl.missionFileExtension - - onFilenameReturned: { - controller.saveMissionToFile(filename) - } + openDialog: false + fileExtension: QGroundControl.missionFileExtension + onFilenameReturned: _syncDropDownController.saveToFile() } } @@ -217,7 +212,7 @@ QGCView { if (toIndex == 0) { toIndex = 1 } - controller.moveMissionItem(_moveDialogMissionItemIndex, toIndex) + missionController.moveMissionItem(_moveDialogMissionItemIndex, toIndex) hideDialog() } @@ -260,8 +255,6 @@ QGCView { anchors.right: parent.right mapName: "MissionEditor" - signal mapClicked(var coordinate) - readonly property real animationDuration: 500 // Initial map position duplicates Fly view position @@ -283,17 +276,25 @@ QGCView { onClicked: { //-- Don't pay attention to items beneath the toolbar. var topLimit = parent.height - ScreenTools.availableHeight - if(mouse.y >= topLimit) { - var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y)) - coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) - coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) - coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) + if(mouse.y < topLimit) { + return + } + + var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y)) + coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) + coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) + coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) + + switch (_editingLayer) { + case _layerMission: if (addMissionItemsButton.checked) { - var sequenceNumber = controller.insertSimpleMissionItem(coordinate, controller.visualItems.count) + var sequenceNumber = missionController.insertSimpleMissionItem(coordinate, missionController.visualItems.count) setCurrentItem(sequenceNumber) - } else { - editorMap.mapClicked(coordinate) } + break + case _layerGeoFence: + geoFenceController.breachReturnPoint = coordinate + break } } } @@ -350,7 +351,8 @@ QGCView { // Add the complex mission item polygon to the map MapItemView { - model: controller.complexVisualItems + model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined + delegate: MapPolygon { color: 'green' path: object.polygonPath @@ -360,7 +362,7 @@ QGCView { // Add the complex mission item grid to the map MapItemView { - model: controller.complexVisualItems + model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined delegate: MapPolyline { line.color: "white" @@ -371,7 +373,7 @@ QGCView { // Add the complex mission item exit coordinates MapItemView { - model: controller.complexVisualItems + model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined delegate: exitCoordinateComponent } @@ -389,7 +391,7 @@ QGCView { // Add the simple mission items to the map MapItemView { - model: controller.visualItems + model: _editingLayer == _layerMission ? missionController.visualItems : undefined delegate: missionItemComponent } @@ -447,7 +449,7 @@ QGCView { // Add lines between waypoints MissionLineView { - model: controller.waypointLines + model: _editingLayer == _layerMission ? missionController.waypointLines : undefined } // Add the vehicles to the map @@ -472,6 +474,7 @@ QGCView { width: _rightPanelWidth opacity: _rightPanelOpacity z: QGroundControl.zOrderTopMost + visible: _editingLayer == _layerMission MouseArea { // This MouseArea prevents the Map below it from getting Mouse events. Without this @@ -488,7 +491,7 @@ QGCView { height: parent.height spacing: _margin / 2 orientation: ListView.Vertical - model: controller.visualItems + model: missionController.visualItems cacheBuffer: height * 2 clip: true currentIndex: _currentMissionIndex @@ -504,19 +507,48 @@ QGCView { onRemove: { itemDragger.clearItem() - controller.removeMissionItem(index) + missionController.removeMissionItem(index) } onInsert: { - var sequenceNumber = controller.insertSimpleMissionItem(editorMap.center, insertAfterIndex) + var sequenceNumber = missionController.insertSimpleMissionItem(editorMap.center, insertAfterIndex) setCurrentItem(sequenceNumber) } - onMoveHomeToMapCenter: controller.visualItems.get(0).coordinate = editorMap.center + onMoveHomeToMapCenter: missionController.visualItems.get(0).coordinate = editorMap.center } } // ListView } // Item - Mission Item editor + // GeoFence Editor + Loader { + anchors.topMargin: parent.height - ScreenTools.availableHeight + anchors.top: parent.top + 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 + } + + // GeoFence breach return point + MapQuickItem { + anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) + coordinate: geoFenceController.breachReturnPoint + sourceItem: MissionItemIndexLabel { + label: "F" + } + } + //-- Dismiss Drop Down (if any) MouseArea { anchors.fill: parent @@ -548,23 +580,66 @@ QGCView { spacing: ScreenTools.defaultFontPixelHeight z: QGroundControl.zOrderWidgets + DropButton { + id: layerButton + dropDirection: dropRight + //buttonImage: "/qmlimages/MapCenter.svg" + viewportMargins: ScreenTools.defaultFontPixelWidth / 2 + exclusiveGroup: _dropButtonsExclusiveGroup + lightBorders: _lightWidgetBorders + + dropDownComponent: Component { + Column { + spacing: ScreenTools.defaultFontPixelWidth * 0.5 + + QGCLabel { text: qsTr("Editing Layer:") } + + Row { + spacing: ScreenTools.defaultFontPixelWidth + + QGCButton { + text: qsTr("Mission") + + onClicked: { + layerButton.hideDropDown() + _editingLayer = _layerMission + _syncDropDownController = missionController + } + } + + QGCButton { + text: qsTr("GeoFence") + + onClicked: { + layerButton.hideDropDown() + _editingLayer = _layerGeoFence + _syncDropDownController = geoFenceController + } + } + } + } + } + } + RoundButton { id: addMissionItemsButton buttonImage: "/qmlimages/MapAddMission.svg" lightBorders: _lightWidgetBorders + visible: _editingLayer == _layerMission } RoundButton { id: addShapeButton buttonImage: "/qmlimages/MapDrawShape.svg" lightBorders: _lightWidgetBorders + visible: _editingLayer == _layerMission onClicked: { var coordinate = editorMap.center coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) - var sequenceNumber = controller.insertComplexMissionItem(coordinate, controller.visualItems.count) + var sequenceNumber = missionController.insertComplexMissionItem(coordinate, missionController.visualItems.count) setCurrentItem(sequenceNumber) checked = false addMissionItemsButton.checked = false @@ -574,12 +649,12 @@ QGCView { DropButton { id: syncButton dropDirection: dropRight - buttonImage: _syncNeeded ? "/qmlimages/MapSyncChanged.svg" : "/qmlimages/MapSync.svg" + buttonImage: _syncDropDownController.dirty ? "/qmlimages/MapSyncChanged.svg" : "/qmlimages/MapSync.svg" viewportMargins: ScreenTools.defaultFontPixelWidth / 2 exclusiveGroup: _dropButtonsExclusiveGroup dropDownComponent: syncDropDownComponent - enabled: !controller.syncInProgress - rotateImage: controller.syncInProgress + enabled: !_syncDropDownController.syncInProgress + rotateImage: _syncDropDownController.syncInProgress lightBorders: _lightWidgetBorders } @@ -602,7 +677,7 @@ QGCView { width: ScreenTools.defaultFontPixelWidth * 10 onClicked: { centerMapButton.hideDropDown() - editorMap.center = controller.visualItems.get(0).coordinate + editorMap.center = missionController.visualItems.get(0).coordinate } } QGCButton { @@ -706,13 +781,13 @@ QGCView { anchors.bottom: parent.bottom z: QGroundControl.zOrderTopMost currentMissionItem: _currentMissionItem - missionItems: controller.visualItems + missionItems: missionController.visualItems expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2) - missionDistance: controller.missionDistance - missionMaxTelemetry: controller.missionMaxTelemetry - cruiseDistance: controller.cruiseDistance - hoverDistance: controller.hoverDistance - visible: !ScreenTools.isShortScreen + missionDistance: missionController.missionDistance + missionMaxTelemetry: missionController.missionMaxTelemetry + cruiseDistance: missionController.cruiseDistance + hoverDistance: missionController.hoverDistance + visible: _editingLayer == _layerMission && !ScreenTools.isShortScreen } } // FlightMap } // Item - split view container @@ -748,7 +823,7 @@ QGCView { message: qsTr("Are you sure you want to delete all mission items?") function accept() { itemDragger.clearItem() - controller.removeAllMissionItems() + missionController.removeAll() hideDialog() } } @@ -756,96 +831,84 @@ QGCView { Component { id: syncDropDownComponent + Column { id: columnHolder spacing: _margin + QGCLabel { width: sendSaveGrid.width wrapMode: Text.WordWrap - text: _syncNeeded && !controller.autoSync ? + text: _syncDropDownController.dirty ? qsTr("You have unsaved changes to your mission. You should send to your vehicle, or save to a file:") : qsTr("Sync:") } + GridLayout { id: sendSaveGrid columns: 2 anchors.margins: _margin rowSpacing: _margin columnSpacing: ScreenTools.defaultFontPixelWidth - visible: true //autoSyncCheckBox.enabled && autoSyncCheckBox.checked + QGCButton { text: qsTr("Send To Vehicle") Layout.fillWidth: true - enabled: _activeVehicle && !controller.syncInProgress + enabled: _activeVehicle && !_syncDropDownController.syncInProgress onClicked: { syncButton.hideDropDown() - controller.sendMissionItems() + _syncDropDownController.sendToVehicle() } } + QGCButton { text: qsTr("Load From Vehicle") Layout.fillWidth: true - enabled: _activeVehicle && !controller.syncInProgress + enabled: _activeVehicle && !_syncDropDownController.syncInProgress onClicked: { syncButton.hideDropDown() - if (_syncNeeded) { + if (_syncDropDownController.dirty) { _root.showDialog(syncLoadFromVehicleOverwrite, qsTr("Mission overwrite"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) } else { - loadFromVehicle() + _syncDropDownController.loadFromVehicle() } } } + QGCButton { text: qsTr("Save To File...") Layout.fillWidth: true - enabled: !controller.syncInProgress + enabled: !_syncDropDownController.syncInProgress onClicked: { syncButton.hideDropDown() - saveToFile() + _syncDropDownController.saveToSelectedFile() } } + QGCButton { text: qsTr("Load From File...") Layout.fillWidth: true - enabled: !controller.syncInProgress + enabled: !_syncDropDownController.syncInProgress onClicked: { syncButton.hideDropDown() - if (_syncNeeded) { + if (_syncDropDownController.dirty) { _root.showDialog(syncLoadFromFileOverwrite, qsTr("Mission overwrite"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) } else { - loadFromFile() + _syncDropDownController.loadFromSelectedFile() } } } + QGCButton { text: qsTr("Remove All") Layout.fillWidth: true onClicked: { syncButton.hideDropDown() + _syncDropDownController.removeAll() _root.showDialog(removeAllPromptDialog, qsTr("Delete all"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) } } } - - -/* - FIXME: autoSync is temporarily disconnected since it's still buggy - - QGCLabel { - id: autoSyncDisallowedLabel - visible: _activeVehicle && _activeVehicle.armed - text: "AutoSync is not allowed whie vehicle is armed" - } - - QGCCheckBox { - id: autoSyncCheckBox - checked: controller.autoSync - text: "Automatically sync changes with vehicle" - enabled: _activeVehicle ? !_activeVehicle.armed : false - - onClicked: controller.autoSync = checked - } -*/ } } } // QGCVIew diff --git a/src/MissionEditor/QGCMapPolygonControls.qml b/src/MissionEditor/QGCMapPolygonControls.qml new file mode 100644 index 000000000..c14ce2e42 --- /dev/null +++ b/src/MissionEditor/QGCMapPolygonControls.qml @@ -0,0 +1,73 @@ +import QtQuick 2.2 +import QtQuick.Controls 1.2 + +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 + +/// Controls for drawing/editing map polygon +Column { + id: root + spacing: _margin + + property var sectionLabel: qsTr("Polygon:") ///< Section label + property var flightMap ///< Must be set to FlightMap control + property var polygon ///< Must be set to MapPolygon + + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + + function polygonCaptureStarted() { + polygon.clear() + } + + function polygonCaptureFinished(coordinates) { + polygon.path = coordinates + } + + function polygonAdjustVertex(vertexIndex, vertexCoordinate) { + polygon.adjustCoordinate(vertexIndex, vertexCoordinate) + } + + function polygonAdjustStarted() { } + function polygonAdjustFinished() { } + + QGCLabel { text: sectionLabel } + + Rectangle { + anchors.left: parent.left + anchors.right: parent.right + height: 1 + color: qgcPal.text + } + + Row { + spacing: ScreenTools.defaultFontPixelWidth + + QGCButton { + text: flightMap.polygonDraw.drawingPolygon ? qsTr("Finish Draw") : qsTr("Draw") + visible: !flightMap.polygonDraw.adjustingPolygon + enabled: ((flightMap.polygonDraw.drawingPolygon && flightMap.polygonDraw.polygonReady) || !flightMap.polygonDraw.drawingPolygon) + + onClicked: { + if (flightMap.polygonDraw.drawingPolygon) { + flightMap.polygonDraw.finishCapturePolygon() + } else { + flightMap.polygonDraw.startCapturePolygon(root) + } + } + } + + QGCButton { + text: flightMap.polygonDraw.adjustingPolygon ? qsTr("Finish Adjust") : qsTr("Adjust") + visible: polygon.path.length > 0 && !flightMap.polygonDraw.drawingPolygon + + onClicked: { + if (flightMap.polygonDraw.adjustingPolygon) { + flightMap.polygonDraw.finishAdjustPolygon() + } else { + flightMap.polygonDraw.startAdjustPolygon(root, polygon.path) + } + } + } + } +} diff --git a/src/MissionEditor/SurveyItemEditor.qml b/src/MissionEditor/SurveyItemEditor.qml index 610f8c13a..81d88cb7a 100644 --- a/src/MissionEditor/SurveyItemEditor.qml +++ b/src/MissionEditor/SurveyItemEditor.qml @@ -54,22 +54,23 @@ Rectangle { missionItem.cameraTriggerDistance.rawValue = cameraTriggerDistance } - Connections { - target: editorMap.polygonDraw - - onPolygonCaptureStarted: { - missionItem.clearPolygon() - } + function polygonCaptureStarted() { + missionItem.clearPolygon() + } - onPolygonCaptureFinished: { - for (var i=0; i + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +/// @file +/// @author Don Gagne + +#include "GeoFenceController.h" +#include "Vehicle.h" +#include "FirmwarePlugin.h" +#include "MAVLinkProtocol.h" +#include "QGCApplication.h" +#include "ParameterLoader.h" + +QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog") + +GeoFenceController::GeoFenceController(QObject* parent) + : PlanElementController(parent) + , _dirty(false) +{ + _clearGeoFence(); +} + +GeoFenceController::~GeoFenceController() +{ + +} + +void GeoFenceController::start(bool editMode) +{ + qCDebug(GeoFenceControllerLog) << "start editMode" << editMode; + + PlanElementController::start(editMode); + + connect(_multiVehicleMgr, &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &GeoFenceController::_parameterReadyVehicleAvailableChanged); + connect(&_geoFence.polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged); +} + +void GeoFenceController::setFenceType(GeoFenceTypeEnum fenceType) +{ + if (_geoFence.fenceType != (GeoFenceManager::GeoFenceType_t)fenceType) { + _geoFence.fenceType = (GeoFenceManager::GeoFenceType_t)fenceType; + emit fenceTypeChanged(fenceType); + } +} + +void GeoFenceController::setCircleRadius(float circleRadius) +{ + if (qFuzzyCompare(_geoFence.circleRadius, circleRadius)) { + _geoFence.circleRadius = circleRadius; + emit circleRadiusChanged(circleRadius); + } +} + +void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint) +{ + if (_geoFence.breachReturnPoint != breachReturnPoint) { + _geoFence.breachReturnPoint = breachReturnPoint; + emit breachReturnPointChanged(breachReturnPoint); + } +} + +void GeoFenceController::_setParams(void) +{ + if (_params.count() == 0 && _activeVehicle && _multiVehicleMgr->parameterReadyVehicleAvailable()) { + QStringList skipList; + skipList << QStringLiteral("FENCE_TOTAL") << QStringLiteral("FENCE_ENABLE"); + + QStringList allNames = _activeVehicle->autopilotPlugin()->parameterNames(-1); + foreach (const QString& paramName, allNames) { + if (paramName.startsWith(QStringLiteral("FENCE_")) && !skipList.contains(paramName)) { + _params << QVariant::fromValue(_activeVehicle->autopilotPlugin()->getParameterFact(-1, paramName)); + } + } + emit paramsChanged(); + } +} + +void GeoFenceController::_activeVehicleBeingRemoved(void) +{ + _clearGeoFence(); + _params.clear(); + emit paramsChanged(); + _activeVehicle->geoFenceManager()->disconnect(this); +} + +void GeoFenceController::_activeVehicleSet(void) +{ + connect(_activeVehicle->geoFenceManager(), &GeoFenceManager::newGeoFenceAvailable, this, &GeoFenceController::_newGeoFenceAvailable); + + _setParams(); + + if (_activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { + // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. + // We don't request mission items for new vehicles since that will happen autamatically. + loadFromVehicle(); + } +} + +void GeoFenceController::_parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable) +{ + Q_UNUSED(parameterReadyVehicleAvailable); + _setParams(); +} + +void GeoFenceController::_newGeoFenceAvailable(void) +{ + _setGeoFence(_activeVehicle->geoFenceManager()->geoFence()); + setDirty(false); +} + +void GeoFenceController::loadFromFilePicker(void) +{ + +} + +void GeoFenceController::loadFromFile(const QString& filename) +{ + Q_UNUSED(filename); +} + +void GeoFenceController::saveToFilePicker(void) +{ + +} + +void GeoFenceController::saveToFile(const QString& filename) +{ + Q_UNUSED(filename); +} + +void GeoFenceController::removeAll(void) +{ + _clearGeoFence(); +} + +void GeoFenceController::loadFromVehicle(void) +{ + if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { + _activeVehicle->geoFenceManager()->requestGeoFence(); + } else { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress(); + } +} + +void GeoFenceController::sendToVehicle(void) +{ + if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { + // FIXME: Hack + setFenceType(GeoFencePolygon); + setDirty(false); + _geoFence.polygon.setDirty(false); + _activeVehicle->geoFenceManager()->setGeoFence(_geoFence); + } else { + qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress(); + } +} + +void GeoFenceController::_clearGeoFence(void) +{ + setFenceType(GeoFenceNone); + setCircleRadius(0.0); + setBreachReturnPoint(QGeoCoordinate()); + _geoFence.polygon.clear(); +} + +void GeoFenceController::_setGeoFence(const GeoFenceManager::GeoFence_t& geoFence) +{ + _clearGeoFence(); + setFenceType(static_cast(geoFence.fenceType)); + setCircleRadius(geoFence.circleRadius); + setBreachReturnPoint(geoFence.breachReturnPoint); + _geoFence.polygon = geoFence.polygon; +} + +bool GeoFenceController::syncInProgress(void) const +{ + if (_activeVehicle) { + return _activeVehicle->geoFenceManager()->inProgress(); + } else { + return false; + } +} + +bool GeoFenceController::dirty(void) const +{ + return _dirty | _geoFence.polygon.dirty(); +} + + +void GeoFenceController::setDirty(bool dirty) +{ + if (dirty != _dirty) { + _dirty = dirty; + if (!dirty) { + _geoFence.polygon.setDirty(dirty); + } + emit dirtyChanged(dirty); + } +} + +void GeoFenceController::_polygonDirtyChanged(bool dirty) +{ + if (dirty) { + setDirty(true); + } +} diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h new file mode 100644 index 000000000..a5a6d88be --- /dev/null +++ b/src/MissionManager/GeoFenceController.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#ifndef GeoFenceController_H +#define GeoFenceController_H + +#include "PlanElementController.h" +#include "GeoFenceManager.h" +#include "QGCMapPolygon.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" +#include "QGCLoggingCategory.h" + +Q_DECLARE_LOGGING_CATEGORY(GeoFenceControllerLog) + +class GeoFenceController : public PlanElementController +{ + Q_OBJECT + +public: + GeoFenceController(QObject* parent = NULL); + ~GeoFenceController(); + + enum GeoFenceTypeEnum { + GeoFenceNone = GeoFenceManager::GeoFenceNone, + GeoFenceCircle = GeoFenceManager::GeoFenceCircle, + GeoFencePolygon = GeoFenceManager::GeoFencePolygon, + }; + + Q_PROPERTY(GeoFenceTypeEnum fenceType READ fenceType WRITE setFenceType NOTIFY fenceTypeChanged) + Q_PROPERTY(float circleRadius READ circleRadius WRITE setCircleRadius NOTIFY circleRadiusChanged) + Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT) + Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged) + + void start (bool editMode) 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; + bool syncInProgress (void) const final; + bool dirty (void) const final; + void setDirty (bool dirty) final; + + GeoFenceTypeEnum fenceType (void) const { return (GeoFenceTypeEnum)_geoFence.fenceType; } + float circleRadius (void) const { return _geoFence.circleRadius; } + QGCMapPolygon* polygon (void) { return &_geoFence.polygon; } + QGeoCoordinate breachReturnPoint (void) const { return _geoFence.breachReturnPoint; } + QVariantList params (void) { return _params; } + + void setFenceType(GeoFenceTypeEnum fenceType); + void setCircleRadius(float circleRadius); + void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint); + +signals: + void fenceTypeChanged (GeoFenceTypeEnum fenceType); + void circleRadiusChanged (float circleRadius); + void polygonPathChanged (const QVariantList& polygonPath); + void breachReturnPointChanged (QGeoCoordinate breachReturnPoint); + void paramsChanged (void); + +private slots: + void _parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable); + void _newGeoFenceAvailable(void); + void _polygonDirtyChanged(bool dirty); + +private: + void _setParams(void); + void _clearGeoFence(void); + void _setGeoFence(const GeoFenceManager::GeoFence_t& geoFence); + + void _activeVehicleBeingRemoved(void) final; + void _activeVehicleSet(void) final; + + bool _dirty; + GeoFenceManager::GeoFence_t _geoFence; + QVariantList _params; +}; + +#endif diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc new file mode 100644 index 000000000..51d7d9c85 --- /dev/null +++ b/src/MissionManager/GeoFenceManager.cc @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + + +/// @file +/// @author Don Gagne + +#include "GeoFenceManager.h" +#include "Vehicle.h" +#include "FirmwarePlugin.h" +#include "MAVLinkProtocol.h" +#include "QGCApplication.h" + +QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog") + +const char* GeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; +const char* GeoFenceManager::_fenceActionParam = "FENCE_ACTION"; + +GeoFenceManager::GeoFenceManager(Vehicle* vehicle) + : _vehicle(vehicle) + , _readTransactionInProgress(false) + , _writeTransactionInProgress(false) +{ + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &GeoFenceManager::_mavlinkMessageReceived); +} + +GeoFenceManager::~GeoFenceManager() +{ + +} + +void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) +{ + qCDebug(GeoFenceManagerLog) << "Sending error" << errorCode << errorMsg; + + emit error(errorCode, errorMsg); +} + +void GeoFenceManager::setGeoFence(const GeoFence_t& geoFence) +{ + if (_readTransactionInProgress) { + _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress.")); + return; + } + + if (!_geoFenceSupported()) { + return; + } + + // Validate + switch (geoFence.fenceType) { + case GeoFencePolygon: + if (geoFence.polygon.count() < 3) { + _sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points.")); + return; + } + if (geoFence.polygon.count() > std::numeric_limits::max()) { + _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(geoFence.polygon.count())); + return; + } + break; + case GeoFenceCircle: + if (geoFence.circleRadius <= 0.0) { + _sendError(InvalidCircleRadius, QStringLiteral("Geo-Fence circle radius must be greater than 0.")); + return; + } + default: + break; + } + + _geoFence.fenceType = geoFence.fenceType; + _geoFence.circleRadius = geoFence.circleRadius; + _geoFence.polygon = geoFence.polygon; + _geoFence.breachReturnPoint = geoFence.breachReturnPoint; + + if (_geoFence.fenceType != GeoFencePolygon) { + // Circle is just params, so no more work to do + return; + } + + emit newGeoFenceAvailable(); + + // First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air + Fact* fenceActionFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceActionParam); + _savedWriteFenceAction = fenceActionFact->rawValue(); + fenceActionFact->setRawValue(0); + + // Fence total param includes: + // index 0: breach return + // last index: polygon close (same as first polygon point) + _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_geoFence.polygon.count() + 2); + + // FIXME: No validation of correct fence received + // Send points: + // breach return + // polygon fence points + // polygon close + // Put back previous fence action to start geofence again + _sendFencePoint(0); + for (uint8_t index=0; index<_geoFence.polygon.count(); index++) { + _sendFencePoint(index + 1); + } + _sendFencePoint(_geoFence.polygon.count() + 1); + fenceActionFact->setRawValue(_savedWriteFenceAction); +} + +void GeoFenceManager::requestGeoFence(void) +{ + _clearGeoFence(); + + if (!_geoFenceSupported()) { + return; + } + + // Point 0: Breach return point + // Point [1,N]: Polygon points + // Point N+1: Close polygon point (same as point 1) + int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); + qCDebug(GeoFenceManagerLog) << "GeoFenceManager::requestGeoFence" << cFencePoints; + if (cFencePoints == 0) { + // No fence, no more work to do, fence data has already been cleared + return; + } + if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < 6)) { + _sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints)); + return; + } + if (cFencePoints > std::numeric_limits::max()) { + _sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints)); + return; + } + + _readTransactionInProgress = true; + _cReadFencePoints = cFencePoints; + _currentFencePoint = 0; + + _requestFencePoint(_currentFencePoint); +} + +/// Called when a new mavlink message for out vehicle is received +void GeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) { + mavlink_fence_point_t fencePoint; + + mavlink_msg_fence_point_decode(&message, &fencePoint); + if (fencePoint.idx != _currentFencePoint) { + // FIXME: Protocol out of whack + qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; + return; + } + + if (fencePoint.idx == 0) { + _geoFence.breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng); + qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _geoFence.breachReturnPoint; + _requestFencePoint(++_currentFencePoint); + } else if (fencePoint.idx < _cReadFencePoints - 1) { + QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng); + _geoFence.polygon.addCoordinate(polyCoord); + qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord; + if (fencePoint.idx < _cReadFencePoints - 2) { + // Still more points to request + _requestFencePoint(++_currentFencePoint); + } else { + // We've finished collecting fence points + _readTransactionInProgress = false; + emit newGeoFenceAvailable(); + } + } + } +} + +void GeoFenceManager::_clearGeoFence(void) +{ + _geoFence.fenceType = GeoFenceNone; + _geoFence.circleRadius = 0.0; + _geoFence.polygon.clear(); + _geoFence.breachReturnPoint = QGeoCoordinate(); + emit newGeoFenceAvailable(); +} + +void GeoFenceManager::_requestFencePoint(uint8_t pointIndex) +{ + mavlink_message_t msg; + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + + qCDebug(GeoFenceManagerLog) << "GeoFenceManager::_requestFencePoint" << pointIndex; + mavlink_msg_fence_fetch_point_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex); + _vehicle->sendMessageOnPriorityLink(msg); +} + +void GeoFenceManager::_sendFencePoint(uint8_t pointIndex) +{ + mavlink_message_t msg; + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + + QGeoCoordinate fenceCoord; + if (pointIndex == 0) { + fenceCoord = _geoFence.breachReturnPoint; + } else if (pointIndex - 1 < _geoFence.polygon.count()) { + fenceCoord = _geoFence.polygon[pointIndex - 1]; + } else { + // Polygon close point + fenceCoord = _geoFence.polygon[0]; + } + + mavlink_msg_fence_point_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex, // Index of point to set + _geoFence.polygon.count() + 2, // Total point count, +1 for breach in index 0, +1 polygon close in last index + fenceCoord.latitude(), + fenceCoord.longitude()); + _vehicle->sendMessageOnPriorityLink(msg); +} + +bool GeoFenceManager::inProgress(void) const +{ + return _readTransactionInProgress || _writeTransactionInProgress; +} + +bool GeoFenceManager::_geoFenceSupported(void) +{ + // FIXME: Hack to get around lack of plugin-ized version of code + if (_vehicle->apmFirmware()) { + if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) || + !_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) { + _sendError(InternalError, QStringLiteral("Vehicle does not support Geo-Fence implementation.")); + return false; + } else { + return true; + } + } else { + return false; + } +} diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h new file mode 100644 index 000000000..bfbbd60f6 --- /dev/null +++ b/src/MissionManager/GeoFenceManager.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#ifndef GeoFenceManager_H +#define GeoFenceManager_H + +#include +#include +#include + +#include "MissionItem.h" +#include "QGCMAVLink.h" +#include "QGCLoggingCategory.h" +#include "LinkInterface.h" +#include "QGCMapPolygon.h" + +class Vehicle; + +Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog) + +class GeoFenceManager : public QObject +{ + Q_OBJECT + +public: + GeoFenceManager(Vehicle* vehicle); + ~GeoFenceManager(); + + typedef enum { + GeoFenceNone, + GeoFenceCircle, + GeoFencePolygon, + } GeoFenceType_t; + + typedef struct { + GeoFenceType_t fenceType; + float circleRadius; + QGCMapPolygon polygon; + QGeoCoordinate breachReturnPoint; + } GeoFence_t; + + bool inProgress(void) const; + + /// Request the geo fence from the vehicle + void requestGeoFence(void); + + /// Returns the current geofence settings + const GeoFence_t& geoFence(void) const { return _geoFence; } + + /// Set and send the specified geo fence to the vehicle + void setGeoFence(const GeoFence_t& geoFence); + + /// Error codes returned in error signal + typedef enum { + InternalError, + TooFewPoints, ///< Too few points for valid geofence + TooManyPoints, ///< Too many points for valid geofence + InvalidCircleRadius, + } ErrorCode_t; + +signals: + void newGeoFenceAvailable(void); + void inProgressChanged(bool inProgress); + void error(int errorCode, const QString& errorMsg); + +private slots: + void _mavlinkMessageReceived(const mavlink_message_t& message); + //void _ackTimeout(void); + +private: + void _sendError(ErrorCode_t errorCode, const QString& errorMsg); + void _clearGeoFence(void); + void _requestFencePoint(uint8_t pointIndex); + void _sendFencePoint(uint8_t pointIndex); + bool _geoFenceSupported(void); + +private: + Vehicle* _vehicle; + + bool _readTransactionInProgress; + bool _writeTransactionInProgress; + + uint8_t _cReadFencePoints; + uint8_t _currentFencePoint; + QVariant _savedWriteFenceAction; + + GeoFence_t _geoFence; + + static const char* _fenceTotalParam; + static const char* _fenceActionParam; +}; + +#endif diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 864d05ff8..63d3ac032 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -36,12 +36,9 @@ const char* MissionController::_jsonComplexItemsKey = "complexItems"; const char* MissionController::_jsonPlannedHomePositionKey = "plannedHomePosition"; MissionController::MissionController(QObject *parent) - : QObject(parent) - , _editMode(false) + : PlanElementController(parent) , _visualItems(NULL) , _complexItems(NULL) - , _activeVehicle(NULL) - , _autoSync(false) , _firstItemsFromVehicle(false) , _missionItemsRequested(false) , _queuedSend(false) @@ -62,12 +59,7 @@ void MissionController::start(bool editMode) { qCDebug(MissionControllerLog) << "start editMode" << editMode; - _editMode = editMode; - - MultiVehicleManager* multiVehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); - - connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionController::_activeVehicleChanged); - _activeVehicleChanged(multiVehicleMgr->activeVehicle()); + PlanElementController::start(editMode); // We start with an empty mission _visualItems = new QmlObjectListModel(this); @@ -111,7 +103,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(void) } } -void MissionController::getMissionItems(void) +void MissionController::loadFromVehicle(void) { Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); @@ -121,7 +113,7 @@ void MissionController::getMissionItems(void) } } -void MissionController::sendMissionItems(void) +void MissionController::sendToVehicle(void) { if (_activeVehicle) { // Convert to MissionItems so we can send to vehicle @@ -240,7 +232,7 @@ void MissionController::removeMissionItem(int index) _visualItems->setDirty(true); } -void MissionController::removeAllMissionItems(void) +void MissionController::removeAll(void) { if (_visualItems) { _deinitAllVisualItems(); @@ -416,7 +408,7 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM return true; } -void MissionController::loadMissionFromFile(const QString& filename) +void MissionController::loadFromFile(const QString& filename) { QString errorString; @@ -476,7 +468,7 @@ void MissionController::loadMissionFromFile(const QString& filename) _initAllVisualItems(); } -void MissionController::loadMissionFromFilePicker(void) +void MissionController::loadFromFilePicker(void) { #ifndef __mobile__ QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Mission File to load", QString(), "Mission file (*.mission);;All Files (*.*)"); @@ -484,11 +476,11 @@ void MissionController::loadMissionFromFilePicker(void) if (filename.isEmpty()) { return; } - loadMissionFromFile(filename); + loadFromFile(filename); #endif } -void MissionController::saveMissionToFile(const QString& filename) +void MissionController::saveToFile(const QString& filename) { qDebug() << filename; @@ -560,7 +552,7 @@ void MissionController::saveMissionToFile(const QString& filename) _visualItems->setDirty(false); } -void MissionController::saveMissionToFilePicker(void) +void MissionController::saveToFilePicker(void) { #ifndef __mobile__ QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save mission to", QString(), "Mission file (*.mission);;All Files (*.*)"); @@ -568,7 +560,7 @@ void MissionController::saveMissionToFilePicker(void) if (filename.isEmpty()) { return; } - saveMissionToFile(filename); + saveToFile(filename); #endif } @@ -979,9 +971,9 @@ void MissionController::_initAllVisualItems(void) emit visualItemsChanged(); emit complexVisualItemsChanged(); - _visualItems->setDirty(false); + connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); - connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged); + _visualItems->setDirty(false); } void MissionController::_deinitAllVisualItems(void) @@ -990,7 +982,7 @@ void MissionController::_deinitAllVisualItems(void) _deinitVisualItem(qobject_cast(_visualItems->get(i))); } - connect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged); + disconnect(_visualItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::dirtyChanged); } void MissionController::_initVisualItem(VisualMissionItem* visualItem) @@ -1029,48 +1021,45 @@ void MissionController::_itemCommandChanged(void) _recalcWaypointLines(); } -void MissionController::_activeVehicleChanged(Vehicle* activeVehicle) +void MissionController::_activeVehicleBeingRemoved(void) { - qCDebug(MissionControllerLog) << "_activeVehicleChanged activeVehicle" << activeVehicle; + qCDebug(MissionControllerLog) << "_activeVehicleSet _activeVehicleBeingRemoved"; - if (_activeVehicle) { - MissionManager* missionManager = _activeVehicle->missionManager(); - - disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); - disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); - disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); - disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); - disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); - _activeVehicle = NULL; - } + MissionManager* missionManager = _activeVehicle->missionManager(); + + disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); + disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); + disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); + disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); + disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); // We always remove all items on vehicle change. This leaves a user model hole: // If the user has unsaved changes in the Plan view they will lose them - removeAllMissionItems(); + removeAll(); +} - _activeVehicle = activeVehicle; +void MissionController::_activeVehicleSet(void) +{ + // We always remove all items on vehicle change. This leaves a user model hole: + // If the user has unsaved changes in the Plan view they will lose them + removeAll(); - if (_activeVehicle) { - MissionManager* missionManager = activeVehicle->missionManager(); - - connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); - connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); - connect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); - connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); - connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); - - if (_activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { - // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. - // We don't request mission items for new vehicles since that will happen autamatically. - getMissionItems(); - } + MissionManager* missionManager = _activeVehicle->missionManager(); + + connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle); + connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); + connect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged); + connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); + connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); - _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); - _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); + if (_activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) { + // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle. + // We don't request mission items for new vehicles since that will happen autamatically. + loadFromVehicle(); } - // Whenever vehicle changes we need to update syncInProgress - emit syncInProgressChanged(syncInProgress()); + _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); + _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); } void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable) @@ -1095,21 +1084,6 @@ void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& } } -void MissionController::setAutoSync(bool autoSync) -{ - // FIXME: AutoSync temporarily turned off -#if 0 - _autoSync = autoSync; - emit autoSyncChanged(_autoSync); - - if (_autoSync) { - _dirtyChanged(true); - } -#else - Q_UNUSED(autoSync) -#endif -} - void MissionController::setMissionDistance(double missionDistance) { if (!qFuzzyCompare(_missionDistance, missionDistance)) { @@ -1142,36 +1116,9 @@ void MissionController::setHoverDistance(double hoverDistance) } } -void MissionController::_dirtyChanged(bool dirty) -{ - if (dirty && _autoSync) { - Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); - - if (activeVehicle && !activeVehicle->armed()) { - if (_activeVehicle->missionManager()->inProgress()) { - _queuedSend = true; - } else { - _autoSyncSend(); - } - } - } -} - -void MissionController::_autoSyncSend(void) -{ - _queuedSend = false; - if (_visualItems) { - sendMissionItems(); - _visualItems->setDirty(false); - } -} - void MissionController::_inProgressChanged(bool inProgress) { emit syncInProgressChanged(inProgress); - if (!inProgress && _queuedSend) { - _autoSyncSend(); - } } bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame) @@ -1292,11 +1239,20 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber) } } -bool MissionController::syncInProgress(void) +bool MissionController::syncInProgress(void) const { - if (_activeVehicle) { - return _activeVehicle->missionManager()->inProgress(); - } else { - return false; + return _activeVehicle ? _activeVehicle->missionManager()->inProgress() : false; +} + +bool MissionController::dirty(void) const +{ + return _visualItems ? _visualItems->dirty() : false; +} + + +void MissionController::setDirty(bool dirty) +{ + if (_visualItems) { + _visualItems->setDirty(dirty); } } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index f0b49285e..ff5f7b227 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -11,22 +11,22 @@ #ifndef MissionController_H #define MissionController_H -#include -#include - +#include "PlanElementController.h" #include "QmlObjectListModel.h" #include "Vehicle.h" #include "QGCLoggingCategory.h" #include "MavlinkQmlSingleton.h" #include "VisualMissionItem.h" +#include + class CoordinateVector; Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog) typedef QPair VisualItemPair; typedef QHash CoordVectHashTable; -class MissionController : public QObject +class MissionController : public PlanElementController { Q_OBJECT @@ -37,24 +37,13 @@ public: Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged) Q_PROPERTY(QmlObjectListModel* complexVisualItems READ complexVisualItems NOTIFY complexVisualItemsChanged) Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) - Q_PROPERTY(bool autoSync READ autoSync WRITE setAutoSync NOTIFY autoSyncChanged) - Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged) Q_PROPERTY(double missionMaxTelemetry READ missionMaxTelemetry NOTIFY missionMaxTelemetryChanged) Q_PROPERTY(double cruiseDistance READ cruiseDistance NOTIFY cruiseDistanceChanged) Q_PROPERTY(double hoverDistance READ hoverDistance NOTIFY hoverDistanceChanged) - - Q_INVOKABLE void start(bool editMode); - Q_INVOKABLE void getMissionItems(void); - Q_INVOKABLE void sendMissionItems(void); - Q_INVOKABLE void loadMissionFromFilePicker(void); - Q_INVOKABLE void loadMissionFromFile(const QString& filename); - Q_INVOKABLE void saveMissionToFilePicker(void); - Q_INVOKABLE void saveMissionToFile(const QString& filename); Q_INVOKABLE void removeMissionItem(int index); - Q_INVOKABLE void removeAllMissionItems(void); /// Add a new simple mission item to the list /// @param i: index to insert at @@ -66,16 +55,25 @@ public: /// @return Sequence number for new item Q_INVOKABLE int insertComplexMissionItem(QGeoCoordinate coordinate, int i); + // Overrides from PlanElementController + void start (bool editMode) 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; + bool syncInProgress (void) const final; + bool dirty (void) const final; + void setDirty (bool dirty) final; + // Property accessors QmlObjectListModel* visualItems (void) { return _visualItems; } QmlObjectListModel* complexVisualItems (void) { return _complexItems; } QmlObjectListModel* waypointLines (void) { return &_waypointLines; } - bool autoSync(void) { return _autoSync; } - void setAutoSync(bool autoSync); - bool syncInProgress(void); - double missionDistance (void) const { return _missionDistance; } double missionMaxTelemetry (void) const { return _missionMaxTelemetry; } double cruiseDistance (void) const { return _cruiseDistance; } @@ -93,9 +91,7 @@ signals: void visualItemsChanged(void); void complexVisualItemsChanged(void); void waypointLinesChanged(void); - void autoSyncChanged(bool autoSync); void newItemsFromVehicle(void); - void syncInProgressChanged(bool syncInProgress); void missionDistanceChanged(double missionDistance); void missionMaxTelemetryChanged(double missionMaxTelemetry); void cruiseDistanceChanged(double cruiseDistance); @@ -104,10 +100,8 @@ signals: private slots: void _newMissionItemsAvailableFromVehicle(); void _itemCommandChanged(void); - void _activeVehicleChanged(Vehicle* activeVehicle); void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable); void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition); - void _dirtyChanged(bool dirty); void _inProgressChanged(bool inProgress); void _currentMissionItemChanged(int sequenceNumber); void _recalcWaypointLines(void); @@ -121,7 +115,6 @@ private: void _deinitAllVisualItems(void); void _initVisualItem(VisualMissionItem* item); void _deinitVisualItem(VisualMissionItem* item); - void _autoSyncSend(void); void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle); static void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference); static void _calcHomeDist(VisualMissionItem* currentItem, VisualMissionItem* homeItem, double* distance); @@ -134,14 +127,15 @@ private: bool _loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString); int _nextSequenceNumber(void); + // Overrides from PlanElementController + void _activeVehicleBeingRemoved(void) final; + void _activeVehicleSet(void) final; + private: - bool _editMode; QmlObjectListModel* _visualItems; QmlObjectListModel* _complexItems; QmlObjectListModel _waypointLines; CoordVectHashTable _linesTable; - Vehicle* _activeVehicle; - bool _autoSync; bool _firstItemsFromVehicle; bool _missionItemsRequested; bool _queuedSend; diff --git a/src/MissionManager/MissionControllerTest.cc b/src/MissionManager/MissionControllerTest.cc index 24107145b..d81084a87 100644 --- a/src/MissionManager/MissionControllerTest.cc +++ b/src/MissionManager/MissionControllerTest.cc @@ -89,9 +89,6 @@ void MissionControllerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType) QmlObjectListModel* waypointLines = _missionController->waypointLines(); QVERIFY(waypointLines); QCOMPARE(waypointLines->count(), 0); - - // AutoSync should be off by default - QCOMPARE(_missionController->autoSync(), false); } void MissionControllerTest::_testEmptyVehicleWorker(MAV_AUTOPILOT firmwareType) diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index f3661e1af..6a4602abe 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -31,7 +31,6 @@ class MissionManager : public QObject Q_OBJECT public: - /// @param uas Uas which this set of facts is associated with MissionManager(Vehicle* vehicle); ~MissionManager(); diff --git a/src/MissionManager/PlanElementController.cc b/src/MissionManager/PlanElementController.cc new file mode 100644 index 000000000..11c2acc96 --- /dev/null +++ b/src/MissionManager/PlanElementController.cc @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * (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 "PlanElementController.h" +#include "QGCApplication.h" + +PlanElementController::PlanElementController(QObject* parent) + : QObject(parent) + , _activeVehicle(NULL) + , _multiVehicleMgr(qgcApp()->toolbox()->multiVehicleManager()) + , _editMode(false) +{ + +} + +PlanElementController::~PlanElementController() +{ + +} + +void PlanElementController::start(bool editMode) +{ + _editMode = editMode; + connect(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &PlanElementController::_activeVehicleChanged); + _activeVehicleChanged(_multiVehicleMgr->activeVehicle()); +} + +void PlanElementController::_activeVehicleChanged(Vehicle* activeVehicle) +{ + if (_activeVehicle) { + _activeVehicleBeingRemoved(); + } + + _activeVehicle = activeVehicle; + + if (_activeVehicle) { + _activeVehicleSet(); + } + + // Whenever vehicle changes we need to update syncInProgress + emit syncInProgressChanged(syncInProgress()); +} diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h new file mode 100644 index 000000000..b7d26c153 --- /dev/null +++ b/src/MissionManager/PlanElementController.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#ifndef PlanElementController_H +#define PlanElementController_H + +#include + +#include "Vehicle.h" +#include "MultiVehicleManager.h" + +/// This is the abstract base clas for Plan Element controllers. +/// Examples of plan elements are: missions (MissionController), geofence (GeoFenceController) +class PlanElementController : public QObject +{ + Q_OBJECT + +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) + + /// true: unsaved/sent changes are present, false: no changes since last save/send + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + + /// Should be called immediately upon Component.onCompleted. + /// @param editMode true: controller being used in Plan view, false: controller being used in Fly view + Q_INVOKABLE virtual void start(bool editMode); + + Q_INVOKABLE virtual void loadFromVehicle(void) = 0; + Q_INVOKABLE virtual void sendToVehicle(void) = 0; + Q_INVOKABLE virtual void loadFromFilePicker(void) = 0; + 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; + + virtual bool syncInProgress (void) const = 0; + virtual bool dirty (void) const = 0; + virtual void setDirty (bool dirty) = 0; + +signals: + void syncInProgressChanged (bool syncInProgress); + void dirtyChanged (bool dirty); + +protected: + Vehicle* _activeVehicle; + MultiVehicleManager* _multiVehicleMgr; + bool _editMode; + + /// Called when the current active vehicle has been removed. Derived classes should override + /// to implement custom behavior. + virtual void _activeVehicleBeingRemoved(void) = 0; + + /// Called when a new active vehicle has been set. Derived classes should override + /// to implement custom behavior. + virtual void _activeVehicleSet(void) = 0; + +private slots: + void _activeVehicleChanged(Vehicle* activeVehicle); +}; + +#endif diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc new file mode 100644 index 000000000..060eb10bf --- /dev/null +++ b/src/MissionManager/QGCMapPolygon.cc @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * (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 "QGCMapPolygon.h" +#include "QGCGeo.h" + +#include +#include +#include + +QGCMapPolygon::QGCMapPolygon(QObject* parent) + : QObject(parent) + , _dirty(false) +{ + +} + +const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other) +{ + _polygonPath = other._polygonPath; + setDirty(true); + + emit pathChanged(); + + return *this; +} + +void QGCMapPolygon::clear(void) +{ + // Bug workaround, see below + while (_polygonPath.count() > 1) { + _polygonPath.takeLast(); + } + emit pathChanged(); + + // Although this code should remove the polygon from the map it doesn't. There appears + // to be a bug in QGCMapPolygon which causes it to not be redrawn if the list is empty. So + // we work around it by using the code above to remove all but the last point which in turn + // will cause the polygon to go away. + _polygonPath.clear(); + + setDirty(true); +} + +void QGCMapPolygon::addCoordinate(const QGeoCoordinate coordinate) +{ + _polygonPath << QVariant::fromValue(coordinate); + emit pathChanged(); + setDirty(true); +} + +void QGCMapPolygon::adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate) +{ + _polygonPath[vertexIndex] = QVariant::fromValue(coordinate); + emit pathChanged(); + setDirty(true); +} + +void QGCMapPolygon::setDirty(bool dirty) +{ + if (_dirty != dirty) { + _dirty = dirty; + emit dirtyChanged(dirty); + } +} + +QGeoCoordinate QGCMapPolygon::center(void) const +{ + QPolygonF polygon; + + QGeoCoordinate tangentOrigin = _polygonPath[0].value(); + + foreach(const QVariant& coordVar, _polygonPath) { + double y, x, down; + + convertGeoToNed(coordVar.value(), tangentOrigin, &y, &x, &down); + polygon << QPointF(x, -y); + } + + QGeoCoordinate centerCoord; + QPointF centerPoint = polygon.boundingRect().center(); + convertNedToGeo(-centerPoint.y(), centerPoint.x(), 0, tangentOrigin, ¢erCoord); + + return centerCoord; +} + +void QGCMapPolygon::setPath(const QList& path) +{ + _polygonPath.clear(); + foreach(const QGeoCoordinate& coord, path) { + _polygonPath << QVariant::fromValue(coord); + } + setDirty(true); + emit pathChanged(); +} + +void QGCMapPolygon::setPath(const QVariantList& path) +{ + _polygonPath = path; + setDirty(true); + emit pathChanged(); +} diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h new file mode 100644 index 000000000..2dc3d8b65 --- /dev/null +++ b/src/MissionManager/QGCMapPolygon.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ + +#ifndef QGCMapPolygon_H +#define QGCMapPolygon_H + +#include +#include +#include + +class QGCMapPolygon : public QObject +{ + Q_OBJECT + +public: + QGCMapPolygon(QObject* parent = NULL); + + const QGCMapPolygon& operator=(const QGCMapPolygon& other); + + Q_PROPERTY(QVariantList path READ path WRITE setPath NOTIFY pathChanged) + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + + Q_INVOKABLE void clear(void); + Q_INVOKABLE void addCoordinate(const QGeoCoordinate coordinate); + Q_INVOKABLE void adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate); + Q_INVOKABLE QGeoCoordinate center(void) const; + Q_INVOKABLE int count(void) const { return _polygonPath.count(); } + + const QVariantList path(void) const { return _polygonPath; } + void setPath(const QList& path); + void setPath(const QVariantList& path); + + const QGeoCoordinate operator[](int index) { return _polygonPath[index].value(); } + + bool dirty(void) const { return _dirty; } + void setDirty(bool dirty); + +signals: + void pathChanged(void); + void dirtyChanged(bool dirty); + +private: + QVariantList _polygonPath; + bool _dirty; +}; + +#endif diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 98ef42261..70c570570 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -93,6 +93,8 @@ #include "PositionManager.h" #include "FollowMe.h" #include "MissionCommandTree.h" +#include "GeoFenceController.h" +#include "QGCMapPolygon.h" #ifndef __ios__ #include "SerialLink.h" @@ -370,14 +372,15 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl", 1, 0, "VideoSurface", "Reference only"); qmlRegisterUncreatableType ("QGroundControl", 1, 0, "MissionCommandTree", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "AutoPilotPlugin", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "Vehicle", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionItem", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionManager", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "AutoPilotPlugin", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "Vehicle", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionItem", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionManager", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.FlightMap", 1, 0, "QGCMapPolygon", "Reference only"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController"); @@ -397,6 +400,7 @@ void QGCApplication::_initCommon(void) qmlRegisterType ("QGroundControl.Controllers", 1, 0, "QGCMobileFileDialogController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "RCChannelMonitorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "JoystickConfigController"); + qmlRegisterType ("QGroundControl.Controllers", 1, 0, "GeoFenceController"); #ifndef __mobile__ qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ViewWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "CustomCommandWidgetController"); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 8320e9234..a54e91abe 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -25,6 +25,7 @@ #include "FollowMe.h" #include "MissionCommandTree.h" #include "QGroundControlQmlGlobal.h" +#include "GeoFenceManager.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -98,6 +99,8 @@ Vehicle::Vehicle(LinkInterface* link, , _connectionLostEnabled(true) , _missionManager(NULL) , _missionManagerInitialRequestComplete(false) + , _geoFenceManager(NULL) + , _geoFenceManagerInitialRequestComplete(false) , _parameterLoader(NULL) , _armed(false) , _base_mode(0) @@ -193,7 +196,11 @@ Vehicle::Vehicle(LinkInterface* link, _loadSettings(); _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); + + _geoFenceManager = new GeoFenceManager(this); + connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); _parameterLoader = new ParameterLoader(this); connect(_parameterLoader, &ParameterLoader::parametersReady, _autopilotPlugin, &AutoPilotPlugin::_parametersReadyPreChecks); @@ -1333,6 +1340,12 @@ void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg) qgcApp()->showMessage(QString("Error during Mission communication with Vehicle: %1").arg(errorMsg)); } +void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg) +{ + Q_UNUSED(errorCode); + qgcApp()->showMessage(QString("Error during Geo-Fence communication with Vehicle: %1").arg(errorMsg)); +} + void Vehicle::_addNewMapTrajectoryPoint(void) { if (_mapTrajectoryHaveFirstCoordinate) { @@ -1805,6 +1818,29 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs) } #endif +/// Returns true if the specifed parameter exists from the default component +bool Vehicle::parameterExists(int componentId, const QString& name) +{ + return _autopilotPlugin->parameterExists(componentId, name); +} + +/// Returns the specified parameter Fact from the default component +/// WARNING: Returns a default Fact if parameter does not exists. If that possibility exists, check for existence first with +/// parameterExists. +Fact* Vehicle::getParameterFact(int componentId, const QString& name) +{ + return _autopilotPlugin->getParameterFact(componentId, name); +} + +void Vehicle::_newMissionItemsAvailable(void) +{ + // After the initial mission request complets we ask for the geofence + if (!_geoFenceManagerInitialRequestComplete) { + _geoFenceManagerInitialRequestComplete = true; + _geoFenceManager->requestGeoFence(); + } +} + const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7dce83d5a..a2cfe9579 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -33,6 +33,7 @@ class FirmwarePluginManager; class AutoPilotPlugin; class AutoPilotPluginManager; class MissionManager; +class GeoFenceManager; class ParameterLoader; class JoystickManager; class UASMessage; @@ -444,6 +445,7 @@ public: int manualControlReservedButtonCount(void); MissionManager* missionManager(void) { return _missionManager; } + GeoFenceManager* geoFenceManager(void) { return _geoFenceManager; } bool homePositionAvailable(void); QGeoCoordinate homePosition(void); @@ -567,6 +569,14 @@ public: /// @return true: X confiuration, false: Plus configuration bool xConfigMotors(void); + /// Returns true if the specifed parameter exists from the default component + bool parameterExists(int componentId, const QString& name); + + /// Returns the specified parameter Fact from the default component + /// WARNING: Returns a default Fact if parameter does not exists. If that possibility exists, check for existence first with + /// parameterExists. + Fact* getParameterFact(int componentId, const QString& name); + public slots: void setLatitude(double latitude); void setLongitude(double longitude); @@ -658,6 +668,7 @@ private slots: void _imageReady (UASInterface* uas); void _connectionLostTimeout(void); void _prearmErrorTimeout(void); + void _newMissionItemsAvailable(void); private: bool _containsLink(LinkInterface* link); @@ -678,6 +689,7 @@ private: void _handleCommandAck(mavlink_message_t& message); void _handleAutopilotVersion(mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); + void _geoFenceManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); void _connectionActive(void); @@ -743,6 +755,9 @@ private: MissionManager* _missionManager; bool _missionManagerInitialRequestComplete; + GeoFenceManager* _geoFenceManager; + bool _geoFenceManagerInitialRequestComplete; + ParameterLoader* _parameterLoader; bool _armed; ///< true: vehicle is armed -- 2.22.0