diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index 9ad5accf4aa78a866c04d4fb33eda72a76152381..24c76808cf7c6b737c51d28aefd104d87fcb8b54 100644 --- a/src/FlightDisplay/VideoManager.cc +++ b/src/FlightDisplay/VideoManager.cc @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2016 QGROUNDCONTROL PROJECT + * (c) 2009-2019 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -206,28 +206,28 @@ VideoManager::_videoSourceChanged() emit hasVideoChanged(); emit isGStreamerChanged(); emit isAutoStreamChanged(); - _restartVideo(); + restartVideo(); } //----------------------------------------------------------------------------- void VideoManager::_udpPortChanged() { - _restartVideo(); + restartVideo(); } //----------------------------------------------------------------------------- void VideoManager::_rtspUrlChanged() { - _restartVideo(); + restartVideo(); } //----------------------------------------------------------------------------- void VideoManager::_tcpUrlChanged() { - _restartVideo(); + restartVideo(); } //----------------------------------------------------------------------------- @@ -275,7 +275,14 @@ VideoManager::_updateSettings() return; //-- Auto discovery if(_activeVehicle && _activeVehicle->dynamicCameras()) { - QGCVideoStreamInfo* pInfo = _activeVehicle->dynamicCameras()->currentStreamInstance(); + QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); + if(pCamera) { + Fact *fact = pCamera->videoEncoding(); + if (fact) { + _videoReceiver->setVideoDecoder(static_cast(fact->rawValue().toInt())); + } + } + QGCVideoStreamInfo* pInfo = _activeVehicle->dynamicCameras()->currentStreamInstance(); if(pInfo) { qCDebug(VideoManagerLog) << "Configure primary stream: " << pInfo->uri(); switch(pInfo->type()) { @@ -329,7 +336,7 @@ VideoManager::_updateSettings() //----------------------------------------------------------------------------- void -VideoManager::_restartVideo() +VideoManager::restartVideo() { #if defined(QGC_GST_STREAMING) qCDebug(VideoManagerLog) << "Restart video streaming"; @@ -350,13 +357,13 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) if(pCamera) { pCamera->stopStream(); } - disconnect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::_restartVideo); + disconnect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo); } } _activeVehicle = vehicle; if(_activeVehicle) { if(_activeVehicle->dynamicCameras()) { - connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::_restartVideo); + connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo); QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); if(pCamera) { pCamera->resumeStream(); @@ -364,7 +371,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) } } emit autoStreamConfiguredChanged(); - _restartVideo(); + restartVideo(); } //---------------------------------------------------------------------------------------- diff --git a/src/FlightDisplay/VideoManager.h b/src/FlightDisplay/VideoManager.h index 5c84003443a49f4051d2e32b2ca172022d926aab..918b1a2695058d7707c698c2b0099847c4da2961 100644 --- a/src/FlightDisplay/VideoManager.h +++ b/src/FlightDisplay/VideoManager.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2016 QGROUNDCONTROL PROJECT + * (c) 2009-2019 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -62,6 +62,7 @@ public: double thermalHfov (); bool autoStreamConfigured(); bool hasThermal (); + void restartVideo (); VideoReceiver* videoReceiver () { return _videoReceiver; } VideoReceiver* thermalVideoReceiver () { return _thermalVideoReceiver; } @@ -99,7 +100,6 @@ private slots: void _updateUVC (); void _setActiveVehicle (Vehicle* vehicle); void _aspectRatioChanged (); - void _restartVideo (); private: void _updateSettings (); diff --git a/src/VideoStreaming/VideoReceiver.cc b/src/VideoStreaming/VideoReceiver.cc index 5e693fc07d415fdc43ec556cef4146a40fc57165..8069895cf7c17c9e3b1f0d2bce0f4da1434f3e7c 100644 --- a/src/VideoStreaming/VideoReceiver.cc +++ b/src/VideoStreaming/VideoReceiver.cc @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2016 QGROUNDCONTROL PROJECT + * (c) 2009-2019 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -58,14 +58,13 @@ VideoReceiver::VideoReceiver(QObject* parent) , _streaming(false) , _starting(false) , _stopping(false) + , _stop(true) , _sink(nullptr) , _tee(nullptr) , _pipeline(nullptr) , _pipelineStopRec(nullptr) , _videoSink(nullptr) - , _socket(nullptr) - , _serverPresent(false) - , _rtspTestInterval_ms(5000) + , _restart_time_ms(1389) , _udpReconnect_us(5000000) #endif , _videoSurface(nullptr) @@ -74,11 +73,12 @@ VideoReceiver::VideoReceiver(QObject* parent) , _videoSettings(nullptr) { _videoSurface = new VideoSurface; - _videoSettings = qgcApp()->toolbox()->settingsManager()->videoSettings(); + _videoSettings = qgcApp()->toolbox()->settingsManager()->videoSettings(); #if defined(QGC_GST_STREAMING) + setVideoDecoder(H264_SW); _setVideoSink(_videoSurface->videoSink()); - _timer.setSingleShot(true); - connect(&_timer, &QTimer::timeout, this, &VideoReceiver::_timeout); + _restart_timer.setSingleShot(true); + connect(&_restart_timer, &QTimer::timeout, this, &VideoReceiver::_restart_timeout); connect(this, &VideoReceiver::msgErrorReceived, this, &VideoReceiver::_handleError); connect(this, &VideoReceiver::msgEOSReceived, this, &VideoReceiver::_handleEOS); connect(this, &VideoReceiver::msgStateChangedReceived, this, &VideoReceiver::_handleStateChanged); @@ -91,9 +91,6 @@ VideoReceiver::~VideoReceiver() { #if defined(QGC_GST_STREAMING) stop(); - if(_socket) { - delete _socket; - } if (_videoSink) { gst_object_unref(_videoSink); } @@ -130,8 +127,7 @@ VideoReceiver::grabImage(QString imageFile) static void newPadCB(GstElement* element, GstPad* pad, gpointer data) { - gchar* name; - name = gst_pad_get_name(pad); + gchar* name = gst_pad_get_name(pad); //g_print("A new pad %s was created\n", name); GstCaps* p_caps = gst_pad_get_pad_template_caps (pad); gchar* description = gst_caps_to_string(p_caps); @@ -145,67 +141,11 @@ newPadCB(GstElement* element, GstPad* pad, gpointer data) #endif //----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) void -VideoReceiver::_connected() +VideoReceiver::_restart_timeout() { - //-- Server showed up. Now we start the stream. - _timer.stop(); - _socket->deleteLater(); - _socket = nullptr; - if(_videoSettings->streamEnabled()->rawValue().toBool()) { - _serverPresent = true; - start(); - } + qgcApp()->toolbox()->videoManager()->restartVideo(); } -#endif - -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_socketError(QAbstractSocket::SocketError socketError) -{ - Q_UNUSED(socketError); - _socket->deleteLater(); - _socket = nullptr; - //-- Try again in a while - if(_videoSettings->streamEnabled()->rawValue().toBool()) { - _timer.start(_rtspTestInterval_ms); - } -} -#endif - -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_timeout() -{ - //-- If socket is live, we got no connection nor a socket error - if(_socket) { - delete _socket; - _socket = nullptr; - } - if(_videoSettings->streamEnabled()->rawValue().toBool()) { - //-- RTSP will try to connect to the server. If it cannot connect, - // it will simply give up and never try again. Instead, we keep - // attempting a connection on this timer. Once a connection is - // found to be working, only then we actually start the stream. - QUrl url(_uri); - //-- If RTSP and no port is defined, set default RTSP port (554) - if(_uri.contains("rtsp://") && url.port() <= 0) { - url.setPort(554); - } - _socket = new QTcpSocket; - QNetworkProxy tempProxy; - tempProxy.setType(QNetworkProxy::DefaultProxy); - _socket->setProxy(tempProxy); - connect(_socket, static_cast(&QTcpSocket::error), this, &VideoReceiver::_socketError); - connect(_socket, &QTcpSocket::connected, this, &VideoReceiver::_connected); - _socket->connectToHost(url.host(), static_cast(url.port())); - _timer.start(_rtspTestInterval_ms); - } -} -#endif //----------------------------------------------------------------------------- // When we finish our pipeline will look like this: @@ -213,14 +153,12 @@ VideoReceiver::_timeout() // +-->queue-->decoder-->_videosink // | // datasource-->demux-->parser-->tee -// // ^ // | // +-Here we will later link elements for recording void VideoReceiver::start() { - qCDebug(VideoReceiverLog) << "start():" << _uri; if(qgcApp()->runningUnitTests()) { return; } @@ -229,18 +167,18 @@ VideoReceiver::start() qCDebug(VideoReceiverLog) << "start() but not enabled/configured"; return; } + #if defined(QGC_GST_STREAMING) _stop = false; - qCDebug(VideoReceiverLog) << "start()"; + qCDebug(VideoReceiverLog) << "start():" << _uri; -#if defined(QGC_GST_TAISYNC_ENABLED) && (defined(__android__) || defined(__ios__)) +#if defined(QGC_GST_TAISYNC_ENABLED) && (defined(__android__) || defined(__ios__)) //-- Taisync on iOS or Android sends a raw h.264 stream bool isTaisyncUSB = qgcApp()->toolbox()->videoManager()->isTaisync(); #else bool isTaisyncUSB = false; #endif bool isUdp = _uri.contains("udp://") && !isTaisyncUSB; - bool isRtsp = _uri.contains("rtsp://") && !isTaisyncUSB; bool isTCP = _uri.contains("tcp://") && !isTaisyncUSB; bool isMPEGTS = _uri.contains("mpegts://") && !isTaisyncUSB; @@ -259,12 +197,6 @@ VideoReceiver::start() _starting = true; - //-- For RTSP and TCP, check to see if server is there first - if(!_serverPresent && (isRtsp || isTCP)) { - _timer.start(100); - return; - } - bool running = false; bool pipelineUp = false; @@ -324,15 +256,15 @@ VideoReceiver::start() } } else { if(!isTaisyncUSB) { - if ((demux = gst_element_factory_make("rtph264depay", "rtp-h264-depacketizer")) == nullptr) { - qCritical() << "VideoReceiver::start() failed. Error with gst_element_factory_make('rtph264depay')"; + if ((demux = gst_element_factory_make(_depayName, "rtp-depacketizer")) == nullptr) { + qCritical() << "VideoReceiver::start() failed. Error with gst_element_factory_make('" << _depayName << "')"; break; } } } - if ((parser = gst_element_factory_make("h264parse", "h264-parser")) == nullptr) { - qCritical() << "VideoReceiver::start() failed. Error with gst_element_factory_make('h264parse')"; + if ((parser = gst_element_factory_make(_parserName, "parser")) == nullptr) { + qCritical() << "VideoReceiver::start() failed. Error with gst_element_factory_make('" << _parserName << "')"; break; } @@ -348,8 +280,8 @@ VideoReceiver::start() break; } - if ((decoder = gst_element_factory_make("avdec_h264", "h264-decoder")) == nullptr) { - qCritical() << "VideoReceiver::start() failed. Error with gst_element_factory_make('avdec_h264')"; + if ((decoder = gst_element_factory_make(_decoderName, "decoder")) == nullptr) { + qCritical() << "VideoReceiver::start() failed. Error with gst_element_factory_make('" << _decoderName << "')"; break; } @@ -525,7 +457,6 @@ VideoReceiver::_shutdownPipeline() { _pipeline = nullptr; delete _sink; _sink = nullptr; - _serverPresent = false; _streaming = false; _recording = false; _stopping = false; @@ -540,7 +471,7 @@ void VideoReceiver::_handleError() { qCDebug(VideoReceiverLog) << "Gstreamer error!"; stop(); - start(); + _restart_timer.start(_restart_time_ms); } #endif @@ -555,8 +486,7 @@ VideoReceiver::_handleEOS() { _shutdownRecordingBranch(); } else { qWarning() << "VideoReceiver: Unexpected EOS!"; - stop(); - start(); + _handleError(); } } #endif @@ -646,6 +576,21 @@ VideoReceiver::_cleanupOldVideos() } #endif +//----------------------------------------------------------------------------- +void +VideoReceiver::setVideoDecoder(VideoEncoding encoding) +{ + if (encoding == H265_HW || encoding == H265_SW) { + _depayName = "rtph265depay"; + _parserName = "h265parse"; + _decoderName = "avdec_h265"; + } else { + _depayName = "rtph264depay"; + _parserName = "h264parse"; + _decoderName = "avdec_h264"; + } +} + //----------------------------------------------------------------------------- // When we finish our pipeline will look like this: // @@ -682,7 +627,7 @@ VideoReceiver::startRecording(const QString &videoFile) _sink = new Sink(); _sink->teepad = gst_element_get_request_pad(_tee, "src_%u"); _sink->queue = gst_element_factory_make("queue", nullptr); - _sink->parse = gst_element_factory_make("h264parse", nullptr); + _sink->parse = gst_element_factory_make(_parserName, nullptr); _sink->mux = gst_element_factory_make(kVideoMuxes[muxIdx], nullptr); _sink->filesink = gst_element_factory_make("filesink", nullptr); _sink->removing = false; @@ -896,10 +841,10 @@ VideoReceiver::_updateTimer() { #if defined(QGC_GST_STREAMING) if(_videoSurface) { - if(stopping() || starting()) { + if(_stopping || _starting) { return; } - if(streaming()) { + if(_streaming) { if(!_videoRunning) { _videoSurface->setLastFrame(0); _videoRunning = true; @@ -927,7 +872,7 @@ VideoReceiver::_updateTimer() _stop = false; } } else { - if(!_stop && !running() && !_uri.isEmpty() && _videoSettings->streamEnabled()->rawValue().toBool()) { + if(!_stop && _running && !_uri.isEmpty() && _videoSettings->streamEnabled()->rawValue().toBool()) { start(); } } diff --git a/src/VideoStreaming/VideoReceiver.h b/src/VideoStreaming/VideoReceiver.h index 7e6bb3776d75e54279f8ae7396a03c72304b9eea..017b937b00f769ec8aecaa67308109184194a839 100644 --- a/src/VideoStreaming/VideoReceiver.h +++ b/src/VideoStreaming/VideoReceiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * (c) 2009-2018 QGROUNDCONTROL PROJECT + * (c) 2009-2019 QGROUNDCONTROL PROJECT * * QGroundControl is licensed according to the terms in the file * COPYING.md in the root of the source code directory. @@ -34,6 +34,13 @@ class VideoReceiver : public QObject { Q_OBJECT public: + enum VideoEncoding { + H264_SW = 1, + H264_HW = 2, + H265_SW = 3, + H265_HW = 4 + }; + #if defined(QGC_GST_STREAMING) Q_PROPERTY(bool recording READ recording NOTIFY recordingChanged) #endif @@ -47,11 +54,7 @@ public: ~VideoReceiver(); #if defined(QGC_GST_STREAMING) - virtual bool running () { return _running; } virtual bool recording () { return _recording; } - virtual bool streaming () { return _streaming; } - virtual bool starting () { return _starting; } - virtual bool stopping () { return _stopping; } #endif virtual VideoSurface* videoSurface () { return _videoSurface; } @@ -64,6 +67,8 @@ public: virtual void setShowFullScreen (bool show) { _showFullScreen = show; emit showFullScreenChanged(); } + void setVideoDecoder (VideoEncoding encoding); + signals: void videoRunningChanged (); void imageFileChanged (); @@ -86,9 +91,7 @@ public slots: protected slots: virtual void _updateTimer (); #if defined(QGC_GST_STREAMING) - virtual void _timeout (); - virtual void _connected (); - virtual void _socketError (QAbstractSocket::SocketError socketError); + virtual void _restart_timeout (); virtual void _handleError (); virtual void _handleEOS (); virtual void _handleStateChanged (); @@ -132,14 +135,11 @@ protected: //-- Wait for Video Server to show up before starting QTimer _frameTimer; - QTimer _timer; - QTcpSocket* _socket; - bool _serverPresent; - int _rtspTestInterval_ms; + QTimer _restart_timer; + int _restart_time_ms; //-- RTSP UDP reconnect timeout uint64_t _udpReconnect_us; - #endif QString _uri; @@ -149,5 +149,8 @@ protected: bool _videoRunning; bool _showFullScreen; VideoSettings* _videoSettings; + const char* _depayName; + const char* _parserName; + const char* _decoderName; };