diff --git a/cmake/CustomOptions.cmake b/cmake/CustomOptions.cmake index f8beca5cfa6b..d04a51f026cd 100644 --- a/cmake/CustomOptions.cmake +++ b/cmake/CustomOptions.cmake @@ -35,7 +35,7 @@ option(QGC_ENABLE_QT_VIDEOSTREAMING "Enable QtMultimedia Video Backend" OFF) # Q # MAVLink set(QGC_MAVLINK_GIT_REPO "https://github.com/mavlink/c_library_v2.git" CACHE STRING "URL to MAVLink Git Repo") -set(QGC_MAVLINK_GIT_TAG "7ea034366ee7f09f3991a5b82f51f0c259023b38" CACHE STRING "Tag of MAVLink Git Repo") +set(QGC_MAVLINK_GIT_TAG "19f9955598af9a9181064619bd2e3c04bd2d848a" CACHE STRING "Tag of MAVLink Git Repo") # APM option(QGC_DISABLE_APM_MAVLINK "Disable APM Dialect" OFF) diff --git a/qgcresources.qrc b/qgcresources.qrc index 3690875865a0..9b00de938759 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -110,4 +110,9 @@ resources/gimbal/payload.svg + + resources/gcscontrolIndicator/gcscontrol_device.svg + resources/gcscontrolIndicator/gcscontrol_gcs.svg + resources/gcscontrolIndicator/gcscontrol_line.svg + diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index af1883aa9681..e74bb3f8837c 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -12,6 +12,7 @@ src/UI/toolbar/TelemetryRSSIIndicator.qml src/UI/toolbar/APMSupportForwardingIndicator.qml src/UI/toolbar/GimbalIndicator.qml + src/UI/toolbar/GCSControlIndicator.qml src/FlightDisplay/DefaultChecklist.qml @@ -208,6 +209,7 @@ src/QmlControls/VehicleRotationCal.qml src/QmlControls/VehicleSummaryRow.qml src/PlanView/VTOLLandingPatternMapVisual.qml + src/QmlControls/TimedProgressTracker.qml src/FactSystem/FactControls/AltitudeFactTextField.qml src/FactSystem/FactControls/FactBitmask.qml src/FactSystem/FactControls/FactCheckBox.qml diff --git a/resources/gcscontrolIndicator/gcscontrol_device.svg b/resources/gcscontrolIndicator/gcscontrol_device.svg new file mode 100644 index 000000000000..fb267ffe5074 --- /dev/null +++ b/resources/gcscontrolIndicator/gcscontrol_device.svg @@ -0,0 +1,20 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/gcscontrol_gcs.svg b/resources/gcscontrolIndicator/gcscontrol_gcs.svg new file mode 100644 index 000000000000..af965e771f2d --- /dev/null +++ b/resources/gcscontrolIndicator/gcscontrol_gcs.svg @@ -0,0 +1,20 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/gcscontrol_line.svg b/resources/gcscontrolIndicator/gcscontrol_line.svg new file mode 100644 index 000000000000..8e695693efd9 --- /dev/null +++ b/resources/gcscontrolIndicator/gcscontrol_line.svg @@ -0,0 +1,20 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 0e2867723f88..d2eb469c5f42 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -212,6 +212,10 @@ const QVariantList &FirmwarePlugin::toolIndicators(const Vehicle*) QVariant::fromValue(QUrl::fromUserInput("qrc:/qml/QGroundControl/Controls/BatteryIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/RemoteIDIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GimbalIndicator.qml")), +// ControlIndicator is only available in debug builds for the moment +#ifdef QT_DEBUG + QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GCSControlIndicator.qml")), +#endif }); } diff --git a/src/QmlControls/QGroundControl/Controls/qmldir b/src/QmlControls/QGroundControl/Controls/qmldir index 27243228ae61..452afe0285d8 100644 --- a/src/QmlControls/QGroundControl/Controls/qmldir +++ b/src/QmlControls/QGroundControl/Controls/qmldir @@ -130,3 +130,5 @@ ValueSlider 1.0 ValueSlider.qml VehicleMessageList 1.0 VehicleMessageList.qml VehicleRotationCal 1.0 VehicleRotationCal.qml VehicleSummaryRow 1.0 VehicleSummaryRow.qml +MAVLinkChart 1.0 MAVLinkChart.qml +TimedProgressTracker 1.0 TimedProgressTracker.qml diff --git a/src/QmlControls/TimedProgressTracker.qml b/src/QmlControls/TimedProgressTracker.qml new file mode 100644 index 000000000000..b98c7311b987 --- /dev/null +++ b/src/QmlControls/TimedProgressTracker.qml @@ -0,0 +1,74 @@ +import QtQuick + +Item { + id: root + // Setable propterties + property real timeoutSeconds: 0 + property string progressLabel: "" + property bool running: false + + // Do not set these properties + property real progress: 0 + + signal timeout() + + property double _lastUpdateTime: 0 + + Timer { + id: timeoutTimer + interval: timeoutSeconds * 1000 + repeat: false + running: running + + onTriggered: { + root.running = false + root.progress = 0 + root.progressLabel = "" + root.timeout() + } + } + + SequentialAnimation on progress { + id: progressAnimation + running: root.running + loops: 1 + + NumberAnimation { + target: root + property: "progress" + to: 1 + duration: timeoutSeconds * 1000 + } + } + + onProgressChanged: { + const currentTime = Date.now() * 0.001 + if (currentTime - _lastUpdateTime < 0.1) { + return + } + + if (running) { + var currentCount = (progress * timeoutSeconds) + progressLabel = (timeoutSeconds - currentCount).toFixed(1) + } else { + progressLabel = "" + } + + _lastUpdateTime = currentTime + } + + function start() { + running = true + progress = 0 + timeoutTimer.restart() + progressAnimation.restart() + } + + function stop() { + running = false + progress = 0 + timeoutTimer.stop() + progressAnimation.stop() + progressLabel = "" + } +} diff --git a/src/Settings/FlyView.SettingsGroup.json b/src/Settings/FlyView.SettingsGroup.json index 99e0c95bd7b1..48e54e0c3ee7 100644 --- a/src/Settings/FlyView.SettingsGroup.json +++ b/src/Settings/FlyView.SettingsGroup.json @@ -74,6 +74,20 @@ "enumStrings": "Integrated Compass & Attitude,Horizontal Compass & Attitude,Large Vertical", "enumValues": "qrc:/qml/IntegratedCompassAttitude.qml,qrc:/qml/HorizontalCompassAttitude.qml,qrc:/qml/VerticalCompassAttitude.qml", "default": "qrc:/qml/IntegratedCompassAttitude.qml" +}, +{ + "name": "requestControlAllowTakeover", + "shortDesc": "When requesting vehicle control, allow other GCS to override control automatically, or require this GCS to accept the request first.", + "type": "bool", + "default": false +}, +{ + "name": "requestControlTimeout", + "shortDesc": "Timeout in seconds before a request to a GCS to allow takeover is assumed to be rejected. This is used to display the timeout graphically on requestor and GCS in control.", + "type": "uint32", + "min": 3, + "max": 60, + "default": 10 } ] } diff --git a/src/Settings/FlyViewSettings.cc b/src/Settings/FlyViewSettings.cc index 2ffa1f024255..947ba39b5751 100644 --- a/src/Settings/FlyViewSettings.cc +++ b/src/Settings/FlyViewSettings.cc @@ -27,3 +27,5 @@ DECLARE_SETTINGSFACT(FlyViewSettings, showSimpleCameraControl) DECLARE_SETTINGSFACT(FlyViewSettings, showObstacleDistanceOverlay) DECLARE_SETTINGSFACT(FlyViewSettings, updateHomePosition) DECLARE_SETTINGSFACT(FlyViewSettings, instrumentQmlFile) +DECLARE_SETTINGSFACT(FlyViewSettings, requestControlAllowTakeover) +DECLARE_SETTINGSFACT(FlyViewSettings, requestControlTimeout) diff --git a/src/Settings/FlyViewSettings.h b/src/Settings/FlyViewSettings.h index d949bf7b05ce..20653281321d 100644 --- a/src/Settings/FlyViewSettings.h +++ b/src/Settings/FlyViewSettings.h @@ -30,4 +30,6 @@ class FlyViewSettings : public SettingsGroup DEFINE_SETTINGFACT(showObstacleDistanceOverlay) DEFINE_SETTINGFACT(updateHomePosition) DEFINE_SETTINGFACT(instrumentQmlFile) + DEFINE_SETTINGFACT(requestControlAllowTakeover) + DEFINE_SETTINGFACT(requestControlTimeout) }; diff --git a/src/UI/MainRootWindow.qml b/src/UI/MainRootWindow.qml index a9f9e3639177..a2d1fa02a669 100644 --- a/src/UI/MainRootWindow.qml +++ b/src/UI/MainRootWindow.qml @@ -677,7 +677,6 @@ ApplicationWindow { modal: true focus: true closePolicy: Popup.CloseOnEscape | Popup.CloseOnPressOutside - dim: false property var sourceComponent property var indicatorItem diff --git a/src/UI/toolbar/GCSControlIndicator.qml b/src/UI/toolbar/GCSControlIndicator.qml new file mode 100644 index 000000000000..7434ac65efbd --- /dev/null +++ b/src/UI/toolbar/GCSControlIndicator.qml @@ -0,0 +1,386 @@ +import QtQuick +import QtQuick.Layouts + +import QGroundControl +import QGroundControl.Controls +import QGroundControl.MultiVehicleManager +import QGroundControl.ScreenTools +import QGroundControl.Palette +import QGroundControl.FactSystem +import QGroundControl.FactControls + +Item { + id: control + width: controlIndicatorIconGCS.width * 1.1 + anchors.top: parent.top + anchors.bottom: parent.bottom + + property var activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property bool showIndicator: activeVehicle && activeVehicle.firstControlStatusReceived + property var sysidInControl: activeVehicle ? activeVehicle.sysidInControl : 0 + property bool gcsControlStatusFlags_SystemManager: activeVehicle ? activeVehicle.gcsControlStatusFlags_SystemManager : false + property bool gcsControlStatusFlags_TakeoverAllowed: activeVehicle ? activeVehicle.gcsControlStatusFlags_TakeoverAllowed : false + property Fact requestControlAllowTakeoverFact: QGroundControl.settingsManager.flyViewSettings.requestControlAllowTakeover + property bool requestControlAllowTakeover: requestControlAllowTakeoverFact.rawValue + property bool isThisGCSinControl: sysidInControl == QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID.rawValue + property bool sendControlRequestAllowed: activeVehicle ? activeVehicle.sendControlRequestAllowed : false + + property var margins: ScreenTools.defaultFontPixelWidth + property var panelRadius: ScreenTools.defaultFontPixelWidth * 0.5 + property var buttonHeight: height * 1.6 + property var squareButtonPadding: ScreenTools.defaultFontPixelWidth + property var separatorHeight: buttonHeight * 0.9 + property var settingsPanelVisible: false + property bool outdoorPalette: qgcPal.globalTheme === QGCPalette.Light + + // Used by control request popup, when other GCS ask us for control + property var receivedRequestTimeoutMs: QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.defaultValue // Use this as default in case something goes wrong. Usually it will be overriden on onRequestOperatorControlReceived + property var requestSysIdRequestingControl: 0 + property var requestAllowTakeover: false + + signal triggerAnimations // Used to trigger animation inside the popup component + + Connections { + target: activeVehicle + // Popup prompting user to accept control from other GCS + onRequestOperatorControlReceived: (sysIdRequestingControl, allowTakeover, requestTimeoutSecs) => { + // If we don't have the indicator visible ( not receiving CONTROL_STATUS ) don't proceed + if (!control.showIndicator) { + return + } + requestSysIdRequestingControl = sysIdRequestingControl + requestAllowTakeover = allowTakeover + // If request came without request timeout, use our default one + receivedRequestTimeoutMs = requestTimeoutSecs !== 0 ? requestTimeoutSecs * 1000 : QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.defaultValue + // First hide current popup, in case the normal control panel is visible + mainWindow.closeIndicatorDrawer() + // When showing the popup, the component will automatically start the count down in controlRequestPopup + mainWindow.showIndicatorDrawer(controlRequestPopup, control) + } + // Animation to blink indicator when any related info changes + onGcsControlStatusChanged: { + backgroundRectangle.doOpacityAnimation() + triggerAnimations() // Needed for animation inside the popup component + } + } + + // Background to have animation when current system in control changes + Rectangle { + id: backgroundRectangle + anchors.centerIn: parent + width: height + height: parent.height * 1.4 + color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text + radius: margins + opacity: 0.0 + + function doOpacityAnimation() { opacityAnimation.restart() } + SequentialAnimation on opacity { + id: opacityAnimation + running: false + loops: 1 + PropertyAnimation { to: 0.4; duration: 150 } + PropertyAnimation { to: 0.5; duration: 100 } + PropertyAnimation { to: 0.0; duration: 150 } + } + } + + // Control request popup. Appears when other GCS requests control, so operator on this one can accept or deny it + Component { + id: controlRequestPopup + ToolIndicatorPage { + + TimedProgressTracker { + id: requestProgressTracker + timeoutSeconds: receivedRequestTimeoutMs * 0.001 + onTimeout: mainWindow.closeIndicatorDrawer() + } + + Component.onCompleted: { + requestProgressTracker.start() + } + + contentComponent: GridLayout { + id: mainLayout + columns: 3 + + // Action label + QGCLabel { + font.pointSize: ScreenTools.defaultFontPointSize * 1.1 + text: qsTr("GCS ") + requestSysIdRequestingControl + qsTr(" is requesting control") + font.bold: true + Layout.columnSpan: 2 + } + QGCButton { + text: qsTr("Allow
takeover") + Layout.rowSpan: 2 + Layout.leftMargin: margins * 2 + Layout.alignment: Qt.AlignBottom + Layout.fillHeight: true + onClicked: { + control.activeVehicle.requestOperatorControl(true) // Allow takeover + mainWindow.closeIndicatorDrawer() + // After allowing takeover, if other GCS does not take control within 10 seconds + // takeover will be set to not allowed again. Notify user about this + control.activeVehicle.startTimerRevertAllowTakeover() + mainWindow.showIndicatorDrawer(allowTakeoverExpirationPopup, control) + } + } + // Action label + QGCLabel { + font.pointSize: ScreenTools.defaultFontPointSize * 1.1 + text: qsTr("Ignoring automatically in ") + requestProgressTracker.progressLabel + qsTr(" seconds") + } + QGCButton { + id: ignoreButton + text: qsTr("Ignore") + onClicked: mainWindow.closeIndicatorDrawer() + Layout.alignment: Qt.AlignHCenter + } + // Actual progress bar + Rectangle { + id: overlayRectangle + height: ScreenTools.defaultFontPixelWidth + width: parent.width * requestProgressTracker.progress + color: qgcPal.buttonHighlight + Layout.columnSpan: 2 + } + } + } + } + + // Allow takeover expiration time popup. When a request is received and takeover was allowed, this popup alerts + // that after vehicle::REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS seconds, this GCS will change back to takeover not allowed, as per mavlink specs + Component { + id: allowTakeoverExpirationPopup + + ToolIndicatorPage { + // Allow takeover expiration time popup. When a request is received and takeover was allowed, this popup alerts + // that after vehicle::REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS seconds, this GCS will change back to takeover not allowed, as per mavlink specs + TimedProgressTracker { + id: revertTakeoverProgressTracker + timeoutSeconds: control.activeVehicle.operatorControlTakeoverTimeoutMsecs * 0.001 + onTimeout: { + mainWindow.closeIndicatorDrawer() + } + } + // After accepting allow takeover after a request, we show the 10 seconds countdown after which takeover will be set again to not allowed. + // If during this time another GCS takes control, which is what we are expecting, remove this popup + property var isThisGCSinControlLocal: control.isThisGCSinControl + onIsThisGCSinControlLocalChanged: { + if (visible && !isThisGCSinControlLocal) { + mainWindow.closeIndicatorDrawer() + } + } + + Component.onCompleted: { + revertTakeoverProgressTracker.start() + } + + contentComponent: GridLayout { + id: mainLayout + columns: 3 + + // Action label + QGCLabel { + font.pointSize: ScreenTools.defaultFontPointSize * 1.1 + text: qsTr("Reverting back to takeover not allowed if GCS ") + requestSysIdRequestingControl + + qsTr(" doesn't take control in ") + revertTakeoverProgressTracker.progressLabel + + qsTr(" seconds ...") + } + QGCButton { + id: ignoreButton + text: qsTr("Ignore") + onClicked: mainWindow.closeIndicatorDrawer() + Layout.alignment: Qt.AlignHCenter + } + } + } + } + + // Popup panel, appears when clicking top control indicator + Component { + id: controlPopup + + ToolIndicatorPage { + + TimedProgressTracker { + id: sendRequestProgressTracker + timeoutSeconds: QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.rawValue + } + // If a request was sent, and we get feedback that takeover has been allowed, stop the progress tracker as the request has been granted + property var takeoverAllowedLocal: control.gcsControlStatusFlags_TakeoverAllowed + onTakeoverAllowedLocalChanged: { + if (takeoverAllowedLocal && sendRequestProgressTracker.running) { + sendRequestProgressTracker.stop() + } + } + + Component.onCompleted: { + // If send control request is not allowed it means we recently sent a request, closed the popup, and opened again + // before the other request timeout expired. This way we can keep track of the time remaining and update UI accordingly + if (!sendControlRequestAllowed) { + // vehicle.requestOperatorControlRemainingMsecs holds the time remaining for the current request + startProgressTracker(control.activeVehicle.requestOperatorControlRemainingMsecs * 0.001) + } + } + + function startProgressTracker(timeoutSeconds) { + sendRequestProgressTracker.timeoutSeconds = timeoutSeconds + sendRequestProgressTracker.start() + } + + contentComponent: GridLayout { + id: mainLayout + columns: 2 + + QGCLabel { + text: qsTr("System in control: ") + font.bold: true + } + QGCLabel { + text: isThisGCSinControl ? (qsTr("This GCS") + " (" + sysidInControl + ")" ) : sysidInControl + font.bold: isThisGCSinControl + color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text + Layout.alignment: Qt.AlignRight + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + } + QGCLabel { + text: gcsControlStatusFlags_TakeoverAllowed ? qsTr("Takeover allowed") : qsTr("Takeover NOT allowed") + Layout.columnSpan: 2 + Layout.alignment: Qt.AlignRight + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: gcsControlStatusFlags_TakeoverAllowed ? qgcPal.colorGreen : qgcPal.text + } + // Separator + Rectangle { + Layout.columnSpan: 2 + Layout.preferredWidth: parent.width + Layout.alignment: Qt.AlignHCenter + color: qgcPal.windowShade + height: outdoorPalette ? 1 : 2 + } + QGCLabel { + text: qsTr("Send Control Request:") + Layout.columnSpan: 2 + visible: !isThisGCSinControl + } + QGCLabel { + text: qsTr("Change takeover condition:") + Layout.columnSpan: 2 + visible: isThisGCSinControl + } + QGCLabel { + id: requestSentTimeoutLabel + text: qsTr("Request sent: ") + sendRequestProgressTracker.progressLabel + Layout.columnSpan: 2 + visible: sendRequestProgressTracker.running + } + FactCheckBox { + text: qsTr("Allow takeover") + fact: requestControlAllowTakeoverFact + enabled: gcsControlStatusFlags_TakeoverAllowed || isThisGCSinControl + } + QGCButton { + text: gcsControlStatusFlags_TakeoverAllowed ? qsTr("Adquire Control") : qsTr("Send Request") + onClicked: { + var timeout = gcsControlStatusFlags_TakeoverAllowed ? 0 : QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.rawValue + control.activeVehicle.requestOperatorControl(requestControlAllowTakeoverFact.rawValue, timeout) + if (timeout > 0) { + // Start UI timeout animation + startProgressTracker(timeout) + } + } + Layout.alignment: Qt.AlignRight + visible: !isThisGCSinControl + enabled: !sendRequestProgressTracker.running + } + QGCLabel { + text: qsTr("Request Timeout (sec):") + visible: !isThisGCSinControl && !gcsControlStatusFlags_TakeoverAllowed + } + FactTextField { + fact: QGroundControl.settingsManager.flyViewSettings.requestControlTimeout + visible: !isThisGCSinControl && !gcsControlStatusFlags_TakeoverAllowed + Layout.alignment: Qt.AlignRight + Layout.preferredWidth: ScreenTools.defaultFontPixelWidth * 7 + } + QGCButton { + text: qsTr("Change") + onClicked: control.activeVehicle.requestOperatorControl(requestControlAllowTakeoverFact.rawValue) + visible: isThisGCSinControl + Layout.alignment: Qt.AlignRight + enabled: gcsControlStatusFlags_TakeoverAllowed != requestControlAllowTakeoverFact.rawValue + } + // Separator + Rectangle { + Layout.columnSpan: 2 + Layout.preferredWidth: parent.width + Layout.alignment: Qt.AlignHCenter + color: qgcPal.windowShade + height: outdoorPalette ? 1 : 2 + } + LabelledFactTextField { + Layout.fillWidth: true + Layout.columnSpan: 2 + label: qsTr("This GCS Mavlink System ID: ") + fact: QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID + } + } + } + } + + // Actual top toolbar indicator + QGCColoredImage { + id: controlIndicatorIconLine + width: height + anchors.top: parent.top + anchors.bottom: parent.bottom + source: "/gcscontrolIndicator/gcscontrol_line.svg" + fillMode: Image.PreserveAspectFit + sourceSize.height: height + color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text + } + QGCColoredImage { + id: controlIndicatorIconAircraft + width: height + anchors.top: parent.top + anchors.bottom: parent.bottom + source: "/gcscontrolIndicator/gcscontrol_device.svg" + fillMode: Image.PreserveAspectFit + sourceSize.height: height + color: (isThisGCSinControl || gcsControlStatusFlags_TakeoverAllowed) ? qgcPal.colorGreen : qgcPal.text + } + QGCColoredImage { + id: controlIndicatorIconGCS + width: height + anchors.top: parent.top + anchors.bottom: parent.bottom + source: "/gcscontrolIndicator/gcscontrol_gcs.svg" + fillMode: Image.PreserveAspectFit + sourceSize.height: height + color: qgcPal.text + + // Current GCS in control indicator + QGCLabel { + id: gcsInControlIndicator + text: sysidInControl + font.bold: true + font.pointSize: ScreenTools.smallFontPointSize * 1.1 + color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text + anchors.bottom: parent.bottom + anchors.bottomMargin: -margins * 0.7 + anchors.right: parent.right + anchors.rightMargin: -margins * 0.1 + } + } + + MouseArea { + anchors.fill: parent + onClicked: { + mainWindow.showIndicatorDrawer(controlPopup, control) + } + } +} diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index cd7cf4b1547e..0b22c5589bdd 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -77,6 +77,10 @@ QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") #define SET_HOME_TERRAIN_ALT_MAX 10000 #define SET_HOME_TERRAIN_ALT_MIN -500 +// After a second GCS has requested control and we have given it permission to takeover, we will remove takeover permission automatically after this timeout +// If the second GCS didn't get control +#define REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS 10000 + const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode not supported by Vehicle."); // Standard connected vehicle @@ -647,6 +651,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _handleMessageInterval(message); break; } + case MAVLINK_MSG_ID_CONTROL_STATUS: + _handleControlStatus(message); + break; + case MAVLINK_MSG_ID_COMMAND_LONG: + _handleCommandLong(message); + break; } // This must be emitted after the vehicle processes the message. This way the vehicle state is up to date when anyone else @@ -3970,6 +3980,157 @@ void Vehicle::_handleMessageInterval(const mavlink_message_t& message) } } +void Vehicle::startTimerRevertAllowTakeover() +{ + _timerRevertAllowTakeover.stop(); + _timerRevertAllowTakeover.setSingleShot(true); + _timerRevertAllowTakeover.setInterval(operatorControlTakeoverTimeoutMsecs()); + // Disconnect any previous connections to avoid multiple handlers + disconnect(&_timerRevertAllowTakeover, &QTimer::timeout, nullptr, nullptr); + + connect(&_timerRevertAllowTakeover, &QTimer::timeout, this, [this](){ + if (MAVLinkProtocol::instance()->getSystemId() == _sysid_in_control) { + this->requestOperatorControl(false); + } + }); + _timerRevertAllowTakeover.start(); +} + +void Vehicle::requestOperatorControl(bool allowOverride, int requestTimeoutSecs) +{ + int safeRequestTimeoutSecs; + int requestTimeoutSecsMin = SettingsManager::instance()->flyViewSettings()->requestControlTimeout()->cookedMin().toInt(); + int requestTimeoutSecsMax = SettingsManager::instance()->flyViewSettings()->requestControlTimeout()->cookedMax().toInt(); + if (requestTimeoutSecs >= requestTimeoutSecsMin && requestTimeoutSecs <= requestTimeoutSecsMax) { + safeRequestTimeoutSecs = requestTimeoutSecs; + } else { + // If out of limits use default value + safeRequestTimeoutSecs = SettingsManager::instance()->flyViewSettings()->requestControlTimeout()->cookedDefaultValue().toInt(); + } + + const MavCmdAckHandlerInfo_t handlerInfo = {&Vehicle::_requestOperatorControlAckHandler, this, nullptr, nullptr}; + sendMavCommandWithHandler( + &handlerInfo, + _defaultComponentId, + MAV_CMD_REQUEST_OPERATOR_CONTROL, + 0, // System ID of GCS requesting control, 0 if it is this GCS + 1, // Action - 0: Release control, 1: Request control. + allowOverride ? 1 : 0, // Allow takeover - Enable automatic granting of ownership on request. 0: Ask current owner and reject request, 1: Allow automatic takeover. + safeRequestTimeoutSecs // Timeout in seconds before a request to a GCS to allow takeover is assumed to be rejected. This is used to display the timeout graphically on requestor and GCS in control. + ); + + // If this is a request we sent to other GCS, start timer so User can not keep sending requests until the current timeout expires + if (requestTimeoutSecs > 0) { + requestOperatorControlStartTimer(requestTimeoutSecs * 1000); + } +} + +void Vehicle::_requestOperatorControlAckHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode) +{ + // For the moment, this will always come from an autopilot, compid 1 + Q_UNUSED(compId); + + // If duplicated or no response, show popup to user. Otherwise only log it. + switch (failureCode) { + case MavCmdResultFailureDuplicateCommand: + qgcApp()->showAppMessage(tr("Waiting for previous operator control request")); + return; + case MavCmdResultFailureNoResponseToCommand: + qgcApp()->showAppMessage(tr("No response to operator control request")); + return; + default: + break; + } + + Vehicle* vehicle = static_cast(resultHandlerData); + if (!vehicle) { + return; + } + + if (ack.result == MAV_RESULT_ACCEPTED) { + qCDebug(VehicleLog) << "Operator control request accepted"; + } else { + qCDebug(VehicleLog) << "Operator control request rejected"; + } +} + +void Vehicle::requestOperatorControlStartTimer(int requestTimeoutMsecs) +{ + // First flag requests not allowed + _sendControlRequestAllowed = false; + emit sendControlRequestAllowedChanged(false); + // Setup timer to re enable it again after timeout + _timerRequestOperatorControl.stop(); + _timerRequestOperatorControl.setSingleShot(true); + _timerRequestOperatorControl.setInterval(requestTimeoutMsecs); + // Disconnect any previous connections to avoid multiple handlers + disconnect(&_timerRequestOperatorControl, &QTimer::timeout, nullptr, nullptr); + connect(&_timerRequestOperatorControl, &QTimer::timeout, this, [this](){ + _sendControlRequestAllowed = true; + emit sendControlRequestAllowedChanged(true); + }); + _timerRequestOperatorControl.start(); +} + +void Vehicle::_handleControlStatus(const mavlink_message_t& message) +{ + mavlink_control_status_t controlStatus; + mavlink_msg_control_status_decode(&message, &controlStatus); + + bool updateControlStatusSignals = false; + if (_gcsControlStatusFlags != controlStatus.flags) { + _gcsControlStatusFlags = controlStatus.flags; + _gcsControlStatusFlags_SystemManager = controlStatus.flags & GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER; + _gcsControlStatusFlags_TakeoverAllowed = controlStatus.flags & GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED; + updateControlStatusSignals = true; + } + + if (_sysid_in_control != controlStatus.sysid_in_control) { + _sysid_in_control = controlStatus.sysid_in_control; + updateControlStatusSignals = true; + } + + if (!_firstControlStatusReceived) { + _firstControlStatusReceived = true; + updateControlStatusSignals = true; + } + + if (updateControlStatusSignals) { + emit gcsControlStatusChanged(); + } + + // If we were waiting for a request to be accepted and now it was accepted, adjust flags accordingly so + // UI unlocks the request/take control button + if (!sendControlRequestAllowed() && _gcsControlStatusFlags_TakeoverAllowed) { + disconnect(&_timerRequestOperatorControl, &QTimer::timeout, nullptr, nullptr); + _sendControlRequestAllowed = true; + emit sendControlRequestAllowedChanged(true); + } +} + +void Vehicle::_handleCommandRequestOperatorControl(const mavlink_command_long_t commandLong) +{ + emit requestOperatorControlReceived(commandLong.param1, commandLong.param3, commandLong.param4); +} + +void Vehicle::_handleCommandLong(const mavlink_message_t& message) +{ + mavlink_command_long_t commandLong; + mavlink_msg_command_long_decode(&message, &commandLong); + // Ignore command if it is not targeted for us + if (commandLong.target_system != MAVLinkProtocol::instance()->getSystemId()) { + return; + } + if (commandLong.command == MAV_CMD_REQUEST_OPERATOR_CONTROL) { + _handleCommandRequestOperatorControl(commandLong); + } +} + +int Vehicle::operatorControlTakeoverTimeoutMsecs() const +{ + return REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS; +} + void Vehicle::_requestMessageMessageIntervalResultHandler(void* resultHandlerData, MAV_RESULT result, RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message) { if((result != MAV_RESULT_ACCEPTED) || (failureCode != RequestMessageNoFailure)) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index b5141d2d9aa2..bc50a5ab6d0a 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -974,6 +974,7 @@ private slots: void _handleCameraFeedback (const mavlink_message_t& message); #endif void _handleCameraImageCaptured (const mavlink_message_t& message); + void _handleCommandLong (const 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); @@ -1322,6 +1323,50 @@ private slots: /*---------------------------------------------------------------------------*/ /*===========================================================================*/ /* Status Text Handler */ +/* CONTROL STATUS HANDLER */ +/*===========================================================================*/ +public: + Q_INVOKABLE void startTimerRevertAllowTakeover(); + Q_INVOKABLE void requestOperatorControl(bool allowOverride, int requestTimeoutSecs = 0); + +private: + void _handleControlStatus(const mavlink_message_t& message); + void _handleCommandRequestOperatorControl(const mavlink_command_long_t commandLong); + static void _requestOperatorControlAckHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, MavCmdResultFailureCode_t failureCode); + + Q_PROPERTY(uint8_t sysidInControl READ sysidInControl NOTIFY gcsControlStatusChanged) + Q_PROPERTY(bool gcsControlStatusFlags_SystemManager READ gcsControlStatusFlags_SystemManager NOTIFY gcsControlStatusChanged) + Q_PROPERTY(bool gcsControlStatusFlags_TakeoverAllowed READ gcsControlStatusFlags_TakeoverAllowed NOTIFY gcsControlStatusChanged) + Q_PROPERTY(bool firstControlStatusReceived READ firstControlStatusReceived NOTIFY gcsControlStatusChanged) + Q_PROPERTY(int operatorControlTakeoverTimeoutMsecs READ operatorControlTakeoverTimeoutMsecs CONSTANT) + Q_PROPERTY(int requestOperatorControlRemainingMsecs READ requestOperatorControlRemainingMsecs CONSTANT) + Q_PROPERTY(bool sendControlRequestAllowed READ sendControlRequestAllowed NOTIFY sendControlRequestAllowedChanged) + + uint8_t sysidInControl() const { return _sysid_in_control; } + bool gcsControlStatusFlags_SystemManager() const { return _gcsControlStatusFlags_SystemManager; } + bool gcsControlStatusFlags_TakeoverAllowed() const { return _gcsControlStatusFlags_TakeoverAllowed; } + bool firstControlStatusReceived() const { return _firstControlStatusReceived; } + int operatorControlTakeoverTimeoutMsecs() const; + int requestOperatorControlRemainingMsecs() const { return _timerRequestOperatorControl.remainingTime(); } + bool sendControlRequestAllowed() const { return _sendControlRequestAllowed; } + void requestOperatorControlStartTimer(int requestTimeoutMsecs); + + uint8_t _sysid_in_control = 0; + uint8_t _gcsControlStatusFlags = 0; + bool _gcsControlStatusFlags_SystemManager = 0; + bool _gcsControlStatusFlags_TakeoverAllowed = 0; + bool _firstControlStatusReceived = false; + QTimer _timerRevertAllowTakeover; + QTimer _timerRequestOperatorControl; + bool _sendControlRequestAllowed = true; + +signals: + void gcsControlStatusChanged(); + void requestOperatorControlReceived(int sysIdRequestingControl, int allowTakeover, int requestTimeoutSecs); + void sendControlRequestAllowedChanged(bool sendControlRequestAllowed); + +/*===========================================================================*/ +/* STATUS TEXT HANDLER */ /*===========================================================================*/ private: Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) diff --git a/src/main.cc b/src/main.cc index 0a8220b91718..b599df106691 100644 --- a/src/main.cc +++ b/src/main.cc @@ -17,6 +17,8 @@ #include "QGCApplication.h" #include "QGCLogging.h" #include "CmdLineOptParser.h" +#include "SettingsManager.h" +#include "MavlinkSettings.h" #if !defined(Q_OS_ANDROID) && !defined(Q_OS_IOS) #include @@ -70,6 +72,30 @@ int WindowsCrtReportHook(int reportType, char* message, int* returnValue) int main(int argc, char *argv[]) { + bool runUnitTests = false; + bool simpleBootTest = false; + QString systemIdStr = QString(); + bool hasSystemId = false; + bool bypassRunGuard = false; + + bool stressUnitTests = false; // Stress test unit tests + bool quietWindowsAsserts = false; // Don't let asserts pop dialog boxes + QString unitTestOptions; + + CmdLineOpt_t rgCmdLineOptions[] = { +#ifdef QT_DEBUG + { "--unittest", &runUnitTests, &unitTestOptions }, + { "--unittest-stress", &stressUnitTests, &unitTestOptions }, + { "--no-windows-assert-ui", &quietWindowsAsserts, nullptr }, + { "--allow-multiple", &bypassRunGuard, nullptr }, +#endif + { "--system-id", &hasSystemId, &systemIdStr }, + { "--simple-boot-test", &simpleBootTest, nullptr }, + // Add additional command line option flags here + }; + + ParseCmdLineOptions(argc, argv, rgCmdLineOptions, std::size(rgCmdLineOptions), false); + #if !defined(Q_OS_ANDROID) && !defined(Q_OS_IOS) // We make the runguard key different for custom and non custom // builds, so they can be executed together in the same device. @@ -78,7 +104,7 @@ int main(int argc, char *argv[]) const QString runguardString = QStringLiteral("%1 RunGuardKey").arg(QGC_APP_NAME); RunGuard guard(runguardString); - if (!guard.tryToRun()) { + if (!bypassRunGuard && !guard.tryToRun()) { QApplication errorApp(argc, argv); QMessageBox::critical(nullptr, QObject::tr("Error"), QObject::tr("A second instance of %1 is already running. Please close the other instance and try again.").arg(QGC_APP_NAME) @@ -132,25 +158,7 @@ int main(int argc, char *argv[]) } #endif - bool runUnitTests = false; - bool simpleBootTest = false; - #ifdef QT_DEBUG - // We parse a small set of command line options here prior to QGCApplication in order to handle the ones - // which need to be handled before a QApplication object is started. - - bool stressUnitTests = false; // Stress test unit tests - bool quietWindowsAsserts = false; // Don't let asserts pop dialog boxes - - QString unitTestOptions; - CmdLineOpt_t rgCmdLineOptions[] = { - { "--unittest", &runUnitTests, &unitTestOptions }, - { "--unittest-stress", &stressUnitTests, &unitTestOptions }, - { "--no-windows-assert-ui", &quietWindowsAsserts, nullptr }, - // Add additional command line option flags here - }; - - ParseCmdLineOptions(argc, argv, rgCmdLineOptions, std::size(rgCmdLineOptions), false); if (stressUnitTests) { runUnitTests = true; } @@ -171,11 +179,6 @@ int main(int argc, char *argv[]) SetErrorMode(dwMode | SEM_NOGPFAULTERRORBOX); } #endif // Q_OS_WIN -#else - CmdLineOpt_t rgCmdLineOptions[] = { - { "--simple-boot-test", &simpleBootTest, nullptr }, - }; - ParseCmdLineOptions(argc, argv, rgCmdLineOptions, std::size(rgCmdLineOptions), false); #endif // QT_DEBUG QGCApplication app(argc, argv, runUnitTests, simpleBootTest); @@ -189,6 +192,18 @@ int main(int argc, char *argv[]) app.init(); + // Set system ID if specified via command line, for example --system-id:255 + if (hasSystemId) { + bool ok; + int systemId = systemIdStr.toInt(&ok); + if (ok && systemId >= 1 && systemId <= 255) { // MAVLink system IDs are 8-bit + qDebug() << "Setting MAVLink System ID to:" << systemId; + SettingsManager::instance()->mavlinkSettings()->gcsMavlinkSystemID()->setRawValue(systemId); + } else { + qDebug() << "Not setting MAVLink System ID. It must be between 0 and 255. Invalid system ID value:" << systemIdStr; + } + } + int exitCode = 0; #ifdef QGC_UNITTEST_BUILD