From 73ea13a13428e642b8143b9675cf71723866d8cc Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Wed, 11 Dec 2019 14:51:19 -0500 Subject: [PATCH] Add ROI option --- QGCExternalLibs.pri | 6 +-- src/FirmwarePlugin/FirmwarePlugin.h | 1 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 5 +- src/FlightDisplay/FlightDisplayViewMap.qml | 53 +++++++++++++++++-- src/FlightDisplay/GuidedActionsController.qml | 13 +++++ src/Vehicle/Vehicle.cc | 37 +++++++++++++ src/Vehicle/Vehicle.h | 6 +++ src/api/QGCOptions.h | 3 ++ 8 files changed, 117 insertions(+), 7 deletions(-) diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index 563907468..0344f034c 100644 --- a/QGCExternalLibs.pri +++ b/QGCExternalLibs.pri @@ -147,9 +147,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/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index a6627e61f..888ac2e09 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 bd6efcbe4..426df8e79 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 214d09097..bfc467191 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -401,6 +401,46 @@ FlightMap { } } + // ROI Location visuals + MapQuickItem { + id: roiLocationItem + visible: false + 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") + } + + Connections { + target: mainWindow + onActiveVehicleChanged: { + if (!activeVehicle) { + roiLocationItem.visible = false + } + } + } + + function show(coord) { + roiLocationItem.coordinate = coord + roiLocationItem.visible = true + } + + function hide() { + roiLocationItem.visible = false + } + + function actionConfirmed() { + hide() + } + + function actionCancelled() { + hide() + } + } + // Orbit telemetry visuals QGCMapCircleVisuals { id: orbitTelemetryCircle @@ -429,9 +469,7 @@ FlightMap { QGCMenu { id: clickMenu - property var coord - QGCMenuItem { text: qsTr("Go to location") visible: guidedActionsController.showGotoLocation @@ -442,7 +480,6 @@ FlightMap { guidedActionsController.confirmAction(guidedActionsController.actionGoto, clickMenu.coord, gotoLocationItem) } } - QGCMenuItem { text: qsTr("Orbit at location") visible: guidedActionsController.showOrbit @@ -453,6 +490,16 @@ FlightMap { guidedActionsController.confirmAction(guidedActionsController.actionOrbit, clickMenu.coord, orbitMapCircle) } } + QGCMenuItem { + text: qsTr("ROI at location") + visible: guidedActionsController.showROI + + onTriggered: { + roiLocationItem.show(clickMenu.coord) + gotoLocationItem.hide() + guidedActionsController.confirmAction(guidedActionsController.actionROI, clickMenu.coord, orbitMapCircle) + } + } } onClicked: { diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 69e3b67fa..c0185989d 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,6 +94,7 @@ 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 @@ -104,6 +107,7 @@ Item { 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 @@ -129,6 +133,7 @@ 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 @@ -342,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 @@ -417,6 +427,9 @@ Item { case actionVtolTransitionToMRFlight: activeVehicle.vtolInFwdFlight = false break + case actionROI: + activeVehicle.guidedModeROI(actionData) + break default: console.warn(qsTr("Internal error: unknown actionCode"), actionCode) break diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 82d8b4586..4f424175d 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2989,6 +2989,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); @@ -3094,6 +3099,38 @@ 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(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + centerCoord.latitude(), centerCoord.longitude(), static_cast(centerCoord.altitude())); + } else { + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_LOCATION, + true, // show error if fails + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(centerCoord.latitude()), + static_cast(centerCoord.longitude()), + static_cast(centerCoord.altitude())); + } +} + void Vehicle::pauseVehicle(void) { if (!pauseVehicleSupported()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 023c5760a..6fc832b93 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -654,6 +654,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 @@ -737,6 +738,10 @@ 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); + /// 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); @@ -786,6 +791,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; diff --git a/src/api/QGCOptions.h b/src/api/QGCOptions.h index ac660fa92..a290f93d1 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 (); -- 2.22.0