From 5c4658462a95842fc835743b6271222ca185fe2e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 28 Sep 2016 12:43:21 -0700 Subject: [PATCH] Initial ArduPilot Rally Point support --- qgroundcontrol.pro | 12 +- qgroundcontrol.qrc | 5 +- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 2 + src/FirmwarePlugin/APM/APMGeoFenceManager.cc | 8 +- .../APM/APMRallyPointManager.cc | 144 ++++++++ src/FirmwarePlugin/APM/APMRallyPointManager.h | 49 +++ src/FirmwarePlugin/FirmwarePlugin.h | 8 +- src/FlightDisplay/FlightDisplayViewMap.qml | 26 +- .../MapItems/MissionItemIndicator.qml | 8 +- src/FlightMap/MapItems/MissionItemView.qml | 6 +- src/JsonHelper.cc | 150 ++++++++- src/JsonHelper.h | 47 ++- src/MissionEditor/MissionEditor.qml | 266 +++++++++++---- .../MissionItemEditor.qml | 0 src/MissionEditor/MissionItemStatus.qml | 2 +- src/MissionEditor/RallyPointEditorHeader.qml | 68 ++++ src/MissionEditor/RallyPointItemEditor.qml | 124 +++++++ src/MissionManager/GeoFenceController.cc | 12 +- src/MissionManager/GeoFenceController.h | 2 + src/MissionManager/MissionCommandTree.cc | 5 +- src/MissionManager/MissionController.cc | 5 + src/MissionManager/MissionController.h | 2 + src/MissionManager/MissionItem.cc | 2 +- src/MissionManager/PlanElementController.h | 4 + src/MissionManager/QGCMapPolygon.cc | 33 +- src/MissionManager/RallyPoint.cc | 110 ++++++ src/MissionManager/RallyPoint.h | 66 ++++ src/MissionManager/RallyPoint.json | 21 ++ src/MissionManager/RallyPointController.cc | 317 ++++++++++++++++++ src/MissionManager/RallyPointController.h | 83 +++++ src/MissionManager/RallyPointManager.cc | 44 +++ src/MissionManager/RallyPointManager.h | 67 ++++ src/MissionManager/SurveyMissionItem.cc | 4 +- src/QGCApplication.cc | 7 +- src/QGCApplication.h | 1 + src/QmlControls/MissionItemIndexLabel.qml | 10 +- .../QGroundControl.Controls.qmldir | 2 + src/QmlControls/QmlObjectListModel.cc | 1 + src/Vehicle/Vehicle.cc | 47 ++- src/Vehicle/Vehicle.h | 15 +- 40 files changed, 1640 insertions(+), 145 deletions(-) create mode 100644 src/FirmwarePlugin/APM/APMRallyPointManager.cc create mode 100644 src/FirmwarePlugin/APM/APMRallyPointManager.h rename src/{QmlControls => MissionEditor}/MissionItemEditor.qml (100%) create mode 100644 src/MissionEditor/RallyPointEditorHeader.qml create mode 100644 src/MissionEditor/RallyPointItemEditor.qml create mode 100644 src/MissionManager/RallyPoint.cc create mode 100644 src/MissionManager/RallyPoint.h create mode 100644 src/MissionManager/RallyPoint.json create mode 100644 src/MissionManager/RallyPointController.cc create mode 100644 src/MissionManager/RallyPointController.h create mode 100644 src/MissionManager/RallyPointManager.cc create mode 100644 src/MissionManager/RallyPointManager.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index e79b38a13..a4963410f 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -293,7 +293,6 @@ HEADERS += \ 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 \ @@ -301,6 +300,10 @@ HEADERS += \ src/MissionManager/MissionItem.h \ src/MissionManager/MissionManager.h \ src/MissionManager/PlanElementController.h \ + src/MissionManager/QGCMapPolygon.h \ + src/MissionManager/RallyPoint.h \ + src/MissionManager/RallyPointController.h \ + src/MissionManager/RallyPointManager.h \ src/MissionManager/SimpleMissionItem.h \ src/MissionManager/SurveyMissionItem.h \ src/MissionManager/VisualMissionItem.h \ @@ -457,7 +460,6 @@ SOURCES += \ 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 \ @@ -465,6 +467,10 @@ SOURCES += \ src/MissionManager/MissionItem.cc \ src/MissionManager/MissionManager.cc \ src/MissionManager/PlanElementController.cc \ + src/MissionManager/QGCMapPolygon.cc \ + src/MissionManager/RallyPoint.cc \ + src/MissionManager/RallyPointController.cc \ + src/MissionManager/RallyPointManager.cc \ src/MissionManager/SimpleMissionItem.cc \ src/MissionManager/SurveyMissionItem.cc \ src/MissionManager/VisualMissionItem.cc \ @@ -696,6 +702,7 @@ HEADERS+= \ src/FirmwarePlugin/APM/APMFirmwarePlugin.h \ src/FirmwarePlugin/APM/APMGeoFenceManager.h \ src/FirmwarePlugin/APM/APMParameterMetaData.h \ + src/FirmwarePlugin/APM/APMRallyPointManager.h \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \ @@ -757,6 +764,7 @@ SOURCES += \ src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \ src/FirmwarePlugin/APM/APMGeoFenceManager.cc \ src/FirmwarePlugin/APM/APMParameterMetaData.cc \ + src/FirmwarePlugin/APM/APMRallyPointManager.cc \ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 9780befa8..52a5fc8fa 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -55,7 +55,7 @@ src/QmlControls/JoystickThumbPad.qml src/ui/toolbar/MainToolBar.qml src/ui/toolbar/MainToolBarIndicators.qml - src/QmlControls/MissionItemEditor.qml + src/MissionEditor/MissionItemEditor.qml src/QmlControls/MissionItemIndexLabel.qml src/MissionEditor/MissionItemStatus.qml src/QmlControls/MissionCommandDialog.qml @@ -63,6 +63,7 @@ src/QmlControls/ModeSwitchDisplay.qml src/QmlControls/ParameterEditor.qml src/QmlControls/ParameterEditorDialog.qml + src/MissionEditor/RallyPointItemEditor.qml src/QmlControls/RCChannelMonitor.qml src/QmlControls/QGCButton.qml src/QmlControls/QGCCheckBox.qml @@ -83,6 +84,7 @@ src/QmlControls/QGCViewDialog.qml src/QmlControls/QGCViewMessage.qml src/QmlControls/QGCViewPanel.qml + src/MissionEditor/RallyPointEditorHeader.qml src/QmlControls/RoundButton.qml src/AutoPilotPlugins/Common/SetupPage.qml src/ui/toolbar/SignalStrength.qml @@ -190,5 +192,6 @@ src/Vehicle/WindFact.json src/Vehicle/VibrationFact.json src/QmlControls/QGroundControlQmlGlobal.json + src/MissionManager/RallyPoint.json diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index fd6824d85..a15051ca6 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -18,6 +18,7 @@ #include "QGCLoggingCategory.h" #include "APMParameterMetaData.h" #include "APMGeoFenceManager.h" +#include "APMRallyPointManager.h" #include @@ -93,6 +94,7 @@ public: void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } QObject* loadParameterMetaData (const QString& metaDataFile); GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) { return new APMGeoFenceManager(vehicle); } + RallyPointManager* newRallyPointManager (Vehicle* vehicle) { return new APMRallyPointManager(vehicle); } QString getParameterMetaDataFile(Vehicle* vehicle); diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 3759459db..18753af2d 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -99,6 +99,7 @@ void APMGeoFenceManager::loadFromVehicle(void) return; } + _breachReturnPoint = QGeoCoordinate(); _polygon.clear(); if (!_geoFenceSupported()) { @@ -112,7 +113,8 @@ void APMGeoFenceManager::loadFromVehicle(void) int minFencePoints = 6; qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints; if (cFencePoints == 0) { - // No fence, no more work to do, fence data has already been cleared + // No fence + emit loadComplete(_breachReturnPoint, _polygon); return; } if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < minFencePoints)) { @@ -142,6 +144,8 @@ void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& messag if (fencePoint.idx != _currentFencePoint) { // FIXME: Protocol out of whack + _readTransactionInProgress = false; + emit inProgressChanged(inProgress()); qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; return; } @@ -159,8 +163,10 @@ void APMGeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& messag _requestFencePoint(++_currentFencePoint); } else { // We've finished collecting fence points + qCDebug(GeoFenceManagerLog) << "Fence point load complete"; _readTransactionInProgress = false; emit loadComplete(_breachReturnPoint, _polygon); + emit inProgressChanged(inProgress()); } } } diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc new file mode 100644 index 000000000..be39947f9 --- /dev/null +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.cc @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * (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 "APMRallyPointManager.h" +#include "Vehicle.h" +#include "FirmwarePlugin.h" +#include "MAVLinkProtocol.h" +#include "QGCApplication.h" +#include "ParameterManager.h" + +const char* APMRallyPointManager::_rallyTotalParam = "RALLY_TOTAL"; + +APMRallyPointManager::APMRallyPointManager(Vehicle* vehicle) + : RallyPointManager(vehicle) + , _readTransactionInProgress(false) + , _writeTransactionInProgress(false) +{ + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMRallyPointManager::_mavlinkMessageReceived); +} + +APMRallyPointManager::~APMRallyPointManager() +{ + +} + +void APMRallyPointManager::sendToVehicle(const QList& rgPoints) +{ + if (_vehicle->isOfflineEditingVehicle()) { + return; + } + + if (_readTransactionInProgress) { + _sendError(InternalError, QStringLiteral("Rally Point write attempted while read in progress.")); + return; + } + + _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->setRawValue(rgPoints.count()); + + // FIXME: No validation of correct point received + _rgPoints = rgPoints; + for (uint8_t index=0; index<_rgPoints.count(); index++) { + _sendRallyPoint(index); + } + + emit loadComplete(_rgPoints); +} + +void APMRallyPointManager::loadFromVehicle(void) +{ + if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) { + return; + } + + _rgPoints.clear(); + + _cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt(); + qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << _cReadRallyPoints; + if (_cReadRallyPoints == 0) { + emit loadComplete(_rgPoints); + return; + } + + _currentRallyPoint = 0; + _readTransactionInProgress = true; + _requestRallyPoint(_currentRallyPoint); +} + +/// Called when a new mavlink message for out vehicle is received +void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + if (message.msgid == MAVLINK_MSG_ID_RALLY_POINT) { + mavlink_rally_point_t rallyPoint; + + mavlink_msg_rally_point_decode(&message, &rallyPoint); + qCDebug(RallyPointManagerLog) << "From vehicle rally_point: idx:lat:lng:alt" << rallyPoint.idx << rallyPoint.lat << rallyPoint.lng << rallyPoint.alt; + + if (rallyPoint.idx != _currentRallyPoint) { + // FIXME: Protocol out of whack + _readTransactionInProgress = false; + emit inProgressChanged(inProgress()); + qCWarning(RallyPointManagerLog) << "Indices out of sync" << rallyPoint.idx << _currentRallyPoint; + return; + } + + QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt); + _rgPoints.append(point); + if (rallyPoint.idx < _cReadRallyPoints - 2) { + // Still more points to request + _requestRallyPoint(++_currentRallyPoint); + } else { + // We've finished collecting rally points + qCDebug(RallyPointManagerLog) << "Rally point load complete"; + _readTransactionInProgress = false; + emit loadComplete(_rgPoints); + emit inProgressChanged(inProgress()); + } + } +} + +void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex) +{ + mavlink_message_t msg; + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + + qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex; + mavlink_msg_rally_fetch_point_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex); + _vehicle->sendMessageOnPriorityLink(msg); +} + +void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex) +{ + mavlink_message_t msg; + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + + QGeoCoordinate point = _rgPoints[pointIndex]; + mavlink_msg_rally_point_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex, + _rgPoints.count(), + point.latitude() * 1e7, + point.longitude() * 1e7, + point.altitude(), + 0, 0, 0); // break_alt, land_dir, flags + _vehicle->sendMessageOnPriorityLink(msg); +} + +bool APMRallyPointManager::inProgress(void) const +{ + return _readTransactionInProgress || _writeTransactionInProgress; +} diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.h b/src/FirmwarePlugin/APM/APMRallyPointManager.h new file mode 100644 index 000000000..80dc758c4 --- /dev/null +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * (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 APMRallyPointManager_H +#define APMRallyPointManager_H + +#include "RallyPointManager.h" +#include "QGCMAVLink.h" + +class APMRallyPointManager : public RallyPointManager +{ + Q_OBJECT + +public: + APMRallyPointManager(Vehicle* vehicle); + ~APMRallyPointManager(); + + // Overrides from RallyPointManager + bool inProgress (void) const final; + void loadFromVehicle (void) final; + void sendToVehicle (const QList& rgPoints) final; + bool rallyPointsSupported (void) const final { return true; } + + QString editorQml(void) const final { return QStringLiteral("qrc:/FirmwarePlugin/APM/APMRallyPointEditor.qml"); } + +private slots: + void _mavlinkMessageReceived(const mavlink_message_t& message); + +private: + void _requestRallyPoint(uint8_t pointIndex); + void _sendRallyPoint(uint8_t pointIndex); + +private: + bool _readTransactionInProgress; + bool _writeTransactionInProgress; + + uint8_t _cReadRallyPoints; + uint8_t _currentRallyPoint; + + static const char* _rallyTotalParam; +}; + +#endif diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 4ddd29ff0..bc464e635 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -18,6 +18,7 @@ #include "VehicleComponent.h" #include "AutoPilotPlugin.h" #include "GeoFenceManager.h" +#include "RallyPointManager.h" #include #include @@ -209,14 +210,15 @@ public: /// @return true: X confiuration, false: Plus configuration virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; } - /// Returns a newly create geofence manager for this vehicle. Returns NULL if this vehicle - /// does not support geofence. You should make sense to check vehicle capabilites for geofence - /// support. + /// Returns a newly created geofence manager for this vehicle. virtual GeoFenceManager* newGeoFenceManager(Vehicle* vehicle) { return new GeoFenceManager(vehicle); } /// Returns the parameter which holds the fence circle radius if supported. virtual QString geoFenceRadiusParam(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); } + /// Returns a newly created rally point manager for this vehicle. + virtual RallyPointManager* newRallyPointManager(Vehicle* vehicle) { return new RallyPointManager(vehicle); } + /// Return the resource file which contains the set of params loaded for offline editing. virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); } }; diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index e1108d57c..decd6d79c 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -63,6 +63,11 @@ FlightMap { Component.onCompleted: start(false /* editMode */) } + RallyPointController { + id: rallyPointController + Component.onCompleted: start(false /* editMode */) + } + // Add trajectory points to the map MapItemView { model: _mainIsMap ? _activeVehicle ? _activeVehicle.trajectoryPoints : 0 : 0 @@ -126,6 +131,23 @@ FlightMap { z: QGroundControl.zOrderMapItems } + // Rally points on map + MapItemView { + model: rallyPointController.points + + delegate: MapQuickItem { + id: itemIndicator + anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) + coordinate: object.coordinate + z: QGroundControl.zOrderMapItems + + sourceItem: MissionItemIndexLabel { + id: itemIndexLabel + label: qsTr("R", "rally point map item label") + } + } + } + // GoTo here waypoint MapQuickItem { coordinate: _gotoHereCoordinate @@ -135,8 +157,8 @@ FlightMap { anchorPoint.y: sourceItem.height / 2 sourceItem: MissionItemIndexLabel { - isCurrentItem: true - label: qsTr("G", "Goto here waypoint") // second string is translator's hint. + checked: true + label: qsTr("G", "Goto here waypoint") // second string is translator's hint. } } diff --git a/src/FlightMap/MapItems/MissionItemIndicator.qml b/src/FlightMap/MapItems/MissionItemIndicator.qml index 5a50651f8..ec325c1ec 100644 --- a/src/FlightMap/MapItems/MissionItemIndicator.qml +++ b/src/FlightMap/MapItems/MissionItemIndicator.qml @@ -29,10 +29,10 @@ MapQuickItem { sourceItem: MissionItemIndexLabel { - id: _label - isCurrentItem: _isCurrentItem - label: missionItem ? missionItem.abbreviation : "" - onClicked: _item.clicked() + id: _label + checked: _isCurrentItem + label: missionItem ? missionItem.abbreviation : "" + onClicked: _item.clicked() property bool _isCurrentItem: missionItem ? missionItem.isCurrentItem : false } diff --git a/src/FlightMap/MapItems/MissionItemView.qml b/src/FlightMap/MapItems/MissionItemView.qml index 9956514c5..2065ca958 100644 --- a/src/FlightMap/MapItems/MissionItemView.qml +++ b/src/FlightMap/MapItems/MissionItemView.qml @@ -43,9 +43,9 @@ MapItemView { model: object.childItems delegate: MissionItemIndexLabel { - label: object.abbreviation - isCurrentItem: object.isCurrentItem - z: 2 + label: object.abbreviation + checked: object.isCurrentItem + z: 2 } } } diff --git a/src/JsonHelper.cc b/src/JsonHelper.cc index 9a8bd14df..e1c4058ae 100644 --- a/src/JsonHelper.cc +++ b/src/JsonHelper.cc @@ -12,6 +12,9 @@ #include #include +#include +#include +#include const char* JsonHelper::_enumStringsJsonKey = "enumStrings"; const char* JsonHelper::_enumValuesJsonKey = "enumValues"; @@ -34,30 +37,33 @@ bool JsonHelper::validateRequiredKeys(const QJsonObject& jsonObject, const QStri } if (missingKeys.count() != 0) { - errorString = QString("The following required keys are missing: %1").arg(missingKeys); + errorString = QObject::tr("The following required keys are missing: %1").arg(missingKeys); return false; } return true; } -bool JsonHelper::toQGeoCoordinate(const QJsonValue& jsonValue, QGeoCoordinate& coordinate, bool altitudeRequired, QString& errorString) +bool JsonHelper::loadGeoCoordinate(const QJsonValue& jsonValue, + bool altitudeRequired, + QGeoCoordinate& coordinate, + QString& errorString) { if (!jsonValue.isArray()) { - errorString = QStringLiteral("JSon value for coordinate is not array"); + errorString = QObject::tr("value for coordinate is not array"); return false; } QJsonArray coordinateArray = jsonValue.toArray(); int requiredCount = altitudeRequired ? 3 : 2; if (coordinateArray.count() != requiredCount) { - errorString = QString("Coordinate array must contain %1 values").arg(requiredCount); + errorString = QObject::tr("Coordinate array must contain %1 values").arg(requiredCount); return false; } foreach(const QJsonValue& jsonValue, coordinateArray) { if (jsonValue.type() != QJsonValue::Double) { - errorString = QString("Coordinate array may only contain double values, found: %1").arg(jsonValue.type()); + errorString = QObject::tr("Coordinate array may only contain double values, found: %1").arg(jsonValue.type()); return false; } } @@ -68,14 +74,16 @@ bool JsonHelper::toQGeoCoordinate(const QJsonValue& jsonValue, QGeoCoordinate& c } if (!coordinate.isValid()) { - errorString = QString("Coordinate is invalid: %1").arg(coordinate.toString()); + errorString = QObject::tr("Coordinate is invalid: %1").arg(coordinate.toString()); return false; } return true; } -void JsonHelper::writeQGeoCoordinate(QJsonValue& jsonValue, const QGeoCoordinate& coordinate, bool writeAltitude) +void JsonHelper::saveGeoCoordinate(const QGeoCoordinate& coordinate, + bool writeAltitude, + QJsonValue& jsonValue) { QJsonArray coordinateArray; @@ -92,7 +100,7 @@ bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringLi for (int i=0; i typeList = { QJsonValue::String, QJsonValue::String }; + if (!validateKeyTypes(jsonObject, requiredKeys, typeList, errorString)) { + return false; + } + + // Make sure file type is correct + QString fileTypeValue = jsonObject[jsonFileTypeKey].toString(); + if (fileTypeValue != expectedFileType) { + errorString = QObject::tr("Incorrect file type key expected:%1 actual:%2").arg(expectedFileType).arg(fileTypeValue); + return false; + } + + // Parse and validate version + + QString incorrectVersionFormatErrorString = QObject::tr("Incorrectly formatted version value"); + QRegularExpression versionRegExp("(\\d+).(\\d+)"); + QRegularExpressionMatch match = versionRegExp.match(jsonObject[jsonVersionKey].toString()); + if (!match.hasMatch()) { + errorString = incorrectVersionFormatErrorString; + return false; + } + QStringList versionParts = match.capturedTexts(); + if (versionParts.count() != 3) { + errorString = incorrectVersionFormatErrorString; + return false; + } + + fileMajorVersion = versionParts[0].toInt(); + fileMinorVersion = versionParts[0].toInt(); + if (fileMajorVersion > supportedMajorVersion || (fileMajorVersion == supportedMajorVersion && fileMinorVersion > supportedMinorVersion)) { + errorString = QObject::tr("File version (%1.%2) is larger than current supported version (%3.%4)").arg(fileMajorVersion).arg(fileMinorVersion).arg(supportedMajorVersion).arg(supportedMinorVersion); + return false; + } + + return true; +} + +bool JsonHelper::loadGeoCoordinateArray(const QJsonValue& jsonValue, + bool altitudeRequired, + QVariantList& rgVarPoints, + QString& errorString) +{ + if (!jsonValue.isArray()) { + errorString = QObject::tr("value for coordinate array is not array"); + return false; + } + QJsonArray rgJsonPoints = jsonValue.toArray(); + + rgVarPoints.clear(); + for (int i=0; i& rgPoints, + QString& errorString) +{ + QVariantList rgVarPoints; + + if (!loadGeoCoordinateArray(jsonValue, altitudeRequired, rgVarPoints, errorString)) { + return false; + } + + rgPoints.clear(); + for (int i=0; i()); + } + + return true; +} + +void JsonHelper::saveGeoCoordinateArray(const QVariantList& rgVarPoints, + bool writeAltitude, + QJsonValue& jsonValue) +{ + QJsonArray rgJsonPoints; + + // Add all points to the array + for (int i=0; i(), writeAltitude, jsonPoint); + rgJsonPoints.append(jsonPoint); + } + + jsonValue = rgJsonPoints; +} + +void JsonHelper::saveGeoCoordinateArray(const QList& rgPoints, + bool writeAltitude, + QJsonValue& jsonValue) +{ + QVariantList rgVarPoints; + + for (int i=0; i +#include #include class JsonHelper @@ -21,12 +22,54 @@ public: /// @return true: file is json, false: file is not json static bool isJsonFile(const QByteArray& bytes, QJsonDocument& jsonDoc); + /// Validates the standard parts of a QGC json file: + /// jsonFileTypeKey - Required and checked to be equal to expectedFileType + /// jsonVersionKey - Required and checked to be below supportedMajorVersion, supportedMinorVersion + /// @return false: validation failed + static bool validateQGCJsonFile(const QJsonObject& jsonObject, ///< top level json object in file + const QString& expectedFileType, ///< correct file type for file + int supportedMajorVersion, ///< maximum supported major version + int supportedMinorVersion, ///< maximum supported minor version + int &fileMajorVersion, ///< returned file major version + int &fileMinorVersion, ///< returned file minor version + QString& errorString); ///< returned error string if validation fails + static bool validateRequiredKeys(const QJsonObject& jsonObject, const QStringList& keys, QString& errorString); static bool validateKeyTypes(const QJsonObject& jsonObject, const QStringList& keys, const QList& types, QString& errorString); - static bool toQGeoCoordinate(const QJsonValue& jsonValue, QGeoCoordinate& coordinate, bool altitudeRequired, QString& errorString); + + /// Loads a QGeoCoordinate + /// @return false: validation failed + static bool loadGeoCoordinate(const QJsonValue& jsonValue, ///< json value to load from + bool altitudeRequired, ///< true: altitude must be specified + QGeoCoordinate& coordinate, ///< returned QGeoCordinate + QString& errorString); ///< returned error string if load failure + + /// Saves a QGeoCoordinate + static void saveGeoCoordinate(const QGeoCoordinate& coordinate, ///< QGeoCoordinate to save + bool writeAltitude, ///< true: write altitude to json + QJsonValue& jsonValue); ///< json value to save to + + /// Loads a list of QGeoCoordinates from a json array + /// @return false: validation failed + static bool loadGeoCoordinateArray(const QJsonValue& jsonValue, ///< json value which contains points + bool altitudeRequired, ///< true: altitude field must be specified + QVariantList& rgVarPoints, ///< returned points + QString& errorString); ///< returned error string if load failure + static bool loadGeoCoordinateArray(const QJsonValue& jsonValue, ///< json value which contains points + bool altitudeRequired, ///< true: altitude field must be specified + QList& rgPoints, ///< returned points + QString& errorString); ///< returned error string if load failure + + /// Saves a list of QGeoCoordinates to a json array + static void saveGeoCoordinateArray(const QVariantList& rgVarPoints, ///< points to save + bool writeAltitude, ///< true: write altitide value + QJsonValue& jsonValue); ///< json value to save to + static void saveGeoCoordinateArray(const QList& rgPoints, ///< points to save + bool writeAltitude, ///< true: write altitide value + QJsonValue& jsonValue); ///< json value to save to + static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString); - static void writeQGeoCoordinate(QJsonValue& jsonValue, const QGeoCoordinate& coordinate, bool writeAltitude); static const char* jsonVersionKey; static const char* jsonGroundStationKey; diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index cefcb2ec1..30f136d41 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -54,6 +54,7 @@ QGCView { readonly property int _layerMission: 1 readonly property int _layerGeoFence: 2 + readonly property int _layerRallyPoints: 3 property int _editingLayer: _layerMission onActiveVehiclePositionChanged: updateMapToVehiclePosition() @@ -154,12 +155,6 @@ QGCView { Component.onCompleted: start(true /* editMode */) - onFenceSupportedChanged: { - if (!fenceSupported && _editingLayer == _layerGeoFence) { - _editingLayer = _layerMission - } - } - function saveToSelectedFile() { if (ScreenTools.isMobile) { qgcView.showDialog(mobileFileSaver, qsTr("Save Fence File"), qgcView.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) @@ -188,6 +183,36 @@ QGCView { } } + RallyPointController { + id: rallyPointController + + onCurrentRallyPointChanged: { + if (_editingLayer == _layerRallyPoints && !currentRallyPoint) { + itemDragger.visible = false + itemDragger.coordinateItem = undefined + itemDragger.mapCoordinateIndicator = undefined + } + } + + Component.onCompleted: start(true /* editMode */) + + function saveToSelectedFile() { + if (ScreenTools.isMobile) { + qgcView.showDialog(mobileFileSaver, qsTr("Save Rally Point File"), qgcView.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel) + } else { + rallyPointController.saveToFilePicker() + } + } + + function loadFromSelectedFile() { + if (ScreenTools.isMobile) { + qgcView.showDialog(mobileFilePicker, qsTr("Select Rally Point File"), qgcView.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel) + } else { + rallyPointController.loadFromFilePicker() + } + } + } + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } ExclusiveGroup { @@ -220,7 +245,7 @@ QGCView { QGCMobileFileDialog { openDialog: true - fileExtension: _syncDropDownController == geoFenceController ? QGroundControl.fenceFileExtension : QGroundControl.missionFileExtension + fileExtension: _syncDropDownController.fileExtension onFilenameReturned: _syncDropDownController.loadFromfile(filename) } } @@ -230,7 +255,7 @@ QGCView { QGCMobileFileDialog { openDialog: false - fileExtension: _syncDropDownController == geoFenceController ? QGroundControl.fenceFileExtension : QGroundControl.missionFileExtension + fileExtension: _syncDropDownController.fileExtension onFilenameReturned: _syncDropDownController.saveToFile() } } @@ -326,8 +351,15 @@ QGCView { } break case _layerGeoFence: - geoFenceController.breachReturnPoint = coordinate - geoFenceController.validateBreachReturn() + if (geoFenceController.breachReturnSupported) { + geoFenceController.breachReturnPoint = coordinate + geoFenceController.validateBreachReturn() + } + break + case _layerRallyPoints: + if (rallyPointController.rallyPointsSupported) { + rallyPointController.addPoint(coordinate) + } break } } @@ -336,16 +368,16 @@ QGCView { // We use this item to support dragging since dragging a MapQuickItem just doesn't seem to work Rectangle { id: itemDragger - x: missionItemIndicator ? (missionItemIndicator.x + missionItemIndicator.anchorPoint.x - (itemDragger.width / 2)) : 100 - y: missionItemIndicator ? (missionItemIndicator.y + missionItemIndicator.anchorPoint.y - (itemDragger.height / 2)) : 100 + x: mapCoordinateIndicator ? (mapCoordinateIndicator.x + mapCoordinateIndicator.anchorPoint.x - (itemDragger.width / 2)) : 100 + y: mapCoordinateIndicator ? (mapCoordinateIndicator.y + mapCoordinateIndicator.anchorPoint.y - (itemDragger.height / 2)) : 100 width: ScreenTools.defaultFontPixelHeight * 2 height: ScreenTools.defaultFontPixelHeight * 2 color: "transparent" visible: false z: QGroundControl.zOrderMapItems + 1 // Above item icons - property var missionItem - property var missionItemIndicator + property var coordinateItem + property var mapCoordinateIndicator property bool preventCoordinateBindingLoop: false onXChanged: liveDrag() @@ -355,17 +387,17 @@ QGCView { if (!itemDragger.preventCoordinateBindingLoop && Drag.active) { var point = Qt.point(itemDragger.x + (itemDragger.width / 2), itemDragger.y + (itemDragger.height / 2)) var coordinate = editorMap.toCoordinate(point) - coordinate.altitude = itemDragger.missionItem.coordinate.altitude + coordinate.altitude = itemDragger.coordinateItem.coordinate.altitude itemDragger.preventCoordinateBindingLoop = true - itemDragger.missionItem.coordinate = coordinate + itemDragger.coordinateItem.coordinate = coordinate itemDragger.preventCoordinateBindingLoop = false } } function clearItem() { itemDragger.visible = false - itemDragger.missionItem = undefined - itemDragger.missionItemIndicator = undefined + itemDragger.coordinateItem = undefined + itemDragger.mapCoordinateIndicator = undefined } Drag.active: itemDrag.drag.active @@ -449,8 +481,8 @@ QGCView { if (object.isCurrentItem && itemIndicator.visible && object.specifiesCoordinate && object.isSimpleItem) { // Setup our drag item itemDragger.visible = true - itemDragger.missionItem = Qt.binding(function() { return object }) - itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator }) + itemDragger.coordinateItem = Qt.binding(function() { return object }) + itemDragger.mapCoordinateIndicator = Qt.binding(function() { return itemIndicator }) } } @@ -470,9 +502,9 @@ QGCView { model: object.childItems delegate: MissionItemIndexLabel { - label: object.abbreviation - isCurrentItem: object.isCurrentItem - z: 2 + label: object.abbreviation + checked: object.isCurrentItem + z: 2 onClicked: setCurrentItem(object.sequenceNumber) } @@ -491,60 +523,113 @@ QGCView { model: QGroundControl.multiVehicleManager.vehicles delegate: VehicleMapItem { - vehicle: object - coordinate: object.coordinate - isSatellite: editorMap.isSatelliteMap - size: ScreenTools.defaultFontPixelHeight * 5 - z: QGroundControl.zOrderMapItems - 1 - } + vehicle: object + coordinate: object.coordinate + isSatellite: editorMap.isSatelliteMap + size: ScreenTools.defaultFontPixelHeight * 5 + z: QGroundControl.zOrderMapItems - 1 + } } - // Mission/GeoFence selector - Item { - id: planElementSelector + // Plan Element selector (Mission/Fence/Rally) + Row { + id: planElementSelectorRow anchors.topMargin: parent.height - ScreenTools.availableHeight + _margin anchors.top: parent.top anchors.leftMargin: parent.width - _rightPanelWidth anchors.left: parent.left - width: planElementSelectorRow.width - height: geoFenceController.fenceSupported ? planElementSelectorRow.height : 0 - visible: geoFenceController.fenceSupported + spacing: _horizontalMargin + + readonly property real _buttonRadius: ScreenTools.defaultFontPixelHeight * 0.75 ExclusiveGroup { id: planElementSelectorGroup onCurrentChanged: { - var layerIsMission = current == planElementMission - _editingLayer = layerIsMission ? _layerMission : _layerGeoFence - _syncDropDownController = layerIsMission ? missionController : geoFenceController + switch (current) { + case planElementMission: + _editingLayer = _layerMission + _syncDropDownController = missionController + break + case planElementGeoFence: + _editingLayer = _layerGeoFence + _syncDropDownController = geoFenceController + break + case planElementRallyPoints: + _editingLayer = _layerRallyPoints + _syncDropDownController = rallyPointController + break + } } } - Row { - id: planElementSelectorRow - spacing: _horizontalMargin + RoundButton { + id: planElementMission + radius: parent._buttonRadius + buttonImage: "/qmlimages/Plan.svg" + lightBorders: _lightWidgetBorders + exclusiveGroup: planElementSelectorGroup + checked: true + } + + QGCLabel { + text: qsTr("Mission") + color: mapPal.text + anchors.verticalCenter: parent.verticalCenter - QGCRadioButton { - id: planElementMission - text: qsTr("Mission") - checked: true - exclusiveGroup: planElementSelectorGroup - color: mapPal.text + MouseArea { + anchors.fill: parent + onClicked: planElementMission.checked = true } + } + + Item { height: 1; width: 1 } + + RoundButton { + id: planElementGeoFence + radius: parent._buttonRadius + buttonImage: "/qmlimages/Plan.svg" + lightBorders: _lightWidgetBorders + exclusiveGroup: planElementSelectorGroup + } + + QGCLabel { + text: qsTr("Fence") + color: mapPal.text + anchors.verticalCenter: parent.verticalCenter - QGCRadioButton { - id: planElementGeoFence - text: qsTr("GeoFence") - exclusiveGroup: planElementSelectorGroup - color: mapPal.text + MouseArea { + anchors.fill: parent + onClicked: planElementGeoFence.checked = true } } - } + + Item { height: 1; width: 1 } + + RoundButton { + id: planElementRallyPoints + radius: parent._buttonRadius + buttonImage: "/qmlimages/Plan.svg" + lightBorders: _lightWidgetBorders + exclusiveGroup: planElementSelectorGroup + } + + QGCLabel { + text: qsTr("Rally") + color: mapPal.text + anchors.verticalCenter: parent.verticalCenter + + MouseArea { + anchors.fill: parent + onClicked: planElementRallyPoints.checked = true + } + } + } // Row - Plan Element Selector // Mission Item Editor Item { id: missionItemEditor anchors.topMargin: _margin - anchors.top: planElementSelector.bottom + anchors.top: planElementSelectorRow.bottom anchors.bottom: parent.bottom anchors.right: parent.right width: _rightPanelWidth @@ -553,14 +638,14 @@ QGCView { visible: _editingLayer == _layerMission MouseArea { - // This MouseArea prevents the Map below it from getting Mouse events. Without this - // things like mousewheel will scroll the Flickable and then scroll the map as well. - anchors.fill: editorListView - onWheel: wheel.accepted = true - } + // This MouseArea prevents the Map below it from getting Mouse events. Without this + // things like mousewheel will scroll the Flickable and then scroll the map as well. + anchors.fill: missionItemEditorListView + onWheel: wheel.accepted = true + } ListView { - id: editorListView + id: missionItemEditorListView anchors.left: parent.left anchors.right: parent.right anchors.top: parent.top @@ -599,7 +684,7 @@ QGCView { // GeoFence Editor Loader { anchors.topMargin: _margin - anchors.top: planElementSelector.bottom + anchors.top: planElementSelectorRow.bottom anchors.right: parent.right opacity: _rightPanelOpacity z: QGroundControl.zOrderTopMost @@ -635,6 +720,63 @@ QGCView { z: QGroundControl.zOrderMapItems } + // Rally Point Editor + + RallyPointEditorHeader { + id: rallyPointHeader + anchors.topMargin: _margin + anchors.top: planElementSelectorRow.bottom + anchors.right: parent.right + width: _rightPanelWidth + opacity: _rightPanelOpacity + z: QGroundControl.zOrderTopMost + visible: _editingLayer == _layerRallyPoints + controller: rallyPointController + } + + RallyPointItemEditor { + id: rallyPointEditor + anchors.topMargin: _margin + anchors.top: rallyPointHeader.bottom + anchors.right: parent.right + width: _rightPanelWidth + opacity: _rightPanelOpacity + z: QGroundControl.zOrderTopMost + visible: _editingLayer == _layerRallyPoints && rallyPointController.points.count + rallyPoint: rallyPointController.currentRallyPoint + controller: rallyPointController + } + + // Rally points on map + + MapItemView { + model: rallyPointController.points + + delegate: MapQuickItem { + id: itemIndicator + anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2) + coordinate: object.coordinate + z: QGroundControl.zOrderMapItems + + sourceItem: MissionItemIndexLabel { + id: itemIndexLabel + label: qsTr("R", "rally point map item label") + checked: _editingLayer == _layerRallyPoints ? object == rallyPointController.currentRallyPoint : false + + onClicked: rallyPointController.currentRallyPoint = object + + onCheckedChanged: { + if (checked) { + // Setup our drag item + itemDragger.visible = true + itemDragger.coordinateItem = Qt.binding(function() { return object }) + itemDragger.mapCoordinateIndicator = Qt.binding(function() { return itemIndicator }) + } + } + } + } + } + //-- Dismiss Drop Down (if any) MouseArea { anchors.fill: parent @@ -881,7 +1023,7 @@ QGCView { id: columnHolder spacing: _margin - property string _overwriteText: (_editingLayer == _layerMission) ? qsTr("Mission overwrite") : qsTr("GeoFence overwrite") + property string _overwriteText: (_editingLayer == _layerMission) ? qsTr("Mission overwrite") : ((_editingLayer == _layerGeoFence) ? qsTr("GeoFence overwrite") : qsTr("Rally Points overwrite")) QGCLabel { width: sendSaveGrid.width diff --git a/src/QmlControls/MissionItemEditor.qml b/src/MissionEditor/MissionItemEditor.qml similarity index 100% rename from src/QmlControls/MissionItemEditor.qml rename to src/MissionEditor/MissionItemEditor.qml diff --git a/src/MissionEditor/MissionItemStatus.qml b/src/MissionEditor/MissionItemStatus.qml index e767d60e2..fcedf7fd4 100644 --- a/src/MissionEditor/MissionItemStatus.qml +++ b/src/MissionEditor/MissionItemStatus.qml @@ -141,7 +141,7 @@ Rectangle { anchors.horizontalCenter: parent.horizontalCenter y: availableHeight - (availableHeight * object.altPercent) small: true - isCurrentItem: object.isCurrentItem + checked: object.isCurrentItem label: object.abbreviation visible: object.relativeAltitude ? true : (object.homePosition || graphAbsolute) } diff --git a/src/MissionEditor/RallyPointEditorHeader.qml b/src/MissionEditor/RallyPointEditorHeader.qml new file mode 100644 index 000000000..8a3828a6f --- /dev/null +++ b/src/MissionEditor/RallyPointEditorHeader.qml @@ -0,0 +1,68 @@ +import QtQuick 2.2 +import QtQuick.Controls 1.2 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 + +QGCFlickable { + height: outerEditorRect.height + contentHeight: outerEditorRect.height + clip: true + + property var controller ///< RallyPointController + + readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 + readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2 + + Rectangle { + id: outerEditorRect + width: parent.width + height: innerEditorRect.y + innerEditorRect.height + (_margin * 2) + radius: _radius + color: qgcPal.buttonHighlight + + QGCLabel { + id: editorLabel + anchors.margins: _margin + anchors.left: parent.left + anchors.top: parent.top + text: qsTr("Rally Points (WIP careful!)") + color: "black" + } + + Rectangle { + id: innerEditorRect + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: editorLabel.bottom + height: helpLabel.y + helpLabel.height + (_margin * 2) + color: qgcPal.windowShadeDark + radius: _radius + + QGCLabel { + id: infoLabel + anchors.margins: _margin + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + font.pointSize: ScreenTools.smallFontPointSize + text: qsTr("Rally Points provide alternate landing points when performing a Return to Launch (RTL).") + } + + QGCLabel { + id: helpLabel + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: infoLabel.bottom + wrapMode: Text.WordWrap + text: controller.rallyPointsSupported ? + qsTr("Click in the map to add new rally points.") : + qsTr("This vehicle does not support Rally Points.") + } + } + } +} diff --git a/src/MissionEditor/RallyPointItemEditor.qml b/src/MissionEditor/RallyPointItemEditor.qml new file mode 100644 index 000000000..86cf98a61 --- /dev/null +++ b/src/MissionEditor/RallyPointItemEditor.qml @@ -0,0 +1,124 @@ +import QtQuick 2.2 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.2 +import QtQuick.Dialogs 1.2 + +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 + +Rectangle { + id: root + height: _currentItem ? valuesRect.y + valuesRect.height + (_margin * 2) : titleBar.y - titleBar.height + _margin + color: _currentItem ? qgcPal.buttonHighlight : qgcPal.windowShade + radius: _radius + + property var rallyPoint ///< RallyPoint object associated with editor + property var controller ///< RallyPointController + + property bool _currentItem: rallyPoint ? rallyPoint == controller.currentRallyPoint : false + property color _outerTextColor: _currentItem ? "black" : qgcPal.text + + readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 12) + readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2 + readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2 + readonly property real _titleHeight: ScreenTools.defaultFontPixelHeight * 2 + + QGCPalette { id: qgcPal; colorGroupEnabled: true } + + Item { + id: titleBar + anchors.margins: _margin + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + height: _titleHeight + + MissionItemIndexLabel { + id: indicator + anchors.verticalCenter: parent.verticalCenter + anchors.left: parent.left + label: "R" + checked: true + } + + QGCLabel { + anchors.leftMargin: _margin + anchors.left: indicator.right + anchors.verticalCenter: parent.verticalCenter + text: qsTr("Rally Point") + color: _outerTextColor + } + + Image { + id: hamburger + anchors.rightMargin: _margin + anchors.right: parent.right + anchors.verticalCenter: parent.verticalCenter + width: ScreenTools.defaultFontPixelWidth * 2 + height: width + sourceSize.height: height + source: "qrc:/qmlimages/Hamburger.svg" + + MouseArea { + anchors.fill: parent + onClicked: hamburgerMenu.popup() + + Menu { + id: hamburgerMenu + + MenuItem { + text: qsTr("Delete") + onTriggered: controller.removePoint(rallyPoint) + } + } + } + } + } // Item - titleBar + + Rectangle { + id: valuesRect + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: titleBar.bottom + height: valuesColumn.height + (_margin * 2) + color: qgcPal.windowShadeDark + visible: _currentItem + radius: _radius + + Column { + id: valuesColumn + anchors.margins: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: parent.top + spacing: _margin + + Repeater { + model: rallyPoint ? rallyPoint.textFieldFacts : 0 + + Item { + width: valuesColumn.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 + } + } + } // Repeater - text fields + } // Column + } // Rectangle +} // Rectangle diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 5768e7642..dbf9a5444 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -92,6 +92,7 @@ void GeoFenceController::_activeVehicleSet(void) connect(geoFenceManager, &GeoFenceManager::paramsChanged, this, &GeoFenceController::paramsChanged); connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged, this, &GeoFenceController::paramLabelsChanged); connect(geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_loadComplete); + connect(geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); if (!geoFenceManager->inProgress()) { _loadComplete(geoFenceManager->breachReturnPoint(), geoFenceManager->polygon()); @@ -134,7 +135,7 @@ bool GeoFenceController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorStr if (breachReturnSupported()) { if (json.contains(_jsonBreachReturnKey) - && !JsonHelper::toQGeoCoordinate(json[_jsonBreachReturnKey], _breachReturnPoint, false /* altitudeRequired */, errorString)) { + && !JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], false /* altitudeRequired */, _breachReturnPoint, errorString)) { return false; } } else { @@ -242,7 +243,7 @@ void GeoFenceController::loadFromFile(const QString& filename) void GeoFenceController::loadFromFilePicker(void) { #ifndef __mobile__ - QString filename = QGCFileDialog::getOpenFileName(NULL, "Select GeoFence File to load", QString(), "Mission file (*.fence);;All Files (*.*)"); + QString filename = QGCFileDialog::getOpenFileName(NULL, "Select GeoFence File to load", QString(), "Fence file (*.fence);;All Files (*.*)"); if (filename.isEmpty()) { return; @@ -287,7 +288,7 @@ void GeoFenceController::saveToFile(const QString& filename) if (breachReturnSupported()) { QJsonValue jsonBreachReturn; - JsonHelper::writeQGeoCoordinate(jsonBreachReturn, _breachReturnPoint, false /* writeAltitude */); + JsonHelper::saveGeoCoordinate(_breachReturnPoint, false /* writeAltitude */, jsonBreachReturn); fenceFileObject[_jsonBreachReturnKey] = jsonBreachReturn; } @@ -433,3 +434,8 @@ void GeoFenceController::_loadComplete(const QGeoCoordinate& breachReturn, const _setPolygonFromManager(polygon); setDirty(false); } + +QString GeoFenceController::fileExtension(void) const +{ + return QGCApplication::fenceFileExtension; +} diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 3488b5097..d5f14cf3f 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -52,6 +52,8 @@ public: bool dirty (void) const final; void setDirty (bool dirty) final; + QString fileExtension(void) const final; + bool fenceSupported (void) const; bool circleSupported (void) const; bool polygonSupported (void) const; diff --git a/src/MissionManager/MissionCommandTree.cc b/src/MissionManager/MissionCommandTree.cc index e6828007b..1a261c18f 100644 --- a/src/MissionManager/MissionCommandTree.cc +++ b/src/MissionManager/MissionCommandTree.cc @@ -102,7 +102,10 @@ void MissionCommandTree::_collapseHierarchy(Vehicle* foreach (MAV_CMD command, cmdList->commandIds()) { // Only add supported command to tree (MAV_CMD_NAV_LAST is used for planned home position) - if (!qgcApp()->runningUnitTests() && !vehicle->firmwarePlugin()->supportedMissionCommands().contains(command) && command != MAV_CMD_NAV_LAST) { + if (!qgcApp()->runningUnitTests() + && !vehicle->firmwarePlugin()->supportedMissionCommands().isEmpty() + && !vehicle->firmwarePlugin()->supportedMissionCommands().contains(command) + && command != MAV_CMD_NAV_LAST) { continue; } diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index a54abc2d3..1574fb2e8 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1284,3 +1284,8 @@ void MissionController::_homeCoordinateChanged(void) emit plannedHomePositionChanged(plannedHomePosition()); _recalcAltitudeRangeBearing(); } + +QString MissionController::fileExtension(void) const +{ + return QGCApplication::missionFileExtension; +} diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 63abcb20d..fc1f288d3 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -69,6 +69,8 @@ public: bool dirty (void) const final; void setDirty (bool dirty) final; + QString fileExtension(void) const final; + // Property accessors QGeoCoordinate plannedHomePosition (void); diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc index 1182cd290..8b7df76ce 100644 --- a/src/MissionManager/MissionItem.cc +++ b/src/MissionManager/MissionItem.cc @@ -196,7 +196,7 @@ bool MissionItem::load(const QJsonObject& json, QString& errorString) setCommand((MAV_CMD)json[_jsonCommandKey].toInt()); QGeoCoordinate coordinate; - if (!JsonHelper::toQGeoCoordinate(json[_jsonCoordinateKey], coordinate, true /* altitudeRequired */, errorString)) { + if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) { return false; } setParam5(coordinate.latitude()); diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h index 2ba043935..63c4abe35 100644 --- a/src/MissionManager/PlanElementController.h +++ b/src/MissionManager/PlanElementController.h @@ -31,6 +31,10 @@ public: /// true: unsaved/sent changes are present, false: no changes since last save/send Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + /// Returns the file extention for plan element file type. + Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) + virtual QString fileExtension(void) const = 0; + /// 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); diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index f85eded79..1667fad9d 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -142,18 +142,10 @@ void QGCMapPolygon::setPath(const QVariantList& path) void QGCMapPolygon::saveToJson(QJsonObject& json) { - QJsonArray rgPoints; - - // Add all points to the array - for (int i=0; i<_polygonPath.count(); i++) { - QJsonValue jsonPoint; - - JsonHelper::writeQGeoCoordinate(jsonPoint, (*this)[i], false /* writeAltitude */); - rgPoints.append(jsonPoint); - } - - json.insert(_jsonPolygonKey, QJsonValue(rgPoints)); + QJsonValue jsonValue; + JsonHelper::saveGeoCoordinateArray(_polygonPath, false /* writeAltitude*/, jsonValue); + json.insert(_jsonPolygonKey, jsonValue); setDirty(false); } @@ -170,26 +162,11 @@ bool QGCMapPolygon::loadFromJson(const QJsonObject& json, bool required, QString return true; } - QList types; - - types << QJsonValue::Array; - if (!JsonHelper::validateKeyTypes(json, QStringList(_jsonPolygonKey), types, errorString)) { + if (!JsonHelper::loadGeoCoordinateArray(json[_jsonPolygonKey], false /* altitudeRequired */, _polygonPath, errorString)) { return false; } - - QList rgPoints; - QJsonArray jsonPoints = json[_jsonPolygonKey].toArray(); - for (int i=0; i + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include +#include + +#include "RallyPoint.h" + +const char* RallyPoint::_longitudeFactName = "Longitude"; +const char* RallyPoint::_latitudeFactName = "Latitude"; +const char* RallyPoint::_altitudeFactName = "Altitude"; + +QMap RallyPoint::_metaDataMap; + +RallyPoint::RallyPoint(const QGeoCoordinate& coordinate, QObject* parent) + : QObject(parent) + , _dirty(false) + , _longitudeFact(0, _longitudeFactName, FactMetaData::valueTypeDouble) + , _latitudeFact(0, _latitudeFactName, FactMetaData::valueTypeDouble) + , _altitudeFact(0, _altitudeFactName, FactMetaData::valueTypeDouble) +{ + setCoordinate(coordinate); + + _factSetup(); +} + +RallyPoint::RallyPoint(const RallyPoint& other, QObject* parent) + : QObject(parent) + , _dirty(false) + , _longitudeFact(0, _longitudeFactName, FactMetaData::valueTypeDouble) + , _latitudeFact(0, _latitudeFactName, FactMetaData::valueTypeDouble) + , _altitudeFact(0, _altitudeFactName, FactMetaData::valueTypeDouble) +{ + _longitudeFact.setRawValue(other._longitudeFact.rawValue()); + _latitudeFact.setRawValue(other._latitudeFact.rawValue()); + _altitudeFact.setRawValue(other._altitudeFact.rawValue()); + + _factSetup(); +} + +const RallyPoint& RallyPoint::operator=(const RallyPoint& other) +{ + _longitudeFact.setRawValue(other._longitudeFact.rawValue()); + _latitudeFact.setRawValue(other._latitudeFact.rawValue()); + _altitudeFact.setRawValue(other._altitudeFact.rawValue()); + + emit coordinateChanged(coordinate()); + + return *this; +} + +RallyPoint::~RallyPoint() +{ + +} + +void RallyPoint::_factSetup(void) +{ + if (_metaDataMap.isEmpty()) { + _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/RallyPoint.json"), NULL /* metaDataParent */); + } + + _longitudeFact.setMetaData(_metaDataMap[_longitudeFactName]); + _latitudeFact.setMetaData(_metaDataMap[_latitudeFactName]); + _altitudeFact.setMetaData(_metaDataMap[_altitudeFactName]); + + _textFieldFacts.append(QVariant::fromValue(&_longitudeFact)); + _textFieldFacts.append(QVariant::fromValue(&_latitudeFact)); + _textFieldFacts.append(QVariant::fromValue(&_altitudeFact)); + + connect(&_longitudeFact, &Fact::valueChanged, this, &RallyPoint::_sendCoordinateChanged); + connect(&_latitudeFact, &Fact::valueChanged, this, &RallyPoint::_sendCoordinateChanged); + connect(&_altitudeFact, &Fact::valueChanged, this, &RallyPoint::_sendCoordinateChanged); +} + +void RallyPoint::setCoordinate(const QGeoCoordinate& coordinate) +{ + if (coordinate != this->coordinate()) { + _longitudeFact.setRawValue(coordinate.longitude()); + _latitudeFact.setRawValue(coordinate.latitude()); + _altitudeFact.setRawValue(coordinate.altitude()); + emit coordinateChanged(coordinate); + setDirty(true); + } +} + +void RallyPoint::setDirty(bool dirty) +{ + if (dirty != _dirty) { + _dirty = dirty; + emit dirtyChanged(dirty); + } +} + +QGeoCoordinate RallyPoint::coordinate(void) const +{ + return QGeoCoordinate(_latitudeFact.rawValue().toDouble(), _longitudeFact.rawValue().toDouble(), _altitudeFact.rawValue().toDouble()); +} + +void RallyPoint::_sendCoordinateChanged(void) +{ + emit coordinateChanged(coordinate()); +} diff --git a/src/MissionManager/RallyPoint.h b/src/MissionManager/RallyPoint.h new file mode 100644 index 000000000..913371659 --- /dev/null +++ b/src/MissionManager/RallyPoint.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * (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 RallyPoint_H +#define RallyPoint_H + +#include +#include + +#include "FactSystem.h" + +/// This class is used to encapsulate the QGeoCoordinate associated with a Rally Point into a QObject such +/// that it can be used in a QmlObjectListMode for Qml. +class RallyPoint : public QObject +{ + Q_OBJECT + +public: + RallyPoint(const QGeoCoordinate& coordinate, QObject* parent = NULL); + RallyPoint(const RallyPoint& other, QObject* parent = NULL); + + ~RallyPoint(); + + const RallyPoint& operator=(const RallyPoint& other); + + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + Q_PROPERTY(QVariantList textFieldFacts MEMBER _textFieldFacts CONSTANT) + + QGeoCoordinate coordinate(void) const; + void setCoordinate(const QGeoCoordinate& coordinate); + + bool dirty(void) const { return _dirty; } + void setDirty(bool dirty); + +signals: + void coordinateChanged (const QGeoCoordinate& coordinate); + void dirtyChanged (bool dirty); + +private slots: + void _sendCoordinateChanged(void); + +private: + void _factSetup(void); + + bool _dirty; + Fact _longitudeFact; + Fact _latitudeFact; + Fact _altitudeFact; + + QVariantList _textFieldFacts; + + static QMap _metaDataMap; + + static const char* _longitudeFactName; + static const char* _latitudeFactName; + static const char* _altitudeFactName; +}; + +#endif diff --git a/src/MissionManager/RallyPoint.json b/src/MissionManager/RallyPoint.json new file mode 100644 index 000000000..6e3c81e1b --- /dev/null +++ b/src/MissionManager/RallyPoint.json @@ -0,0 +1,21 @@ +[ +{ + "name": "Latitude", + "shortDescription": "Latitude of rally point position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Longitude", + "shortDescription": "Longitude of rally point position", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "Altitude", + "shortDescription": "Altitude of rally point position (home relative)", + "type": "double", + "decimalPlaces": 2, + "units": "m" +} +] diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc new file mode 100644 index 000000000..f7e5a9fc9 --- /dev/null +++ b/src/MissionManager/RallyPointController.cc @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * (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 "RallyPointController.h" +#include "RallyPoint.h" +#include "Vehicle.h" +#include "FirmwarePlugin.h" +#include "MAVLinkProtocol.h" +#include "QGCApplication.h" +#include "ParameterManager.h" +#include "JsonHelper.h" +#include "SimpleMissionItem.h" + +#ifndef __mobile__ +#include "QGCFileDialog.h" +#endif + +#include +#include + +QGC_LOGGING_CATEGORY(RallyPointControllerLog, "RallyPointControllerLog") + +const char* RallyPointController::_jsonFileTypeValue = "RallyPoints"; +const char* RallyPointController::_jsonPointsKey = "points"; + +RallyPointController::RallyPointController(QObject* parent) + : PlanElementController(parent) + , _dirty(false) + , _currentRallyPoint(NULL) +{ + +} + +RallyPointController::~RallyPointController() +{ + +} + +void RallyPointController::start(bool editMode) +{ + qCDebug(RallyPointControllerLog) << "start editMode" << editMode; + + PlanElementController::start(editMode); +} + +void RallyPointController::_activeVehicleBeingRemoved(void) +{ + _activeVehicle->rallyPointManager()->disconnect(this); + _points.clearAndDeleteContents(); +} + +void RallyPointController::_activeVehicleSet(void) +{ + RallyPointManager* rallyPointManager = _activeVehicle->rallyPointManager(); + connect(rallyPointManager, &RallyPointManager::loadComplete, this, &RallyPointController::_loadComplete); + connect(rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); + + if (!rallyPointManager->inProgress()) { + _loadComplete(rallyPointManager->points()); + } + emit rallyPointsSupportedChanged(rallyPointsSupported()); +} + +bool RallyPointController::_loadJsonFile(QJsonDocument& jsonDoc, QString& errorString) +{ + QJsonObject json = jsonDoc.object(); + + int fileMajorVersion, fileMinorVersion; + if (!JsonHelper::validateQGCJsonFile(json, _jsonFileTypeValue, 1 /* supportedMajorVersion */, 0 /* supportedMinorVersion */, fileMajorVersion, fileMinorVersion, errorString)) { + return false; + } + + // Check for required keys + QStringList requiredKeys = { _jsonPointsKey }; + if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorString)) { + return false; + } + + // Load points + + QList rgPoints; + if (!JsonHelper::loadGeoCoordinateArray(json[_jsonPointsKey], true /* altitudeRequired */, rgPoints, errorString)) { + return false; + } + _points.clearAndDeleteContents(); + QObjectList pointList; + for (int i=0; ishowMessage("Rall Point file is in incorrect format."); + return; + } + } + + if (!errorString.isEmpty()) { + qgcApp()->showMessage(errorString); + } + + setDirty(true); + _setFirstPointCurrent(); +} + +void RallyPointController::loadFromFilePicker(void) +{ +#ifndef __mobile__ + QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Rally Point File to load", QString(), "Rally point file (*.rally);;All Files (*.*)"); + + if (filename.isEmpty()) { + return; + } + loadFromFile(filename); +#endif +} + +void RallyPointController::saveToFile(const QString& filename) +{ + if (filename.isEmpty()) { + return; + } + + QString rallyFilename = filename; + if (!QFileInfo(filename).fileName().contains(".")) { + rallyFilename += QString(".%1").arg(QGCApplication::rallyPointFileExtension); + } + + QFile file(rallyFilename); + + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + qgcApp()->showMessage(file.errorString()); + } else { + QJsonObject jsonObject; + + jsonObject[JsonHelper::jsonFileTypeKey] = _jsonFileTypeValue; + jsonObject[JsonHelper::jsonVersionKey] = QStringLiteral("1.0"); + jsonObject[JsonHelper::jsonGroundStationKey] = JsonHelper::jsonGroundStationValue; + + QJsonArray rgPoints; + QJsonValue jsonPoint; + for (int i=0; i<_points.count(); i++) { + JsonHelper::saveGeoCoordinate(qobject_cast(_points[i])->coordinate(), true /* writeAltitude */, jsonPoint); + rgPoints.append(jsonPoint); + } + jsonObject[_jsonPointsKey] = QJsonValue(rgPoints); + + QJsonDocument saveDoc(jsonObject); + file.write(saveDoc.toJson()); + } + + setDirty(false); +} + +void RallyPointController::saveToFilePicker(void) +{ +#ifndef __mobile__ + QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save Rally Points to", QString(), "Rally point file (*.rally);;All Files (*.*)"); + + if (filename.isEmpty()) { + return; + } + saveToFile(filename); +#endif +} + +void RallyPointController::removeAll(void) +{ + _points.clearAndDeleteContents(); + setDirty(true); + setCurrentRallyPoint(NULL); +} + +void RallyPointController::loadFromVehicle(void) +{ + if (_activeVehicle->parameterManager()->parametersReady() && !syncInProgress()) { + _activeVehicle->rallyPointManager()->loadFromVehicle(); + } else { + qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); + } +} + +void RallyPointController::sendToVehicle(void) +{ + if (!syncInProgress()) { + setDirty(false); + QList rgPoints; + for (int i=0; i<_points.count(); i++) { + rgPoints.append(qobject_cast(_points[i])->coordinate()); + } + _activeVehicle->rallyPointManager()->sendToVehicle(rgPoints); + } else { + qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle call at wrong time" << _activeVehicle->parameterManager()->parametersReady() << syncInProgress(); + } +} + +bool RallyPointController::syncInProgress(void) const +{ + return _activeVehicle->rallyPointManager()->inProgress(); +} + +void RallyPointController::setDirty(bool dirty) +{ + if (dirty != _dirty) { + _dirty = dirty; + emit dirtyChanged(dirty); + } +} + +QString RallyPointController::editorQml(void) const +{ + return _activeVehicle->rallyPointManager()->editorQml(); +} + +void RallyPointController::_loadComplete(const QList rgPoints) +{ + _points.clearAndDeleteContents(); + QObjectList pointList; + for (int i=0; i(_points[_points.count() - 1])->coordinate().altitude(); + } else { + defaultAlt = SimpleMissionItem::defaultAltitude; + } + point.setAltitude(defaultAlt); + RallyPoint* newPoint = new RallyPoint(point, this); + _points.append(newPoint); + setCurrentRallyPoint(newPoint); + setDirty(true); +} + +bool RallyPointController::rallyPointsSupported(void) const +{ + return _activeVehicle->rallyPointManager()->rallyPointsSupported(); +} + +void RallyPointController::removePoint(QObject* rallyPoint) +{ + QObject* prevPoint = _points[0]; + + int foundIndex = 0; + for (foundIndex=0; foundIndex<_points.count(); foundIndex++) { + prevPoint = _points[foundIndex]; + if (_points[foundIndex] == rallyPoint) { + _points.removeOne(rallyPoint); + rallyPoint->deleteLater(); + } + } + + if (_points.count()) { + int newIndex = qMin(foundIndex, _points.count() - 1); + newIndex = qMax(newIndex, 0); + setCurrentRallyPoint(_points[newIndex]); + } else { + setCurrentRallyPoint(NULL); + } +} + +void RallyPointController::setCurrentRallyPoint(QObject* rallyPoint) +{ + if (_currentRallyPoint != rallyPoint) { + _currentRallyPoint = rallyPoint; + emit currentRallyPointChanged(rallyPoint); + } +} + +void RallyPointController::_setFirstPointCurrent(void) +{ + setCurrentRallyPoint(_points.count() ? _points[0] : NULL); +} diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h new file mode 100644 index 000000000..7ac1af8be --- /dev/null +++ b/src/MissionManager/RallyPointController.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * (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 RallyPointController_H +#define RallyPointController_H + +#include "PlanElementController.h" +#include "RallyPointManager.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" +#include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" + +Q_DECLARE_LOGGING_CATEGORY(RallyPointControllerLog) + +class GeoFenceManager; + +class RallyPointController : public PlanElementController +{ + Q_OBJECT + +public: + RallyPointController(QObject* parent = NULL); + ~RallyPointController(); + + Q_PROPERTY(bool rallyPointsSupported READ rallyPointsSupported NOTIFY rallyPointsSupportedChanged) + Q_PROPERTY(QmlObjectListModel* points READ points CONSTANT) + Q_PROPERTY(QString editorQml READ editorQml CONSTANT) + Q_PROPERTY(QObject* currentRallyPoint READ currentRallyPoint WRITE setCurrentRallyPoint NOTIFY currentRallyPointChanged) + + Q_INVOKABLE void addPoint(QGeoCoordinate point); + Q_INVOKABLE void removePoint(QObject* rallyPoint); + + 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 { return _dirty; } + void setDirty (bool dirty) final; + + QString fileExtension(void) const final; + + bool rallyPointsSupported (void) const; + QmlObjectListModel* points (void) { return &_points; } + QString editorQml (void) const; + QObject* currentRallyPoint (void) const { return _currentRallyPoint; } + + void setCurrentRallyPoint(QObject* rallyPoint); + +signals: + void rallyPointsSupportedChanged(bool rallyPointsSupported); + void currentRallyPointChanged(QObject* rallyPoint); + +private slots: + void _loadComplete(const QList rgPoints); + void _setFirstPointCurrent(void); + +private: + bool _loadJsonFile(QJsonDocument& jsonDoc, QString& errorString); + + void _activeVehicleBeingRemoved(void) final; + void _activeVehicleSet(void) final; + + bool _dirty; + QmlObjectListModel _points; + QObject* _currentRallyPoint; + + static const char* _jsonFileTypeValue; + static const char* _jsonPointsKey; +}; + +#endif diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc new file mode 100644 index 000000000..2698fc5df --- /dev/null +++ b/src/MissionManager/RallyPointManager.cc @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * (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 "RallyPointManager.h" +#include "Vehicle.h" + +QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog") + +RallyPointManager::RallyPointManager(Vehicle* vehicle) + : QObject(vehicle) + , _vehicle(vehicle) +{ + +} + +RallyPointManager::~RallyPointManager() +{ + +} + +void RallyPointManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) +{ + qCDebug(RallyPointManagerLog) << "Sending error" << errorCode << errorMsg; + + emit error(errorCode, errorMsg); +} + +void RallyPointManager::loadFromVehicle(void) +{ + // No support in generic vehicle + loadComplete(QList()); +} + +void RallyPointManager::sendToVehicle(const QList& rgPoints) +{ + Q_UNUSED(rgPoints); + // No support in generic vehicle +} diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h new file mode 100644 index 000000000..92132b5a6 --- /dev/null +++ b/src/MissionManager/RallyPointManager.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * (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 RallyPointManager_H +#define RallyPointManager_H + +#include +#include + +#include "QGCLoggingCategory.h" + +class Vehicle; + +Q_DECLARE_LOGGING_CATEGORY(RallyPointManagerLog) + +/// This is the base class for firmware specific rally point managers. A rally point manager is responsible +/// for communicating with the vehicle to set/get rally points. +class RallyPointManager : public QObject +{ + Q_OBJECT + +public: + RallyPointManager(Vehicle* vehicle); + ~RallyPointManager(); + + /// Returns true if the manager is currently communicating with the vehicle + virtual bool inProgress(void) const { return false; } + + /// Load the current settings from the vehicle + virtual void loadFromVehicle(void); + + /// Send the current settings to the vehicle + virtual void sendToVehicle(const QList& rgPoints); + + virtual bool rallyPointsSupported(void) const { return false; } + + QList points(void) const { return _rgPoints; } + + virtual QString editorQml(void) const { return QStringLiteral("qrc:/FirmwarePlugin/RallyPointEditor.qml"); } + + /// 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 loadComplete (const QList rgPoints); + void inProgressChanged (bool inProgress); + void error (int errorCode, const QString& errorMsg); + +protected: + void _sendError(ErrorCode_t errorCode, const QString& errorMsg); + + Vehicle* _vehicle; + QList _rgPoints; +}; + +#endif diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index a2c0d4bdd..1f5e61428 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -216,7 +216,7 @@ void SurveyMissionItem::save(QJsonObject& saveObject) const const QVariant& polyVar = _polygonPath[i]; QJsonValue jsonValue; - JsonHelper::writeQGeoCoordinate(jsonValue, polyVar.value(), false /* writeAltitude */); + JsonHelper::saveGeoCoordinate(polyVar.value(), false /* writeAltitude */, jsonValue); polygonArray += jsonValue; } @@ -294,7 +294,7 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, QString& errorStr const QJsonValue& pointValue = polygonArray[i]; QGeoCoordinate pointCoord; - if (!JsonHelper::toQGeoCoordinate(pointValue, pointCoord, false /* altitudeRequired */, errorString)) { + if (!JsonHelper::loadGeoCoordinate(pointValue, false /* altitudeRequired */, pointCoord, errorString)) { _clear(); return false; } diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index a9a182763..e5e5582d5 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -82,6 +82,8 @@ #include "CoordinateVector.h" #include "MainToolBarController.h" #include "MissionController.h" +#include "GeoFenceController.h" +#include "RallyPointController.h" #include "VideoManager.h" #include "VideoSurface.h" #include "VideoReceiver.h" @@ -93,7 +95,6 @@ #include "PositionManager.h" #include "FollowMe.h" #include "MissionCommandTree.h" -#include "GeoFenceController.h" #include "QGCMapPolygon.h" #include "ParameterManager.h" @@ -126,6 +127,7 @@ QGCApplication* QGCApplication::_app = NULL; const char* QGCApplication::parameterFileExtension = "params"; const char* QGCApplication::missionFileExtension = "mission"; const char* QGCApplication::fenceFileExtension = "fence"; +const char* QGCApplication::rallyPointFileExtension = "rally"; const char* QGCApplication::telemetryFileExtension = "tlog"; const char* QGCApplication::_deleteAllSettingsKey = "DeleteAllSettingsNextBoot"; @@ -400,11 +402,12 @@ void QGCApplication::_initCommon(void) qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ScreenToolsController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "MainToolBarController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "MissionController"); + qmlRegisterType ("QGroundControl.Controllers", 1, 0, "GeoFenceController"); + qmlRegisterType ("QGroundControl.Controllers", 1, 0, "RallyPointController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ValuesWidgetController"); 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/QGCApplication.h b/src/QGCApplication.h index 0d9d38efd..f6805a7e6 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -68,6 +68,7 @@ public: static const char* parameterFileExtension; static const char* missionFileExtension; static const char* fenceFileExtension; + static const char* rallyPointFileExtension; static const char* telemetryFileExtension; /// @brief Sets the persistent flag to delete all settings the next time QGroundControl is started. diff --git a/src/QmlControls/MissionItemIndexLabel.qml b/src/QmlControls/MissionItemIndexLabel.qml index e6b70357f..9561f61c0 100644 --- a/src/QmlControls/MissionItemIndexLabel.qml +++ b/src/QmlControls/MissionItemIndexLabel.qml @@ -6,9 +6,11 @@ import QGroundControl.ScreenTools 1.0 import QGroundControl.Palette 1.0 Rectangle { - property alias label: _label.text - property bool isCurrentItem: false - property bool small: false + id: root + + property alias label: _label.text + property bool checked: false + property bool small: false signal clicked @@ -17,7 +19,7 @@ Rectangle { radius: _width / 2 border.width: small ? 1 : 2 border.color: "white" - color: isCurrentItem ? "green" : qgcPal.mapButtonHighlight + color: checked ? "green" : qgcPal.mapButtonHighlight property real _width: small ? ScreenTools.defaultFontPixelHeight * ScreenTools.smallFontPointRatio * 1.75 : ScreenTools.defaultFontPixelHeight * 1.75 diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 472abd579..e1ce19072 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -16,6 +16,8 @@ ModeSwitchDisplay 1.0 ModeSwitchDisplay.qml MultiRotorMotorDisplay 1.0 MultiRotorMotorDisplay.qml ParameterEditor 1.0 ParameterEditor.qml ParameterEditorDialog 1.0 ParameterEditorDialog.qml +RallyPointEditorHeader 1.0 RallyPointEditorHeader.qml +RallyPointItemEditor 1.0 RallyPointItemEditor.qml RCChannelMonitor 1.0 RCChannelMonitor.qml QGCButton 1.0 QGCButton.qml QGCCheckBox 1.0 QGCCheckBox.qml diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index e7687aa16..c85b1c3a0 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -185,6 +185,7 @@ QObjectList QmlObjectListModel::swapObjectList(const QObjectList& newlist) beginResetModel(); _objectList = newlist; endResetModel(); + emit countChanged(count()); return oldlist; } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b7849664e..f5e5c13bd 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -17,6 +17,8 @@ #include "UAS.h" #include "JoystickManager.h" #include "MissionManager.h" +#include "GeoFenceManager.h" +#include "RallyPointManager.h" #include "CoordinateVector.h" #include "ParameterManager.h" #include "QGCApplication.h" @@ -25,7 +27,6 @@ #include "FollowMe.h" #include "MissionCommandTree.h" #include "QGroundControlQmlGlobal.h" -#include "GeoFenceManager.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -98,9 +99,11 @@ Vehicle::Vehicle(LinkInterface* link, , _connectionLost(false) , _connectionLostEnabled(true) , _missionManager(NULL) - , _missionManagerInitialRequestComplete(false) + , _missionManagerInitialRequestSent(false) , _geoFenceManager(NULL) - , _geoFenceManagerInitialRequestComplete(false) + , _geoFenceManagerInitialRequestSent(false) + , _rallyPointManager(NULL) + , _rallyPointManagerInitialRequestSent(false) , _parameterManager(NULL) , _armed(false) , _base_mode(0) @@ -202,6 +205,10 @@ Vehicle::Vehicle(LinkInterface* link, // GeoFenceManager needs to access ParameterManager so make sure to create after _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); + connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_newGeoFenceAvailable); + + _rallyPointManager = _firmwarePlugin->newRallyPointManager(this); + connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); // Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet. @@ -288,9 +295,11 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _connectionLost(false) , _connectionLostEnabled(true) , _missionManager(NULL) - , _missionManagerInitialRequestComplete(false) + , _missionManagerInitialRequestSent(false) , _geoFenceManager(NULL) - , _geoFenceManagerInitialRequestComplete(false) + , _geoFenceManagerInitialRequestSent(false) + , _rallyPointManager(NULL) + , _rallyPointManagerInitialRequestSent(false) , _parameterManager(NULL) , _armed(false) , _base_mode(0) @@ -335,6 +344,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this); connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError); + _rallyPointManager = _firmwarePlugin->newRallyPointManager(this); + connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError); + // Build FactGroup object model _addFact(&_rollFact, _rollFactName); @@ -1381,7 +1393,13 @@ void Vehicle::_missionManagerError(int errorCode, const QString& 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)); + qgcApp()->showMessage(QString("Error during GeoFence communication with Vehicle: %1").arg(errorMsg)); +} + +void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg) +{ + Q_UNUSED(errorCode); + qgcApp()->showMessage(QString("Error during Rally Point communication with Vehicle: %1").arg(errorMsg)); } void Vehicle::_addNewMapTrajectoryPoint(void) @@ -1411,8 +1429,8 @@ void Vehicle::_mapTrajectoryStop() void Vehicle::_parametersReady(bool parametersReady) { - if (parametersReady && !_missionManagerInitialRequestComplete) { - _missionManagerInitialRequestComplete = true; + if (parametersReady && !_missionManagerInitialRequestSent) { + _missionManagerInitialRequestSent = true; _missionManager->requestMissionItems(); } @@ -1854,12 +1872,21 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs) void Vehicle::_newMissionItemsAvailable(void) { // After the initial mission request complets we ask for the geofence - if (!_geoFenceManagerInitialRequestComplete) { - _geoFenceManagerInitialRequestComplete = true; + if (!_geoFenceManagerInitialRequestSent) { + _geoFenceManagerInitialRequestSent = true; _geoFenceManager->loadFromVehicle(); } } +void Vehicle::_newGeoFenceAvailable(void) +{ + // After the initial mission request complets we ask for the geofence + if (!_rallyPointManagerInitialRequestSent) { + _rallyPointManagerInitialRequestSent = true; + _rallyPointManager->loadFromVehicle(); + } +} + 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 b23339c3e..a5f13de86 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -34,6 +34,7 @@ class AutoPilotPlugin; class AutoPilotPluginManager; class MissionManager; class GeoFenceManager; +class RallyPointManager; class ParameterManager; class JoystickManager; class UASMessage; @@ -448,8 +449,9 @@ public: int manualControlReservedButtonCount(void); - MissionManager* missionManager(void) { return _missionManager; } - GeoFenceManager* geoFenceManager(void) { return _geoFenceManager; } + MissionManager* missionManager(void) { return _missionManager; } + GeoFenceManager* geoFenceManager(void) { return _geoFenceManager; } + RallyPointManager* rallyPointManager(void) { return _rallyPointManager; } bool homePositionAvailable(void); QGeoCoordinate homePosition(void); @@ -666,6 +668,7 @@ private slots: void _connectionLostTimeout(void); void _prearmErrorTimeout(void); void _newMissionItemsAvailable(void); + void _newGeoFenceAvailable(void); private: bool _containsLink(LinkInterface* link); @@ -688,6 +691,7 @@ private: void _handleHilActuatorControls(mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); void _geoFenceManagerError(int errorCode, const QString& errorMsg); + void _rallyPointManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); void _connectionActive(void); @@ -751,10 +755,13 @@ private: QTimer _connectionLostTimer; MissionManager* _missionManager; - bool _missionManagerInitialRequestComplete; + bool _missionManagerInitialRequestSent; GeoFenceManager* _geoFenceManager; - bool _geoFenceManagerInitialRequestComplete; + bool _geoFenceManagerInitialRequestSent; + + RallyPointManager* _rallyPointManager; + bool _rallyPointManagerInitialRequestSent; ParameterManager* _parameterManager; -- 2.22.0