diff --git a/custom/herelink/VideoStreamControl.cc b/custom/herelink/VideoStreamControl.cc index a7c2eeea66..90c9000885 100644 --- a/custom/herelink/VideoStreamControl.cc +++ b/custom/herelink/VideoStreamControl.cc @@ -26,9 +26,11 @@ VideoStreamControl::VideoStreamControl() _resolutionSwitchPending = false; _currentHdmiInput = 0; // Default to HDMI 1 _currentResolution = 1; // Default to 1080p + _currentActiveSource = _videoSettings->herelinkActiveSource()->rawValue().toUInt(); connect(_videoSettings->cameraId(), &Fact::rawValueChanged, this, &VideoStreamControl::_cameraIdChanged); connect(_videoSettings->resolution(), &Fact::rawValueChanged, this, &VideoStreamControl::_resolutionChanged); + connect(_videoSettings->herelinkActiveSource(), &Fact::rawValueChanged, this, &VideoStreamControl::_activeSourceChanged); //connect(&_infoRequestTimer, &QTimer::timeout, this, &VideoStreamControl::_requestVideoStreamInfo); //_infoRequestTimer.setInterval(1000); } @@ -61,6 +63,36 @@ void VideoStreamControl::_cameraIdChanged() _setCameraIdLockUi(true); } +void VideoStreamControl::_activeSourceChanged() +{ + uint32_t source = _videoSettings->herelinkActiveSource()->rawValue().toUInt(); + qCDebug(VideoStreamControlLog) << "Active source changed to:" << source + << "(0=HDMI1, 1=HDMI2, 2=RTSP1, 3=RTSP2)"; + + if (source == _currentActiveSource) { + return; + } + _currentActiveSource = source; + emit currentActiveSourceChanged(); + + if (source < 2) { + // HDMI 1 or HDMI 2 — drive the existing cameraId path + uint32_t newCameraId = source; + if (newCameraId != _videoSettings->cameraId()->rawValue().toUInt()) { + // Cascades to _cameraIdChanged → _setCameraIdLockUi which sends VIDEO_START_STREAMING + _videoSettings->cameraId()->setRawValue(newCameraId); + } else { + // Coming back from RTSP with the same HDMI as before — cameraId hasn't changed + // so _cameraIdChanged won't fire. Re-send VIDEO_START_STREAMING in case the + // air unit's stream stopped while we were on RTSP. + _startVideoStreaming(); + } + } else { + // RTSP 1 or RTSP 2 — VideoManager handles the URI swap based on herelinkActiveSource. + qCDebug(VideoStreamControlLog) << "RTSP source selected, no MAVLink HDMI command sent"; + } +} + void VideoStreamControl::_requestVideoStreamInfo() { if (_linkInterface == NULL) { diff --git a/custom/herelink/VideoStreamControl.h b/custom/herelink/VideoStreamControl.h index 2a1c37dfa7..11d5d7b373 100644 --- a/custom/herelink/VideoStreamControl.h +++ b/custom/herelink/VideoStreamControl.h @@ -28,6 +28,9 @@ class VideoStreamControl : public QObject Q_PROPERTY(uint8_t currentResolution READ currentResolution NOTIFY currentResolutionChanged) uint8_t currentResolution() { return _currentResolution; } + Q_PROPERTY(uint8_t currentActiveSource READ currentActiveSource NOTIFY currentActiveSourceChanged) + uint8_t currentActiveSource() { return _currentActiveSource; } + signals: void settingInProgressChanged(); void videoNeedsReset(); @@ -35,10 +38,12 @@ class VideoStreamControl : public QObject void currentHdmiInputChanged(); void cameraInfoReceivedChanged(); void currentResolutionChanged(); + void currentActiveSourceChanged(); private slots: void _mavlinkMessageReceived(LinkInterface *link, mavlink_message_t message); void _cameraIdChanged(); + void _activeSourceChanged(); void _resolutionChanged(); void _requestVideoStreamInfo(); @@ -56,6 +61,7 @@ private slots: bool _resolutionSwitchPending; // Suppress encoder resets during resolution change uint8_t _currentHdmiInput; uint8_t _currentResolution; + uint8_t _currentActiveSource; // 0=HDMI1, 1=HDMI2, 2=RTSP1, 3=RTSP2 void _handleHeartbeatInfo(LinkInterface* link, mavlink_message_t& message); void _handleVideoStreamInformation(mavlink_message_t& message); diff --git a/src/FlightMap/Widgets/PhotoVideoControl.qml b/src/FlightMap/Widgets/PhotoVideoControl.qml index d4f8ea9194..a41dfb11d3 100644 --- a/src/FlightMap/Widgets/PhotoVideoControl.qml +++ b/src/FlightMap/Widgets/PhotoVideoControl.qml @@ -375,14 +375,14 @@ Rectangle { } QGCLabel { - text: qsTr("HDMI Input") + text: qsTr("Video Source") visible: QGroundControl.corePlugin.isHerelink onVisibleChanged: gridLayout.dynamicRows += visible ? 1 : -1 } QGCLabel { text: qsTr("Resolution") - visible: QGroundControl.corePlugin.isHerelink + visible: QGroundControl.corePlugin.isHerelink && _videoStreamSettings.herelinkActiveSource.rawValue < 2 onVisibleChanged: gridLayout.dynamicRows += visible ? 1 : -1 } @@ -472,27 +472,48 @@ Rectangle { spacing: ScreenTools.defaultFontPixelWidth / 2 visible: QGroundControl.corePlugin.isHerelink + property var _streamControl: QGroundControl.videoManager.videoStreamControl + QGCButton { text: qsTr("HDMI 1") - enabled: QGroundControl.videoManager.videoStreamControl.cameraInfoReceived && !QGroundControl.videoManager.videoStreamControl.settingInProgress && QGroundControl.videoManager.videoStreamControl.currentHdmiInput !== 0 + visible: _videoStreamSettings.herelinkHdmi1Enabled.rawValue + enabled: parent._streamControl.cameraInfoReceived && !parent._streamControl.settingInProgress && parent._streamControl.currentActiveSource !== 0 onClicked: { - console.log("HDMI 1 button clicked") - _videoStreamSettings.cameraId.rawValue = 0 + _videoStreamSettings.herelinkActiveSource.rawValue = 0 } } QGCButton { text: qsTr("HDMI 2") - enabled: QGroundControl.videoManager.videoStreamControl.cameraInfoReceived && !QGroundControl.videoManager.videoStreamControl.settingInProgress && QGroundControl.videoManager.videoStreamControl.currentHdmiInput !== 1 + visible: _videoStreamSettings.herelinkHdmi2Enabled.rawValue + enabled: parent._streamControl.cameraInfoReceived && !parent._streamControl.settingInProgress && parent._streamControl.currentActiveSource !== 1 + onClicked: { + _videoStreamSettings.herelinkActiveSource.rawValue = 1 + } + } + + QGCButton { + text: qsTr("RTSP 1") + visible: _videoStreamSettings.herelinkRtsp1Url.rawValue.length > 0 + enabled: parent._streamControl.currentActiveSource !== 2 onClicked: { - _videoStreamSettings.cameraId.rawValue = 1 + _videoStreamSettings.herelinkActiveSource.rawValue = 2 + } + } + + QGCButton { + text: qsTr("RTSP 2") + visible: _videoStreamSettings.herelinkRtsp2Url.rawValue.length > 0 + enabled: parent._streamControl.currentActiveSource !== 3 + onClicked: { + _videoStreamSettings.herelinkActiveSource.rawValue = 3 } } } Row { spacing: ScreenTools.defaultFontPixelWidth / 2 - visible: QGroundControl.corePlugin.isHerelink + visible: QGroundControl.corePlugin.isHerelink && _videoStreamSettings.herelinkActiveSource.rawValue < 2 QGCComboBox { model: [ qsTr("720p"), qsTr("1080p") ] diff --git a/src/Settings/Video.SettingsGroup.json b/src/Settings/Video.SettingsGroup.json index 4ea7d58063..dafe828c5f 100644 --- a/src/Settings/Video.SettingsGroup.json +++ b/src/Settings/Video.SettingsGroup.json @@ -151,6 +151,43 @@ "enumStrings": "720p,1080p", "enumValues": "0,1", "default": 1 +}, +{ + "name": "herelinkHdmi1Enabled", + "shortDesc": "Enable HDMI 1 source", + "longDesc": "When enabled, the HDMI 1 button is available in the camera config dialog.", + "type": "bool", + "default": true +}, +{ + "name": "herelinkHdmi2Enabled", + "shortDesc": "Enable HDMI 2 source", + "longDesc": "When enabled, the HDMI 2 button is available in the camera config dialog.", + "type": "bool", + "default": true +}, +{ + "name": "herelinkRtsp1Url", + "shortDesc": "RTSP 1 URL", + "longDesc": "RTSP URL for the RTSP 1 source. Leave empty to hide the RTSP 1 button. Example: rtsp://10.0.0.5:554/stream", + "type": "string", + "default": "" +}, +{ + "name": "herelinkRtsp2Url", + "shortDesc": "RTSP 2 URL", + "longDesc": "RTSP URL for the RTSP 2 source. Leave empty to hide the RTSP 2 button. Example: rtsp://10.0.0.6:554/stream", + "type": "string", + "default": "" +}, +{ + "name": "herelinkActiveSource", + "shortDesc": "Active video source", + "longDesc": "Currently selected video source: 0=HDMI 1, 1=HDMI 2, 2=RTSP 1, 3=RTSP 2.", + "type": "uint32", + "enumStrings": "HDMI 1,HDMI 2,RTSP 1,RTSP 2", + "enumValues": "0,1,2,3", + "default": 0 } ] } diff --git a/src/Settings/VideoSettings.cc b/src/Settings/VideoSettings.cc index 142bb93486..7e0bc11d3d 100644 --- a/src/Settings/VideoSettings.cc +++ b/src/Settings/VideoSettings.cc @@ -96,6 +96,12 @@ DECLARE_SETTINGSFACT(VideoSettings, disableWhenDisarmed) DECLARE_SETTINGSFACT(VideoSettings, cameraId) // Herelink resolution switching DECLARE_SETTINGSFACT(VideoSettings, resolution) +// Herelink multi-source (HDMI 1/2 + RTSP 1/2) switching +DECLARE_SETTINGSFACT(VideoSettings, herelinkHdmi1Enabled) +DECLARE_SETTINGSFACT(VideoSettings, herelinkHdmi2Enabled) +DECLARE_SETTINGSFACT(VideoSettings, herelinkRtsp1Url) +DECLARE_SETTINGSFACT(VideoSettings, herelinkRtsp2Url) +DECLARE_SETTINGSFACT(VideoSettings, herelinkActiveSource) DECLARE_SETTINGSFACT_NO_FUNC(VideoSettings, videoSource) { diff --git a/src/Settings/VideoSettings.h b/src/Settings/VideoSettings.h index e668a110b1..077a062a23 100644 --- a/src/Settings/VideoSettings.h +++ b/src/Settings/VideoSettings.h @@ -39,6 +39,12 @@ class VideoSettings : public SettingsGroup DEFINE_SETTINGFACT(cameraId) // Herelink resolution switching DEFINE_SETTINGFACT(resolution) + // Herelink multi-source (HDMI 1/2 + RTSP 1/2) switching + DEFINE_SETTINGFACT(herelinkHdmi1Enabled) + DEFINE_SETTINGFACT(herelinkHdmi2Enabled) + DEFINE_SETTINGFACT(herelinkRtsp1Url) + DEFINE_SETTINGFACT(herelinkRtsp2Url) + DEFINE_SETTINGFACT(herelinkActiveSource) Q_PROPERTY(bool streamConfigured READ streamConfigured NOTIFY streamConfiguredChanged) Q_PROPERTY(QString rtspVideoSource READ rtspVideoSource CONSTANT) diff --git a/src/UI/AppSettings/VideoSettings.qml b/src/UI/AppSettings/VideoSettings.qml index 98efdf6ff4..b131afd70b 100644 --- a/src/UI/AppSettings/VideoSettings.qml +++ b/src/UI/AppSettings/VideoSettings.qml @@ -80,6 +80,38 @@ SettingsPage { } } + SettingsGroupLayout { + Layout.fillWidth: true + heading: qsTr("Herelink Sources") + visible: QGroundControl.corePlugin.isHerelink + + FactCheckBoxSlider { + Layout.fillWidth: true + text: qsTr("HDMI 1") + fact: _videoSettings.herelinkHdmi1Enabled + } + + FactCheckBoxSlider { + Layout.fillWidth: true + text: qsTr("HDMI 2") + fact: _videoSettings.herelinkHdmi2Enabled + } + + LabelledFactTextField { + Layout.fillWidth: true + textFieldPreferredWidth: _urlFieldWidth + label: qsTr("RTSP 1 URL") + fact: _videoSettings.herelinkRtsp1Url + } + + LabelledFactTextField { + Layout.fillWidth: true + textFieldPreferredWidth: _urlFieldWidth + label: qsTr("RTSP 2 URL") + fact: _videoSettings.herelinkRtsp2Url + } + } + SettingsGroupLayout { Layout.fillWidth: true heading: qsTr("Settings") diff --git a/src/VideoManager/VideoManager.cc b/src/VideoManager/VideoManager.cc index a966189c77..aead1b5d37 100644 --- a/src/VideoManager/VideoManager.cc +++ b/src/VideoManager/VideoManager.cc @@ -108,6 +108,9 @@ void VideoManager::init(QQuickWindow *window) (void) connect(_videoSettings->udpUrl(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); (void) connect(_videoSettings->rtspUrl(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); (void) connect(_videoSettings->tcpUrl(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); + (void) connect(_videoSettings->herelinkActiveSource(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); + (void) connect(_videoSettings->herelinkRtsp1Url(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); + (void) connect(_videoSettings->herelinkRtsp2Url(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); (void) connect(_videoSettings->aspectRatio(), &Fact::rawValueChanged, this, &VideoManager::aspectRatioChanged); (void) connect(_videoSettings->lowLatencyMode(), &Fact::rawValueChanged, this, [this](const QVariant &value) { Q_UNUSED(value); _restartAllVideos(); }); (void) connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &VideoManager::_setActiveVehicle); @@ -552,7 +555,25 @@ bool VideoManager::_updateSettings(VideoReceiver *receiver) } else if (source == VideoSettings::videoSourceYuneecMantisG) { settingsChanged |= _updateVideoUri(receiver, QStringLiteral("rtsp://192.168.42.1:554/live")); } else if (source == VideoSettings::videoSourceHerelinkAirUnit) { - settingsChanged |= _updateVideoUri(receiver, QStringLiteral("rtsp://192.168.0.10:8554/H264Video")); + QString uri; + const uint32_t activeSource = _videoSettings->herelinkActiveSource()->rawValue().toUInt(); + switch (activeSource) { + case 0: + case 1: + // HDMI 1 / HDMI 2 — Air Unit multiplexes the chosen HDMI into this fixed RTSP URL + uri = QStringLiteral("rtsp://192.168.0.10:8554/H264Video"); + break; + case 2: + uri = _videoSettings->herelinkRtsp1Url()->rawValue().toString(); + break; + case 3: + uri = _videoSettings->herelinkRtsp2Url()->rawValue().toString(); + break; + default: + uri = QStringLiteral("rtsp://192.168.0.10:8554/H264Video"); + break; + } + settingsChanged |= _updateVideoUri(receiver, uri); } else if (source == VideoSettings::videoSourceHerelinkHotspot) { settingsChanged |= _updateVideoUri(receiver, QStringLiteral("rtsp://192.168.43.1:8554/fpv_stream")); } else if ((source == VideoSettings::videoDisabled) || (source == VideoSettings::videoSourceNoVideo)) {