From 392a7a86222425ba06b5e558b7bd2810af9bfcfb Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Sun, 6 Jan 2019 12:54:14 -0500 Subject: [PATCH] Allow assigning buttons to set zoom, cameras and video streams. --- src/FlightDisplay/VideoManager.cc | 92 +++++++++++++++++++++++++++++-- src/FlightDisplay/VideoManager.h | 30 ++++++++-- src/Joystick/Joystick.cc | 36 ++++++++---- src/Joystick/Joystick.h | 11 +++- 4 files changed, 147 insertions(+), 22 deletions(-) diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index 49b7be4e6..5451dad21 100644 --- a/src/FlightDisplay/VideoManager.cc +++ b/src/FlightDisplay/VideoManager.cc @@ -28,6 +28,7 @@ #include "MultiVehicleManager.h" #include "Settings/SettingsManager.h" #include "Vehicle.h" +#include "JoystickManager.h" QGC_LOGGING_CATEGORY(VideoManagerLog, "VideoManagerLog") @@ -36,6 +37,8 @@ VideoManager::VideoManager(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) { _streamInfo = {}; + _lastZoomChange.start(); + _lastStreamChange.start(); } //----------------------------------------------------------------------------- @@ -61,9 +64,11 @@ VideoManager::setToolbox(QGCToolbox *toolbox) connect(_videoSettings->udpPort(), &Fact::rawValueChanged, this, &VideoManager::_udpPortChanged); connect(_videoSettings->rtspUrl(), &Fact::rawValueChanged, this, &VideoManager::_rtspUrlChanged); connect(_videoSettings->tcpUrl(), &Fact::rawValueChanged, this, &VideoManager::_tcpUrlChanged); - connect(_videoSettings->aspectRatio(), &Fact::rawValueChanged, this, &VideoManager::_aspectRatioChanged); - MultiVehicleManager *manager = qgcApp()->toolbox()->multiVehicleManager(); - connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &VideoManager::_setActiveVehicle); + connect(_videoSettings->aspectRatio(), &Fact::rawValueChanged, this, &VideoManager::_streamInfoChanged); + MultiVehicleManager *pVehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + connect(pVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &VideoManager::_setActiveVehicle); + JoystickManager *pJoyMgr = qgcApp()->toolbox()->joystickManager(); + connect(pJoyMgr, &JoystickManager::activeJoystickChanged, this, &VideoManager::_activeJoystickChanged); #if defined(QGC_GST_STREAMING) #ifndef QGC_DISABLE_UVC @@ -272,16 +277,91 @@ VideoManager::_vehicleMessageReceived(const mavlink_message_t& message) { //-- For now we only handle one stream. There is no UI to pick different streams. if(message.msgid == MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION) { + _videoStreamCompID = message.compid; mavlink_msg_video_stream_information_decode(&message, &_streamInfo); + _hasAutoStream = true; qCDebug(VideoManagerLog) << "Received video stream info:" << _streamInfo.uri; _restartVideo(); - emit aspectRatioChanged(); + emit streamInfoChanged(); } } //---------------------------------------------------------------------------------------- void -VideoManager::_aspectRatioChanged() +VideoManager::_activeJoystickChanged(Joystick* joystick) { - emit aspectRatioChanged(); + if(_activeJoystick) { + disconnect(_activeJoystick, &Joystick::stepZoom, this, &VideoManager::stepZoom); + disconnect(_activeJoystick, &Joystick::stepStream, this, &VideoManager::stepStream); + } + _activeJoystick = joystick; + if(_activeJoystick) { + connect(_activeJoystick, &Joystick::stepZoom, this, &VideoManager::stepZoom); + connect(_activeJoystick, &Joystick::stepStream, this, &VideoManager::stepStream); + } +} + +//----------------------------------------------------------------------------- +void +VideoManager::stepZoom(int direction) +{ + if(_lastZoomChange.elapsed() > 250) { + _lastZoomChange.start(); + qCDebug(VideoManagerLog) << "Step Stream Zoom" << direction; + if(_activeVehicle && hasZoom()) { + _activeVehicle->sendMavCommand( + _videoStreamCompID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_STEP, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } + } +} + +//----------------------------------------------------------------------------- +void +VideoManager::stepStream(int direction) +{ + if(_lastStreamChange.elapsed() > 250) { + _lastStreamChange.start(); + int s = _currentStream + direction; + if(s < 1) s = _streamInfo.count; + if(s > _streamInfo.count) s = 1; + setCurrentStream(s); + } +} + +//----------------------------------------------------------------------------- +void +VideoManager::setCurrentStream(int stream) +{ + qCDebug(VideoManagerLog) << "Set Stream" << stream; + //-- TODO: Handle multiple streams + if(_hasAutoStream && stream <= _streamInfo.count && stream > 0 && _activeVehicle) { + if(_currentStream != stream) { + //-- Stop current stream + _activeVehicle->sendMavCommand( + _videoStreamCompID, // Target component + MAV_CMD_VIDEO_STOP_STREAMING, // Command id + false, // ShowError + _currentStream); // Stream ID + //-- Start new stream + _currentStream = stream; + _activeVehicle->sendMavCommand( + _videoStreamCompID, // Target component + MAV_CMD_VIDEO_START_STREAMING, // Command id + false, // ShowError + _currentStream); // Stream ID + _currentStream = stream; + emit currentStreamChanged(); + } + } +} + +//---------------------------------------------------------------------------------------- +void +VideoManager::_streamInfoChanged() +{ + emit streamInfoChanged(); } diff --git a/src/FlightDisplay/VideoManager.h b/src/FlightDisplay/VideoManager.h index a21d1391b..0831725f7 100644 --- a/src/FlightDisplay/VideoManager.h +++ b/src/FlightDisplay/VideoManager.h @@ -13,6 +13,7 @@ #include #include +#include #include #include "QGCMAVLink.h" @@ -24,6 +25,7 @@ Q_DECLARE_LOGGING_CATEGORY(VideoManagerLog) class VideoSettings; class Vehicle; +class Joystick; class VideoManager : public QGCTool { @@ -41,7 +43,10 @@ public: Q_PROPERTY(bool uvcEnabled READ uvcEnabled CONSTANT) Q_PROPERTY(bool fullScreen READ fullScreen WRITE setfullScreen NOTIFY fullScreenChanged) Q_PROPERTY(VideoReceiver* videoReceiver READ videoReceiver CONSTANT) - Q_PROPERTY(double aspectRatio READ aspectRatio NOTIFY aspectRatioChanged) + Q_PROPERTY(double aspectRatio READ aspectRatio NOTIFY streamInfoChanged) + Q_PROPERTY(bool hasZoom READ hasZoom NOTIFY streamInfoChanged) + Q_PROPERTY(int streamCount READ streamCount NOTIFY streamCountChanged) + Q_PROPERTY(int currentStream READ currentStream WRITE setCurrentStream NOTIFY currentStreamChanged) bool hasVideo (); bool isGStreamer (); @@ -51,6 +56,9 @@ public: QString videoSourceID () { return _videoSourceID; } QString autoURL () { return QString(_streamInfo.uri); } double aspectRatio (); + bool hasZoom () { return _streamInfo.flags & VIDEO_STREAM_HAS_BASIC_ZOOM; } + int currentStream () { return _currentStream; } + int streamCount () { return _streamInfo.count; } VideoReceiver* videoReceiver () { return _videoReceiver; } @@ -62,12 +70,15 @@ public: void setfullScreen (bool f) { _fullScreen = f; emit fullScreenChanged(); } void setIsTaisync (bool t) { _isTaisync = t; emit isTaisyncChanged(); } + void setCurrentStream (int stream); // Override from QGCTool void setToolbox (QGCToolbox *toolbox); - Q_INVOKABLE void startVideo() {_videoReceiver->start();} - Q_INVOKABLE void stopVideo() {_videoReceiver->stop(); } + Q_INVOKABLE void startVideo () { _videoReceiver->start(); } + Q_INVOKABLE void stopVideo () { _videoReceiver->stop(); } + Q_INVOKABLE void stepZoom (int direction); + Q_INVOKABLE void stepStream (int direction); signals: void hasVideoChanged (); @@ -75,8 +86,10 @@ signals: void videoSourceIDChanged (); void fullScreenChanged (); void isAutoStreamChanged (); - void aspectRatioChanged (); + void streamInfoChanged (); void isTaisyncChanged (); + void currentStreamChanged (); + void streamCountChanged (); private slots: void _videoSourceChanged (); @@ -84,8 +97,9 @@ private slots: void _rtspUrlChanged (); void _tcpUrlChanged (); void _updateUVC (); - void _aspectRatioChanged (); + void _streamInfoChanged (); void _setActiveVehicle (Vehicle* vehicle); + void _activeJoystickChanged (Joystick* joystick); void _vehicleMessageReceived (const mavlink_message_t& message); private: @@ -99,7 +113,13 @@ private: QString _videoSourceID; bool _fullScreen = false; Vehicle* _activeVehicle = nullptr; + Joystick* _activeJoystick = nullptr; mavlink_video_stream_information_t _streamInfo; + bool _hasAutoStream = false; + uint8_t _videoStreamCompID = 0; + int _currentStream = 1; + QTime _lastZoomChange; + QTime _lastStreamChange; }; #endif diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index b14f810ea..e06d9d550 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -12,6 +12,10 @@ #include "QGC.h" #include "AutoPilotPlugin.h" #include "UAS.h" +#include "QGCApplication.h" +#include "VideoManager.h" +#include "QGCCameraManager.h" +#include "QGCCameraControl.h" #include @@ -27,7 +31,7 @@ const char* Joystick::_accumulatorSettingsKey = "Accumulator"; const char* Joystick::_deadbandSettingsKey = "Deadband"; const char* Joystick::_circleCorrectionSettingsKey = "Circle_Correction"; const char* Joystick::_frequencySettingsKey = "Frequency"; -const char* Joystick::_txModeSettingsKey = NULL; +const char* Joystick::_txModeSettingsKey = nullptr; const char* Joystick::_fixedWingTXModeSettingsKey = "TXMode_FixedWing"; const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover"; @@ -38,6 +42,12 @@ const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); const char* Joystick::_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); const char* Joystick::_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); +const char* Joystick::_buttonActionZoomIn = QT_TR_NOOP("Zoom In"); +const char* Joystick::_buttonActionZoomOut = QT_TR_NOOP("Zoom Out"); +const char* Joystick::_buttonActionNextStream = QT_TR_NOOP("Next Video Stream"); +const char* Joystick::_buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream"); +const char* Joystick::_buttonActionNextCamera = QT_TR_NOOP("Next Camera"); +const char* Joystick::_buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera"); const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { "RollAxis", @@ -57,9 +67,9 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _hatButtonCount(4*hatCount) , _totalButtonCount(_buttonCount+_hatButtonCount) , _calibrationMode(false) - , _rgAxisValues(NULL) - , _rgCalibration(NULL) - , _rgButtonValues(NULL) + , _rgAxisValues(nullptr) + , _rgCalibration(nullptr) + , _rgButtonValues(nullptr) , _lastButtonBits(0) , _throttleMode(ThrottleModeCenterZero) , _negativeThrust(false) @@ -68,7 +78,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _deadband(false) , _circleCorrection(true) , _frequency(25.0f) - , _activeVehicle(NULL) + , _activeVehicle(nullptr) , _pollingStartedForCalibration(false) , _multiVehicleManager(multiVehicleManager) { @@ -150,12 +160,12 @@ void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle) } else if(activeVehicle->sub()) { _txModeSettingsKey = _submarineTXModeSettingsKey; } else { - _txModeSettingsKey = NULL; + _txModeSettingsKey = nullptr; qWarning() << "No valid joystick TXmode settings key for selected vehicle"; return; } } else { - _txModeSettingsKey = NULL; + _txModeSettingsKey = nullptr; } } @@ -659,14 +669,14 @@ int Joystick::getFunctionAxis(AxisFunction_t function) QStringList Joystick::actions(void) { QStringList list; - list << _buttonActionArm << _buttonActionDisarm; - if (_activeVehicle) { list << _activeVehicle->flightModes(); } list << _buttonActionVTOLFixedWing << _buttonActionVTOLMultiRotor; - + list << _buttonActionZoomIn << _buttonActionZoomOut; + list << _buttonActionNextStream << _buttonActionPreviousStream; + list << _buttonActionNextCamera << _buttonActionPreviousCamera; return list; } @@ -838,6 +848,12 @@ void Joystick::_buttonAction(const QString& action) _activeVehicle->setVtolInFwdFlight(false); } else if (_activeVehicle->flightModes().contains(action)) { _activeVehicle->setFlightMode(action); + } else if(action == _buttonActionZoomIn || action == _buttonActionZoomOut) { + emit stepZoom(action == _buttonActionZoomIn ? 1 : -1); + } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) { + emit stepStream(action == _buttonActionNextStream ? 1 : -1); + } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) { + emit stepCamera(action == _buttonActionNextCamera ? 1 : -1); } else { qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index e6e7aa9eb..c65dd6ab4 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -164,7 +164,10 @@ signals: void buttonActionTriggered(int action); - void frequencyChanged(); + void frequencyChanged (); + void stepZoom (int direction); + void stepCamera (int direction); + void stepStream (int direction); protected: void _setDefaultCalibration(void); @@ -252,6 +255,12 @@ private: static const char* _buttonActionDisarm; static const char* _buttonActionVTOLFixedWing; static const char* _buttonActionVTOLMultiRotor; + static const char* _buttonActionZoomIn; + static const char* _buttonActionZoomOut; + static const char* _buttonActionNextStream; + static const char* _buttonActionPreviousStream; + static const char* _buttonActionNextCamera; + static const char* _buttonActionPreviousCamera; private slots: void _activeVehicleChanged(Vehicle* activeVehicle); -- 2.22.0