diff --git a/cmake/CustomOptions.cmake b/cmake/CustomOptions.cmake index 94d26af3f558..aa6f551a4162 100644 --- a/cmake/CustomOptions.cmake +++ b/cmake/CustomOptions.cmake @@ -95,7 +95,7 @@ option(QGC_ENABLE_QT_VIDEOSTREAMING "Enable QtMultimedia video backend" OFF) # ============================================================================ set(QGC_MAVLINK_GIT_REPO "https://github.com/mavlink/mavlink.git" CACHE STRING "MAVLink repository URL") -set(QGC_MAVLINK_GIT_TAG "c409cf690454db6d3e004bd14173bc6c7ff1e0ff" CACHE STRING "MAVLink repository commit/tag") +set(QGC_MAVLINK_GIT_TAG "362f7e0f8f37cacd79f85b567a8ae6193e8afbd0" CACHE STRING "MAVLink repository commit/tag") set(QGC_MAVLINK_DIALECT "all" CACHE STRING "MAVLink dialect") set(QGC_MAVLINK_VERSION "2.0" CACHE STRING "MAVLink protocol version") diff --git a/resources/gcscontrolIndicator/gcscontrol_device.svg b/resources/gcscontrolIndicator/multigcs_aircraft.svg similarity index 55% rename from resources/gcscontrolIndicator/gcscontrol_device.svg rename to resources/gcscontrolIndicator/multigcs_aircraft.svg index fb267ffe5074..85a841045772 100644 --- a/resources/gcscontrolIndicator/gcscontrol_device.svg +++ b/resources/gcscontrolIndicator/multigcs_aircraft.svg @@ -11,10 +11,10 @@ } - + - - + + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/gcscontrol_gcs.svg b/resources/gcscontrolIndicator/multigcs_device.svg similarity index 58% rename from resources/gcscontrolIndicator/gcscontrol_gcs.svg rename to resources/gcscontrolIndicator/multigcs_device.svg index af965e771f2d..1e7a475c049f 100644 --- a/resources/gcscontrolIndicator/gcscontrol_gcs.svg +++ b/resources/gcscontrolIndicator/multigcs_device.svg @@ -11,10 +11,10 @@ } - + - + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/multigcs_device_alt.svg b/resources/gcscontrolIndicator/multigcs_device_alt.svg new file mode 100644 index 000000000000..6aafe1969d6d --- /dev/null +++ b/resources/gcscontrolIndicator/multigcs_device_alt.svg @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/gcscontrol_line.svg b/resources/gcscontrolIndicator/multigcs_line.svg similarity index 63% rename from resources/gcscontrolIndicator/gcscontrol_line.svg rename to resources/gcscontrolIndicator/multigcs_line.svg index 8e695693efd9..9b1529f5fc81 100644 --- a/resources/gcscontrolIndicator/gcscontrol_line.svg +++ b/resources/gcscontrolIndicator/multigcs_line.svg @@ -11,10 +11,10 @@ } - + - + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/multigcs_lock_closed.svg b/resources/gcscontrolIndicator/multigcs_lock_closed.svg new file mode 100644 index 000000000000..f2377409b20f --- /dev/null +++ b/resources/gcscontrolIndicator/multigcs_lock_closed.svg @@ -0,0 +1,20 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/resources/gcscontrolIndicator/multigcs_lock_open.svg b/resources/gcscontrolIndicator/multigcs_lock_open.svg new file mode 100644 index 000000000000..449fbc740bdc --- /dev/null +++ b/resources/gcscontrolIndicator/multigcs_lock_open.svg @@ -0,0 +1,20 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index 46357f841282..2897788c9697 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -1733,6 +1733,10 @@ void MockLink::_handleCommandLong(const mavlink_message_t &msg) _handleTakeoff(request); commandResult = MAV_RESULT_ACCEPTED; break; + case MAV_CMD_REQUEST_OPERATOR_CONTROL: + // Result is settable so tests can exercise the pending (FAILED) vs rejected paths + commandResult = _requestOperatorControlResult; + break; case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED: // Test command which always returns MAV_RESULT_ACCEPTED commandResult = MAV_RESULT_ACCEPTED; diff --git a/src/Comms/MockLink/MockLink.h b/src/Comms/MockLink/MockLink.h index 544feb4bc9b5..111f2e1f5c35 100644 --- a/src/Comms/MockLink/MockLink.h +++ b/src/Comms/MockLink/MockLink.h @@ -99,6 +99,8 @@ class MockLink : public LinkInterface void clearReceivedMavCommandCounts() { _receivedMavCommandCountMap.clear(); _receivedMavCommandByCompCountMap.clear(); _receivedRequestMessageByCompAndMsgCountMap.clear(); } int receivedMavCommandCount(MAV_CMD command) const { return _receivedMavCommandCountMap.value(command, 0); } int receivedMavCommandCount(MAV_CMD command, int compId) const { return _receivedMavCommandByCompCountMap.value(command).value(compId, 0); } + /// Result acked for MAV_CMD_REQUEST_OPERATOR_CONTROL (defaults to MAV_RESULT_UNSUPPORTED: not implemented) + void setRequestOperatorControlResult(MAV_RESULT result) { _requestOperatorControlResult = result; } int receivedRequestMessageCount(int compId, int messageId) const { return _receivedRequestMessageByCompAndMsgCountMap.value(compId).value(messageId, 0); } void clearReceivedRequestMessageCounts() { _receivedRequestMessageCountMap.clear(); _receivedRequestMessageByCompAndMsgCountMap.clear(); } int receivedRequestMessageCount(uint32_t messageId) const { return _receivedRequestMessageCountMap.value(messageId, 0); } @@ -384,6 +386,7 @@ private slots: bool _paramRequestReadFailureFirstAttemptPending = false; bool _hashCheckNoResponse = false; int _hashCheckRequestCount = 0; + MAV_RESULT _requestOperatorControlResult = MAV_RESULT_UNSUPPORTED; bool _paramRequestListHashCheckSent = false; bool _resetSysAutostartOnParamReset = false; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 5cf1d2d7bb5f..34b842a1597e 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -209,10 +209,7 @@ const QVariantList &FirmwarePlugin::toolIndicators(const Vehicle*) QVariant::fromValue(QUrl::fromUserInput("qrc:/qml/QGroundControl/Toolbar/EscIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/qml/QGroundControl/Toolbar/JoystickIndicator.qml")), QVariant::fromValue(QUrl::fromUserInput("qrc:/qml/QGroundControl/Toolbar/MultiVehicleSelector.qml")), -#ifdef QT_DEBUG - // ControlIndicator is only available in debug builds for the moment QVariant::fromValue(QUrl::fromUserInput("qrc:/qml/QGroundControl/Toolbar/GCSControlIndicator.qml")), -#endif }); } diff --git a/src/Toolbar/CMakeLists.txt b/src/Toolbar/CMakeLists.txt index 6a2fd68dac4f..feb21e5df5d4 100644 --- a/src/Toolbar/CMakeLists.txt +++ b/src/Toolbar/CMakeLists.txt @@ -101,7 +101,10 @@ qt_add_resources(${CMAKE_PROJECT_NAME} toolbar_gcscontrol_resources PREFIX "/gcscontrolIndicator" BASE "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator" FILES - "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/gcscontrol_device.svg" - "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/gcscontrol_gcs.svg" - "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/gcscontrol_line.svg" + "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/multigcs_aircraft.svg" + "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/multigcs_line.svg" + "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/multigcs_device.svg" + "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/multigcs_device_alt.svg" + "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/multigcs_lock_open.svg" + "${CMAKE_SOURCE_DIR}/resources/gcscontrolIndicator/multigcs_lock_closed.svg" ) diff --git a/src/Toolbar/GCSControlIndicator.qml b/src/Toolbar/GCSControlIndicator.qml index 2a4be5cb1787..e16a631a9d43 100644 --- a/src/Toolbar/GCSControlIndicator.qml +++ b/src/Toolbar/GCSControlIndicator.qml @@ -7,19 +7,32 @@ import QGroundControl.FactControls Item { id: control - width: controlIndicatorIconGCS.width * 1.1 + width: controlIndicatorIconRole.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 gcsMain: activeVehicle ? activeVehicle.gcsMain : 0 - property bool gcsControlStatusFlags_SystemManager: activeVehicle ? activeVehicle.gcsControlStatusFlags_SystemManager : false - property bool gcsControlStatusFlags_TakeoverAllowed: activeVehicle ? activeVehicle.gcsControlStatusFlags_TakeoverAllowed : false + property var gcsControlManager: activeVehicle ? activeVehicle.gcsControlManager : null + property bool showIndicator: gcsControlManager && gcsControlManager.firstControlStatusReceived + property var sysidInControl: gcsControlManager ? gcsControlManager.sysidInControl : 0 + property var secondaryGCSList: gcsControlManager ? gcsControlManager.secondaryGCSList : [] + property bool gcsControlStatusFlags_TakeoverAllowed: gcsControlManager ? gcsControlManager.gcsControlStatusFlags_TakeoverAllowed : false property Fact requestControlAllowTakeoverFact: QGroundControl.settingsManager.flyViewSettings.requestControlAllowTakeover property bool requestControlAllowTakeover: requestControlAllowTakeoverFact.rawValue - property bool isThisGCSinControl: gcsMain == QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID.rawValue - property bool sendControlRequestAllowed: activeVehicle ? activeVehicle.sendControlRequestAllowed : false + property bool isThisGCSinControl: sysidInControl == QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID.rawValue + property bool sendControlRequestAllowed: gcsControlManager ? gcsControlManager.sendControlRequestAllowed : false + // When nobody is in control (uncontrolled) or takeover is allowed, the autopilot grants control + // immediately, so there is no owner to ask and no request countdown + property bool controlGrantedImmediately: sysidInControl == 0 || gcsControlStatusFlags_TakeoverAllowed + // Someone (anyone) holds control of the vehicle + property bool someoneInControl: sysidInControl != 0 + // This GCS is a recognized secondary operator: the vehicle lists us in its secondary range. + // This holds even when uncontrolled (gcs_main == 0): a GCS within the recognized range is an + // owner that can command the vehicle, just not the one holding manual control. + property bool isThisGCSsecondary: !isThisGCSinControl && + secondaryGCSList.indexOf(Number(QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID.rawValue)) >= 0 + // This GCS has an operator role (primary or secondary) on the vehicle + property bool isThisGCSoperator: isThisGCSinControl || isThisGCSsecondary property var margins: ScreenTools.defaultFontPixelWidth property var panelRadius: ScreenTools.defaultFontPixelWidth * 0.5 @@ -30,14 +43,14 @@ Item { 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 receivedRequestTimeoutMs: QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.defaultValue * 1000 // Use this as default in case something goes wrong. Usually it will be overriden on onRequestOperatorControlReceived (defaultValue is in seconds, this is in ms) property var requestSysIdRequestingControl: 0 property var requestAllowTakeover: false signal triggerAnimations // Used to trigger animation inside the popup component Connections { - target: activeVehicle + target: gcsControlManager // Popup prompting user to accept control from other GCS function onRequestOperatorControlReceived(sysIdRequestingControl, allowTakeover, requestTimeoutSecs) { // If we don't have the indicator visible ( not receiving CONTROL_STATUS ) don't proceed @@ -47,7 +60,7 @@ Item { requestSysIdRequestingControl = sysIdRequestingControl requestAllowTakeover = allowTakeover // If request came without request timeout, use our default one - receivedRequestTimeoutMs = requestTimeoutSecs !== 0 ? requestTimeoutSecs * 1000 : QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.defaultValue + receivedRequestTimeoutMs = requestTimeoutSecs !== 0 ? requestTimeoutSecs * 1000 : QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.defaultValue * 1000 // 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 @@ -114,11 +127,11 @@ Item { Layout.alignment: Qt.AlignBottom Layout.fillHeight: true onClicked: { - control.activeVehicle.requestOperatorControl(true) // Allow takeover + control.gcsControlManager.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() + control.gcsControlManager.startTimerRevertAllowTakeover() mainWindow.showIndicatorDrawer(allowTakeoverExpirationPopup, control) } } @@ -146,16 +159,14 @@ Item { } // 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 + // that after gcsControlManager.operatorControlTakeoverTimeoutMsecs, 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 + timeoutSeconds: control.gcsControlManager.operatorControlTakeoverTimeoutMsecs * 0.001 onTimeout: { mainWindow.closeIndicatorDrawer() } @@ -199,6 +210,7 @@ Item { id: controlPopup ToolIndicatorPage { + showExpand: true TimedProgressTracker { id: sendRequestProgressTracker @@ -211,13 +223,27 @@ Item { sendRequestProgressTracker.stop() } } + // Also stop it if we gained control, or the vehicle became uncontrolled/the request expired, + // which the C++ side reports through sendControlRequestAllowed + property bool isThisGCSinControlLocal: control.isThisGCSinControl + onIsThisGCSinControlLocalChanged: { + if (isThisGCSinControlLocal && sendRequestProgressTracker.running) { + sendRequestProgressTracker.stop() + } + } + property bool sendControlRequestAllowedLocal: control.sendControlRequestAllowed + onSendControlRequestAllowedLocalChanged: { + if (sendControlRequestAllowedLocal && 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) + startProgressTracker(control.gcsControlManager.requestOperatorControlRemainingMsecs * 0.001) } } @@ -230,12 +256,36 @@ Item { id: mainLayout columns: 2 + // --- Status (read-only) --- + // 1. My situation: in control (main or secondary), else whether control is acquirable + QGCLabel { + text: qsTr("Control status:") + font.bold: true + } + QGCLabel { + text: isThisGCSinControl ? qsTr("In Control, FULL") + : (isThisGCSsecondary ? qsTr("In control, COMMANDS ONLY") + : (controlGrantedImmediately ? qsTr("Unlocked") : qsTr("Request Needed"))) + font.bold: isThisGCSoperator + color: isThisGCSoperator ? qgcPal.colorGreen : qgcPal.text + Layout.alignment: Qt.AlignRight + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + } + // 2. Takeover permission + QGCLabel { + text: controlGrantedImmediately ? qsTr("Takeover ALLOWED") : qsTr("Takeover NOT ALLOWED") + Layout.columnSpan: 2 + Layout.alignment: Qt.AlignRight + } + // 3. Ownership roster: the main GCS and any secondaries, each labelled (this GCS marked) QGCLabel { - text: qsTr("System in control: ") + text: qsTr("Main GCS:") font.bold: true } QGCLabel { - text: isThisGCSinControl ? (qsTr("This GCS") + " (" + gcsMain + ")" ) : gcsMain + text: isThisGCSinControl ? (sysidInControl + qsTr(" (This GCS)")) + : (someoneInControl ? ("" + sysidInControl) : qsTr("Nobody")) font.bold: isThisGCSinControl color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text Layout.alignment: Qt.AlignRight @@ -243,31 +293,40 @@ Item { horizontalAlignment: Text.AlignRight } QGCLabel { - text: gcsControlStatusFlags_TakeoverAllowed ? qsTr("Takeover allowed") : qsTr("Takeover NOT allowed") - Layout.columnSpan: 2 + text: qsTr("Secondary GCS:") + font.bold: true + visible: secondaryGCSList.length > 0 + } + QGCLabel { + visible: secondaryGCSList.length > 0 + color: qgcPal.text + textFormat: Text.StyledText Layout.alignment: Qt.AlignRight Layout.fillWidth: true horizontalAlignment: Text.AlignRight - color: gcsControlStatusFlags_TakeoverAllowed ? qgcPal.colorGreen : qgcPal.text + // List secondaries; colour only the entry that is this GCS green + text: { + var myId = Number(QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID.rawValue) + var out = [] + for (var i = 0; i < secondaryGCSList.length; i++) { + var id = secondaryGCSList[i] + if (id === myId) { + out.push('' + id + qsTr(" (This GCS)") + '') + } else { + out.push("" + id) + } + } + return out.join(", ") + } } // Separator Rectangle { Layout.columnSpan: 2 - Layout.preferredWidth: parent.width - Layout.alignment: Qt.AlignHCenter + Layout.fillWidth: true 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 - } + // --- Actions --- QGCLabel { id: requestSentTimeoutLabel text: qsTr("Request sent: ") + sendRequestProgressTracker.progressLabel @@ -280,12 +339,13 @@ Item { enabled: gcsControlStatusFlags_TakeoverAllowed || isThisGCSinControl } QGCButton { - text: gcsControlStatusFlags_TakeoverAllowed ? qsTr("Adquire Control") : qsTr("Send Request") + // Requesting always targets the main (full) role, so a secondary that already has + // command authority sees that this upgrades it to full/manual control + text: controlGrantedImmediately ? qsTr("Take full control") : qsTr("Request full control") onClicked: { - var timeout = gcsControlStatusFlags_TakeoverAllowed ? 0 : QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.rawValue - control.activeVehicle.requestOperatorControl(requestControlAllowTakeoverFact.rawValue, timeout) + var timeout = controlGrantedImmediately ? 0 : QGroundControl.settingsManager.flyViewSettings.requestControlTimeout.rawValue + control.gcsControlManager.requestOperatorControl(requestControlAllowTakeoverFact.rawValue, timeout) if (timeout > 0) { - // Start UI timeout animation startProgressTracker(timeout) } } @@ -293,79 +353,125 @@ Item { 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) + onClicked: control.gcsControlManager.requestOperatorControl(requestControlAllowTakeoverFact.rawValue) visible: isThisGCSinControl Layout.alignment: Qt.AlignRight enabled: gcsControlStatusFlags_TakeoverAllowed != requestControlAllowTakeoverFact.rawValue + // padding to the right, otherwise the panel will get too narrow and the UI will look inconsistent when only this button is present. + Layout.leftMargin: ScreenTools.defaultFontPixelWidth * 5 } - // Separator - Rectangle { + QGCButton { + text: qsTr("Release Control") + onClicked: { + control.gcsControlManager.releaseOperatorControl() + } Layout.columnSpan: 2 - Layout.preferredWidth: parent.width - Layout.alignment: Qt.AlignHCenter - color: qgcPal.windowShade - height: outdoorPalette ? 1 : 2 + Layout.alignment: Qt.AlignRight + visible: isThisGCSinControl } - LabelledFactTextField { - Layout.fillWidth: true - Layout.columnSpan: 2 - label: qsTr("This GCS Mavlink System ID: ") - fact: QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID + } + + // Editable settings, shown to the right of the status/actions panel via the standard + // ToolIndicatorPage expand button, matching the rest of the toolbar indicators. + // Wrapped in an Item with an explicit implicitWidth: the grid is loaded inside a Loader, + // so Layout.* on it is ignored. Without a width source the wrapping labels below + // (preferredWidth 0 + fillWidth) collapse and pile up; the fixed-width wrapper gives the + // anchored grid a stable width to wrap within regardless of which rows are visible. + expandedComponent: Item { + id: settingsRoot + implicitWidth: ScreenTools.defaultFontPixelWidth * 30 + implicitHeight: settingsLayout.implicitHeight + + GridLayout { + id: settingsLayout + anchors.left: parent.left + anchors.right: parent.right + anchors.top: parent.top + columns: 2 + + // This GCS system ID setting. Label on its own wrapping row so the long text doesn't + // drive the panel width; small field below. + QGCLabel { + text: qsTr("This GCS System ID:") + Layout.fillWidth: true + Layout.preferredWidth: 0 + wrapMode: Text.WordWrap + } + FactTextField { + fact: QGroundControl.settingsManager.mavlinkSettings.gcsMavlinkSystemID + Layout.preferredWidth: ScreenTools.defaultFontPixelWidth * 8 + Layout.alignment: Qt.AlignRight + } + // Request timeout setting (same wrapping-label treatment) + QGCLabel { + text: qsTr("Takeover request timeout (s):") + Layout.fillWidth: true + Layout.preferredWidth: 0 + wrapMode: Text.WordWrap + } + FactTextField { + fact: QGroundControl.settingsManager.flyViewSettings.requestControlTimeout + Layout.preferredWidth: ScreenTools.defaultFontPixelWidth * 8 + Layout.alignment: Qt.AlignRight + } } } } } - // Actual top toolbar indicator + // Actual top toolbar indicator. Three stacked layers occupying different quadrants: + // aircraft (top-left) - green when the vehicle has a controller or this GCS is an operator + // line (bottom-left) - green only when THIS GCS has an operator role (its control link) + // role glyph (right) - solid device = primary, outlined device = secondary, + // lock open/closed = no role (open when control is acquirable now). + // Always white: it's your station; green is reserved for the control + // relationship (aircraft + line + MAIN/SEC label). QGCColoredImage { - id: controlIndicatorIconLine + id: controlIndicatorIconAircraft width: height anchors.top: parent.top anchors.bottom: parent.bottom - source: "/gcscontrolIndicator/gcscontrol_line.svg" + source: "/gcscontrolIndicator/multigcs_aircraft.svg" fillMode: Image.PreserveAspectFit sourceSize.height: height - color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text + // Green when the vehicle has a controller (a main) or when this GCS is itself an operator + // (covers the gcs_main==0 case where this GCS is a recognized secondary) + color: (someoneInControl || isThisGCSoperator) ? qgcPal.colorGreen : qgcPal.text } QGCColoredImage { - id: controlIndicatorIconAircraft + id: controlIndicatorIconLine width: height anchors.top: parent.top anchors.bottom: parent.bottom - source: "/gcscontrolIndicator/gcscontrol_device.svg" + source: "/gcscontrolIndicator/multigcs_line.svg" fillMode: Image.PreserveAspectFit sourceSize.height: height - color: (isThisGCSinControl || gcsControlStatusFlags_TakeoverAllowed) ? qgcPal.colorGreen : qgcPal.text + color: isThisGCSoperator ? qgcPal.colorGreen : qgcPal.text } QGCColoredImage { - id: controlIndicatorIconGCS + id: controlIndicatorIconRole width: height anchors.top: parent.top anchors.bottom: parent.bottom - source: "/gcscontrolIndicator/gcscontrol_gcs.svg" + source: isThisGCSoperator + ? (isThisGCSinControl ? "/gcscontrolIndicator/multigcs_device.svg" + : "/gcscontrolIndicator/multigcs_device_alt.svg") + : (controlGrantedImmediately ? "/gcscontrolIndicator/multigcs_lock_open.svg" + : "/gcscontrolIndicator/multigcs_lock_closed.svg") fillMode: Image.PreserveAspectFit sourceSize.height: height color: qgcPal.text - // Current GCS in control indicator + // MAIN/SEC role label, only shown when this GCS has an operator role QGCLabel { - id: gcsInControlIndicator - text: gcsMain + id: roleLabel + text: isThisGCSinControl ? qsTr("MAIN") : qsTr("SEC") + visible: isThisGCSoperator font.bold: true - font.pointSize: ScreenTools.smallFontPointSize * 1.1 - color: isThisGCSinControl ? qgcPal.colorGreen : qgcPal.text + font.pointSize: ScreenTools.smallFontPointSize + color: qgcPal.colorGreen anchors.bottom: parent.bottom anchors.bottomMargin: -margins * 0.7 anchors.right: parent.right diff --git a/src/Vehicle/CMakeLists.txt b/src/Vehicle/CMakeLists.txt index 6384f82f9754..49f13edc8cc7 100644 --- a/src/Vehicle/CMakeLists.txt +++ b/src/Vehicle/CMakeLists.txt @@ -11,6 +11,8 @@ target_sources(${CMAKE_PROJECT_NAME} FTPController.h FTPManager.cc FTPManager.h + GCSControlManager.cc + GCSControlManager.h InitialConnectStateMachine.cc InitialConnectStateMachine.h MavCommandQueue.cc diff --git a/src/Vehicle/GCSControlManager.cc b/src/Vehicle/GCSControlManager.cc new file mode 100644 index 000000000000..31565e16e176 --- /dev/null +++ b/src/Vehicle/GCSControlManager.cc @@ -0,0 +1,286 @@ +#include "GCSControlManager.h" +#include "Vehicle.h" +#include "MultiVehicleManager.h" +#include "VehicleLinkManager.h" +#include "MAVLinkProtocol.h" +#include "SettingsManager.h" +#include "FlyViewSettings.h" +#include "AppMessages.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(GCSControlManagerLog, "Vehicle.GCSControlManager") + +#define REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS 10000 + +GCSControlManager::GCSControlManager(Vehicle* vehicle) + : QObject(vehicle) + , _vehicle(vehicle) +{ +} + +int GCSControlManager::operatorControlTakeoverTimeoutMsecs() const +{ + return REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS; +} + +void GCSControlManager::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 GCSControlManager::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 { + safeRequestTimeoutSecs = SettingsManager::instance()->flyViewSettings()->requestControlTimeout()->cookedDefaultValue().toInt(); + } + + // Secondary/owner membership is the flight stack's configuration: the request + // only asks for the gcs_main role for this GCS, it never carries a range. + const VehicleTypes::MavCmdAckHandlerInfo_t handlerInfo = {&GCSControlManager::_requestOperatorControlAckHandler, this, nullptr, nullptr}; + _vehicle->sendMavCommandWithHandler( + &handlerInfo, + (_operatorControlCompId != 0) ? _operatorControlCompId : _vehicle->defaultComponentId(), + MAV_CMD_REQUEST_OPERATOR_CONTROL, + 1, // param1: Action - 1: Request control + allowOverride ? 1.0f : 0.0f, // param2: Allow takeover + static_cast(safeRequestTimeoutSecs), // param3: Timeout in seconds + static_cast(MAVLinkProtocol::instance()->getSystemId()), // param4: GCS sysid requesting control + 0 // param5: unused (removed from spec) + ); + + if (safeRequestTimeoutSecs > 0) { + _requestOperatorControlStartTimer(safeRequestTimeoutSecs * 1000); + } +} + +void GCSControlManager::releaseOperatorControl() +{ + // Releasing control makes any pending request countdown or takeover revert meaningless + _timerRevertAllowTakeover.stop(); + _cancelRequestOperatorControlCountdown(); + + const VehicleTypes::MavCmdAckHandlerInfo_t handlerInfo = {&GCSControlManager::_requestOperatorControlAckHandler, this, nullptr, nullptr}; + _vehicle->sendMavCommandWithHandler( + &handlerInfo, + (_operatorControlCompId != 0) ? _operatorControlCompId : _vehicle->defaultComponentId(), + MAV_CMD_REQUEST_OPERATOR_CONTROL, + 0, // param1: Action - 0: Release control + 0, // param2: Allow takeover (irrelevant for release) + 0, // param3: Timeout (irrelevant for release) + static_cast(MAVLinkProtocol::instance()->getSystemId()), // param4: GCS sysid + 0 // param5: unused (removed from spec) + ); +} + +void GCSControlManager::_requestOperatorControlAckHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, VehicleTypes::MavCmdResultFailureCode_t failureCode) +{ + // COMMAND_ACK may come from the system-manager component (not necessarily the autopilot) + Q_UNUSED(compId); + + // If duplicated or no response, show popup to user. Otherwise only log it. + switch (failureCode) { + case VehicleTypes::MavCmdResultFailureDuplicateCommand: + QGC::showAppMessage(tr("Waiting for previous operator control request")); + return; + case VehicleTypes::MavCmdResultFailureNoResponseToCommand: + QGC::showAppMessage(tr("No response to operator control request")); + return; + default: + break; + } + + GCSControlManager* manager = static_cast(resultHandlerData); + if (!manager) { + return; + } + + switch (ack.result) { + case MAV_RESULT_ACCEPTED: + qCDebug(GCSControlManagerLog) << "Operator control request accepted"; + break; + case MAV_RESULT_FAILED: + case MAV_RESULT_IN_PROGRESS: + // The owner was notified and the request is genuinely pending vehicle-side, so keep the + // request-lockout countdown running until it is granted or times out. + qCDebug(GCSControlManagerLog) << "Operator control request pending owner response"; + break; + case MAV_RESULT_DENIED: + // Requester is not an authorized operator of the vehicle, so nothing is pending + // vehicle-side. Cancel the local countdown so the UI stops showing a bogus wait. + manager->_cancelRequestOperatorControlCountdown(); + QGC::showAppMessage(tr("Control request denied: this ground station is not an authorized operator of vehicle %1").arg(manager->_vehicle->id())); + break; + default: + // Any other rejection (unsupported, temporarily rejected, ...) is not pending either. + manager->_cancelRequestOperatorControlCountdown(); + QGC::showAppMessage(tr("Control request rejected by vehicle (%1)").arg(static_cast(ack.result))); + break; + } +} + +void GCSControlManager::_cancelRequestOperatorControlCountdown() +{ + _timerRequestOperatorControl.stop(); + disconnect(&_timerRequestOperatorControl, &QTimer::timeout, nullptr, nullptr); + if (!_sendControlRequestAllowed) { + _sendControlRequestAllowed = true; + emit sendControlRequestAllowedChanged(true); + } +} + +void GCSControlManager::_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 GCSControlManager::handleControlStatus(const mavlink_message_t& message) +{ + mavlink_control_status_t controlStatus; + mavlink_msg_control_status_decode(&message, &controlStatus); + + if (!(controlStatus.flags & GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER)) { + // Per spec, a CONTROL_STATUS without the SYSTEM_MANAGER flag describes only the emitting + // component, not the system, so it must not drive system-level control state. + qCDebug(GCSControlManagerLog) << "Ignoring component-local CONTROL_STATUS (no SYSTEM_MANAGER flag) from compId" << message.compid; + return; + } + + // This component manages GCS control of the whole system. Operator control + // commands must be addressed to it, which is not necessarily the autopilot. + // Follow the most recent claimant so a manager that restarts under a new + // compid keeps receiving our requests, but make the switch visible: it can + // also mean two components are claiming SYSTEM_MANAGER, which the spec + // requires integrators to prevent. + if ((_operatorControlCompId != 0) && (message.compid != _operatorControlCompId)) { + qCWarning(GCSControlManagerLog) << "System manager component changed from compId" + << _operatorControlCompId << "to" << message.compid + << "- component restart, or two components claiming GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER?"; + } + _operatorControlCompId = message.compid; + + 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.gcs_main) { + _sysid_in_control = controlStatus.gcs_main; + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + _vehicle->setJoystickSendAllowed(_sysid_in_control == 0 || _sysid_in_control == myId); + if (_sysid_in_control != myId) { + // Control moved away from this GCS, so a pending revert to takeover not allowed no longer applies + _timerRevertAllowTakeover.stop(); + } + updateControlStatusSignals = true; + } + + QList newSecondaryList; + for (int i = 0; i < 10; i++) { + if (controlStatus.gcs_secondary[i] != 0) { + newSecondaryList.append(controlStatus.gcs_secondary[i]); + } + } + if (_secondaryGCSList != newSecondaryList) { + _secondaryGCSList = newSecondaryList; + updateControlStatusSignals = true; + } + + if (!_firstControlStatusReceived) { + _firstControlStatusReceived = true; + updateControlStatusSignals = true; + } + + if (updateControlStatusSignals) { + emit gcsControlStatusChanged(); + } + + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + if (!sendControlRequestAllowed() && (_sysid_in_control == myId || _sysid_in_control == 0 || _gcsControlStatusFlags_TakeoverAllowed)) { + _timerRequestOperatorControl.stop(); + disconnect(&_timerRequestOperatorControl, &QTimer::timeout, nullptr, nullptr); + _sendControlRequestAllowed = true; + emit sendControlRequestAllowedChanged(true); + } +} + +void GCSControlManager::handleCommandRequestOperatorControl(const mavlink_message_t& message, const mavlink_command_long_t& commandLong) +{ + // Acknowledge the takeover notification so the autopilot can clear its pending notification state + SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock(); + if (sharedLink) { + mavlink_message_t ackMessage{}; + (void) mavlink_msg_command_ack_pack_chan( + MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), + sharedLink->mavlinkChannel(), + &ackMessage, + MAV_CMD_REQUEST_OPERATOR_CONTROL, + MAV_RESULT_ACCEPTED, + 0, // progress + 0, // result_param2 + message.sysid, + message.compid); + (void) _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), ackMessage); + } + + // Only a request for control (param1 == 1) raises a takeover prompt. A forwarded release + // (param1 == 0) from a non-ArduPilot stack must not surface a bogus takeover popup. + if (static_cast(commandLong.param1) != 1) { + return; + } + + // Within our own takeover-allowed window (we just approved a request and the revert timer + // is running) the vehicle grants any takeover without asking, so a repeated or crossed + // request must not re-prompt the operator for consent they already gave. + if (_timerRevertAllowTakeover.isActive()) { + qCDebug(GCSControlManagerLog) << "Suppressing takeover prompt: takeover already allowed (revert timer active)"; + return; + } + + // The takeover popup is bound to the active vehicle only, so a request arriving on a + // non-active vehicle would otherwise be ACKed and silently dropped. Surface it at the app + // level so the operator knows to switch to that vehicle to respond. + if (MultiVehicleManager::instance()->activeVehicle() != _vehicle) { + QGC::showAppMessage(tr("GCS %1 is requesting control of vehicle %2 — switch to that vehicle to respond") + .arg(static_cast(commandLong.param4)) + .arg(_vehicle->id())); + } + + emit requestOperatorControlReceived( + static_cast(commandLong.param4), // GCS sysid requesting control + static_cast(commandLong.param2), // Allow takeover + static_cast(commandLong.param3) // Request timeout in seconds + ); +} diff --git a/src/Vehicle/GCSControlManager.h b/src/Vehicle/GCSControlManager.h new file mode 100644 index 000000000000..9708c5d10383 --- /dev/null +++ b/src/Vehicle/GCSControlManager.h @@ -0,0 +1,77 @@ +#pragma once + +#include +#include +#include +#include + +#include "MAVLinkMessageType.h" +#include "VehicleTypes.h" + +class Vehicle; + +/// Manages GCS / operator control of a single Vehicle (MAVLink #2313). +/// +/// Tracks which GCS currently holds control, the configured secondary GCS list +/// and the request / release / takeover lifecycle, and gates joystick output on +/// the owning Vehicle accordingly. Owned by and parented to that Vehicle, which +/// routes the relevant MAVLink messages here. +class GCSControlManager : public QObject +{ + Q_OBJECT + QML_ELEMENT + QML_UNCREATABLE("") + +public: + GCSControlManager(Vehicle* vehicle); + + Q_PROPERTY(uint8_t sysidInControl READ sysidInControl NOTIFY gcsControlStatusChanged) + Q_PROPERTY(QList secondaryGCSList READ secondaryGCSList 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 NOTIFY sendControlRequestAllowedChanged) + Q_PROPERTY(bool sendControlRequestAllowed READ sendControlRequestAllowed NOTIFY sendControlRequestAllowedChanged) + + Q_INVOKABLE void startTimerRevertAllowTakeover(); + Q_INVOKABLE void requestOperatorControl(bool allowOverride, int requestTimeoutSecs = 0); + Q_INVOKABLE void releaseOperatorControl(); + + uint8_t sysidInControl() const { return _sysid_in_control; } + QList secondaryGCSList() const { return _secondaryGCSList; } + 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; } + + /// Handle an incoming CONTROL_STATUS message. Called by Vehicle message routing. + void handleControlStatus(const mavlink_message_t& message); + /// Handle an incoming MAV_CMD_REQUEST_OPERATOR_CONTROL command. Called by Vehicle message routing. + void handleCommandRequestOperatorControl(const mavlink_message_t& message, const mavlink_command_long_t& commandLong); + +signals: + void gcsControlStatusChanged(); + void requestOperatorControlReceived(int sysIdRequestingControl, int allowTakeover, int requestTimeoutSecs); + void sendControlRequestAllowedChanged(bool sendControlRequestAllowed); + +private: + static void _requestOperatorControlAckHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, VehicleTypes::MavCmdResultFailureCode_t failureCode); + void _requestOperatorControlStartTimer(int requestTimeoutMsecs); + void _cancelRequestOperatorControlCountdown(); + + Vehicle* _vehicle = nullptr; + + uint8_t _sysid_in_control = 0; + uint8_t _operatorControlCompId = 0; // compid of the system manager component, learned from CONTROL_STATUS + QList _secondaryGCSList; + uint8_t _gcsControlStatusFlags = 0; + bool _gcsControlStatusFlags_SystemManager = false; + bool _gcsControlStatusFlags_TakeoverAllowed = false; + bool _firstControlStatusReceived = false; + QTimer _timerRevertAllowTakeover; + QTimer _timerRequestOperatorControl; + bool _sendControlRequestAllowed = true; +}; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index c19094d300b4..6fd70898be99 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -29,6 +29,7 @@ #include "FirmwarePlugin.h" #include "FirmwarePluginManager.h" #include "FTPManager.h" +#include "GCSControlManager.h" #include "GeoFenceManager.h" #include "ImageProtocolManager.h" #include "InitialConnectStateMachine.h" @@ -91,10 +92,6 @@ QGC_LOGGING_CATEGORY(VehicleLog, "Vehicle.Vehicle") #define DEFAULT_LAT 38.965767f #define DEFAULT_LON -120.083923f -// 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 @@ -304,6 +301,7 @@ void Vehicle::_commonInit(LinkInterface* link) // Remote ID manager might want to acces parameters so make sure to create it after _remoteIDManager = new RemoteIDManager(this); + _gcsControlManager = new GCSControlManager(this); // Flight modes can differ based on advanced mode connect(QGCCorePlugin::instance(), &QGCCorePlugin::showAdvancedUIChanged, this, &Vehicle::flightModesChanged); @@ -741,7 +739,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes break; } case MAVLINK_MSG_ID_CONTROL_STATUS: - _handleControlStatus(message); + _gcsControlManager->handleControlStatus(message); break; case MAVLINK_MSG_ID_COMMAND_LONG: _handleCommandLong(message); @@ -2977,6 +2975,10 @@ void Vehicle::clearAllParamMapRC(void) void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, float thrust, quint16 buttons, quint16 buttons2, float pitchExtension, float rollExtension, float aux1, float aux2, float aux3, float aux4, float aux5, float aux6) { + if (!_joystickSendAllowed.load(std::memory_order_acquire)) { + return; + } + SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); if (!sharedLink) { qCDebug(VehicleLog)<< "sendJoystickDataThreadSafe: primary link gone!"; @@ -3037,6 +3039,10 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo // Channels 1–4 (attitude axes) always carry UINT16_MAX (ignore) and channels 11–18 are unused. void Vehicle::sendJoystickAuxRcOverrideThreadSafe(const std::array &channelValues, const std::array &channelEnabled, bool useRcOverride) { + if (!_joystickSendAllowed.load(std::memory_order_acquire)) { + return; + } + SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); if (!sharedLink) { qCDebug(VehicleLog) << "sendJoystickAuxRcOverrideThreadSafe: primary link gone!"; @@ -3173,139 +3179,6 @@ void Vehicle::pairRX(int rxType, int rxSubType) rxSubType); } -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() == _gcsMain) { - 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: - QGC::showAppMessage(tr("Waiting for previous operator control request")); - return; - case MavCmdResultFailureNoResponseToCommand: - QGC::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 (_gcsMain != controlStatus.gcs_main) { - _gcsMain = controlStatus.gcs_main; - 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; @@ -3315,15 +3188,10 @@ void Vehicle::_handleCommandLong(const mavlink_message_t& message) return; } if (commandLong.command == MAV_CMD_REQUEST_OPERATOR_CONTROL) { - _handleCommandRequestOperatorControl(commandLong); + _gcsControlManager->handleCommandRequestOperatorControl(message, commandLong); } } -int Vehicle::operatorControlTakeoverTimeoutMsecs() const -{ - return REQUEST_OPERATOR_CONTROL_ALLOW_TAKEOVER_TIMEOUT_MSECS; -} - int32_t Vehicle::getMessageRate(uint8_t compId, uint16_t msgId) { return _messageIntervalManager->getMessageRate(compId, msgId); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2986f7f9ebf7..7b692756dc78 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -70,6 +70,7 @@ class RequestMessageCoordinator; class QGCCameraManager; class RallyPointManager; class RemoteIDManager; +class GCSControlManager; class RequestMessageTest; class RetryableRequestMessageStateTest; class SendMavCommandWithHandlerTest; @@ -97,6 +98,7 @@ class Vehicle : public VehicleFactGroup, public VehicleTypes Q_MOC_INCLUDE("QGCMapCircle.h") Q_MOC_INCLUDE("QmlObjectListModel.h") Q_MOC_INCLUDE("RemoteIDManager.h") + Q_MOC_INCLUDE("GCSControlManager.h") Q_MOC_INCLUDE("TrajectoryPoints.h") Q_MOC_INCLUDE("VehicleLinkManager.h") Q_MOC_INCLUDE("VehicleObjectAvoidance.h") @@ -229,6 +231,7 @@ class Vehicle : public VehicleFactGroup, public VehicleTypes Q_PROPERTY(VehicleObjectAvoidance* objectAvoidance READ objectAvoidance CONSTANT) Q_PROPERTY(Autotune* autotune READ autotune CONSTANT) Q_PROPERTY(RemoteIDManager* remoteIDManager READ remoteIDManager CONSTANT) + Q_PROPERTY(GCSControlManager* gcsControlManager READ gcsControlManager CONSTANT) // FactGroup object model properties @@ -578,6 +581,7 @@ class Vehicle : public VehicleFactGroup, public VehicleTypes VehicleObjectAvoidance* objectAvoidance () { return _objectAvoidance; } Autotune* autotune () const { return _autotune; } RemoteIDManager* remoteIDManager () { return _remoteIDManager; } + GCSControlManager* gcsControlManager () { return _gcsControlManager; } static void showCommandAckError(const mavlink_command_ack_t& ack); @@ -947,6 +951,7 @@ private slots: bool _allSensorsHealthy = true; VehicleSigningController* _signingController = nullptr; std::atomic _joystickAuxRcOverrideActive = false; + std::atomic _joystickSendAllowed = true; std::unique_ptr _sysStatusSensorInfo; @@ -1118,6 +1123,7 @@ private slots: InitialConnectStateMachine* _initialConnectStateMachine = nullptr; Actuators* _actuators = nullptr; RemoteIDManager* _remoteIDManager = nullptr; + GCSControlManager* _gcsControlManager = nullptr; StandardModes* _standardModes = nullptr; // All terrain query workflows (doSetHome, ROI, altAboveTerrain) live in the coordinator. @@ -1149,44 +1155,12 @@ private slots: /* 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 gcsMain READ gcsMain 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 gcsMain() const { return _gcsMain; } - 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 _gcsMain = 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); + /// Gate for joystick / RC override output. Set by GCSControlManager from the + /// MAVLink receive thread when CONTROL_STATUS changes which GCS is in control; + /// read on the joystick send path. The atomic lives here because it gates the + /// Vehicle's own send methods. Uses release/acquire ordering so the gate change + /// is properly ordered with the control status update that caused it. + void setJoystickSendAllowed(bool allowed) { _joystickSendAllowed.store(allowed, std::memory_order_release); } /*===========================================================================*/ /* STATUS TEXT HANDLER */ diff --git a/test/Vehicle/CMakeLists.txt b/test/Vehicle/CMakeLists.txt index 563c019d96e3..d653a05c61b8 100644 --- a/test/Vehicle/CMakeLists.txt +++ b/test/Vehicle/CMakeLists.txt @@ -11,6 +11,8 @@ target_sources(${CMAKE_PROJECT_NAME} FTPControllerTest.h FirmwareUpgradeControllerTest.cc FirmwareUpgradeControllerTest.h + GCSControlManagerTest.cc + GCSControlManagerTest.h InitialConnectTest.cc InitialConnectTest.h InitialConnectPeripheralStartupTest.cc @@ -46,6 +48,7 @@ add_qgc_test(APMAirframeComponentControllerTest LABELS Integration Vehicle) add_qgc_test(FTPControllerTest LABELS Integration Vehicle RESOURCE_LOCK TempFiles) add_qgc_test(FTPManagerTest LABELS Integration Vehicle) add_qgc_test(FirmwareUpgradeControllerTest LABELS Unit Vehicle) +add_qgc_test(GCSControlManagerTest LABELS Integration Vehicle) add_qgc_test(InitialConnectTest LABELS Integration Vehicle) add_qgc_test(InitialConnectPeripheralStartupTest LABELS Integration Vehicle) add_qgc_test(MAVLinkLogManagerTest LABELS Integration Vehicle) diff --git a/test/Vehicle/GCSControlManagerTest.cc b/test/Vehicle/GCSControlManagerTest.cc new file mode 100644 index 000000000000..80b6797ea84a --- /dev/null +++ b/test/Vehicle/GCSControlManagerTest.cc @@ -0,0 +1,505 @@ +#include "GCSControlManagerTest.h" + +#include +#include + +#include "MAVLinkLib.h" +#include "MAVLinkProtocol.h" +#include "Vehicle.h" + +namespace { + +mavlink_message_t makeControlStatusMessage(uint8_t systemId, uint8_t senderCompId, uint8_t flags, uint8_t gcsMain, + const QList& secondaryIds) +{ + uint8_t secondary[10] = {}; + for (int i = 0; i < secondaryIds.count() && i < 10; ++i) { + secondary[i] = static_cast(secondaryIds.at(i)); + } + + mavlink_message_t message{}; + (void) mavlink_msg_control_status_pack(systemId, senderCompId, &message, flags, gcsMain, secondary); + return message; +} + +mavlink_message_t makeForwardedRequestOperatorControlMessage(uint8_t systemId, uint8_t senderCompId, float action, + float allowTakeover, float timeoutSecs, + float requestingSysid) +{ + mavlink_message_t message{}; + (void) mavlink_msg_command_long_pack( + systemId, senderCompId, &message, static_cast(MAVLinkProtocol::instance()->getSystemId()), + static_cast(MAVLinkProtocol::getComponentId()), MAV_CMD_REQUEST_OPERATOR_CONTROL, + 0, // confirmation + action, allowTakeover, timeoutSecs, requestingSysid, 0, 0, 0); + return message; +} + +} // namespace + +void GCSControlManagerTest::_controlStatusUpdatesState() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + QVERIFY(controlManager); + QCOMPARE(controlManager->firstControlStatusReceived(), false); + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, myId, {1, 2})); + + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + QCOMPARE(controlManager->firstControlStatusReceived(), true); + QCOMPARE(controlManager->sysidInControl(), myId); + QCOMPARE(controlManager->secondaryGCSList(), QList({1, 2})); + QCOMPARE(controlManager->gcsControlStatusFlags_SystemManager(), true); + QCOMPARE(controlManager->gcsControlStatusFlags_TakeoverAllowed(), false); +} + +void GCSControlManagerTest::_controlStatusIgnoredWithoutSystemManagerFlag() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + + // A component-local CONTROL_STATUS (no SYSTEM_MANAGER flag) must be ignored. Proven by + // sending it first, then a system-level one and observing exactly one state change once + // both have necessarily been processed (messages are delivered in order over one link). + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, 0, 99, {})); + mockLink()->respondWithMavlinkMessage( + makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 5, {})); + + QVERIFY_TRUE_WAIT(controlManager->sysidInControl() == 5, TestTimeout::shortMs()); + QCOMPARE(statusSpy.count(), 1); +} + +void GCSControlManagerTest::_controlStatusTracksTakeoverAllowedFlag() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER | GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED, 0, {})); + + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + QCOMPARE(controlManager->gcsControlStatusFlags_TakeoverAllowed(), true); + QCOMPARE(controlManager->sysidInControl(), static_cast(0)); +} + +void GCSControlManagerTest::_controlStatusGatesJoystickSend() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + const uint8_t otherGcsId = (myId == 1) ? 2 : 1; + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + // Control held by another GCS: joystick output must be suppressed. + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, otherGcsId, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + mockLink()->clearReceivedMavlinkMessageCounts(); + vehicle()->sendJoystickDataThreadSafe(0, 0, 0, 0, 0, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), + qQNaN(), qQNaN()); + QTest::qWait(TestTimeout::shortMs()); + QCOMPARE(mockLink()->receivedMavlinkMessageCount(MAVLINK_MSG_ID_MANUAL_CONTROL), 0); + + // Control reverts to this GCS: joystick output must resume. + statusSpy.clear(); + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, myId, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + vehicle()->sendJoystickDataThreadSafe(0, 0, 0, 0, 0, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), + qQNaN(), qQNaN()); + QVERIFY_TRUE_WAIT(mockLink()->receivedMavlinkMessageCount(MAVLINK_MSG_ID_MANUAL_CONTROL) == 1, + TestTimeout::shortMs()); +} + +void GCSControlManagerTest::_controlStatusSecondaryListChangeEmitsSignal() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 0, {1, 2, 3})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + QCOMPARE(controlManager->secondaryGCSList(), QList({1, 2, 3})); + + // Shrinking the secondary list (e.g. a GCS deregistering) must also be observed. + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 0, {2})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 2, TestTimeout::shortMs()); + QCOMPARE(controlManager->secondaryGCSList(), QList({2})); + + // Clearing it entirely must be observed too. + mockLink()->respondWithMavlinkMessage( + makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 0, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 3, TestTimeout::shortMs()); + QCOMPARE(controlManager->secondaryGCSList(), QList()); +} + +void GCSControlManagerTest::_requestOperatorControlRejectedByVehicle() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + QVERIFY(controlManager->sendControlRequestAllowed()); + + // MockLink's default result for MAV_CMD_REQUEST_OPERATOR_CONTROL is MAV_RESULT_UNSUPPORTED + // (protocol not implemented), which the manager surfaces as a generic rejection. + expectAppMessage(QRegularExpression("Control request rejected by vehicle")); + + mockLink()->clearReceivedMavCommandCounts(); + controlManager->requestOperatorControl(false, 5); + + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); + QVERIFY_TRUE_WAIT(controlManager->sendControlRequestAllowed(), TestTimeout::shortMs()); + + verifyExpectedLogMessage(); +} + +void GCSControlManagerTest::_requestOperatorControlPendingKeepsCountdown() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + QVERIFY(controlManager->sendControlRequestAllowed()); + + // MAV_RESULT_FAILED means the owner was notified and the request is genuinely pending + // vehicle-side: unlike DENIED, the request-lockout countdown must keep running. + mockLink()->setRequestOperatorControlResult(MAV_RESULT_FAILED); + + QSignalSpy allowedSpy(controlManager, &GCSControlManager::sendControlRequestAllowedChanged); + QVERIFY(allowedSpy.isValid()); + + mockLink()->clearReceivedMavCommandCounts(); + controlManager->requestOperatorControl(false, 5); + + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); + QTest::qWait(TestTimeout::shortMs()); + + // Exactly one transition (the lockout on send); a wrongly-cancelled countdown would have + // emitted a second, re-enabling transition after the FAILED ack. + QCOMPARE(allowedSpy.count(), 1); + QCOMPARE(controlManager->sendControlRequestAllowed(), false); +} + +void GCSControlManagerTest::_requestOperatorControlAcceptedKeepsLockoutUntilConfirmed() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + QVERIFY(controlManager->sendControlRequestAllowed()); + + // MAV_RESULT_ACCEPTED means the vehicle granted the request, but the lockout only lifts + // once CONTROL_STATUS actually confirms this GCS is in control - the ack alone is not enough. + mockLink()->setRequestOperatorControlResult(MAV_RESULT_ACCEPTED); + + QSignalSpy allowedSpy(controlManager, &GCSControlManager::sendControlRequestAllowedChanged); + QVERIFY(allowedSpy.isValid()); + + mockLink()->clearReceivedMavCommandCounts(); + controlManager->requestOperatorControl(false, 30); + + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); + QTest::qWait(TestTimeout::shortMs()); + + // Exactly one transition so far (the lockout on send); the ACCEPTED ack must not itself + // re-enable requests. + QCOMPARE(allowedSpy.count(), 1); + QCOMPARE(controlManager->sendControlRequestAllowed(), false); + QVERIFY(controlManager->requestOperatorControlRemainingMsecs() > 0); + + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, myId, {})); + + QVERIFY_TRUE_WAIT(controlManager->sendControlRequestAllowed(), TestTimeout::shortMs()); + QCOMPARE(allowedSpy.count(), 2); + // QTimer::remainingTime() reports -1 once the countdown has been stopped. + QCOMPARE(controlManager->requestOperatorControlRemainingMsecs(), -1); +} + +void GCSControlManagerTest::_requestOperatorControlLockoutClearedByTakeoverAllowedFlag() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + const uint8_t otherGcsId = (myId == 1) ? 2 : 1; + + mockLink()->setRequestOperatorControlResult(MAV_RESULT_ACCEPTED); + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + controlManager->requestOperatorControl(false, 30); + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); + QTest::qWait(TestTimeout::shortMs()); + QCOMPARE(controlManager->sendControlRequestAllowed(), false); + + // Control is still held by another GCS, but TAKEOVER_ALLOWED means a request is no longer + // pointless: the lockout must clear even though sysidInControl != this GCS. + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER | GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED, otherGcsId, {})); + + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + QCOMPARE(controlManager->sysidInControl(), otherGcsId); + QVERIFY_TRUE_WAIT(controlManager->sendControlRequestAllowed(), TestTimeout::shortMs()); +} + +void GCSControlManagerTest::_requestOperatorControlLockoutClearedWhenUncontrolled() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + const uint8_t otherGcsId = (myId == 1) ? 2 : 1; + + mockLink()->setRequestOperatorControlResult(MAV_RESULT_ACCEPTED); + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + // Another GCS holds control, takeover not allowed. + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, otherGcsId, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + controlManager->requestOperatorControl(false, 30); + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); + QTest::qWait(TestTimeout::shortMs()); + QCOMPARE(controlManager->sendControlRequestAllowed(), false); + + // The controlling GCS releases: CONTROL_STATUS reports uncontrolled with takeover still + // not allowed. The lockout must clear -- requesting is meaningful again. This is the + // third re-enable condition (in-control / takeover-allowed / uncontrolled) and was the + // original stuck-countdown bug: the old check missed the sysid_in_control == 0 case. + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 0, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 2, TestTimeout::shortMs()); + QVERIFY_TRUE_WAIT(controlManager->sendControlRequestAllowed(), TestTimeout::shortMs()); +} + +void GCSControlManagerTest::_requestOperatorControlLockoutExpiresAfterTimeout() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + + // Accepted and never confirmed by CONTROL_STATUS: only the countdown timer should re-enable + // requests. Uses the minimum allowed timeout (FlyViewSettings requestControlTimeout, 3s) to + // keep this bounded. + mockLink()->setRequestOperatorControlResult(MAV_RESULT_ACCEPTED); + + controlManager->requestOperatorControl(false, 3); + QVERIFY(!controlManager->sendControlRequestAllowed()); + + QVERIFY_TRUE_WAIT(controlManager->sendControlRequestAllowed(), TestTimeout::mediumMs()); +} + +void GCSControlManagerTest::_requestOperatorControlDeniedShowsAuthorizationMessageAndClearsLockout() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + QVERIFY(controlManager->sendControlRequestAllowed()); + + // MAV_RESULT_DENIED means this GCS is not an authorized operator of the vehicle: unlike a + // pending (FAILED/IN_PROGRESS) result, the lockout must clear immediately and a distinct, + // authorization-specific message must be shown (not the generic rejection message). + mockLink()->setRequestOperatorControlResult(MAV_RESULT_DENIED); + expectAppMessage(QRegularExpression("not an authorized operator of vehicle")); + + mockLink()->clearReceivedMavCommandCounts(); + controlManager->requestOperatorControl(false, 5); + + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); + QVERIFY_TRUE_WAIT(controlManager->sendControlRequestAllowed(), TestTimeout::shortMs()); + + verifyExpectedLogMessage(); +} + +void GCSControlManagerTest::_requestOperatorControlDuplicateRequestShowsWaitingMessage() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + + // ACCEPTED so the first request's own (later, real) ack stays silent - this test is only + // about the second, duplicate call's immediate synchronous response. + mockLink()->setRequestOperatorControlResult(MAV_RESULT_ACCEPTED); + expectAppMessage(QRegularExpression("Waiting for previous operator control request")); + + mockLink()->clearReceivedMavCommandCounts(); + // Two back-to-back calls with no intervening event-loop turn: the second is guaranteed to + // find the first still pending in Vehicle's command queue and be treated as a duplicate, + // resolved synchronously without a second COMMAND_LONG reaching the wire. + controlManager->requestOperatorControl(false, 5); + controlManager->requestOperatorControl(false, 5); + + verifyExpectedLogMessage(); + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::shortMs()); +} + +void GCSControlManagerTest::_startTimerRevertAllowTakeoverRequestsOnExpiry() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + + // This GCS must be the current controller for the revert-to-no-takeover request to fire. + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, myId, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + // ACCEPTED so the auto-issued revert request's own ack stays silent - this test is only + // about the timer actually firing and issuing that request. + mockLink()->setRequestOperatorControlResult(MAV_RESULT_ACCEPTED); + mockLink()->clearReceivedMavCommandCounts(); + controlManager->startTimerRevertAllowTakeover(); + + // operatorControlTakeoverTimeoutMsecs() is a fixed 10s; wait past it for the timer to fire + // and issue a non-override request that reverts "allow takeover" back off. + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL) == 1, + TestTimeout::longMs()); +} + +void GCSControlManagerTest::_releaseOperatorControlTargetsLearnedCompId() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + constexpr uint8_t kSystemManagerCompId = MAV_COMP_ID_ONBOARD_COMPUTER; + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + mockLink()->respondWithMavlinkMessage( + makeControlStatusMessage(vehicleSysId, kSystemManagerCompId, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 0, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + mockLink()->clearReceivedMavCommandCounts(); + controlManager->releaseOperatorControl(); + + // The release must target the component that last sent CONTROL_STATUS, not the autopilot. + // (MockLink always ACKs as the autopilot component regardless of target, so the resulting + // ack is a compId mismatch from Vehicle's perspective and is not asserted on here.) + QVERIFY_TRUE_WAIT(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL, kSystemManagerCompId) == 1, + TestTimeout::shortMs()); + QCOMPARE(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL, MAV_COMP_ID_AUTOPILOT1), 0); +} + +void GCSControlManagerTest::_systemManagerCompIdChangeAdoptsNewManager() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + + // First manager claimant: the autopilot. + mockLink()->respondWithMavlinkMessage( + makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 0, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + + // The manager role moves to another component: the switch must be adopted (so requests + // keep reaching whoever actually manages control) and warned about (it can also mean two + // components are claiming SYSTEM_MANAGER, which the spec requires integrators to prevent). + // gcs_main changes alongside so the second processing is observable via the status signal. + expectLogMessage("Vehicle.GCSControlManager", QtWarningMsg, QRegularExpression("System manager component changed")); + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_ONBOARD_COMPUTER, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, 5, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 2, TestTimeout::shortMs()); + verifyExpectedLogMessage(); + + // Subsequent commands must target the new manager, not the autopilot. + mockLink()->clearReceivedMavCommandCounts(); + controlManager->releaseOperatorControl(); + QVERIFY_TRUE_WAIT( + mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL, MAV_COMP_ID_ONBOARD_COMPUTER) == 1, + TestTimeout::shortMs()); + QCOMPARE(mockLink()->receivedMavCommandCount(MAV_CMD_REQUEST_OPERATOR_CONTROL, MAV_COMP_ID_AUTOPILOT1), 0); +} + +void GCSControlManagerTest::_forwardedRequestOperatorControlEmitsSignalAndAcks() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + + QSignalSpy requestSpy(controlManager, &GCSControlManager::requestOperatorControlReceived); + QVERIFY(requestSpy.isValid()); + + mockLink()->clearReceivedMavlinkMessageCounts(); + mockLink()->respondWithMavlinkMessage(makeForwardedRequestOperatorControlMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, 1 /* request */, 1 /* allow takeover */, 30 /* timeout secs */, + 7 /* requesting sysid */)); + + QVERIFY_SIGNAL_COUNT_WAIT(requestSpy, 1, TestTimeout::shortMs()); + const QList args = requestSpy.takeFirst(); + QCOMPARE(args.at(0).toInt(), 7); + QCOMPARE(args.at(1).toInt(), 1); + QCOMPARE(args.at(2).toInt(), 30); + + // The takeover notification must always be ACKed (command receipt, per spec; the autopilot + // clears its own pending state on send and does not consume the ack). + QVERIFY_TRUE_WAIT(mockLink()->receivedMavlinkMessageCount(MAVLINK_MSG_ID_COMMAND_ACK) == 1, TestTimeout::shortMs()); +} + +void GCSControlManagerTest::_forwardedRequestSuppressedWhileTakeoverWindowActive() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + const uint8_t myId = static_cast(MAVLinkProtocol::instance()->getSystemId()); + + // This GCS holds control and has just approved a takeover (revert timer running). + QSignalSpy statusSpy(controlManager, &GCSControlManager::gcsControlStatusChanged); + QVERIFY(statusSpy.isValid()); + mockLink()->respondWithMavlinkMessage(makeControlStatusMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, + GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER, myId, {})); + QVERIFY_SIGNAL_COUNT_WAIT(statusSpy, 1, TestTimeout::shortMs()); + controlManager->startTimerRevertAllowTakeover(); + + QSignalSpy requestSpy(controlManager, &GCSControlManager::requestOperatorControlReceived); + QVERIFY(requestSpy.isValid()); + + // A repeated/crossed request arrives while the window is open: the vehicle grants any + // takeover without asking during this window, so the operator must not be re-prompted + // for consent they already gave. The notification is still ACKed (command receipt). + mockLink()->clearReceivedMavlinkMessageCounts(); + mockLink()->respondWithMavlinkMessage(makeForwardedRequestOperatorControlMessage( + vehicleSysId, MAV_COMP_ID_AUTOPILOT1, 1 /* request */, 0, 30, 7)); + + QVERIFY_TRUE_WAIT(mockLink()->receivedMavlinkMessageCount(MAVLINK_MSG_ID_COMMAND_ACK) == 1, TestTimeout::shortMs()); + QCOMPARE(requestSpy.count(), 0); +} + +void GCSControlManagerTest::_forwardedReleaseOperatorControlDoesNotEmitSignal() +{ + GCSControlManager* const controlManager = vehicle()->gcsControlManager(); + const uint8_t vehicleSysId = static_cast(mockLink()->vehicleId()); + + QSignalSpy requestSpy(controlManager, &GCSControlManager::requestOperatorControlReceived); + QVERIFY(requestSpy.isValid()); + + mockLink()->clearReceivedMavlinkMessageCounts(); + mockLink()->respondWithMavlinkMessage( + makeForwardedRequestOperatorControlMessage(vehicleSysId, MAV_COMP_ID_AUTOPILOT1, 0 /* release */, 0, 0, 7)); + + // A forwarded release is still ACKed, but must not raise a takeover prompt. + QVERIFY_TRUE_WAIT(mockLink()->receivedMavlinkMessageCount(MAVLINK_MSG_ID_COMMAND_ACK) == 1, TestTimeout::shortMs()); + QCOMPARE(requestSpy.count(), 0); +} + +UT_REGISTER_TEST(GCSControlManagerTest, TestLabel::Integration, TestLabel::Vehicle) diff --git a/test/Vehicle/GCSControlManagerTest.h b/test/Vehicle/GCSControlManagerTest.h new file mode 100644 index 000000000000..65d4057425bd --- /dev/null +++ b/test/Vehicle/GCSControlManagerTest.h @@ -0,0 +1,36 @@ +#pragma once + +#include "BaseClasses/VehicleTest.h" +#include "GCSControlManager.h" + +class GCSControlManagerTest : public VehicleTestNoInitialConnect +{ + Q_OBJECT + +public: + explicit GCSControlManagerTest(QObject* parent = nullptr) : VehicleTestNoInitialConnect(parent) + { + setAutopilotType(MAV_AUTOPILOT_INVALID); + } + +private slots: + void _controlStatusUpdatesState(); + void _controlStatusIgnoredWithoutSystemManagerFlag(); + void _controlStatusTracksTakeoverAllowedFlag(); + void _controlStatusGatesJoystickSend(); + void _controlStatusSecondaryListChangeEmitsSignal(); + void _requestOperatorControlRejectedByVehicle(); + void _requestOperatorControlPendingKeepsCountdown(); + void _requestOperatorControlAcceptedKeepsLockoutUntilConfirmed(); + void _requestOperatorControlLockoutClearedByTakeoverAllowedFlag(); + void _requestOperatorControlLockoutClearedWhenUncontrolled(); + void _requestOperatorControlLockoutExpiresAfterTimeout(); + void _requestOperatorControlDeniedShowsAuthorizationMessageAndClearsLockout(); + void _requestOperatorControlDuplicateRequestShowsWaitingMessage(); + void _startTimerRevertAllowTakeoverRequestsOnExpiry(); + void _releaseOperatorControlTargetsLearnedCompId(); + void _systemManagerCompIdChangeAdoptsNewManager(); + void _forwardedRequestOperatorControlEmitsSignalAndAcks(); + void _forwardedRequestSuppressedWhileTakeoverWindowActive(); + void _forwardedReleaseOperatorControlDoesNotEmitSignal(); +};