diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 5823d8ef4c8b20ab041ee3de06b919bdff95a45d..ffad98fcd0d1889d5388d4d0d7cf3b774bebf3ce 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -58,6 +58,15 @@ static const char* kPhotoMode = "PhotoMode"; static const char* kPhotoLapse = "PhotoLapse"; static const char* kPhotoLapseCount = "PhotoLapseCount"; +//----------------------------------------------------------------------------- +// Known Parameters +static const char *kCAM_EV = "CAM_EV"; +static const char *kCAM_EXPMODE = "CAM_EXPMODE"; +static const char *kCAM_ISO = "CAM_ISO"; +static const char* kCAM_SHUTTER = "CAM_SHUTTER"; +static const char* kCAM_APERTURE = "CAM_APERTURE"; +static const char* kCAM_WBMODE = "CAM_WBMODE"; + //----------------------------------------------------------------------------- QGCCameraOptionExclusion::QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) : QObject(parent) @@ -146,6 +155,7 @@ QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Veh , _compID(compID) , _version(0) , _cached(false) + , _paramComplete(false) , _storageFree(0) , _storageTotal(0) , _netManager(nullptr) @@ -465,6 +475,44 @@ QGCCameraControl::setPhotoMode() } } +//----------------------------------------------------------------------------- +void +QGCCameraControl::setZoomLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setZoomLevel()" << level; + if(hasZoom()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_RANGE, // Zoom type + static_cast(level)); // Level + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setFocusLevel(qreal level) +{ + qCDebug(CameraControlLog) << "setFocusLevel()" << level; + if(hasFocus()) { + //-- Limit + level = std::min(std::max(level, 0.0), 100.0); + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_FOCUS, // Command id + false, // ShowError + FOCUS_TYPE_RANGE, // Focus type + static_cast(level)); // Level + } + } +} + //----------------------------------------------------------------------------- void QGCCameraControl::resetSettings() @@ -497,6 +545,51 @@ QGCCameraControl::formatCard(int id) } } +//----------------------------------------------------------------------------- +void +QGCCameraControl::stepZoom(int direction) +{ + qCDebug(CameraControlLog) << "stepZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + false, // ShowError + ZOOM_TYPE_STEP, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::startZoom(int direction) +{ + qCDebug(CameraControlLog) << "startZoom()" << direction; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + true, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + direction); // Direction (-1 wide, 1 tele) + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopZoom() +{ + qCDebug(CameraControlLog) << "stopZoom()"; + if(_vehicle && hasZoom()) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_ZOOM, // Command id + true, // ShowError + ZOOM_TYPE_CONTINUOUS, // Zoom type + 0); // Direction (-1 wide, 1 tele) + } +} + //----------------------------------------------------------------------------- void QGCCameraControl::_requestCaptureStatus() @@ -1069,6 +1162,10 @@ QGCCameraControl::_updateActiveList() qCDebug(CameraControlLogVerbose) << "Excluding" << exclusionList; _activeSettings = active; emit activeSettingsChanged(); + //-- Force validity of "Facts" based on active set + if(_paramComplete) { + emit parametersReady(); + } } } @@ -1273,6 +1370,16 @@ QGCCameraControl::handleSettings(const mavlink_camera_settings_t& settings) { qCDebug(CameraControlLog) << "handleSettings() Mode:" << settings.mode_id; _setCameraMode(static_cast(settings.mode_id)); + qreal z = static_cast(settings.zoomLevel); + qreal f = static_cast(settings.focusLevel); + if(std::isfinite(z) && z != _zoomLevel) { + _zoomLevel = z; + emit zoomLevelChanged(); + } + if(std::isfinite(f) && f != _focusLevel) { + _focusLevel = f; + emit focusLevelChanged(); + } } //----------------------------------------------------------------------------- @@ -1563,6 +1670,7 @@ QGCCameraControl::_paramDone() } } //-- All parameters loaded (or timed out) + _paramComplete = true; emit parametersReady(); } @@ -1591,3 +1699,45 @@ QGCCameraControl::activeSettings() qCDebug(CameraControlLog) << "Active:" << _activeSettings; return _activeSettings; } + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::exposureMode() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EXPMODE)) ? getFact(kCAM_EXPMODE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::ev() +{ + return (_paramComplete && _activeSettings.contains(kCAM_EV)) ? getFact(kCAM_EV) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::iso() +{ + return (_paramComplete && _activeSettings.contains(kCAM_ISO)) ? getFact(kCAM_ISO) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::shutter() +{ + return (_paramComplete && _activeSettings.contains(kCAM_SHUTTER)) ? getFact(kCAM_SHUTTER) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::aperture() +{ + return (_paramComplete && _activeSettings.contains(kCAM_APERTURE)) ? getFact(kCAM_APERTURE) : nullptr; +} + +//----------------------------------------------------------------------------- +Fact* +QGCCameraControl::wb() +{ + return (_paramComplete && _activeSettings.contains(kCAM_WBMODE)) ? getFact(kCAM_WBMODE) : nullptr; +} diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h index ed76e45f1a7b0f061d9c7931711256fceee7e7c7..87542920cc48f63f6c35397bb9b261ed0c72d830 100644 --- a/src/Camera/QGCCameraControl.h +++ b/src/Camera/QGCCameraControl.h @@ -97,12 +97,25 @@ public: Q_PROPERTY(bool capturesVideo READ capturesVideo NOTIFY infoChanged) Q_PROPERTY(bool capturesPhotos READ capturesPhotos NOTIFY infoChanged) Q_PROPERTY(bool hasModes READ hasModes NOTIFY infoChanged) + Q_PROPERTY(bool hasZoom READ hasZoom NOTIFY infoChanged) + Q_PROPERTY(bool hasFocus READ hasFocus NOTIFY infoChanged) Q_PROPERTY(bool photosInVideoMode READ photosInVideoMode NOTIFY infoChanged) Q_PROPERTY(bool videoInPhotoMode READ videoInPhotoMode NOTIFY infoChanged) Q_PROPERTY(bool isBasic READ isBasic NOTIFY infoChanged) Q_PROPERTY(quint32 storageFree READ storageFree NOTIFY storageFreeChanged) Q_PROPERTY(QString storageFreeStr READ storageFreeStr NOTIFY storageFreeChanged) Q_PROPERTY(quint32 storageTotal READ storageTotal NOTIFY storageTotalChanged) + Q_PROPERTY(bool paramComplete READ paramComplete NOTIFY parametersReady) + + Q_PROPERTY(qreal zoomLevel READ zoomLevel WRITE setZoomLevel NOTIFY zoomLevelChanged) + Q_PROPERTY(qreal focusLevel READ focusLevel WRITE setFocusLevel NOTIFY focusLevelChanged) + + Q_PROPERTY(Fact* exposureMode READ exposureMode NOTIFY parametersReady) + Q_PROPERTY(Fact* ev READ ev NOTIFY parametersReady) + Q_PROPERTY(Fact* iso READ iso NOTIFY parametersReady) + Q_PROPERTY(Fact* shutter READ shutter NOTIFY parametersReady) + Q_PROPERTY(Fact* aperture READ aperture NOTIFY parametersReady) + Q_PROPERTY(Fact* wb READ wb NOTIFY parametersReady) Q_PROPERTY(QStringList activeSettings READ activeSettings NOTIFY activeSettingsChanged) Q_PROPERTY(VideoStatus videoStatus READ videoStatus NOTIFY videoStatusChanged) @@ -122,6 +135,9 @@ public: Q_INVOKABLE virtual bool toggleVideo (); Q_INVOKABLE virtual void resetSettings (); Q_INVOKABLE virtual void formatCard (int id = 1); + Q_INVOKABLE virtual void stepZoom (int direction); + Q_INVOKABLE virtual void startZoom (int direction); + Q_INVOKABLE virtual void stopZoom (); virtual int version () { return _version; } virtual QString modelName () { return _modelName; } @@ -133,6 +149,8 @@ public: virtual bool capturesVideo () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO; } virtual bool capturesPhotos () { return _info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE; } virtual bool hasModes () { return _info.flags & CAMERA_CAP_FLAGS_HAS_MODES; } + virtual bool hasZoom () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM; } + virtual bool hasFocus () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS; } virtual bool photosInVideoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE; } virtual bool videoInPhotoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE; } @@ -148,7 +166,19 @@ public: virtual quint32 storageFree () { return _storageFree; } virtual QString storageFreeStr (); virtual quint32 storageTotal () { return _storageTotal; } + virtual bool paramComplete () { return _paramComplete; } + virtual qreal zoomLevel () { return _zoomLevel; } + virtual qreal focusLevel () { return _focusLevel; } + virtual Fact* exposureMode (); + virtual Fact* ev (); + virtual Fact* iso (); + virtual Fact* shutter (); + virtual Fact* aperture (); + virtual Fact* wb (); + + virtual void setZoomLevel (qreal level); + virtual void setFocusLevel (qreal level); virtual void setCameraMode (CameraMode mode); virtual void setPhotoMode (PhotoMode mode); virtual void setPhotoLapse (qreal interval); @@ -180,6 +210,8 @@ signals: void storageTotalChanged (); void dataReady (QByteArray data); void parametersReady (); + void zoomLevelChanged (); + void focusLevelChanged (); protected: virtual void _setVideoStatus (VideoStatus status); @@ -198,6 +230,7 @@ protected slots: virtual void _dataReady (QByteArray data); virtual void _paramDone (); + private: bool _handleLocalization (QByteArray& bytes); bool _replaceLocaleStrings (const QDomNode node, QByteArray& bytes); @@ -224,6 +257,9 @@ protected: mavlink_camera_information_t _info; int _version; bool _cached; + bool _paramComplete; + qreal _zoomLevel; + qreal _focusLevel; uint32_t _storageFree; uint32_t _storageTotal; QNetworkAccessManager* _netManager; diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index f69b74e66cbbdd1daf1091cc4c4d52eb24ef3ab7..8610a6f22cee840ef088f6c0c41546fea7d30c54 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -7,20 +7,20 @@ #include "QGCApplication.h" #include "QGCCameraManager.h" +#include "JoystickManager.h" QGC_LOGGING_CATEGORY(CameraManagerLog, "CameraManagerLog") //----------------------------------------------------------------------------- QGCCameraManager::QGCCameraManager(Vehicle *vehicle) : _vehicle(vehicle) - , _vehicleReadyState(false) - , _currentTask(0) - , _currentCamera(0) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qCDebug(CameraManagerLog) << "QGCCameraManager Created"; connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady); connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived); + _lastZoomChange.start(); + _lastCameraChange.start(); } //----------------------------------------------------------------------------- @@ -46,6 +46,9 @@ QGCCameraManager::_vehicleReady(bool ready) if(ready) { if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle() == _vehicle) { _vehicleReadyState = true; + JoystickManager *pJoyMgr = qgcApp()->toolbox()->joystickManager(); + _activeJoystickChanged(pJoyMgr->activeJoystick()); + connect(pJoyMgr, &JoystickManager::activeJoystickChanged, this, &QGCCameraManager::_activeJoystickChanged); } } } @@ -114,7 +117,7 @@ QGCCameraManager::_findCamera(int id) } } qWarning() << "Camera component id not found:" << id; - return NULL; + return nullptr; } //----------------------------------------------------------------------------- @@ -127,7 +130,7 @@ QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) _cameraInfoRequested[message.compid] = false; mavlink_camera_information_t info; mavlink_msg_camera_information_decode(&message, &info); - qCDebug(CameraManagerLog) << "_handleCameraInfo:" << (const char*)(void*)&info.model_name[0] << (const char*)(void*)&info.vendor_name[0] << "Comp ID:" << message.compid; + qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast(info.model_name) << reinterpret_cast(info.vendor_name) << "Comp ID:" << message.compid; QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); if(pCamera) { QQmlEngine::setObjectOwnership(pCamera, QQmlEngine::CppOwnership); @@ -212,3 +215,51 @@ QGCCameraManager::_requestCameraInfo(int compID) 1); // Do Request } } + +//---------------------------------------------------------------------------------------- +void +QGCCameraManager::_activeJoystickChanged(Joystick* joystick) +{ + qCDebug(CameraManagerLog) << "Joystick changed"; + if(_activeJoystick) { + disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + } + _activeJoystick = joystick; + if(_activeJoystick) { + connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepZoom(int direction) +{ + if(_lastZoomChange.elapsed() > 250) { + _lastZoomChange.start(); + qCDebug(CameraManagerLog) << "Step Camera Zoom" << direction; + if(_cameras.count() && _cameras[_currentCamera]) { + QGCCameraControl* pCamera = qobject_cast(_cameras[_currentCamera]); + if(pCamera) { + pCamera->stepZoom(direction); + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_stepCamera(int direction) +{ + if(_lastCameraChange.elapsed() > 1000) { + _lastCameraChange.start(); + qCDebug(CameraManagerLog) << "Step Camera" << direction; + int c = _currentCamera + direction; + if(c < 0) c = _cameras.count() - 1; + if(c >= _cameras.count()) c = 0; + setCurrentCamera(c); + } +} + + diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 891d24c31639ce4e727c2014eb471aaa8679c67d..b0c8b9c60292ffd4af530441f47748b904ecd63c 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -17,6 +17,8 @@ Q_DECLARE_LOGGING_CATEGORY(CameraManagerLog) +class Joystick; + //----------------------------------------------------------------------------- class QGCCameraManager : public QObject { @@ -46,6 +48,9 @@ signals: protected slots: virtual void _vehicleReady (bool ready); virtual void _mavlinkMessageReceived (const mavlink_message_t& message); + virtual void _activeJoystickChanged (Joystick* joystick); + virtual void _stepZoom (int direction); + virtual void _stepCamera (int direction); protected: virtual QGCCameraControl* _findCamera (int id); @@ -59,11 +64,14 @@ protected: virtual void _handleCaptureStatus (const mavlink_message_t& message); protected: - Vehicle* _vehicle; - bool _vehicleReadyState; - int _currentTask; + Vehicle* _vehicle = nullptr; + Joystick* _activeJoystick = nullptr; + bool _vehicleReadyState = false; + int _currentTask = 0; QmlObjectListModel _cameras; QStringList _cameraLabels; QMap _cameraInfoRequested; - int _currentCamera; + int _currentCamera = 0; + QTime _lastZoomChange; + QTime _lastCameraChange; }; diff --git a/src/FlightDisplay/VideoManager.cc b/src/FlightDisplay/VideoManager.cc index 49b7be4e64a3367fdc03db1a9d69ba400c86bf41..5451dad2144f074c7e8b9120d19b915d2b0a34f6 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 a21d1391be8305f390cd0ea525dff919b0b0b314..0831725f7a9f2d03fa9ef08126ac3007f4346ef5 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 b14f810ea68b3885177c9519f1bc85959d570f49..e06d9d550257beaa5ee0a828f8517cc0b8564d47 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 e6e7aa9eb8d22e14f067d6d59771ced599d4115a..c65dd6ab4d59f3ada5bde8809f73a3aa36d97a30 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); diff --git a/src/QmlControls/DropPanel.qml b/src/QmlControls/DropPanel.qml index b764d9fe4c3c9e331f1ae38c4a237405c0202070..7e1ef2a145c2b5c52ecd9cddb91ce4f7efcd7de3 100644 --- a/src/QmlControls/DropPanel.qml +++ b/src/QmlControls/DropPanel.qml @@ -37,10 +37,7 @@ Item { readonly property real _arrowBaseHeight: radius // Height of vertical side of arrow readonly property real _arrowPointWidth: radius * 0.666 // Distance from vertical side to point - readonly property real _dropCornerRadius: ScreenTools.defaultFontPixelWidth * 0.5 - readonly property real _dropCornerRadiusX2: _dropCornerRadius * 2 - readonly property real _dropMargin: _dropCornerRadius - readonly property real _dropMarginX2: _dropMargin * 2 + readonly property real _dropMargin: ScreenTools.defaultFontPixelWidth property var _dropEdgeTopPoint property real _dropEdgeHeight @@ -74,8 +71,8 @@ Item { var panelComponentWidth = panelLoader.item.width var panelComponentHeight = panelLoader.item.height - dropDownItem.width = panelComponentWidth + (_dropMarginX2 * 2) + _arrowPointWidth - dropDownItem.height = panelComponentHeight + (_dropMarginX2 * 2) + dropDownItem.width = panelComponentWidth + (_dropMargin * 2) + _arrowPointWidth + dropDownItem.height = panelComponentHeight + (_dropMargin * 2) dropDownItem.x = _dropEdgeTopPoint.x + _dropMargin dropDownItem.y = _dropEdgeTopPoint.y -(dropDownItem.height / 2) + radius