diff --git a/ChangeLog.md b/ChangeLog.md index 5658188..040f1bc 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -6,6 +6,7 @@ Note: This file only contains high level features or important fixes. ### 4.0.0 - Daily Build +* Added ROI option during manual flight. * Windows: Move builds to 64 bit, Qt 5.12.5 * Plan: ROI button will switch to Cancel ROI at appropriate times * Plan: When ROI is selected the flight path lines which are affected by the ROI will change color diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index 890f514..0cf4f15 100644 --- a/QGCExternalLibs.pri +++ b/QGCExternalLibs.pri @@ -131,9 +131,9 @@ AndroidBuild { contains(DEFINES, QGC_ENABLE_PAIRING) { MacBuild { #- Pairing is generally not supported on macOS. This is here solely for development. - exists(/usr/local/Cellar/openssl/1.0.2s/include) { - INCLUDEPATH += /usr/local/Cellar/openssl/1.0.2s/include - LIBS += -L/usr/local/Cellar/openssl/1.0.2s/lib + exists(/usr/local/Cellar/openssl/1.0.2t/include) { + INCLUDEPATH += /usr/local/Cellar/openssl/1.0.2t/include + LIBS += -L/usr/local/Cellar/openssl/1.0.2t/lib LIBS += -lcrypto -lz } else { # There is some circular reference settings going on between QGCExternalLibs.pri and gqgroundcontrol.pro. diff --git a/qgcimages.qrc b/qgcimages.qrc index 4c0d059..a363127 100644 --- a/qgcimages.qrc +++ b/qgcimages.qrc @@ -147,6 +147,7 @@ <file alias="RCLossLight.svg">src/AutoPilotPlugins/PX4/Images/RCLossLight.svg</file> <file alias="ReturnToHomeAltitude.svg">src/AutoPilotPlugins/PX4/Images/ReturnToHomeAltitude.svg</file> <file alias="ReturnToHomeAltitudeCopter.svg">src/AutoPilotPlugins/PX4/Images/ReturnToHomeAltitudeCopter.svg</file> + <file alias="roi.svg">src/ui/toolbar/Images/roi.svg</file> <file alias="rollDialWhite.svg">src/FlightMap/Images/rollDialWhite.svg</file> <file alias="rollPointerWhite.svg">src/FlightMap/Images/rollPointerWhite.svg</file> <file alias="RTK.svg">src/ui/toolbar/Images/RTK.svg</file> diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index e82ba2e..542cba6 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -15,6 +15,7 @@ <file alias="ModeIndicator.qml">src/ui/toolbar/ModeIndicator.qml</file> <file alias="MultiVehicleSelector.qml">src/ui/toolbar/MultiVehicleSelector.qml</file> <file alias="RCRSSIIndicator.qml">src/ui/toolbar/RCRSSIIndicator.qml</file> + <file alias="ROIIndicator.qml">src/ui/toolbar/ROIIndicator.qml</file> <file alias="TelemetryRSSIIndicator.qml">src/ui/toolbar/TelemetryRSSIIndicator.qml</file> <file alias="VTOLModeIndicator.qml">src/ui/toolbar/VTOLModeIndicator.qml</file> </qresource> diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 22d5f3d..f4150ac 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -271,18 +271,15 @@ void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordina qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } -void FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) +void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double) { // Not supported by generic vehicle - Q_UNUSED(vehicle); - Q_UNUSED(altitudeRel); qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } -void FirmwarePlugin::startMission(Vehicle* vehicle) +void FirmwarePlugin::startMission(Vehicle*) { // Not supported by generic vehicle - Q_UNUSED(vehicle); qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } @@ -293,33 +290,28 @@ const FirmwarePlugin::remapParamNameMajorVersionMap_t& FirmwarePlugin::paramName return remap; } -int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const +int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int) const { - Q_UNUSED(majorVersionNumber); return 0; } -QString FirmwarePlugin::vehicleImageOpaque(const Vehicle* vehicle) const +QString FirmwarePlugin::vehicleImageOpaque(const Vehicle*) const { - Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/vehicleArrowOpaque.svg"); } -QString FirmwarePlugin::vehicleImageOutline(const Vehicle* vehicle) const +QString FirmwarePlugin::vehicleImageOutline(const Vehicle*) const { - Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/vehicleArrowOutline.svg"); } -QString FirmwarePlugin::vehicleImageCompass(const Vehicle* vehicle) const +QString FirmwarePlugin::vehicleImageCompass(const Vehicle*) const { - Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/compassInstrumentArrow.svg"); } -const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle) +const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle*) { - Q_UNUSED(vehicle); //-- Default list of indicators for all vehicles. if(_toolBarIndicatorList.size() == 0) { _toolBarIndicatorList = QVariantList({ @@ -329,6 +321,7 @@ const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle) QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/RCRSSIIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/BatteryIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml")), + QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ROIIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ModeIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/VTOLModeIndicator.qml")), @@ -339,10 +332,8 @@ const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle) return _toolBarIndicatorList; } -const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle) +const QVariantList& FirmwarePlugin::cameraList(const Vehicle*) { - Q_UNUSED(vehicle); - if (_cameraList.size() == 0) { CameraMetaData* metaData; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index a6627e6..888ac2e 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -48,6 +48,7 @@ public: GuidedModeCapability = 1 << 2, ///< Vehicle supports guided mode commands OrbitModeCapability = 1 << 3, ///< Vehicle supports orbit mode TakeoffVehicleCapability = 1 << 4, ///< Vehicle supports guided takeoff + ROIModeCapability = 1 << 5, ///< Vehicle supports ROI } FirmwareCapabilities; /// Maps from on parameter name to another diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index bd6efcb..426df8e 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -226,10 +226,13 @@ bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_m bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) { int available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability; + //-- This is arbitrary until I find how to really tell if ROI is avaiable + if (vehicle->multiRotor()) { + available |= ROIModeCapability; + } if (vehicle->multiRotor() || vehicle->vtol()) { available |= TakeoffVehicleCapability | OrbitModeCapability; } - return (capabilities & available) == capabilities; } diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 92bf418..0cb2a31 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -44,7 +44,6 @@ FlightMap { property var _geoFenceController: missionController.geoFenceController property var _rallyPointController: missionController.rallyPointController - property var activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property var _activeVehicleCoordinate: activeVehicle ? activeVehicle.coordinate : QtPositioning.coordinate() property real _toolButtonTopMargin: parent.height - mainWindow.height + (ScreenTools.defaultFontPixelHeight / 2) property bool _airspaceEnabled: QGroundControl.airmapSupported ? (QGroundControl.settingsManager.airMapSettings.enableAirMap.rawValue && QGroundControl.airspaceManager.connected): false @@ -409,6 +408,34 @@ FlightMap { } } + // ROI Location visuals + MapQuickItem { + id: roiLocationItem + visible: activeVehicle && activeVehicle.isROIEnabled + z: QGroundControl.zOrderMapItems + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + sourceItem: MissionItemIndexLabel { + checked: true + index: -1 + label: qsTr("ROI here", "Make this a Region Of Interest") + } + + //-- Visibilty controlled by actual state + function show(coord) { + roiLocationItem.coordinate = coord + } + + function hide() { + } + + function actionConfirmed() { + } + + function actionCancelled() { + } + } + // Orbit telemetry visuals QGCMapCircleVisuals { id: orbitTelemetryCircle @@ -437,9 +464,7 @@ FlightMap { QGCMenu { id: clickMenu - property var coord - QGCMenuItem { text: qsTr("Go to location") visible: guidedActionsController.showGotoLocation @@ -450,7 +475,6 @@ FlightMap { guidedActionsController.confirmAction(guidedActionsController.actionGoto, clickMenu.coord, gotoLocationItem) } } - QGCMenuItem { text: qsTr("Orbit at location") visible: guidedActionsController.showOrbit @@ -461,6 +485,15 @@ FlightMap { guidedActionsController.confirmAction(guidedActionsController.actionOrbit, clickMenu.coord, orbitMapCircle) } } + QGCMenuItem { + text: qsTr("ROI at location") + visible: guidedActionsController.showROI + + onTriggered: { + roiLocationItem.show(clickMenu.coord) + guidedActionsController.confirmAction(guidedActionsController.actionROI, clickMenu.coord, orbitMapCircle) + } + } } onClicked: { diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index bb5831b..9df7acc 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -51,6 +51,7 @@ Item { readonly property string setWaypointTitle: qsTr("Set Waypoint") readonly property string gotoTitle: qsTr("Go To Location") readonly property string vtolTransitionTitle: qsTr("VTOL Transition") + readonly property string roiTitle: qsTr("ROI") readonly property string armMessage: qsTr("Arm the vehicle.") readonly property string disarmMessage: qsTr("Disarm the vehicle") @@ -70,6 +71,7 @@ Item { readonly property string mvPauseMessage: qsTr("Pause all vehicles at their current position.") readonly property string vtolTransitionFwdMessage: qsTr("Transition VTOL to fixed wing flight.") readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.") + readonly property string roiMessage: qsTr("Make the specified location a Region Of Interest.") readonly property int actionRTL: 1 readonly property int actionLand: 2 @@ -92,35 +94,36 @@ Item { readonly property int actionMVStartMission: 19 readonly property int actionVtolTransitionToFwdFlight: 20 readonly property int actionVtolTransitionToMRFlight: 21 + readonly property int actionROI: 22 property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying property bool showArm: _guidedActionsEnabled && !_vehicleArmed property bool showDisarm: _guidedActionsEnabled && _vehicleArmed && !_vehicleFlying - property bool showRTL: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode - property bool showTakeoff: _guidedActionsEnabled && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying - property bool showLand: _guidedActionsEnabled && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode + property bool showRTL: _guidedActionsEnabled && _vehicleArmed && activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode + property bool showTakeoff: _guidedActionsEnabled && activeVehicle.takeoffVehicleSupported && !_vehicleFlying + property bool showLand: _guidedActionsEnabled && activeVehicle.guidedModeSupported && _vehicleArmed && !activeVehicle.fixedWing && !_vehicleInLandMode property bool showStartMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && !_vehicleFlying property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleArmed && _vehicleFlying && (_currentMissionIndex < _missionItemCount - 1) - property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused && !_fixedWingOnApproach - property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive - property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && !_missionActive + property bool showPause: _guidedActionsEnabled && _vehicleArmed && activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused && !_fixedWingOnApproach + property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive + property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && activeVehicle.orbitModeSupported && !_missionActive + property bool showROI: _guidedActionsEnabled && !_hideROI && _vehicleFlying && activeVehicle.roiModeSupported && !_missionActive property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _fixedWingOnApproach property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying // Note: The '_missionItemCount - 2' is a hack to not trigger resume mission when a mission ends with an RTL item - property bool showResumeMission: _activeVehicle && !_vehicleArmed && _vehicleWasFlying && _missionAvailable && _resumeMissionIndex > 0 && (_resumeMissionIndex < _missionItemCount - 2) + property bool showResumeMission: activeVehicle && !_vehicleArmed && _vehicleWasFlying && _missionAvailable && _resumeMissionIndex > 0 && (_resumeMissionIndex < _missionItemCount - 2) property bool guidedUIVisible: guidedActionConfirm.visible || guidedActionList.visible property var _corePlugin: QGroundControl.corePlugin - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle - property bool _guidedActionsEnabled: (!ScreenTools.isDebug && QGroundControl.corePlugin.options.guidedActionsRequireRCRSSI && _activeVehicle) ? _rcRSSIAvailable : _activeVehicle - property string _flightMode: _activeVehicle ? _activeVehicle.flightMode : "" + property bool _guidedActionsEnabled: (!ScreenTools.isDebug && QGroundControl.corePlugin.options.guidedActionsRequireRCRSSI && activeVehicle) ? _rcRSSIAvailable : activeVehicle + property string _flightMode: activeVehicle ? activeVehicle.flightMode : "" property bool _missionAvailable: missionController.containsItems - property bool _missionActive: _activeVehicle ? _vehicleArmed && (_vehicleInLandMode || _vehicleInRTLMode || _vehicleInMissionMode) : false - property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false - property bool _vehicleFlying: _activeVehicle ? _activeVehicle.flying : false - property bool _vehicleLanding: _activeVehicle ? _activeVehicle.landing : false + property bool _missionActive: activeVehicle ? _vehicleArmed && (_vehicleInLandMode || _vehicleInRTLMode || _vehicleInMissionMode) : false + property bool _vehicleArmed: activeVehicle ? activeVehicle.armed : false + property bool _vehicleFlying: activeVehicle ? activeVehicle.flying : false + property bool _vehicleLanding: activeVehicle ? activeVehicle.landing : false property bool _vehiclePaused: false property bool _vehicleInMissionMode: false property bool _vehicleInRTLMode: false @@ -130,18 +133,19 @@ Item { property int _resumeMissionIndex: missionController.resumeMissionIndex property bool _hideEmergenyStop: !QGroundControl.corePlugin.options.guidedBarShowEmergencyStop property bool _hideOrbit: !QGroundControl.corePlugin.options.guidedBarShowOrbit + property bool _hideROI: !QGroundControl.corePlugin.options.guidedBarShowOrbit property bool _vehicleWasFlying: false - property bool _rcRSSIAvailable: _activeVehicle ? _activeVehicle.rcRSSI > 0 && _activeVehicle.rcRSSI <= 100 : false - property bool _fixedWingOnApproach: _activeVehicle ? _activeVehicle.fixedWing && _vehicleLanding : false + property bool _rcRSSIAvailable: activeVehicle ? activeVehicle.rcRSSI > 0 && activeVehicle.rcRSSI <= 100 : false + property bool _fixedWingOnApproach: activeVehicle ? activeVehicle.fixedWing && _vehicleLanding : false // You can turn on log output for GuidedActionsController by turning on GuidedActionsControllerLog category - property bool __guidedModeSupported: _activeVehicle ? _activeVehicle.guidedModeSupported : false - property bool __pauseVehicleSupported: _activeVehicle ? _activeVehicle.pauseVehicleSupported : false + property bool __guidedModeSupported: activeVehicle ? activeVehicle.guidedModeSupported : false + property bool __pauseVehicleSupported: activeVehicle ? activeVehicle.pauseVehicleSupported : false property bool __flightMode: _flightMode function _outputState() { if (_corePlugin.guidedActionsControllerLogging()) { - console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleWasFlying(%5) _vehicleInRTLMode(%6) pauseVehicleSupported(%7) _vehiclePaused(%8) _flightMode(%9) _missionItemCount(%10)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleWasFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode).arg(_missionItemCount)) + console.log(qsTr("activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleWasFlying(%5) _vehicleInRTLMode(%6) pauseVehicleSupported(%7) _vehiclePaused(%8) _flightMode(%9) _missionItemCount(%10)").arg(activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleWasFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode).arg(_missionItemCount)) } } @@ -209,10 +213,10 @@ Item { property var _actionData on_FlightModeChanged: { - _vehiclePaused = _activeVehicle ? _flightMode === _activeVehicle.pauseFlightMode : false - _vehicleInRTLMode = _activeVehicle ? _flightMode === _activeVehicle.rtlFlightMode || _flightMode === _activeVehicle.smartRTLFlightMode : false - _vehicleInLandMode = _activeVehicle ? _flightMode === _activeVehicle.landFlightMode : false - _vehicleInMissionMode = _activeVehicle ? _flightMode === _activeVehicle.missionFlightMode : false // Must be last to get correct signalling for showStartMission popups + _vehiclePaused = activeVehicle ? _flightMode === activeVehicle.pauseFlightMode : false + _vehicleInRTLMode = activeVehicle ? _flightMode === activeVehicle.rtlFlightMode || _flightMode === activeVehicle.smartRTLFlightMode : false + _vehicleInLandMode = activeVehicle ? _flightMode === activeVehicle.landFlightMode : false + _vehicleInMissionMode = activeVehicle ? _flightMode === activeVehicle.missionFlightMode : false // Must be last to get correct signalling for showStartMission popups } // Called when an action is about to be executed in order to confirm @@ -287,7 +291,7 @@ Item { case actionRTL: confirmDialog.title = rtlTitle confirmDialog.message = rtlMessage - if (_activeVehicle.supportsSmartRTL) { + if (activeVehicle.supportsSmartRTL) { confirmDialog.optionText = qsTr("Smart RTL") confirmDialog.optionChecked = false } @@ -343,6 +347,11 @@ Item { confirmDialog.message = vtolTransitionMRMessage confirmDialog.hideTrigger = true break + case actionROI: + confirmDialog.title = roiTitle + confirmDialog.message = roiMessage + confirmDialog.hideTrigger = Qt.binding(function() { return !showROI }) + break; default: console.warn("Unknown actionCode", actionCode) return @@ -356,13 +365,13 @@ Item { var rgVehicle; switch (actionCode) { case actionRTL: - _activeVehicle.guidedModeRTL(optionChecked) + activeVehicle.guidedModeRTL(optionChecked) break case actionLand: - _activeVehicle.guidedModeLand() + activeVehicle.guidedModeLand() break case actionTakeoff: - _activeVehicle.guidedModeTakeoff(actionAltitudeChange) + activeVehicle.guidedModeTakeoff(actionAltitudeChange) break case actionResumeMission: case actionResumeMissionUploadFail: @@ -370,7 +379,7 @@ Item { break case actionStartMission: case actionContinueMission: - _activeVehicle.startMission() + activeVehicle.startMission() break case actionMVStartMission: rgVehicle = QGroundControl.multiVehicleManager.vehicles @@ -379,32 +388,32 @@ Item { } break case actionArm: - _activeVehicle.armed = true + activeVehicle.armed = true break case actionDisarm: - _activeVehicle.armed = false + activeVehicle.armed = false break case actionEmergencyStop: - _activeVehicle.emergencyStop() + activeVehicle.emergencyStop() break case actionChangeAlt: - _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange) + activeVehicle.guidedModeChangeAltitude(actionAltitudeChange) break case actionGoto: - _activeVehicle.guidedModeGotoLocation(actionData) + activeVehicle.guidedModeGotoLocation(actionData) break case actionSetWaypoint: - _activeVehicle.setCurrentMissionSequence(actionData) + activeVehicle.setCurrentMissionSequence(actionData) break case actionOrbit: - _activeVehicle.guidedModeOrbit(orbitMapCircle.center, orbitMapCircle.radius() * (orbitMapCircle.clockwiseRotation ? 1 : -1), _activeVehicle.altitudeAMSL.rawValue + actionAltitudeChange) + activeVehicle.guidedModeOrbit(orbitMapCircle.center, orbitMapCircle.radius() * (orbitMapCircle.clockwiseRotation ? 1 : -1), activeVehicle.altitudeAMSL.rawValue + actionAltitudeChange) break case actionLandAbort: - _activeVehicle.abortLanding(50) // hardcoded value for climbOutAltitude that is currently ignored + activeVehicle.abortLanding(50) // hardcoded value for climbOutAltitude that is currently ignored break case actionPause: - _activeVehicle.pauseVehicle() - _activeVehicle.guidedModeChangeAltitude(actionAltitudeChange) + activeVehicle.pauseVehicle() + activeVehicle.guidedModeChangeAltitude(actionAltitudeChange) break case actionMVPause: rgVehicle = QGroundControl.multiVehicleManager.vehicles @@ -413,10 +422,13 @@ Item { } break case actionVtolTransitionToFwdFlight: - _activeVehicle.vtolInFwdFlight = true + activeVehicle.vtolInFwdFlight = true break case actionVtolTransitionToMRFlight: - _activeVehicle.vtolInFwdFlight = false + activeVehicle.vtolInFwdFlight = false + break + case actionROI: + activeVehicle.guidedModeROI(actionData) break default: console.warn(qsTr("Internal error: unknown actionCode"), actionCode) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 1757cdc..0a8a6db 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2979,6 +2979,11 @@ bool Vehicle::orbitModeSupported() const return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability); } +bool Vehicle::roiModeSupported() const +{ + return _firmwarePlugin->isCapable(this, FirmwarePlugin::ROIModeCapability); +} + bool Vehicle::takeoffVehicleSupported() const { return _firmwarePlugin->isCapable(this, FirmwarePlugin::TakeoffVehicleCapability); @@ -3084,6 +3089,74 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, } } +void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord) +{ + if (!roiModeSupported()) { + qgcApp()->showMessage(QStringLiteral("ROI mode not supported by Vehicle.")); + return; + } + if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { + sendMavCommandInt( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_LOCATION, + MAV_FRAME_GLOBAL, + true, // show error if fails + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + centerCoord.latitude(), + centerCoord.longitude(), + static_cast<float>(centerCoord.altitude())); + } else { + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_LOCATION, + true, // show error if fails + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(centerCoord.latitude()), + static_cast<float>(centerCoord.longitude()), + static_cast<float>(centerCoord.altitude())); + } +} + +void Vehicle::stopGuidedModeROI() +{ + if (!roiModeSupported()) { + qgcApp()->showMessage(QStringLiteral("ROI mode not supported by Vehicle.")); + return; + } + if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { + sendMavCommandInt( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_NONE, + MAV_FRAME_GLOBAL, + true, // show error if fails + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<double>(qQNaN()), // Empty + static_cast<double>(qQNaN()), // Empty + static_cast<float>(qQNaN())); // Empty + } else { + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_NONE, + true, // show error if fails + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN()), // Empty + static_cast<float>(qQNaN())); // Empty + } +} + void Vehicle::pauseVehicle(void) { if (!pauseVehicleSupported()) { @@ -3352,6 +3425,20 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) } } + if (ack.command == MAV_CMD_DO_SET_ROI_LOCATION) { + if (ack.result == MAV_RESULT_ACCEPTED) { + _isROIEnabled = true; + emit isROIEnabledChanged(); + } + } + + if (ack.command == MAV_CMD_DO_SET_ROI_NONE) { + if (ack.result == MAV_RESULT_ACCEPTED) { + _isROIEnabled = false; + emit isROIEnabledChanged(); + } + } + #if !defined(NO_ARDUPILOT_DIALECT) if (ack.command == MAV_CMD_FLASH_BOOTLOADER && ack.result == MAV_RESULT_ACCEPTED) { qgcApp()->showMessage(tr("Bootloader flash succeeded")); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 9368bf8..ed395f1 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -634,6 +634,7 @@ public: Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged) Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged) Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged) + Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) // The following properties relate to Orbit status Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged) @@ -646,6 +647,7 @@ public: Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) ///< Guided mode commands are supported by this vehicle Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT) ///< Pause vehicle command is supported Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle + Q_PROPERTY(bool roiModeSupported READ roiModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported Q_PROPERTY(QString gotoFlightMode READ gotoFlightMode CONSTANT) ///< Flight mode vehicle is in while performing goto @@ -729,6 +731,11 @@ public: /// @param amslAltitude Desired vehicle altitude Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude); + /// Command vehicle to keep given point as ROI + /// @param centerCoord ROI coordinates + Q_INVOKABLE void guidedModeROI(const QGeoCoordinate& centerCoord); + Q_INVOKABLE void stopGuidedModeROI(); + /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left /// in guided mode after pause. Q_INVOKABLE void pauseVehicle(void); @@ -778,6 +785,7 @@ public: bool guidedModeSupported (void) const; bool pauseVehicleSupported (void) const; bool orbitModeSupported (void) const; + bool roiModeSupported (void) const; bool takeoffVehicleSupported (void) const; QString gotoFlightMode (void) const; @@ -1100,6 +1108,7 @@ public: qreal gimbalPitch () { return static_cast<qreal>(_curGimbalPitch); } qreal gimbalYaw () { return static_cast<qreal>(_curGinmbalYaw); } bool gimbalData () { return _haveGimbalData; } + bool isROIEnabled () { return _isROIEnabled; } public slots: void setVtolInFwdFlight (bool vtolInFwdFlight); @@ -1215,6 +1224,7 @@ signals: void gimbalPitchChanged (); void gimbalYawChanged (); void gimbalDataChanged (); + void isROIEnabledChanged (); private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); @@ -1481,6 +1491,7 @@ private: float _curGimbalPitch = 0.0f; float _curGinmbalYaw = 0.0f; bool _haveGimbalData = false; + bool _isROIEnabled = false; Joystick* _activeJoystick = nullptr; int _firmwareMajorVersion; diff --git a/src/api/QGCOptions.h b/src/api/QGCOptions.h index ac660fa..a290f93 100644 --- a/src/api/QGCOptions.h +++ b/src/api/QGCOptions.h @@ -49,6 +49,7 @@ public: Q_PROPERTY(QString firmwareUpgradeSingleURL READ firmwareUpgradeSingleURL CONSTANT) Q_PROPERTY(bool guidedBarShowEmergencyStop READ guidedBarShowEmergencyStop NOTIFY guidedBarShowEmergencyStopChanged) Q_PROPERTY(bool guidedBarShowOrbit READ guidedBarShowOrbit NOTIFY guidedBarShowOrbitChanged) + Q_PROPERTY(bool guidedBarShowROI READ guidedBarShowROI NOTIFY guidedBarShowROIChanged) Q_PROPERTY(bool missionWaypointsOnly READ missionWaypointsOnly NOTIFY missionWaypointsOnlyChanged) Q_PROPERTY(bool multiVehicleEnabled READ multiVehicleEnabled NOTIFY multiVehicleEnabledChanged) Q_PROPERTY(bool showOfflineMapExport READ showOfflineMapExport NOTIFY showOfflineMapExportChanged) @@ -110,6 +111,7 @@ public: virtual bool showFirmwareUpgrade () const { return true; } virtual bool guidedBarShowEmergencyStop () const { return true; } virtual bool guidedBarShowOrbit () const { return true; } + virtual bool guidedBarShowROI () const { return true; } virtual bool missionWaypointsOnly () const { return false; } ///< true: Only allow waypoints and complex items in Plan virtual bool multiVehicleEnabled () const { return true; } ///< false: multi vehicle support is disabled virtual bool guidedActionsRequireRCRSSI () const { return false; } ///< true: Guided actions will be disabled is there is no RC RSSI @@ -147,6 +149,7 @@ signals: void showFirmwareUpgradeChanged (bool show); void guidedBarShowEmergencyStopChanged (bool show); void guidedBarShowOrbitChanged (bool show); + void guidedBarShowROIChanged (bool show); void missionWaypointsOnlyChanged (bool missionWaypointsOnly); void multiVehicleEnabledChanged (bool multiVehicleEnabled); void showOfflineMapExportChanged (); diff --git a/src/ui/MainRootWindow.qml b/src/ui/MainRootWindow.qml index 21b3c87..656395e 100644 --- a/src/ui/MainRootWindow.qml +++ b/src/ui/MainRootWindow.qml @@ -629,6 +629,12 @@ ApplicationWindow { indicatorDropdown.open() } + function hidePopUp() { + indicatorDropdown.close() + indicatorDropdown.currentItem = null + indicatorDropdown.currentIndicator = null + } + Popup { id: indicatorDropdown y: ScreenTools.defaultFontPixelHeight diff --git a/src/ui/toolbar/Images/roi.svg b/src/ui/toolbar/Images/roi.svg new file mode 100644 index 0000000..6983926 --- /dev/null +++ b/src/ui/toolbar/Images/roi.svg @@ -0,0 +1,10 @@ +<?xml version="1.0" encoding="utf-8"?> +<!-- Generator: Adobe Illustrator 23.1.1, SVG Export Plug-In . SVG Version: 6.00 Build 0) --> +<svg version="1.1" id="Layer_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" + viewBox="0 0 288 288" style="enable-background:new 0 0 288 288;" xml:space="preserve"> +<path d="M188.3,144c0,24.5-19.8,44.3-44.3,44.3S99.7,168.5,99.7,144s19.8-44.3,44.3-44.3S188.3,119.5,188.3,144z M288,144 + c0,6.1-5,11.1-11.1,11.1h-28.3c-5.2,49.2-44.4,88.4-93.6,93.6v28.3c0,6.1-5,11.1-11.1,11.1s-11.1-5-11.1-11.1v-28.3 + c-49.2-5.2-88.4-44.4-93.6-93.6H11.1C5,155.1,0,150.1,0,144s5-11.1,11.1-11.1h28.3c5.2-49.2,44.4-88.4,93.6-93.6V11.1 + C132.9,5,137.9,0,144,0s11.1,5,11.1,11.1v28.3c49.2,5.2,88.4,44.4,93.6,93.6h28.3C283,132.9,288,137.9,288,144z M227.1,144 + c0-45.8-37.3-83.1-83.1-83.1S60.9,98.2,60.9,144s37.3,83.1,83.1,83.1S227.1,189.8,227.1,144z"/> +</svg> diff --git a/src/ui/toolbar/ROIIndicator.qml b/src/ui/toolbar/ROIIndicator.qml new file mode 100644 index 0000000..c6f52b9 --- /dev/null +++ b/src/ui/toolbar/ROIIndicator.qml @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * (c) 2009-2019 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org> + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + * @file + * @author Gus Grubba <gus@auterion.com> + */ + +import QtQuick 2.11 +import QtQuick.Controls 1.4 +import QtQuick.Layouts 1.11 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.MultiVehicleManager 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 + +//------------------------------------------------------------------------- +//-- ROI Indicator +Item { + id: _root + width: showIndicator ? roiIcon.width : 0 + visible: showIndicator + anchors.top: parent.top + anchors.bottom: parent.bottom + + property bool showIndicator: activeVehicle && activeVehicle.roiModeSupported + + Component { + id: roiInfo + + Rectangle { + width: roiCol.width + ScreenTools.defaultFontPixelWidth * 6 + height: roiCol.height + ScreenTools.defaultFontPixelHeight * 2 + radius: ScreenTools.defaultFontPixelHeight * 0.5 + color: qgcPal.window + + Column { + id: roiCol + spacing: ScreenTools.defaultFontPixelHeight * 0.5 + width: Math.max(roiButton.width, roiLabel.width) + anchors.margins: ScreenTools.defaultFontPixelHeight + anchors.centerIn: parent + + QGCLabel { + id: roiLabel + text: qsTr("ROI Disabled") + font.family: ScreenTools.demiboldFontFamily + visible: !roiButton.visible + anchors.horizontalCenter: parent.horizontalCenter + } + + QGCButton { + id: roiButton + visible: activeVehicle && activeVehicle.isROIEnabled + text: qsTr("Disable ROI") + onClicked: { + if(activeVehicle) + activeVehicle.stopGuidedModeROI() + mainWindow.hidePopUp() + } + } + } + } + } + + QGCColoredImage { + id: roiIcon + width: height + anchors.top: parent.top + anchors.bottom: parent.bottom + sourceSize.height: height + source: "/qmlimages/roi.svg" + color: activeVehicle && activeVehicle.isROIEnabled ? qgcPal.colorGreen : qgcPal.text + fillMode: Image.PreserveAspectFit + opacity: activeVehicle && activeVehicle.isROIEnabled ? 1 : 0.5 + } + + MouseArea { + anchors.fill: parent + onClicked: { + mainWindow.showPopUp(_root, roiInfo) + } + } +}