diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 43658f300829b6d22042640cf61a05e431e99399..2e37ae704a9fe57185e87a6a160890e163461437 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -212,6 +212,7 @@ src/Settings/APMMavlinkStreamRate.SettingsGroup.json + src/MissionManager/BreachReturn.FactMetaData.json src/MissionManager/CameraCalc.FactMetaData.json src/MissionManager/CameraSpec.FactMetaData.json src/MissionManager/CorridorScan.SettingsGroup.json diff --git a/src/MissionManager/BreachReturn.FactMetaData.json b/src/MissionManager/BreachReturn.FactMetaData.json new file mode 100644 index 0000000000000000000000000000000000000000..fa7536ee26e79f2e7ea61e492407ff1c8a87f839 --- /dev/null +++ b/src/MissionManager/BreachReturn.FactMetaData.json @@ -0,0 +1,21 @@ +[ +{ + "name": "Latitude", + "shortDescription": "Latitude of breach return point position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Longitude", + "shortDescription": "Longitude of breach return point position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Altitude", + "shortDescription": "Altitude of breach return point position (Rel)", + "type": "double", + "decimalPlaces": 2, + "units": "m" +} +] diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index f3e004a117e5b4617af6f776fb8fba26614da4f6..1eacafd4ee5c55b181567b393cee3085acb8d284 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -21,6 +21,8 @@ #include "QGCQGeoCoordinate.h" #include "AppSettings.h" #include "PlanMasterController.h" +#include "SettingsManager.h" +#include "AppSettings.h" #ifndef __mobile__ #include "MainWindow.h" @@ -32,24 +34,43 @@ QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog") +QMap GeoFenceController::_metaDataMap; + const char* GeoFenceController::_jsonFileTypeValue = "GeoFence"; const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn"; const char* GeoFenceController::_jsonPolygonsKey = "polygons"; const char* GeoFenceController::_jsonCirclesKey = "circles"; +const char* GeoFenceController::_breachReturnAltitudeFactName = "Altitude"; + const char* GeoFenceController::_px4ParamCircularFence = "GF_MAX_HOR_DIST"; GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent) - : PlanElementController (masterController, parent) - , _geoFenceManager (_managerVehicle->geoFenceManager()) - , _dirty (false) + : PlanElementController (masterController, parent) + , _geoFenceManager (_managerVehicle->geoFenceManager()) + , _dirty (false) + , _breachReturnAltitudeFact (0, _breachReturnAltitudeFactName, FactMetaData::valueTypeDouble) + , _breachReturnDefaultAltitude (qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble()) + , _itemsRequested (false) - , _px4ParamCircularFenceFact(NULL) + , _px4ParamCircularFenceFact(nullptr) { + if (_metaDataMap.isEmpty()) { + _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/BreachReturn.FactMetaData.json"), nullptr /* metaDataParent */); + } + + _breachReturnAltitudeFact.setMetaData(_metaDataMap[_breachReturnAltitudeFactName]); + _breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude); + connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems); managerVehicleChanged(_managerVehicle); + + connect(this, &GeoFenceController::breachReturnPointChanged, this, &GeoFenceController::_setDirty); + connect(&_breachReturnAltitudeFact, &Fact::rawValueChanged, this, &GeoFenceController::_setDirty); + connect(&_polygons, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty); + connect(&_circles, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty); } GeoFenceController::~GeoFenceController() @@ -79,26 +100,19 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn } } -void GeoFenceController::_signalAll(void) -{ - emit breachReturnPointChanged(breachReturnPoint()); - emit dirtyChanged(dirty()); - emit supportedChanged(supported()); -} - void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) { if (_managerVehicle) { _geoFenceManager->disconnect(this); _managerVehicle->disconnect(this); _managerVehicle->parameterManager()->disconnect(this); - _managerVehicle = NULL; - _geoFenceManager = NULL; + _managerVehicle = nullptr; + _geoFenceManager = nullptr; } _managerVehicle = managerVehicle; if (!_managerVehicle) { - qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=NULL"; + qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=nullptr"; return; } @@ -114,7 +128,6 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle) _parametersReady(); emit supportedChanged(supported()); - _signalAll(); } bool GeoFenceController::load(const QJsonObject& json, QString& errorString) @@ -133,6 +146,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) { JsonHelper::jsonVersionKey, QJsonValue::Double, true }, { _jsonCirclesKey, QJsonValue::Array, true }, { _jsonPolygonsKey, QJsonValue::Array, true }, + { _jsonBreachReturnKey, QJsonValue::Array, false }, }; if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) { return false; @@ -144,7 +158,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) } QJsonArray jsonPolygonArray = json[_jsonPolygonsKey].toArray(); - for (const QJsonValue& jsonPolygonValue: jsonPolygonArray) { + for (const QJsonValue jsonPolygonValue: jsonPolygonArray) { if (jsonPolygonValue.type() != QJsonValue::Object) { errorString = tr("GeoFence polygon not stored as object"); return false; @@ -158,7 +172,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) } QJsonArray jsonCircleArray = json[_jsonCirclesKey].toArray(); - for (const QJsonValue& jsonCircleValue: jsonCircleArray) { + for (const QJsonValue jsonCircleValue: jsonCircleArray) { if (jsonCircleValue.type() != QJsonValue::Object) { errorString = tr("GeoFence circle not stored as object"); return false; @@ -171,8 +185,18 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString) _circles.append(fenceCircle); } + if (json.contains(_jsonBreachReturnKey)) { + if (!JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], true /* altitudeRequred */, _breachReturnPoint, errorString)) { + return false; + } + _breachReturnAltitudeFact.setRawValue(_breachReturnPoint.altitude()); + } else { + _breachReturnPoint = QGeoCoordinate(); + _breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude); + } + emit breachReturnPointChanged(_breachReturnPoint); + setDirty(false); - _signalAll(); return true; } @@ -198,6 +222,14 @@ void GeoFenceController::save(QJsonObject& json) jsonCircleArray.append(jsonCircle); } json[_jsonCirclesKey] = jsonCircleArray; + + if (_breachReturnPoint.isValid()) { + QJsonValue jsonCoordinate; + + _breachReturnPoint.setAltitude(_breachReturnAltitudeFact.rawValue().toDouble()); + JsonHelper::saveGeoCoordinate(_breachReturnPoint, true /* writeAltitude */, jsonCoordinate); + json[_jsonBreachReturnKey] = jsonCoordinate; + } } void GeoFenceController::removeAll(void) @@ -305,6 +337,11 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP { _breachReturnPoint = breachReturnPoint; emit breachReturnPointChanged(_breachReturnPoint); + if (_breachReturnPoint.isValid()) { + _breachReturnAltitudeFact.setRawValue(_breachReturnPoint.altitude()); + } else { + _breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude); + } } void GeoFenceController::_managerLoadComplete(void) @@ -315,7 +352,6 @@ void GeoFenceController::_managerLoadComplete(void) _setReturnPointFromManager(_geoFenceManager->breachReturnPoint()); _setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles()); setDirty(false); - _signalAll(); emit loadComplete(); } _itemsRequested = false; @@ -476,7 +512,7 @@ void GeoFenceController::_parametersReady(void) { if (_px4ParamCircularFenceFact) { _px4ParamCircularFenceFact->disconnect(this); - _px4ParamCircularFenceFact = NULL; + _px4ParamCircularFenceFact = nullptr; } if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) { diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 9412fc00530c0ca81c0408bf10af327ec5e59502..65c098d6916272fd8639ee41c7d669987b0f25d6 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -27,12 +27,13 @@ class GeoFenceController : public PlanElementController Q_OBJECT public: - GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL); + GeoFenceController(PlanMasterController* masterController, QObject* parent = nullptr); ~GeoFenceController(); - Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) - Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) - Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT) + Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT) + Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged) + Q_PROPERTY(Fact* breachReturnAltitude READ breachReturnAltitude CONSTANT) // Hack to expose PX4 circular fence controlled by GF_MAX_HOR_DIST Q_PROPERTY(double paramCircularFence READ paramCircularFence NOTIFY paramCircularFenceChanged) @@ -58,7 +59,8 @@ public: /// Clears the interactive bit from all fence items Q_INVOKABLE void clearAllInteractive(void); - double paramCircularFence(void); + double paramCircularFence (void); + Fact* breachReturnAltitude(void) { return &_breachReturnAltitudeFact; } // Overrides from PlanElementController bool supported (void) const final; @@ -101,16 +103,19 @@ private slots: private: void _init(void); - void _signalAll(void); GeoFenceManager* _geoFenceManager; bool _dirty; QmlObjectListModel _polygons; QmlObjectListModel _circles; QGeoCoordinate _breachReturnPoint; + Fact _breachReturnAltitudeFact; + double _breachReturnDefaultAltitude; bool _itemsRequested; Fact* _px4ParamCircularFenceFact; + static QMap _metaDataMap; + static const char* _px4ParamCircularFence; static const int _jsonCurrentVersion = 2; @@ -119,6 +124,8 @@ private: static const char* _jsonBreachReturnKey; static const char* _jsonPolygonsKey; static const char* _jsonCirclesKey; + + static const char* _breachReturnAltitudeFactName; }; #endif diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 05ce7ae5473d02ff67cdcfedb656b3d339da8960..7702e7ad717af053e72747af6b9052651593c085 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -51,8 +51,6 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygons, QmlObjectListModel& circles) { - Q_UNUSED(breachReturn); - QList fenceItems; _sendPolygons.clear(); @@ -64,6 +62,7 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, for (int i=0; i(i)); } + _breachReturnPoint = breachReturn; for (int i=0; i<_sendPolygons.count(); i++) { const QGCFencePolygon& polygon = _sendPolygons[i]; @@ -103,12 +102,30 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, fenceItems.append(item); } + if (_breachReturnPoint.isValid()) { + MissionItem* item = new MissionItem(0, + MAV_CMD_NAV_FENCE_RETURN_POINT, + MAV_FRAME_GLOBAL_RELATIVE_ALT, + 0, 0, 0, 0, // param 1-4 unused + breachReturn.latitude(), + breachReturn.longitude(), + breachReturn.altitude(), + false, // autocontinue + false, // isCurrentItem + this); // parent + fenceItems.append(item); + } + // Plan manager takes control of MissionItems, so no need to delete _planManager.writeMissionItems(fenceItems); } void GeoFenceManager::removeAll(void) { + _polygons.clear(); + _circles.clear(); + _breachReturnPoint = QGeoCoordinate(); + _planManager.removeAll(); } @@ -117,6 +134,7 @@ void GeoFenceManager::_sendComplete(bool error) if (error) { _polygons.clear(); _circles.clear(); + _breachReturnPoint = QGeoCoordinate(); } else { _polygons = _sendPolygons; _circles = _sendCircles; @@ -174,6 +192,8 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested) } QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */); _circles.append(circle); + } else if (command == MAV_CMD_NAV_FENCE_RETURN_POINT) { + _breachReturnPoint = QGeoCoordinate(item->param5(), item->param6(), item->param7()); } else { emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command())); break; @@ -183,6 +203,7 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested) if (loadFailed) { _polygons.clear(); _circles.clear(); + _breachReturnPoint = QGeoCoordinate(); } emit loadComplete(); diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index 0db0f7342c1a3633c315fa7c3c7dac869765b281..6230cff8c7d29a7b32a83611220c5430f78fb2e6 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -72,6 +72,7 @@ void RallyPointManager::sendToVehicle(const QList& rgPoints) void RallyPointManager::removeAll(void) { + _rgPoints.clear(); _planManager.removeAll(); } diff --git a/src/PlanView/GeoFenceEditor.qml b/src/PlanView/GeoFenceEditor.qml index 9788aea347a030ac59dbf6733fea0080641d8e02..40bfa0e62d1f1b0b9fa996e5ebcc6867579f47fe 100644 --- a/src/PlanView/GeoFenceEditor.qml +++ b/src/PlanView/GeoFenceEditor.qml @@ -1,6 +1,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtQuick.Layouts 1.2 +import QtPositioning 5.2 import QGroundControl 1.0 import QGroundControl.ScreenTools 1.0 @@ -53,7 +54,7 @@ QGCFlickable { anchors.top: parent.top anchors.left: parent.left anchors.right: parent.right - spacing: ScreenTools.defaultFontPixelHeight / 2 + spacing: _margin QGCLabel { anchors.left: parent.left @@ -66,10 +67,10 @@ QGCFlickable { } Column { - anchors.left: parent.left - anchors.right: parent.right - spacing: ScreenTools.defaultFontPixelHeight / 2 - visible: myGeoFenceController.supported + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: myGeoFenceController.supported Repeater { model: myGeoFenceController.params @@ -114,9 +115,8 @@ QGCFlickable { } QGCButton { - anchors.left: parent.left - anchors.right: parent.right - text: qsTr("Polygon Fence") + Layout.fillWidth: true + text: qsTr("Polygon Fence") onClicked: { var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height) @@ -127,9 +127,8 @@ QGCFlickable { } QGCButton { - anchors.left: parent.left - anchors.right: parent.right - text: qsTr("Circular Fence") + Layout.fillWidth: true + text: qsTr("Circular Fence") onClicked: { var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height) @@ -150,11 +149,10 @@ QGCFlickable { } GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columns: 3 - flow: GridLayout.TopToBottom - visible: polygonSection.checked && myGeoFenceController.polygons.count > 0 + Layout.fillWidth: true + columns: 3 + flow: GridLayout.TopToBottom + visible: polygonSection.checked && myGeoFenceController.polygons.count > 0 QGCLabel { text: qsTr("Inclusion") @@ -224,11 +222,11 @@ QGCFlickable { } GridLayout { - anchors.left: parent.left - anchors.right: parent.right - columns: 4 - flow: GridLayout.TopToBottom - visible: polygonSection.checked && myGeoFenceController.circles.count > 0 + anchors.left: parent.left + anchors.right: parent.right + columns: 4 + flow: GridLayout.TopToBottom + visible: polygonSection.checked && myGeoFenceController.circles.count > 0 QGCLabel { text: qsTr("Inclusion") @@ -302,6 +300,46 @@ QGCFlickable { } } } // GridLayout + + SectionHeader { + id: breachReturnSection + text: qsTr("Breach Return Point") + } + + QGCButton { + text: qsTr("Add Breach Return Point") + visible: breachReturnSection.visible && !myGeoFenceController.breachReturnPoint.isValid + anchors.left: parent.left + anchors.right: parent.right + + onClicked: myGeoFenceController.breachReturnPoint = flightMap.center + } + + QGCButton { + text: qsTr("Remove Breach Return Point") + visible: breachReturnSection.visible && myGeoFenceController.breachReturnPoint.isValid + anchors.left: parent.left + anchors.right: parent.right + + onClicked: myGeoFenceController.breachReturnPoint = QtPositioning.coordinate() + } + + ColumnLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: breachReturnSection.visible && myGeoFenceController.breachReturnPoint.isValid + + QGCLabel { + text: qsTr("Altitude") + } + + AltitudeFactTextField { + fact: myGeoFenceController.breachReturnAltitude + altitudeMode: QGroundControl.AltitudeModeRelative + } + } + } } } diff --git a/src/PlanView/GeoFenceMapVisuals.qml b/src/PlanView/GeoFenceMapVisuals.qml index 371a64a4ca546f00aa288da2e7d77c367918e4cc..18bc19b37bacd8639d7cf676090e55fcd929bf70 100644 --- a/src/PlanView/GeoFenceMapVisuals.qml +++ b/src/PlanView/GeoFenceMapVisuals.qml @@ -28,8 +28,8 @@ Item { property bool planView: false ///< true: visuals showing in plan view property var homePosition - //property var _breachReturnPointComponent - //property var _mouseAreaComponent + property var _breachReturnPointComponent + property var _breachReturnDragComponent property var _paramCircleFenceComponent property var _polygons: myGeoFenceController.polygons property var _circles: myGeoFenceController.circles @@ -75,30 +75,19 @@ Item { } Component.onCompleted: { - //_breachReturnPointComponent = breachReturnPointComponent.createObject(map) - //map.addMapItem(_breachReturnPointComponent) - //_mouseAreaComponent = mouseAreaComponent.createObject(map) + _breachReturnPointComponent = breachReturnPointComponent.createObject(map) + map.addMapItem(_breachReturnPointComponent) + _breachReturnDragComponent = breachReturnDragComponent.createObject(map, { "itemIndicator": _breachReturnPointComponent }) _paramCircleFenceComponent = paramCircleFenceComponent.createObject(map) map.addMapItem(_paramCircleFenceComponent) } Component.onDestruction: { - //_breachReturnPointComponent.destroy() - //_mouseAreaComponent.destroy() + _breachReturnPointComponent.destroy() + _breachReturnDragComponent.destroy() _paramCircleFenceComponent.destroy() } - // 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 */) - } - } - Instantiator { model: _polygons @@ -144,6 +133,19 @@ Item { } } + Component { + id: breachReturnDragComponent + + MissionItemIndicatorDrag { + mapControl: map + itemCoordinate: myGeoFenceController.breachReturnPoint + //visible: itemCoordinate.isValid + + onItemCoordinateChanged: myGeoFenceController.breachReturnPoint = itemCoordinate + } + } + + // Breach return point Component { id: breachReturnPointComponent @@ -155,7 +157,8 @@ Item { coordinate: myGeoFenceController.breachReturnPoint sourceItem: MissionItemIndexLabel { - label: "B" + label: qsTr("B", "Breach Return Point item indicator") + checked: true } } } diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 15f81051084a0e17a4f27c23f544907f47fe33a4..53c31a0e08ae8adcd3178785b44ec12f5be6a076 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -938,9 +938,9 @@ QGCView { (_planMasterController.offline ? "" : qsTr("This will also remove all items from the vehicle.")) function accept() { if (_planMasterController.offline) { - masterController.removeAll() + _planMasterController.removeAll() } else { - masterController.removeAllFromVehicle() + _planMasterController.removeAllFromVehicle() } hideDialog() } @@ -1042,7 +1042,7 @@ QGCView { QGCButton { text: qsTr("New...") Layout.fillWidth: true - enabled: _visualItems.count > 1 + enabled: _planMasterController.containsItems onClicked: { dropPanel.hide() _qgcView.showDialog(removeAllPromptDialog, qsTr("New Plan"), _qgcView.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No) @@ -1080,7 +1080,7 @@ QGCView { QGCButton { text: qsTr("Save As...") Layout.fillWidth: true - enabled: !masterController.syncInProgress && _visualItems.count > 1 + enabled: !masterController.syncInProgress && _planMasterController.containsItems onClicked: { dropPanel.hide() masterController.saveToSelectedFile() @@ -1115,7 +1115,7 @@ QGCView { QGCButton { text: qsTr("Upload") Layout.fillWidth: true - enabled: !masterController.offline && !masterController.syncInProgress && _visualItems.count > 1 + enabled: !masterController.offline && !masterController.syncInProgress && _planMasterController.containsItems visible: !QGroundControl.corePlugin.options.disableVehicleConnection onClicked: { dropPanel.hide()