From a0321b80dd40a9942de464ae056d0c300fdf8e2b Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Thu, 6 Jul 2017 06:25:33 -0400 Subject: [PATCH] Camera definition implementation --- libs/mavlink/include/mavlink/v2.0 | 2 +- qgcresources.qrc | 3 + qgroundcontrol.pro | 7 + qgroundcontrol.qrc | 1 + src/Camera/CameraControl.qml | 341 +++++++ src/Camera/QGCCameraControl.cc | 866 ++++++++++++++++++ src/Camera/QGCCameraControl.h | 168 ++++ src/Camera/QGCCameraIO.cc | 235 +++++ src/Camera/QGCCameraIO.h | 49 + src/Camera/QGCCameraManager.cc | 172 ++++ src/Camera/QGCCameraManager.h | 60 ++ src/Camera/camera_definition_example.json | 279 ++++++ src/Camera/camera_definition_example.xml | 301 ++++++ src/Camera/images/camera_photo.svg | 13 + src/Camera/images/camera_settings.svg | 102 +++ src/Camera/images/camera_video.svg | 16 + src/FactSystem/Fact.cc | 103 ++- src/FactSystem/Fact.h | 9 +- src/FactSystem/FactGroup.cc | 25 +- src/FactSystem/FactGroup.h | 9 +- src/FactSystem/FactMetaData.cc | 48 +- src/FactSystem/FactMetaData.h | 6 + src/FirmwarePlugin/FirmwarePlugin.cc | 17 + src/FirmwarePlugin/FirmwarePlugin.h | 9 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 25 +- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 4 + src/FlightDisplay/FlightDisplayView.qml | 4 +- src/FlightDisplay/FlightDisplayViewVideo.qml | 37 +- src/FlightDisplay/GuidedActionsController.qml | 2 +- src/JsonHelper.cc | 31 +- src/JsonHelper.h | 2 +- src/MissionManager/CameraSection.cc | 3 +- src/MissionManager/CameraSectionTest.cc | 11 +- src/MissionManager/MavCmdInfoCommon.json | 6 - src/PlanView/SurveyItemEditor.qml | 6 +- src/QGCApplication.cc | 11 +- src/QmlControls/ScreenTools.qml | 2 +- src/QmlControls/SliderSwitch.qml | 2 +- src/Vehicle/Vehicle.cc | 9 +- src/Vehicle/Vehicle.h | 13 +- src/api/QGCCorePlugin.h | 16 +- src/ui/MainWindowInner.qml | 4 +- src/ui/preferences/MavlinkSettings.qml | 8 +- 43 files changed, 2917 insertions(+), 120 deletions(-) create mode 100644 src/Camera/CameraControl.qml create mode 100644 src/Camera/QGCCameraControl.cc create mode 100644 src/Camera/QGCCameraControl.h create mode 100644 src/Camera/QGCCameraIO.cc create mode 100644 src/Camera/QGCCameraIO.h create mode 100644 src/Camera/QGCCameraManager.cc create mode 100644 src/Camera/QGCCameraManager.h create mode 100644 src/Camera/camera_definition_example.json create mode 100644 src/Camera/camera_definition_example.xml create mode 100644 src/Camera/images/camera_photo.svg create mode 100644 src/Camera/images/camera_settings.svg create mode 100644 src/Camera/images/camera_video.svg diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 6bbc8a51d..05c8fa042 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 6bbc8a51d8f37537732d3f5170093d49e64c6f4b +Subproject commit 05c8fa042e67888f98ce63e2a76bb35f38dd218f diff --git a/qgcresources.qrc b/qgcresources.qrc index 4abea6ec7..b7a5656b1 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -47,6 +47,9 @@ src/AutoPilotPlugins/Common/Images/wifi.svg src/QmlControls/arrow-down.png resources/camera.svg + src/Camera/images/camera_photo.svg + src/Camera/images/camera_settings.svg + src/Camera/images/camera_video.svg src/QmlControls/check.png src/VehicleSetup/FirmwareUpgradeIcon.png src/AutoPilotPlugins/PX4/Images/FlightModesComponentIcon.png diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 06ff98ffd..69406dffc 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -327,6 +327,7 @@ INCLUDEPATH += \ src \ src/api \ src/AnalyzeView \ + src/Camera \ src/AutoPilotPlugins \ src/FlightDisplay \ src/FlightMap \ @@ -479,6 +480,9 @@ HEADERS += \ src/AnalyzeView/ExifParser.h \ src/AnalyzeView/ULogParser.h \ src/AnalyzeView/PX4LogParser.h \ + src/Camera/QGCCameraControl.h \ + src/Camera/QGCCameraIO.h \ + src/Camera/QGCCameraManager.h \ src/CmdLineOptParser.h \ src/FirmwarePlugin/PX4/px4_custom_mode.h \ src/FlightDisplay/VideoManager.h \ @@ -660,6 +664,9 @@ SOURCES += \ src/AnalyzeView/ExifParser.cc \ src/AnalyzeView/ULogParser.cc \ src/AnalyzeView/PX4LogParser.cc \ + src/Camera/QGCCameraControl.cc \ + src/Camera/QGCCameraIO.cc \ + src/Camera/QGCCameraManager.cc \ src/CmdLineOptParser.cc \ src/FlightDisplay/VideoManager.cc \ src/FlightMap/Widgets/ValuesWidgetController.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index dfcda9c3f..26ada3e12 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -21,6 +21,7 @@ src/ui/preferences/BluetoothSettings.qml src/AutoPilotPlugins/PX4/CameraComponent.qml src/AutoPilotPlugins/PX4/CameraComponentSummary.qml + src/Camera/CameraControl.qml src/ViewWidgets/CustomCommandWidget.qml src/ui/preferences/DebugWindow.qml src/AutoPilotPlugins/Common/ESP8266Component.qml diff --git a/src/Camera/CameraControl.qml b/src/Camera/CameraControl.qml new file mode 100644 index 000000000..fa855feb5 --- /dev/null +++ b/src/Camera/CameraControl.qml @@ -0,0 +1,341 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Dialogs 1.2 +import QtGraphicalEffects 1.0 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controllers 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 + +Rectangle { + id: mainRect + height: mainRow.height + (ScreenTools.defaultFontPixelWidth * 2) + width: mainRow.width + (ScreenTools.defaultFontPixelWidth * 2) + radius: ScreenTools.defaultFontPixelWidth * 0.5 + color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(1,1,1,0.95) : Qt.rgba(0,0,0,0.75) + border.width: 1 + border.color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(0,0,0,0.35) : Qt.rgba(1,1,1,0.35) + + QGCPalette { id: qgcPal; colorGroupEnabled: true } + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property var _dynamicCameras: _activeVehicle ? _activeVehicle.dynamicCameras : null + property bool _isCamera: _dynamicCameras ? _dynamicCameras.cameras.count > 0 : false + property bool _cameraModeUndefined: _isCamera ? _dynamicCameras.cameras.get(0).cameraMode === QGCCameraControl.CAMERA_MODE_UNDEFINED : true + property bool _cameraVideoMode: _isCamera ? _dynamicCameras.cameras.get(0).cameraMode === QGCCameraControl.CAMERA_MODE_VIDEO : false + property bool _cameraPhotoMode: _isCamera ? _dynamicCameras.cameras.get(0).cameraMode === QGCCameraControl.CAMERA_MODE_PHOTO : false + property var _camera: _isCamera ? _dynamicCameras.cameras.get(0) : null // Single camera support for the time being + property real _spacers: ScreenTools.defaultFontPixelHeight * 0.5 + property real _labelFieldWidth: ScreenTools.defaultFontPixelWidth * 30 + property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 30 + property bool _communicationLost: _activeVehicle ? _activeVehicle.connectionLost : false + property bool _hasModes: _isCamera && _camera && _camera.hasModes + + MouseArea { + anchors.fill: parent + onWheel: { wheel.accepted = true; } + onPressed: { mouse.accepted = true; } + onReleased: { mouse.accepted = true; } + } + + Connections { + target: QGroundControl.multiVehicleManager.activeVehicle + onConnectionLostChanged: { + if(_communicationLost) { + if(rootLoader.sourceComponent === cameraSettingsComponent) { + rootLoader.sourceComponent = null + } + } + } + } + + Row { + id: mainRow + spacing: _spacers + anchors.centerIn: parent + Column { + spacing: _spacers + anchors.verticalCenter: parent.verticalCenter + //----------------------------------------------------------------- + QGCLabel { + id: cameraLabel + text: _isCamera ? _dynamicCameras.cameras.get(0).modelName : qsTr("Camera") + font.pointSize: ScreenTools.smallFontPointSize + anchors.horizontalCenter: parent.horizontalCenter + } + //-- Camera Mode (visible only if camera has modes) + Rectangle { + width: _hasModes ? ScreenTools.defaultFontPixelWidth * 12 : 0 + height: _hasModes ? ScreenTools.defaultFontPixelWidth * 4 : 0 + color: qgcPal.window + radius: height * 0.5 + visible: _hasModes + anchors.horizontalCenter: parent.horizontalCenter + //-- Video Mode + Rectangle { + width: parent.height * 0.9 + height: parent.height * 0.9 + color: qgcPal.windowShadeDark + radius: height * 0.5 + anchors.left: parent.left + anchors.leftMargin: 4 + anchors.verticalCenter: parent.verticalCenter + QGCColoredImage { + anchors.fill: parent + source: "/qmlimages/camera_video.svg" + fillMode: Image.PreserveAspectFit + sourceSize.height: height + color: _cameraVideoMode ? qgcPal.colorGreen : qgcPal.text + MouseArea { + anchors.fill: parent + enabled: _cameraPhotoMode + onClicked: { + _camera.setVideoMode() + } + } + } + } + //-- Photo Mode + Rectangle { + width: parent.height * 0.9 + height: parent.height * 0.9 + color: qgcPal.window + radius: height * 0.5 + anchors.right: parent.right + anchors.rightMargin: 4 + anchors.verticalCenter: parent.verticalCenter + QGCColoredImage { + anchors.fill: parent + source: "/qmlimages/camera_photo.svg" + fillMode: Image.PreserveAspectFit + sourceSize.height: height + color: _cameraPhotoMode ? qgcPal.colorGreen : qgcPal.text + MouseArea { + anchors.fill: parent + enabled: _cameraVideoMode + onClicked: { + _camera.setPhotoMode() + } + } + } + } + } + //-- Settings + QGCColoredImage { + width: ScreenTools.defaultFontPixelWidth * 3 + height: width + sourceSize.width: width + source: "/qmlimages/camera_settings.svg" + fillMode: Image.PreserveAspectFit + color: _cameraModeUndefined ? qgcPal.colorGrey : qgcPal.text + anchors.horizontalCenter: parent.horizontalCenter + MouseArea { + anchors.fill: parent + enabled: !_cameraModeUndefined + onClicked: { + if(rootLoader.sourceComponent === null) { + rootLoader.sourceComponent = cameraSettingsComponent + } else { + rootLoader.sourceComponent = null + } + } + } + } + } + //-- Shutter + Rectangle { + color: Qt.rgba(0,0,0,0) + width: ScreenTools.defaultFontPixelWidth * 6 + height: width + radius: width * 0.5 + border.color: qgcPal.buttonText + border.width: 3 + anchors.verticalCenter: parent.verticalCenter + Rectangle { + width: parent.width * 0.75 + height: width + radius: width * 0.5 + color: _cameraModeUndefined ? qgcPal.colorGrey : qgcPal.colorRed + anchors.centerIn: parent + } + MouseArea { + anchors.fill: parent + enabled: !_cameraModeUndefined + onClicked: { + if(_cameraVideoMode) { + //-- Start/Stop Video + } else { + _camera.takePhoto() + } + } + } + } + } + + Component { + id: cameraSettingsComponent + Item { + id: cameraSettingsRect + width: mainWindow.width + height: mainWindow.height + anchors.centerIn: parent + MouseArea { + anchors.fill: parent + onClicked: { + rootLoader.sourceComponent = null + } + } + Rectangle { + id: camSettingsRect + width: _labelFieldWidth + _editFieldWidth + (ScreenTools.defaultFontPixelWidth * 8) + height: Math.max(mainWindow.height * 0.65, ScreenTools.defaultFontPixelHeight * 20) + radius: ScreenTools.defaultFontPixelWidth + color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(1,1,1,0.95) : Qt.rgba(0,0,0,0.75) + border.width: 1 + border.color: qgcPal.globalTheme === QGCPalette.Light ? Qt.rgba(0,0,0,0.35) : Qt.rgba(1,1,1,0.35) + anchors.centerIn: parent + QGCLabel { + id: cameraSettingsLabel + text: _cameraVideoMode ? qsTr("Video Settings") : qsTr("Camera Settings") + font.family: ScreenTools.demiboldFontFamily + font.pointSize: ScreenTools.mediumFontPointSize + anchors.margins: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.top: parent.top + anchors.left: parent.left + } + QGCFlickable { + clip: true + anchors.top: cameraSettingsLabel.bottom + anchors.topMargin: ScreenTools.defaultFontPixelHeight + anchors.bottom: parent.bottom + width: cameraSettingsCol.width + contentHeight: cameraSettingsCol.height + contentWidth: cameraSettingsCol.width + anchors.horizontalCenter: parent.horizontalCenter + Column { + id: cameraSettingsCol + spacing: ScreenTools.defaultFontPixelHeight * 0.5 + width: camSettingsRect.width + anchors.margins: ScreenTools.defaultFontPixelHeight + //------------------------------------------- + //-- Camera Settings + Repeater { + model: _camera ? _camera.activeSettings : [] + Row { + spacing: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + QGCLabel { + text: _camera.getFact(modelData).shortDescription + width: _labelFieldWidth + anchors.verticalCenter: parent.verticalCenter + } + FactComboBox { + width: _editFieldWidth + fact: _camera.getFact(modelData) + indexModel: false + visible: !_camera.getFact(modelData).typeIsBool + anchors.verticalCenter: parent.verticalCenter + } + Item { + width: _editFieldWidth + height: factSwitch.height + visible: _camera.getFact(modelData).typeIsBool + anchors.verticalCenter: parent.verticalCenter + Switch { + id: factSwitch + anchors.left: parent.left + checked: fact ? fact.value : false + onClicked: fact.value = checked ? 1 : 0 + property var fact: _camera.getFact(modelData) + } + } + } + } + //------------------------------------------- + //-- Reset Camera + Row { + spacing: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + QGCLabel { + text: qsTr("Reset Camera Defaults") + width: _labelFieldWidth + anchors.verticalCenter: parent.verticalCenter + } + QGCButton { + text: qsTr("Reset") + onClicked: resetPrompt.open() + width: _editFieldWidth + anchors.verticalCenter: parent.verticalCenter + MessageDialog { + id: resetPrompt + title: qsTr("Reset Camera to Factory Settings") + text: qsTr("Confirm resetting all settings?") + standardButtons: StandardButton.Yes | StandardButton.No + onNo: resetPrompt.close() + onYes: { + // TODO + resetPrompt.close() + } + } + } + } + //------------------------------------------- + //-- Format Storage + Row { + spacing: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + QGCLabel { + text: qsTr("Storage") + width: _labelFieldWidth + anchors.verticalCenter: parent.verticalCenter + } + QGCButton { + text: qsTr("Format") + enabled: false + onClicked: formatPrompt.open() + width: _editFieldWidth + anchors.verticalCenter: parent.verticalCenter + MessageDialog { + id: formatPrompt + title: qsTr("Format Camera Storage") + text: qsTr("Confirm erasing all files?") + standardButtons: StandardButton.Yes | StandardButton.No + onNo: formatPrompt.close() + onYes: { + // TODO + formatPrompt.close() + } + } + } + } + } + } + } + Component.onCompleted: { + rootLoader.width = cameraSettingsRect.width + rootLoader.height = cameraSettingsRect.height + } + Keys.onBackPressed: { + rootLoader.sourceComponent = null + } + Keys.onEscapePressed: { + rootLoader.sourceComponent = null + } + } + } +} diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc new file mode 100644 index 000000000..bca7919c0 --- /dev/null +++ b/src/Camera/QGCCameraControl.cc @@ -0,0 +1,866 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" +#include +#include + +QGC_LOGGING_CATEGORY(CameraControlLog, "CameraControlLog") +QGC_LOGGING_CATEGORY(CameraControlLogVerbose, "CameraControlLogVerbose") + +static const char* kDefnition = "definition"; +static const char* kParameters = "parameters"; +static const char* kParameter = "parameter"; +static const char* kVersion = "version"; +static const char* kModel = "model"; +static const char* kVendor = "vendor"; +static const char* kLocalization = "localization"; +static const char* kLocale = "locale"; +static const char* kStrings = "strings"; +static const char* kName = "name"; +static const char* kValue = "value"; +static const char* kControl = "control"; +static const char* kOptions = "options"; +static const char* kOption = "option"; +static const char* kType = "type"; +static const char* kDefault = "default"; +static const char* kDescription = "description"; +static const char* kExclusions = "exclusions"; +static const char* kExclusion = "exclude"; +static const char* kRoption = "roption"; +static const char* kCondition = "condition"; +static const char* kParameterranges = "parameterranges"; +static const char* kParameterrange = "parameterrange"; +static const char* kOriginal = "original"; +static const char* kTranslated = "translated"; + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, bool& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue() != "0"; + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, int& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue().toInt(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_attribute(QDomNode& node, const char* tagName, QString& target) +{ + QDomNamedNodeMap attrs = node.attributes(); + if(!attrs.count()) { + return false; + } + QDomNode subNode = attrs.namedItem(tagName); + if(subNode.isNull()) { + return false; + } + target = subNode.nodeValue(); + return true; +} + +//----------------------------------------------------------------------------- +static bool +read_value(QDomNode& element, const char* tagName, QString& target) +{ + QDomElement de = element.firstChildElement(tagName); + if(de.isNull()) { + return false; + } + target = de.text(); + return true; +} + +//----------------------------------------------------------------------------- +QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Vehicle* vehicle, int compID, QObject* parent) + : FactGroup(0, parent) + , _vehicle(vehicle) + , _compID(compID) + , _version(0) + , _cameraMode(CAMERA_MODE_UNDEFINED) +{ + memcpy(&_info, &info, sizeof(mavlink_camera_information_t)); + if(1 /*!_info.definition_uri[0]*/) { + //-- Process camera definition file + _loadCameraDefinitionFile("/Users/gus/github/Yuneec/camera_repeater/yuneec_udp/def_files/e90.xml"); + } + + if(isBasic()) { + _requestCameraSettings(); + } else { + _requestAllParameters(); + QTimer::singleShot(2000, this, &QGCCameraControl::_requestCameraSettings); + } + + emit infoChanged(); +} + +//----------------------------------------------------------------------------- +QGCCameraControl::~QGCCameraControl() +{ + //-- Clear param IO queue (if any) + foreach(QString paramName, _paramIO.keys()) { + if(_paramIO[paramName]) { + delete _paramIO[paramName]; + } + } +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::firmwareVersion() +{ + int major = (_info.firmware_version >> 24) & 0xFF; + int minor = (_info.firmware_version >> 16) & 0xFF; + int build = _info.firmware_version & 0xFFFF; + QString ver; + ver.sprintf("%d.%d.%d", major, minor, build); + return ver; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setCameraMode(CameraMode mode) +{ + qCDebug(CameraControlLog) << "setCameraMode(" << mode << ")"; + if(mode == CAMERA_MODE_VIDEO) { + setVideoMode(); + } else if(mode == CAMERA_MODE_PHOTO) { + setPhotoMode(); + } else { + qCDebug(CameraControlLog) << "setCameraMode() Invalid mode:" << mode; + return; + } + _cameraMode = mode; + emit cameraModeChanged(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::toggleMode() +{ + if(cameraMode() == CAMERA_MODE_PHOTO) { + setVideoMode(); + } else if(cameraMode() == CAMERA_MODE_VIDEO) { + setPhotoMode(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::takePhoto() +{ + +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::startVideo() +{ + +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::stopVideo() +{ + +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setVideoMode() +{ + if(_vehicle && _cameraMode != CAMERA_MODE_VIDEO) { + qCDebug(CameraControlLog) << "setVideoMode()"; + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Camera ID (0 for all, 1 for first, 2 for second, etc.) ==> TODO: Remove + CAMERA_MODE_VIDEO); // Camera mode (0: photo, 1: video) + _cameraMode = CAMERA_MODE_VIDEO; + emit cameraModeChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::setPhotoMode() +{ + if(_vehicle && _cameraMode != CAMERA_MODE_PHOTO) { + qCDebug(CameraControlLog) << "setPhotoMode()"; + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_SET_CAMERA_MODE, // Command id + true, // ShowError + 0, // Camera ID (0 for all, 1 for first, 2 for second, etc.) ==> TODO: Remove + CAMERA_MODE_PHOTO); // Camera mode (0: photo, 1: video) + _cameraMode = CAMERA_MODE_PHOTO; + emit cameraModeChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::resetSettings() +{ + qCDebug(CameraControlLog) << "resetSettings()"; + if(_vehicle) { + //-- Use basic MAVLink message + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_RESET_CAMERA_SETTINGS, // Command id + true, // ShowError + 0, // Camera ID (0 for all, 1 for first, 2 for second, etc.) ==> TODO: Remove + 1); // Do Reset + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::formatCard(int id) +{ + qCDebug(CameraControlLog) << "formatCard()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_STORAGE_FORMAT, // Command id + true, // ShowError + id, // Storage ID (1 for first, 2 for second, etc.) + 1); // Do Format + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::factChanged(Fact* pFact) +{ + _updateActiveList(); + _updateRanges(pFact); +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadCameraDefinitionFile(const QString& file) +{ + QFile xmlFile(file); + if (!xmlFile.open(QIODevice::ReadOnly | QIODevice::Text)) { + qWarning() << "Unable to open camera definition file" << file << xmlFile.errorString(); + return false; + } + QByteArray bytes = xmlFile.readAll(); + xmlFile.close(); + //-- Handle localization + if(!_handleLocalization(bytes)) { + return false; + } + int errorLine; + QString errorMsg; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCritical() << "Unable to parse camera definition file on line:" << errorLine; + qCritical() << errorMsg; + return false; + } + //-- Load camera constants + QDomNodeList defElements = doc.elementsByTagName(kDefnition); + if(!defElements.size() || !_loadConstants(defElements)) { + qWarning() << "Unable to load camera constants from camera definition file" << file; + return false; + } + //-- Load camera parameters + QDomNodeList paramElements = doc.elementsByTagName(kParameters); + if(!paramElements.size() || !_loadSettings(paramElements)) { + qWarning() << "Unable to load camera parameters from camera definition file" << file; + return false; + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadConstants(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + if(!read_attribute(node, kVersion, _version)) { + return false; + } + if(!read_value(node, kModel, _modelName)) { + return false; + } + if(!read_value(node, kVendor, _vendor)) { + return false; + } + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadSettings(const QDomNodeList nodeList) +{ + QDomNode node = nodeList.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameters = elem.elementsByTagName(kParameter); + //-- Pre-process settings (maintain order and skip non-controls) + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString name; + if(read_attribute(parameterNode, kName, name)) { + bool control = true; + read_attribute(parameterNode, kControl, control); + if(control) { + _settings << name; + } + } else { + qCritical() << "Parameter entry missing parameter name"; + return false; + } + } + //-- Load parameters + for(int i = 0; i < parameters.size(); i++) { + QDomNode parameterNode = parameters.item(i); + QString factName; + read_attribute(parameterNode, kName, factName); + QString type; + if(!read_attribute(parameterNode, kType, type)) { + qCritical() << QString("Parameter %1 missing parameter type").arg(factName); + return false; + } + bool unknownType; + FactMetaData::ValueType_t factType = FactMetaData::stringToType(type, unknownType); + if (unknownType) { + qCritical() << QString("Unknown type for parameter %1").arg(factName); + return false; + } + QString description; + if(!read_value(parameterNode, kDescription, description)) { + qCritical() << QString("Parameter %1 missing parameter description").arg(factName); + return false; + } + //-- Build metadata + FactMetaData* metaData = new FactMetaData(factType, factName, this); + metaData->setShortDescription(description); + metaData->setLongDescription(description); + //-- Options (enums) + QDomElement optionElem = parameterNode.toElement(); + QDomNodeList optionsRoot = optionElem.elementsByTagName(kOptions); + if(optionsRoot.size()) { + //-- Iterate options + QDomNode node = optionsRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList options = elem.elementsByTagName(kOption); + for(int i = 0; i < options.size(); i++) { + QDomNode option = options.item(i); + QString optName; + QString optValue; + QVariant optVariant; + if(!_loadNameValue(option, factName, metaData, optName, optValue, optVariant)) { + delete metaData; + return false; + } + metaData->addEnumInfo(optName, optVariant); + _originalOptNames[factName] << optName; + _originalOptValues[factName] << optVariant; + //-- Check for exclusions + QStringList exclusions = _loadExclusions(option); + if(exclusions.size()) { + qCDebug(CameraControlLogVerbose) << "New exclusions:" << factName << optValue << exclusions; + QGCCameraOptionExclusion* pExc = new QGCCameraOptionExclusion(this, factName, optValue, exclusions); + _valueExclusions.append(pExc); + } + //-- Check for range rules + if(!_loadRanges(option, factName, optValue)) { + delete metaData; + return false; + } + } + } + QString defaultValue; + if(read_attribute(parameterNode, kDefault, defaultValue)) { + QVariant defaultVariant; + QString errorString; + if (metaData->convertAndValidateRaw(defaultValue, false, defaultVariant, errorString)) { + metaData->setRawDefaultValue(defaultVariant); + } else { + qWarning() << "Invalid default value for" << factName + << " type:" << metaData->type() + << " value:" << defaultValue + << " error:" << errorString; + } + } + //-- Set metadata and Fact + if (_nameToFactMetaDataMap.contains(factName)) { + qWarning() << QStringLiteral("Duplicate fact name:") << factName; + delete metaData; + } else { + _nameToFactMetaDataMap[factName] = metaData; + Fact* pFact = new Fact(_compID, factName, factType, this); + pFact->setMetaData(metaData); + pFact->setRawValue(metaData->rawDefaultValue(), true); + QGCCameraParamIO* pIO = new QGCCameraParamIO(this, pFact, _vehicle); + _paramIO[factName] = pIO; + _addFact(pFact, factName); + } + } + if(_nameToFactMetaDataMap.size() > 0) { + _addFactGroup(this, "camera"); + _processRanges(); + _activeSettings = _settings; + emit activeSettingsChanged(); + return true; + } + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_handleLocalization(QByteArray& bytes) +{ + QString errorMsg; + int errorLine; + QDomDocument doc; + if(!doc.setContent(bytes, false, &errorMsg, &errorLine)) { + qCritical() << "Unable to parse camera definition file on line:" << errorLine; + qCritical() << errorMsg; + return false; + } + //-- Find out where we are + QLocale locale = QLocale::system(); +#if defined (__macos__) + locale = QLocale(locale.name()); +#endif + QString localeName = locale.name().toLower().replace("-", "_"); + qCDebug(CameraControlLog) << "Current locale:" << localeName; + if(localeName == "en_us") { + // Nothing to do + return true; + } + QDomNodeList locRoot = doc.elementsByTagName(kLocalization); + if(!locRoot.size()) { + // Nothing to do + return true; + } + //-- Iterate locales + QDomNode node = locRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList locales = elem.elementsByTagName(kLocale); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + if(!read_attribute(locale, kName, name)) { + qWarning() << "Localization entry is missing its name attribute"; + continue; + } + // If we found a direct match, deal with it now + if(localeName == name.toLower().replace("-", "_")) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- No direct match. Pick first matching language (if any) + localeName = localeName.left(3); + for(int i = 0; i < locales.size(); i++) { + QDomNode locale = locales.item(i); + QString name; + read_attribute(locale, kName, name); + if(name.toLower().startsWith(localeName)) { + return _replaceLocaleStrings(locale, bytes); + } + } + //-- Could not find a language to use + qWarning() << "No match for" << QLocale::system().name() << "in camera definition file"; + //-- Just use default, en_US + return true; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_replaceLocaleStrings(const QDomNode node, QByteArray& bytes) +{ + QDomElement stringElem = node.toElement(); + QDomNodeList strings = stringElem.elementsByTagName(kStrings); + for(int i = 0; i < strings.size(); i++) { + QDomNode stringNode = strings.item(i); + QString original; + QString translated; + if(read_attribute(stringNode, kOriginal, original)) { + if(read_attribute(stringNode, kTranslated, translated)) { + QString o; o = "\"" + original + "\""; + QString t; t = "\"" + translated + "\""; + bytes.replace(o.toUtf8(), t.toUtf8()); + o = ">" + original + "<"; + t = ">" + translated + "<"; + bytes.replace(o.toUtf8(), t.toUtf8()); + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestAllParameters() +{ + //-- Reset receive list + foreach(QString pramName, _paramIO.keys()) { + _paramIO[pramName]->setParamRequest(); + } + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_msg_param_ext_request_list_pack_chan( + mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + compID()); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + qCDebug(CameraControlLogVerbose) << "Request all parameters"; +} + +//----------------------------------------------------------------------------- +QString +QGCCameraControl::_getParamName(const char* param_id) +{ + QByteArray bytes(param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName(bytes); + return parameterName; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + QString paramName = _getParamName(ack.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_ACK for unknown param:" << paramName; + return; + } + _paramIO[paramName]->handleParamAck(ack); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleParamValue(const mavlink_param_ext_value_t& value) +{ + QString paramName = _getParamName(value.param_id); + if(!_paramIO.contains(paramName)) { + qCWarning(CameraControlLog) << "Received PARAM_EXT_VALUE for unknown param:" << paramName; + return; + } + _paramIO[paramName]->handleParamValue(value); +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateActiveList() +{ + //-- Clear out excluded parameters based on exclusion rules + QStringList exclusionList; + foreach(QGCCameraOptionExclusion* param, _valueExclusions) { + Fact* pFact = getFact(param->param); + if(pFact) { + QString option = pFact->rawValueString(); + if(param->value == option) { + exclusionList << param->exclusions; + } + } + } + qCDebug(CameraControlLogVerbose) << "Excluding" << exclusionList; + _activeSettings.clear(); + foreach(QString key, _settings) { + if(!exclusionList.contains(key)) { + _activeSettings.append(key); + } + } + emit activeSettingsChanged(); +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processConditionTest(const QString conditionTest) +{ + enum { + TEST_NONE, + TEST_EQUAL, + TEST_NOT_EQUAL, + TEST_GREATER, + TEST_SMALLER + }; + qCDebug(CameraControlLogVerbose) << "_processConditionTest(" << conditionTest << ")"; + int op = TEST_NONE; + QStringList test; + if(conditionTest.contains("!=")) { + test = conditionTest.split("!=", QString::SkipEmptyParts); + op = TEST_NOT_EQUAL; + } else if(conditionTest.contains("=")) { + test = conditionTest.split("=", QString::SkipEmptyParts); + op = TEST_EQUAL; + } else if(conditionTest.contains(">")) { + test = conditionTest.split(">", QString::SkipEmptyParts); + op = TEST_GREATER; + } else if(conditionTest.contains("<")) { + test = conditionTest.split("<", QString::SkipEmptyParts); + op = TEST_SMALLER; + } + if(test.size() == 2) { + Fact* pFact = getFact(test[0]); + if(pFact) { + switch(op) { + case TEST_EQUAL: + return pFact->rawValueString() == test[1]; + case TEST_NOT_EQUAL: + return pFact->rawValueString() != test[1]; + case TEST_GREATER: + return pFact->rawValueString() > test[1]; + case TEST_SMALLER: + return pFact->rawValueString() < test[1]; + case TEST_NONE: + break; + } + } else { + qWarning() << "Invalid condition parameter:" << test[0] << "in" << conditionTest; + return false; + } + } + qWarning() << "Invalid condition" << conditionTest; + return false; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_processCondition(const QString condition) +{ + qCDebug(CameraControlLogVerbose) << "_processCondition(" << condition << ")"; + bool result = true; + bool andOp = true; + if(!condition.isEmpty()) { + QStringList scond = condition.split(" ", QString::SkipEmptyParts); + while(scond.size()) { + QString test = scond.first(); + scond.removeFirst(); + if(andOp) { + result = result && _processConditionTest(test); + } else { + result = result || _processConditionTest(test); + } + if(!scond.size()) { + return result; + } + andOp = scond.first().toUpper() == "AND"; + scond.removeFirst(); + } + } + return result; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateRanges(Fact* pFact) +{ + QStringList changedList; + QStringList resetList; + qCDebug(CameraControlLogVerbose) << "_updateRanges(" << pFact->name() << ")"; + //-- Iterate range sets + foreach(QGCCameraOptionRange* pRange, _optionRanges) { + //-- If this fact or one of its conditions is part of this range set + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pRFact = getFact(pRange->param); //-- This parameter + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(pRFact && pTFact) { + QString option = pRFact->rawValueString(); //-- This parameter value + //-- If this value (and condition) triggers a change in the target range + if(pRange->value == option && _processCondition(pRange->condition)) { + //-- Set limited range set + pTFact->setEnumInfo(pRange->optNames, pRange->optVariants); + emit pTFact->enumStringsChanged(); + emit pTFact->enumValuesChanged(); + qCDebug(CameraControlLogVerbose) << "Limited:" << pRange->targetParam << pRange->optNames; + changedList << pRange->targetParam; + } else { + if(!resetList.contains(pRange->targetParam)) { + //-- Restore full option set + pTFact->setEnumInfo(_originalOptNames[pRange->targetParam], _originalOptValues[pRange->targetParam]); + emit pTFact->enumStringsChanged(); + emit pTFact->enumValuesChanged(); + qCDebug(CameraControlLogVerbose) << "Full:" << pRange->targetParam << _originalOptNames[pRange->targetParam]; + resetList << pRange->targetParam; + } + } + } + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_requestCameraSettings() +{ + qCDebug(CameraControlLog) << "_requestCameraSettings()"; + if(_vehicle) { + _vehicle->sendMavCommand( + _compID, // Target component + MAV_CMD_REQUEST_CAMERA_SETTINGS, // command id + false, // showError + 0, // Camera ID (0 for all, 1 for first, 2 for second, etc.) ==> TODO: Remove + 1); // Do Request + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleSettings(const mavlink_camera_settings_t& settings) +{ + _cameraMode = (CameraMode)settings.mode_id; + emit cameraModeChanged(); + qCDebug(CameraControlLog) << "handleSettings() Mode:" << _cameraMode; +} + +//----------------------------------------------------------------------------- +QStringList +QGCCameraControl::_loadExclusions(QDomNode option) +{ + QStringList exclusionList; + QDomElement optionElem = option.toElement(); + QDomNodeList excRoot = optionElem.elementsByTagName(kExclusions); + if(excRoot.size()) { + //-- Iterate options + QDomNode node = excRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList exclusions = elem.elementsByTagName(kExclusion); + for(int i = 0; i < exclusions.size(); i++) { + QString exclude = exclusions.item(i).toElement().text(); + if(!exclude.isEmpty()) { + exclusionList << exclude; + } + } + } + return exclusionList; +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadRanges(QDomNode option, const QString factName, QString paramValue) +{ + QDomElement optionElem = option.toElement(); + QDomNodeList rangeRoot = optionElem.elementsByTagName(kParameterranges); + if(rangeRoot.size()) { + QDomNode node = rangeRoot.item(0); + QDomElement elem = node.toElement(); + QDomNodeList parameterRanges = elem.elementsByTagName(kParameterrange); + //-- Iterate parameter ranges + for(int i = 0; i < parameterRanges.size(); i++) { + QString param; + QString condition; + QMap rangeList; + QDomNode paramRange = parameterRanges.item(i); + if(!read_attribute(paramRange, kParameter, param)) { + qCritical() << QString("Malformed option range for parameter %1").arg(factName); + return false; + } + read_attribute(paramRange, kCondition, condition); + QDomElement pelem = paramRange.toElement(); + QDomNodeList rangeOptions = pelem.elementsByTagName(kRoption); + QStringList optNames; + QStringList optValues; + //-- Iterate options + for(int i = 0; i < rangeOptions.size(); i++) { + QString optName; + QString optValue; + QDomNode roption = rangeOptions.item(i); + if(!read_attribute(roption, kName, optName)) { + qCritical() << QString("Malformed roption for parameter %1").arg(factName); + return false; + } + if(!read_attribute(roption, kValue, optValue)) { + qCritical() << QString("Malformed rvalue for parameter %1").arg(factName); + return false; + } + optNames << optName; + optValues << optValue; + } + if(optNames.size()) { + QGCCameraOptionRange* pRange = new QGCCameraOptionRange(this, factName, paramValue, param, condition, optNames, optValues); + _optionRanges.append(pRange); + qCDebug(CameraControlLogVerbose) << "New range limit:" << factName << paramValue << param << condition << optNames << optValues; + } + } + } + return true; +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_processRanges() +{ + //-- After all parameter are loaded, process parameter ranges + foreach(QGCCameraOptionRange* pRange, _optionRanges) { + Fact* pRFact = getFact(pRange->targetParam); + if(pRFact) { + for(int i = 0; i < pRange->optNames.size(); i++) { + QVariant optVariant; + QString errorString; + if (!pRFact->metaData()->convertAndValidateRaw(pRange->optValues[i], false, optVariant, errorString)) { + qWarning() << "Invalid roption value, name:" << pRange->targetParam + << " type:" << pRFact->metaData()->type() + << " value:" << pRange->optValues[i] + << " error:" << errorString; + } else { + pRange->optVariants << optVariant; + } + } + } + } +} + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::_loadNameValue(QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant) +{ + if(!read_attribute(option, kName, optName)) { + qCritical() << QString("Malformed option for parameter %1").arg(factName); + return false; + } + if(!read_attribute(option, kValue, optValue)) { + qCritical() << QString("Malformed value for parameter %1").arg(factName); + return false; + } + QString errorString; + if (!metaData->convertAndValidateRaw(optValue, false, optVariant, errorString)) { + qWarning() << "Invalid option value, name:" << factName + << " type:" << metaData->type() + << " value:" << optValue + << " error:" << errorString; + } + return true; +} diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h new file mode 100644 index 000000000..7bf7c7731 --- /dev/null +++ b/src/Camera/QGCCameraControl.h @@ -0,0 +1,168 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +#pragma once + +#include "QGCApplication.h" +#include + +class QDomNode; +class QDomNodeList; +class QGCCameraParamIO; + +Q_DECLARE_LOGGING_CATEGORY(CameraControlLog) +Q_DECLARE_LOGGING_CATEGORY(CameraControlLogVerbose) + +//----------------------------------------------------------------------------- +class QGCCameraOptionExclusion : public QObject +{ +public: + QGCCameraOptionExclusion(QObject* parent, QString param_, QString value_, QStringList exclusions_) + : QObject(parent) + , param(param_) + , value(value_) + , exclusions(exclusions_) + { + } + QString param; + QString value; + QStringList exclusions; +}; + +//----------------------------------------------------------------------------- +class QGCCameraOptionRange : public QObject +{ +public: + QGCCameraOptionRange(QObject* parent, QString param_, QString value_, QString targetParam_, QString condition_, QStringList optNames_, QStringList optValues_) + : QObject(parent) + , param(param_) + , value(value_) + , targetParam(targetParam_) + , condition(condition_) + , optNames(optNames_) + , optValues(optValues_) + { + } + QString param; + QString value; + QString targetParam; + QString condition; + QStringList optNames; + QStringList optValues; + QVariantList optVariants; +}; + +//----------------------------------------------------------------------------- +class QGCCameraControl : public FactGroup +{ + Q_OBJECT +public: + QGCCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL); + ~QGCCameraControl(); + + //-- cam_mode + enum CameraMode { + CAMERA_MODE_UNDEFINED = -1, + CAMERA_MODE_PHOTO = 0, + CAMERA_MODE_VIDEO = 1, + }; + + Q_ENUMS(CameraMode) + + Q_PROPERTY(int version READ version NOTIFY infoChanged) + Q_PROPERTY(QString modelName READ modelName NOTIFY infoChanged) + Q_PROPERTY(QString vendor READ vendor NOTIFY infoChanged) + Q_PROPERTY(QString firmwareVersion READ firmwareVersion NOTIFY infoChanged) + Q_PROPERTY(qreal focalLength READ focalLength NOTIFY infoChanged) + Q_PROPERTY(QSizeF sensorSize READ sensorSize NOTIFY infoChanged) + Q_PROPERTY(QSize resolution READ resolution NOTIFY infoChanged) + 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 photosInVideoMode READ photosInVideoMode NOTIFY infoChanged) + Q_PROPERTY(bool videoInPhotoMode READ videoInPhotoMode NOTIFY infoChanged) + Q_PROPERTY(bool isBasic READ isBasic NOTIFY infoChanged) + + Q_PROPERTY(QStringList activeSettings READ activeSettings NOTIFY activeSettingsChanged) + Q_PROPERTY(CameraMode cameraMode READ cameraMode WRITE setCameraMode NOTIFY cameraModeChanged) + + Q_INVOKABLE void setVideoMode (); + Q_INVOKABLE void setPhotoMode (); + Q_INVOKABLE void toggleMode (); + Q_INVOKABLE void takePhoto (); + Q_INVOKABLE void startVideo (); + Q_INVOKABLE void stopVideo (); + Q_INVOKABLE void resetSettings (); + Q_INVOKABLE void formatCard (int id = 1); + + int version () { return _version; } + QString modelName () { return _modelName; } + QString vendor () { return _vendor; } + QString firmwareVersion (); + qreal focalLength () { return (qreal)_info.focal_length; } + QSizeF sensorSize () { return QSizeF(_info.sensor_size_h, _info.sensor_size_v); } + QSize resolution () { return QSize(_info.resolution_h, _info.resolution_v); } + bool capturesVideo () { return true /*_info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO*/ ; } + bool capturesPhotos () { return true /*_info.flags & CAMERA_CAP_FLAGS_CAPTURE_PHOTO*/ ; } + bool hasModes () { return true /*_info.flags & CAMERA_CAP_FLAGS_HAS_MODES*/ ; } + bool photosInVideoMode () { return true /*_info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_PHOTO_IN_VIDEO_MODE*/ ; } + bool videoInPhotoMode () { return false /*_info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_PHOTO_MODE*/ ; } + + int compID () { return _compID; } + bool isBasic () { return _settings.size() == 0; } + CameraMode cameraMode () { return _cameraMode; } + QStringList activeSettings () { return _activeSettings; } + + void setCameraMode (CameraMode mode); + + void handleSettings (const mavlink_camera_settings_t& settings); + void handleParamAck (const mavlink_param_ext_ack_t& ack); + void handleParamValue (const mavlink_param_ext_value_t& value); + void factChanged (Fact* pFact); + +signals: + void infoChanged (); + void cameraModeChanged (); + void activeSettingsChanged (); + +private slots: + void _requestCameraSettings (); + void _requestAllParameters (); + +private: + bool _handleLocalization (QByteArray& bytes); + bool _replaceLocaleStrings (const QDomNode node, QByteArray& bytes); + bool _loadCameraDefinitionFile (const QString& file); + bool _loadConstants (const QDomNodeList nodeList); + bool _loadSettings (const QDomNodeList nodeList); + void _processRanges (); + bool _processCondition (const QString condition); + bool _processConditionTest (const QString conditionTest); + bool _loadNameValue (QDomNode option, const QString factName, FactMetaData* metaData, QString& optName, QString& optValue, QVariant& optVariant); + bool _loadRanges (QDomNode option, const QString factName, QString paramValue); + void _updateActiveList (); + void _updateRanges (Fact* pFact); + + QStringList _loadExclusions (QDomNode option); + QString _getParamName (const char* param_id); + +private: + Vehicle* _vehicle; + int _compID; + mavlink_camera_information_t _info; + int _version; + QString _modelName; + QString _vendor; + CameraMode _cameraMode; + QStringList _activeSettings; + QStringList _settings; + QList _valueExclusions; + QList _optionRanges; + QMap _originalOptNames; + QMap _originalOptValues; + QMap _paramIO; +}; diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc new file mode 100644 index 000000000..55c1e1117 --- /dev/null +++ b/src/Camera/QGCCameraIO.cc @@ -0,0 +1,235 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +#include "QGCCameraControl.h" +#include "QGCCameraIO.h" + +QGC_LOGGING_CATEGORY(CameraIOLog, "CameraIOLog") +QGC_LOGGING_CATEGORY(CameraIOLogVerbose, "CameraIOLogVerbose") + +//----------------------------------------------------------------------------- +QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicle *vehicle) + : QObject(control) + , _control(control) + , _fact(fact) + , _vehicle(vehicle) + , _sentRetries(0) + , _requestRetries(0) +{ + _paramWriteTimer.setSingleShot(true); + _paramWriteTimer.setInterval(3000); + _paramRequestTimer.setSingleShot(true); + _paramRequestTimer.setInterval(3000); + connect(&_paramWriteTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramWriteTimeout); + connect(&_paramRequestTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramRequestTimeout); + connect(_fact, &Fact::rawValueChanged, this, &QGCCameraParamIO::_factChanged); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::setParamRequest() +{ + _paramRequestReceived = false; + _requestRetries = 0; + _paramRequestTimer.start(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_factChanged(QVariant value) +{ + Q_UNUSED(value); + qCDebug(CameraIOLog) << "Fact" << _fact->name() << "changed"; + _sendParameter(); + //-- TODO: Do we really want to update the UI now or only when we receive the ACK? + _control->factChanged(_fact); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_sendParameter() +{ + //-- TODO: We should use something other than mavlink_param_union_t for PARAM_EXT_SET + mavlink_param_ext_set_t p; + mavlink_param_union_t union_value; + memset(&p, 0, sizeof(p)); + FactMetaData::ValueType_t factType = _fact->type(); + p.param_type = _factTypeToMavType(factType); + switch (factType) { + case FactMetaData::valueTypeUint8: + union_value.param_uint8 = (uint8_t)_fact->rawValue().toUInt(); + break; + case FactMetaData::valueTypeInt8: + union_value.param_int8 = (int8_t)_fact->rawValue().toInt(); + break; + case FactMetaData::valueTypeUint16: + union_value.param_uint16 = (uint16_t)_fact->rawValue().toUInt(); + break; + case FactMetaData::valueTypeInt16: + union_value.param_int16 = (int16_t)_fact->rawValue().toInt(); + break; + case FactMetaData::valueTypeUint32: + union_value.param_uint32 = (uint32_t)_fact->rawValue().toUInt(); + break; + case FactMetaData::valueTypeFloat: + union_value.param_float = _fact->rawValue().toFloat(); + break; + default: + qCritical() << "Unsupported fact type" << factType; + // fall through + case FactMetaData::valueTypeInt32: + union_value.param_int32 = (int32_t)_fact->rawValue().toInt(); + break; + } + memcpy(&p.param_value, &union_value.param_float, sizeof(float)); + p.target_system = (uint8_t)_vehicle->id(); + p.target_component = (uint8_t)_control->compID(); + strncpy(p.param_id, _fact->name().toStdString().c_str(), sizeof(p.param_id)); + MAVLinkProtocol* pMavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_msg_param_ext_set_encode_chan(pMavlink->getSystemId(), + pMavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &_msg, + &p); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), _msg); + _paramWriteTimer.start(); +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_paramWriteTimeout() +{ + if(++_sentRetries > 3) { + qCWarning(CameraIOLog) << "No response for param set:" << _fact->name(); + } else { + //-- Send it again + qCDebug(CameraIOLog) << "Param set retry:" << _fact->name(); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), _msg); + _paramWriteTimer.start(); + } +} + +//----------------------------------------------------------------------------- +MAV_PARAM_TYPE +QGCCameraParamIO::_factTypeToMavType(FactMetaData::ValueType_t factType) +{ + //-- TODO: Even though we don't use anything larger than 32-bit, this should + // probably be updated. + switch (factType) { + case FactMetaData::valueTypeUint8: + return MAV_PARAM_TYPE_UINT8; + case FactMetaData::valueTypeInt8: + return MAV_PARAM_TYPE_INT8; + case FactMetaData::valueTypeUint16: + return MAV_PARAM_TYPE_UINT16; + case FactMetaData::valueTypeInt16: + return MAV_PARAM_TYPE_INT16; + case FactMetaData::valueTypeUint32: + return MAV_PARAM_TYPE_UINT32; + case FactMetaData::valueTypeFloat: + return MAV_PARAM_TYPE_REAL32; + default: + qWarning() << "Unsupported fact type" << factType; + // fall through + case FactMetaData::valueTypeInt32: + return MAV_PARAM_TYPE_INT32; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack) +{ + _paramWriteTimer.stop(); + if(ack.param_result == PARAM_ACK_ACCEPTED) { + _fact->setRawValue(_valueFromMessage(ack.param_value, ack.param_type), true); + } + if(ack.param_result == PARAM_ACK_IN_PROGRESS) { + //-- Wait a bit longer for this one + qCDebug(CameraIOLogVerbose) << "Param set in progress:" << _fact->name(); + _paramWriteTimer.start(); + } else { + if(ack.param_result == PARAM_ACK_FAILED) { + qCWarning(CameraIOLog) << "Param set failed:" << _fact->name(); + } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) { + qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name(); + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value) +{ + _paramRequestTimer.stop(); + _fact->setRawValue(_valueFromMessage(value.param_value, value.param_type), true); + _paramRequestReceived = true; + qCDebug(CameraIOLog) << QString("handleParamValue() %1 %2").arg(_fact->name()).arg(_fact->rawValueString()); +} + +//----------------------------------------------------------------------------- +QVariant +QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type) +{ + //-- TODO: Even though we don't use anything larger than 32-bit, this should + // probably be updated. + QVariant var; + mavlink_param_union_t u; + memcpy(&u.param_float, value, sizeof(float)); + switch (param_type) { + case MAV_PARAM_TYPE_REAL32: + var = QVariant(u.param_float); + break; + case MAV_PARAM_TYPE_UINT8: + var = QVariant(u.param_uint8); + break; + case MAV_PARAM_TYPE_INT8: + var = QVariant(u.param_int8); + break; + case MAV_PARAM_TYPE_UINT16: + var = QVariant(u.param_uint16); + break; + case MAV_PARAM_TYPE_INT16: + var = QVariant(u.param_int16); + break; + case MAV_PARAM_TYPE_UINT32: + var = QVariant(u.param_uint32); + break; + case MAV_PARAM_TYPE_INT32: + var = QVariant(u.param_int32); + break; + default: + var = QVariant(0); + qCritical() << "Invalid param_type used for camera setting:" << param_type; + } + return var; +} + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::_paramRequestTimeout() +{ + if(++_requestRetries > 3) { + qCWarning(CameraIOLog) << "No response for param request:" << _fact->name(); + } else { + //-- Request it again + qCDebug(CameraIOLog) << "Param request retry:" << _fact->name(); + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_msg_param_ext_request_read_pack_chan( + mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _control->compID(), + _fact->name().toStdString().c_str(), + -1); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _paramRequestTimer.start(); + } +} diff --git a/src/Camera/QGCCameraIO.h b/src/Camera/QGCCameraIO.h new file mode 100644 index 000000000..8f05cc775 --- /dev/null +++ b/src/Camera/QGCCameraIO.h @@ -0,0 +1,49 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +#pragma once + +#include "QGCApplication.h" +#include + +class QGCCameraControl; + +Q_DECLARE_LOGGING_CATEGORY(CameraIOLog) +Q_DECLARE_LOGGING_CATEGORY(CameraIOLogVerbose) + +//----------------------------------------------------------------------------- +class QGCCameraParamIO : public QObject +{ +public: + QGCCameraParamIO(QGCCameraControl* control, Fact* fact, Vehicle* vehicle); + + void handleParamAck (const mavlink_param_ext_ack_t& ack); + void handleParamValue (const mavlink_param_ext_value_t& value); + void setParamRequest (); + +private slots: + void _factChanged (QVariant value); + void _paramWriteTimeout (); + void _paramRequestTimeout(); + +private: + void _sendParameter (); + QVariant _valueFromMessage (const char* value, uint8_t param_type); + MAV_PARAM_TYPE _factTypeToMavType (FactMetaData::ValueType_t factType); + +private: + QGCCameraControl* _control; + Fact* _fact; + Vehicle* _vehicle; + int _sentRetries; + int _requestRetries; + bool _paramRequestReceived; + mavlink_message_t _msg; + QTimer _paramWriteTimer; + QTimer _paramRequestTimer; +}; + diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc new file mode 100644 index 000000000..882890646 --- /dev/null +++ b/src/Camera/QGCCameraManager.cc @@ -0,0 +1,172 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +#include "QGCApplication.h" +#include "QGCCameraManager.h" + +QGC_LOGGING_CATEGORY(CameraManagerLog, "CameraManagerLog") + +//----------------------------------------------------------------------------- +QGCCameraManager::QGCCameraManager(Vehicle *vehicle) + : _vehicle(vehicle) + , _vehicleReadyState(false) + , _cameraCount(0) + , _currentTask(0) +{ + qCDebug(CameraManagerLog) << "QGCCameraManager Created"; + connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady); + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived); +} + +//----------------------------------------------------------------------------- +QGCCameraManager::~QGCCameraManager() +{ +} + +//----------------------------------------------------------------------------- +QString +QGCCameraManager::controllerSource() +{ + return QStringLiteral("/qml/CameraControl.qml"); +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_vehicleReady(bool ready) +{ + qCDebug(CameraManagerLog) << "_vehicleReady(" << ready << ")"; + if(ready) { + if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle() == _vehicle) { + _vehicleReadyState = true; + } + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + _handleHeartbeat(message); + break; + case MAVLINK_MSG_ID_CAMERA_INFORMATION: + _handleCameraInfo(message); + break; + case MAVLINK_MSG_ID_CAMERA_SETTINGS: + _handleCameraSettings(message); + break; + case MAVLINK_MSG_ID_PARAM_EXT_ACK: + _handleParamAck(message); + break; + case MAVLINK_MSG_ID_PARAM_EXT_VALUE: + _handleParamValue(message); + break; + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) +{ + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(&message, &heartbeat); + //-- If this heartbeat is from a different node within the vehicle + if(_vehicleReadyState && _vehicle->id() == message.sysid && _vehicle->defaultComponentId() != message.compid) { + if(!_cameraInfoRequested[message.compid]) { + _cameraInfoRequested[message.compid] = true; + //-- Request camera info + _requestCameraInfo(message.compid); + } + } +} + +//----------------------------------------------------------------------------- +QGCCameraControl* +QGCCameraManager::_findCamera(int id) +{ + for(int i = 0; i < _cameras.count(); i++) { + if(_cameras[i]) { + QGCCameraControl* pCamera = qobject_cast(_cameras[i]); + if(pCamera) { + if(pCamera->compID() == id) { + return pCamera; + } + } else { + qCritical() << "Null QGCCameraControl instance"; + } + } + } + qWarning() << "Camera id not found:" << id; + return NULL; +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) +{ + 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]; + _cameraCount = info.camera_count; + QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); + if(pCamera) { + _cameras.append(pCamera); + emit camerasChanged(); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleCameraSettings(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_camera_settings_t settings; + mavlink_msg_camera_settings_decode(&message, &settings); + pCamera->handleSettings(settings); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleParamAck(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_param_ext_ack_t ack; + mavlink_msg_param_ext_ack_decode(&message, &ack); + pCamera->handleParamAck(ack); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleParamValue(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_param_ext_value_t value; + mavlink_msg_param_ext_value_decode(&message, &value); + pCamera->handleParamValue(value); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraManager::_requestCameraInfo(int compID) +{ + qCDebug(CameraManagerLog) << "_requestCameraInfo(" << compID << ")"; + if(_vehicle) { + _vehicle->sendMavCommand( + compID, // target component + MAV_CMD_REQUEST_CAMERA_INFORMATION, // command id + false, // showError + 0, // Camera ID (0 for all, 1 for first, 2 for second, etc.) + 1); // Do Request + } +} diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h new file mode 100644 index 000000000..0ba36be2e --- /dev/null +++ b/src/Camera/QGCCameraManager.h @@ -0,0 +1,60 @@ +/*! + * @file + * @brief Camera Controller + * @author Gus Grubba + * + */ + +#pragma once + +#include "QGCApplication.h" +#include +#include "QmlObjectListModel.h" +#include "QGCCameraControl.h" + +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(CameraManagerLog) + +//----------------------------------------------------------------------------- +class QGCCameraManager : public QObject +{ + Q_OBJECT +public: + QGCCameraManager(Vehicle* vehicle); + ~QGCCameraManager(); + + Q_PROPERTY(QmlObjectListModel* cameras READ cameras NOTIFY camerasChanged) + Q_PROPERTY(QString controllerSource READ controllerSource NOTIFY controllerSourceChanged) + + //-- Return a list of cameras provided by this vehicle + virtual QmlObjectListModel* cameras () { return &_cameras; } + //-- Camera controller source (QML) + virtual QString controllerSource (); + +private slots: + void _vehicleReady (bool ready); + void _mavlinkMessageReceived (const mavlink_message_t& message); + +signals: + void camerasChanged (); + void controllerSourceChanged (); + +private: + QGCCameraControl* _findCamera (int id); + void _requestCameraInfo (int compID); + void _handleHeartbeat (const mavlink_message_t& message); + void _handleCameraInfo (const mavlink_message_t& message); + void _handleCameraSettings (const mavlink_message_t& message); + void _handleParamAck (const mavlink_message_t& message); + void _handleParamValue (const mavlink_message_t& message); + +private: + Vehicle* _vehicle; + bool _vehicleReadyState; + int _cameraCount; + int _currentTask; + QmlObjectListModel _cameras; + QMap _cameraInfoRequested; +}; diff --git a/src/Camera/camera_definition_example.json b/src/Camera/camera_definition_example.json new file mode 100644 index 000000000..27a833df2 --- /dev/null +++ b/src/Camera/camera_definition_example.json @@ -0,0 +1,279 @@ +{ + "definition": { + "version": 1, + "schema_version": 1, + "model": "SD II", + "manufacturer": "Super Dupper Industries", + "minFocalLength": 9.2, + "maxFocalLength": 9.2, + "sensorWidthPix": 4000, + "sensorHeightPix": 3000, + "sensorWidth": 12.4, + "sensorHeight": 9.3, + "capturePhoto": 1, + "captureVideo": 1, + "hasSeparateModes": 1, + "spotWsize": 0.1, + "spotHsize": 0.1, + "centerWSize": 0.1, + "centerHSize": 0.1 + }, + "settings": + [ + { + "name": "CAM_MODE", + "shortDescription": "Camera Mode", + "type": "uint32", + "enumStrings": ["Photo" , "Video"], + "enumValues": [0, 1], + "defaultValue": 1 + }, + { + "name": "CAM_WBMODE", + "shortDescription": "White Balance Mode", + "type": "uint32", + "enumStrings": ["Auto" , "Sunny", "Cloudy", "Fluorescent", "Incandescent", "Sunset"], + "enumValues": [0, 1, 2, 3 , 4, 5], + "defaultValue": 0 + }, + { + "name": "CAM_EXPMODE", + "shortDescription": "Exposure Mode", + "type": "uint32", + "enumStrings": ["Auto" , "Manual", "Shutter Priority", "Aperture Priority"], + "enumValues": [0, 1, 2, 3], + "defaultValue": 0 + }, + { + "name": "CAM_APERTURE", + "shortDescription": "Aperture", + "type": "uint32", + "enumStrings": ["1.8", "2.0", "2.4", "2.8"], + "enumValues": [0, 1, 2, 3], + "defaultValue": 0 + }, + { + "name": "CAM_SHUTTERSPD", + "shortDescription": "Shutter Speed", + "type": "float", + "enumStrings": ["4", "3", "2", "1", "1/4", "1/8", "1/15", "1/30", "1/60", "1/125", "1/250", "1/500", "1/1000"], + "enumValues": [4.0, 3.0, 2.0, 1.0, 0.25, 0.125, 0.066666, 0.033333, 0.016666, 0.008, 0.004, 0.002, 0.001], + "defaultValue": 8 + }, + { + "name": "CAM_ISO", + "shortDescription": "ISO", + "type": "uint32", + "enumStrings": ["100", "200", "400", "800", "1600", "3200", "6400"], + "enumValues": [100, 200, 400, 800, 1600, 3200, 6400], + "defaultValue": 100 + }, + { + "name": "CAM_EV", + "shortDescription": "Exposure Compensation", + "type": "float", + "enumStrings": ["-2.0", "-1.5", "-1.0", "-0.5", "0", "+0.5", "+1.0", "+1.5", "+2.0"], + "enumValues": [-2.0, -1.5, -1.0, -0.5, 0, 0.5, 1.0, 1.5, 2.0], + "defaultValue": 0 + }, + { + "name": "CAM_VIDRES", + "shortDescription": "Video Resolution", + "type": "uint32", + "enumStrings": ["3840x2160 30P", "3840x2160 24P", "1920x1080 60P", "1920x1080 30P", "1920x1080 24P"], + "enumValues": [0, 1, 2, 3, 4], + "defaultValue": 0 + }, + { + "name": "CAM_VIDFMT", + "shortDescription": "Video Format", + "type": "uint32", + "enumStrings": ["h.264", "HEVC"], + "enumValues": [0, 1], + "defaultValue": 0 + }, + { + "name": "CAM_AUDIOREC", + "shortDescription": "Record Audio", + "type": "bool", + "defaultValue": 0 + }, + { + "name": "CAM_METERING", + "shortDescription": "Metering Mode", + "type": "uint32", + "enumStrings": ["Average", "Center", "Spot"], + "enumValues": [0, 1, 2], + "defaultValue": 0 + }, + { + "name": "CAM_SPOTX", + "shortDescription": "Spot Metering X Coordinate", + "type": "double", + "defaultValue": 0.45, + "min": 0, + "max": 0.9, + "control": false + }, + { + "name": "CAM_SPOTY", + "shortDescription": "Spot Metering Y Coordinate", + "type": "double", + "defaultValue": 0.45, + "min": 0, + "max": 0.9, + "control": false + }, + { + "name": "CAM_COLORMODE", + "shortDescription": "Color Mode", + "type": "uint32", + "enumStrings": ["Neutral", "Enhanced", "Unprocessed", "Night"], + "enumValues": [0, 1, 2, 4], + "defaultValue": 1 + }, + { + "name": "CAM_PHOTORES", + "shortDescription": "Photo Resolution", + "type": "uint32", + "enumStrings": ["Large", "Medium", "Small"], + "enumValues": [0, 1, 2], + "defaultValue": 0 + }, + { + "name": "CAM_PHOTOFMT", + "shortDescription": "Image Format", + "type": "uint32", + "enumStrings": ["Jpeg", "Raw", "Jpeg+Raw"], + "enumValues": [0, 1, 2], + "defaultValue": 0 + }, + { + "name": "CAM_PHOTOQUAL", + "shortDescription": "Image Quality", + "type": "uint32", + "enumStrings": ["Fine", "Medium", "Low"], + "enumValues": [0, 1, 2], + "defaultValue": 1 + }, + { + "name": "CAM_FLICKER", + "shortDescription": "Flickering Mode", + "type": "uint32", + "enumStrings": ["Auto", "50Hz", "60Hz"], + "enumValues": [0, 1, 2], + "defaultValue": 0 + } + ], + "videoStreams": + [ + { + "VisibleLight": + { + "description": "Visible Light", + "url": "rtsp://192.168.92.1:1525/live", + "exclusive": 0, + "layer": 0 + } + }, + { + "ThermalImaging": + { + "description": "Thermal Imaging", + "url": "rtsp://192.168.92.1:4525/live", + "exclusive": 0, + "layer": 1 + } + } + ], + "settingRangeComment1": "Example: If CAM_MODE is set to 1, the valid enum range for CAM_ISO is 0-5", + "settingRangeComment2": "Example: If CAM_VIDRES is set to 0 and CAM_EXPMODE is 1-3 and CAM_MODE is 1, the valid enum range for CAM_SHUTTERSPD is 7-12", + "settingRange": + { + "CAM_MODE": + { + "options": + { + "1": + { + "CAM_ISO": [0, 1, 2, 3, 4, 5] + } + } + }, + "CAM_VIDRES": + { + "conditions": + [ + { + "parameter": "CAM_EXPMODE", + "operator": ">", + "value": 0 + }, + { + "parameter": "CAM_MODE", + "operator": "=", + "value": 1 + } + ], + "options": + { + "0": + { + "CAM_SHUTTERSPD": [7, 8, 9, 10, 11, 12] + }, + "1": + { + "CAM_SHUTTERSPD": [7, 8, 9, 10, 11, 12] + }, + "2": + { + "CAM_SHUTTERSPD": [8, 9, 10, 11, 12] + }, + "3": + { + "CAM_SHUTTERSPD": [7, 8, 9, 10, 11, 12] + }, + "4": + { + "CAM_SHUTTERSPD": [7, 8, 9, 10, 11, 12] + } + } + } + }, + "settingExclusionComment": "Example: If CAM_EXPMODE is set to 0, CAM_ISO, CAM_SHUTTERSPD and CAM_APERTURE are excluded (not shown)", + "settingExclusions": + { + "CAM_EXPMODE": + { + "0": ["CAM_ISO", "CAM_SHUTTERSPD", "CAM_APERTURE"], + "1": ["CAM_EV"] + }, + "CAM_MODE": + { + "0": ["CAM_VIDRES", "CAM_VIDFMT", "CAM_AUDIOREC"], + "1": ["CAM_PHOTOFMT", "CAM_PHOTOQUAL", "CAM_PHOTORES"] + }, + "CAM_PHOTOFMT": + { + "1": ["CAM_PHOTOQUAL", "CAM_PHOTORES"] + } + }, + "localization": + [ + { + "locale": "pt_BR", + "strings": + { + "Exposure Mode": "Modo de Exposição", + "Auto": "Automático", + "Manual": "Manual", + "Shutter Priority": "Prioridade de Exposição", + "Aperture Priority": "Prioridade de Abertura", + "Aperture": "Abertura", + "Shutter Speed": "Velocidade", + "Exposure Compensation": "Compensação de Exposição", + "Video Resolution": "Resolução de Video" + } + } + ] +} \ No newline at end of file diff --git a/src/Camera/camera_definition_example.xml b/src/Camera/camera_definition_example.xml new file mode 100644 index 000000000..fa8fd9eee --- /dev/null +++ b/src/Camera/camera_definition_example.xml @@ -0,0 +1,301 @@ + + + + SD II + Super Dupper Industries + + + + Camera Mode + + + + + + + White Balance Mode + + + + + Exposure Mode + + + + + + + Aperture + + + + + Shutter Speed + + + + + ISO + + + + + Exposure Compensation + + + + + Video Resolution + + + + + + + + + + Video Format + + + + + Record Audio + + + Metering Mode + + + + + Color Mode + + + + + Photo Resolution + + + + + Image Format + + + + + + Image Quality + + + + + + + + + + Visible Light + rtsp://192.168.92.1:1525/live + + + Thermal Imaging + rtsp://192.168.92.1:4525/live + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Camera/images/camera_photo.svg b/src/Camera/images/camera_photo.svg new file mode 100644 index 000000000..5348c2262 --- /dev/null +++ b/src/Camera/images/camera_photo.svg @@ -0,0 +1,13 @@ + + + + + + + + diff --git a/src/Camera/images/camera_settings.svg b/src/Camera/images/camera_settings.svg new file mode 100644 index 000000000..78a6a1393 --- /dev/null +++ b/src/Camera/images/camera_settings.svg @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 31 + + 31 + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Camera/images/camera_video.svg b/src/Camera/images/camera_video.svg new file mode 100644 index 000000000..5d6fa1097 --- /dev/null +++ b/src/Camera/images/camera_video.svg @@ -0,0 +1,16 @@ + + + + + + + + + diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 606a0ce23..924758cf3 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -17,6 +17,8 @@ #include #include +static const char* kMissingMetadata = "Meta data pointer missing"; + Fact::Fact(QObject* parent) : QObject(parent) , _componentId(-1) @@ -86,11 +88,11 @@ void Fact::forceSetRawValue(const QVariant& value) emit rawValueChanged(_rawValue); } } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } } -void Fact::setRawValue(const QVariant& value) +void Fact::setRawValue(const QVariant& value, bool skipSignal) { if (_metaData) { QVariant typedValue; @@ -100,12 +102,14 @@ void Fact::setRawValue(const QVariant& value) if (typedValue != _rawValue) { _rawValue.setValue(typedValue); _sendValueChangedSignal(cookedValue()); - emit _containerRawValueChanged(rawValue()); - emit rawValueChanged(_rawValue); + if(!skipSignal) { + emit _containerRawValueChanged(rawValue()); + emit rawValueChanged(_rawValue); + } } } } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } } @@ -114,7 +118,7 @@ void Fact::setCookedValue(const QVariant& value) if (_metaData) { setRawValue(_metaData->cookedTranslator()(value)); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } } @@ -126,7 +130,7 @@ void Fact::setEnumStringValue(const QString& value) setCookedValue(_metaData->enumValues()[index]); } } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } } @@ -135,7 +139,7 @@ void Fact::setEnumIndex(int index) if (_metaData) { setCookedValue(_metaData->enumValues()[index]); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } } @@ -162,7 +166,7 @@ QVariant Fact::cookedValue(void) const if (_metaData) { return _metaData->rawTranslator()(_rawValue); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return _rawValue; } } @@ -175,7 +179,7 @@ QString Fact::enumStringValue(void) return _metaData->enumStrings()[enumIndex]; } } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } return QString(); @@ -183,23 +187,29 @@ QString Fact::enumStringValue(void) int Fact::enumIndex(void) { + static const double accuracy = 1.0 / 1000000.0; if (_metaData) { int index = 0; - foreach (QVariant enumValue, _metaData->enumValues()) { if (enumValue == rawValue()) { return index; } + //-- Float comparissons don't always work + if(type() == FactMetaData::valueTypeFloat || type() == FactMetaData::valueTypeDouble) { + double diff = fabs(enumValue.toDouble() - rawValue().toDouble()); + if(diff < accuracy) { + return index; + } + } index ++; } - // Current value is not in list, add it manually _metaData->addEnumInfo(QString("Unknown: %1").arg(rawValue().toString()), rawValue()); emit enumStringsChanged(); emit enumValuesChanged(); return index; } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } return -1; @@ -210,7 +220,7 @@ QStringList Fact::enumStrings(void) const if (_metaData) { return _metaData->enumStrings(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QStringList(); } } @@ -220,17 +230,26 @@ QVariantList Fact::enumValues(void) const if (_metaData) { return _metaData->enumValues(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariantList(); } } +void Fact::setEnumInfo(const QStringList& strings, const QVariantList& values) +{ + if (_metaData) { + return _metaData->setEnumInfo(strings, values); + } else { + qWarning() << kMissingMetadata; + } +} + QStringList Fact::bitmaskStrings(void) const { if (_metaData) { return _metaData->bitmaskStrings(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QStringList(); } } @@ -240,7 +259,7 @@ QVariantList Fact::bitmaskValues(void) const if (_metaData) { return _metaData->bitmaskValues(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariantList(); } } @@ -314,7 +333,7 @@ QVariant Fact::rawDefaultValue(void) const } return _metaData->rawDefaultValue(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariant(0); } } @@ -327,7 +346,7 @@ QVariant Fact::cookedDefaultValue(void) const } return _metaData->cookedDefaultValue(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariant(0); } } @@ -347,7 +366,7 @@ QString Fact::shortDescription(void) const if (_metaData) { return _metaData->shortDescription(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QString(); } } @@ -357,7 +376,7 @@ QString Fact::longDescription(void) const if (_metaData) { return _metaData->longDescription(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QString(); } } @@ -367,7 +386,7 @@ QString Fact::rawUnits(void) const if (_metaData) { return _metaData->rawUnits(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QString(); } } @@ -377,7 +396,7 @@ QString Fact::cookedUnits(void) const if (_metaData) { return _metaData->cookedUnits(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QString(); } } @@ -387,7 +406,7 @@ QVariant Fact::rawMin(void) const if (_metaData) { return _metaData->rawMin(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariant(0); } } @@ -397,7 +416,7 @@ QVariant Fact::cookedMin(void) const if (_metaData) { return _metaData->cookedMin(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariant(0); } } @@ -412,7 +431,7 @@ QVariant Fact::rawMax(void) const if (_metaData) { return _metaData->rawMax(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariant(0); } } @@ -422,7 +441,7 @@ QVariant Fact::cookedMax(void) const if (_metaData) { return _metaData->cookedMax(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QVariant(0); } } @@ -437,7 +456,7 @@ bool Fact::minIsDefaultForType(void) const if (_metaData) { return _metaData->minIsDefaultForType(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return false; } } @@ -447,7 +466,7 @@ bool Fact::maxIsDefaultForType(void) const if (_metaData) { return _metaData->maxIsDefaultForType(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return false; } } @@ -457,7 +476,7 @@ int Fact::decimalPlaces(void) const if (_metaData) { return _metaData->decimalPlaces(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return FactMetaData::defaultDecimalPlaces; } } @@ -467,7 +486,7 @@ QString Fact::group(void) const if (_metaData) { return _metaData->group(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QString(); } } @@ -487,7 +506,7 @@ bool Fact::valueEqualsDefault(void) const return false; } } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return false; } } @@ -497,7 +516,7 @@ bool Fact::defaultValueAvailable(void) const if (_metaData) { return _metaData->defaultValueAvailable(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return false; } } @@ -512,7 +531,7 @@ QString Fact::validate(const QString& cookedValue, bool convertOnly) return errorString; } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return QString("Internal error: Meta data pointer missing"); } } @@ -522,7 +541,7 @@ bool Fact::rebootRequired(void) const if (_metaData) { return _metaData->rebootRequired(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; return false; } } @@ -562,7 +581,7 @@ QString Fact::enumOrValueString(void) return cookedValueString(); } } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } return QString(); } @@ -572,7 +591,17 @@ double Fact::increment(void) const if (_metaData) { return _metaData->increment(); } else { - qWarning() << "Meta data pointer missing"; + qWarning() << kMissingMetadata; } return std::numeric_limits::quiet_NaN(); } + +bool Fact::hasControl(void) const +{ + if (_metaData) { + return _metaData->hasControl(); + } else { + qWarning() << kMissingMetadata; + return false; + } +} diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index c3db1f46b..6ffe373b3 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -64,6 +64,7 @@ public: Q_PROPERTY(double increment READ increment CONSTANT) Q_PROPERTY(bool typeIsString READ typeIsString CONSTANT) Q_PROPERTY(bool typeIsBool READ typeIsBool CONSTANT) + Q_PROPERTY(bool hasControl READ hasControl CONSTANT) /// Convert and validate value /// @param convertOnly true: validate type conversion only, false: validate against meta data as well @@ -106,11 +107,12 @@ public: double increment (void) const; bool typeIsString (void) const { return type() == FactMetaData::valueTypeString; } bool typeIsBool (void) const { return type() == FactMetaData::valueTypeBool; } + bool hasControl (void) const; /// Returns the values as a string with full 18 digit precision if float/double. QString rawValueStringFullPrecision(void) const; - void setRawValue (const QVariant& value); + void setRawValue (const QVariant& value, bool skipSignal = false); void setCookedValue (const QVariant& value); void setEnumIndex (int index); void setEnumStringValue (const QString& value); @@ -132,12 +134,15 @@ public: /// Sets the meta data associated with the Fact. void setMetaData(FactMetaData* metaData); + FactMetaData* metaData() { return _metaData; } void _containerSetRawValue(const QVariant& value); /// Generally you should not change the name of a fact. But if you know what you are doing, you can. void _setName(const QString& name) { _name = name; } - + /// Generally this is done during parsing. But if you know what you are doing, you can. + void setEnumInfo(const QStringList& strings, const QVariantList& values); + signals: void bitmaskStringsChanged(void); void bitmaskValuesChanged(void); diff --git a/src/FactSystem/FactGroup.cc b/src/FactSystem/FactGroup.cc index f7d594ff3..ee41fdc34 100644 --- a/src/FactSystem/FactGroup.cc +++ b/src/FactSystem/FactGroup.cc @@ -23,14 +23,30 @@ QGC_LOGGING_CATEGORY(FactGroupLog, "FactGroupLog") FactGroup::FactGroup(int updateRateMsecs, const QString& metaDataFile, QObject* parent) : QObject(parent) , _updateRateMSecs(updateRateMsecs) +{ + _setupTimer(); + _nameToFactMetaDataMap = FactMetaData::createMapFromJsonFile(metaDataFile, this); +} + +FactGroup::FactGroup(int updateRateMsecs, QObject* parent) + : QObject(parent) + , _updateRateMSecs(updateRateMsecs) +{ + _setupTimer(); +} + +void FactGroup::_loadFromJsonArray(const QJsonArray jsonArray) +{ + _nameToFactMetaDataMap = FactMetaData::createMapFromJsonArray(jsonArray, this); +} + +void FactGroup::_setupTimer() { if (_updateRateMSecs > 0) { connect(&_updateTimer, &QTimer::timeout, this, &FactGroup::_updateAllValues); _updateTimer.setSingleShot(false); _updateTimer.start(_updateRateMSecs); } - - _loadMetaData(metaDataFile); } Fact* FactGroup::getFact(const QString& name) @@ -107,8 +123,3 @@ void FactGroup::_updateAllValues(void) fact->sendDeferredValueChangedSignal(); } } - -void FactGroup::_loadMetaData(const QString& jsonFilename) -{ - _nameToFactMetaDataMap = FactMetaData::createMapFromJsonFile(jsonFilename, this); -} diff --git a/src/FactSystem/FactGroup.h b/src/FactSystem/FactGroup.h index a56c12f30..a05169964 100644 --- a/src/FactSystem/FactGroup.h +++ b/src/FactSystem/FactGroup.h @@ -27,6 +27,7 @@ class FactGroup : public QObject public: FactGroup(int updateRateMsecs, const QString& metaDataFile, QObject* parent = NULL); + FactGroup(int updateRateMsecs, QObject* parent = NULL); Q_PROPERTY(QStringList factNames READ factNames CONSTANT) Q_PROPERTY(QStringList factGroupNames READ factGroupNames CONSTANT) @@ -39,10 +40,11 @@ public: QStringList factNames(void) const { return _nameToFactMap.keys(); } QStringList factGroupNames(void) const { return _nameToFactGroupMap.keys(); } - + protected: void _addFact(Fact* fact, const QString& name); void _addFactGroup(FactGroup* factGroup, const QString& name); + void _loadFromJsonArray(const QJsonArray jsonArray); int _updateRateMSecs; ///< Update rate for Fact::valueChanged signals, 0: immediate update @@ -50,13 +52,14 @@ private slots: void _updateAllValues(void); private: - void _loadMetaData(const QString& filename); + void _setupTimer(); + QTimer _updateTimer; +protected: QMap _nameToFactMap; QMap _nameToFactGroupMap; QMap _nameToFactMetaDataMap; - QTimer _updateTimer; }; #endif diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index b55b1b954..a0a4d65f6 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -70,6 +70,7 @@ const char* FactMetaData::_unitsJsonKey = "units"; const char* FactMetaData::_defaultValueJsonKey = "defaultValue"; const char* FactMetaData::_minJsonKey = "min"; const char* FactMetaData::_maxJsonKey = "max"; +const char* FactMetaData::_hasControlJsonKey = "control"; FactMetaData::FactMetaData(QObject* parent) : QObject(parent) @@ -86,6 +87,7 @@ FactMetaData::FactMetaData(QObject* parent) , _cookedTranslator(_defaultTranslator) , _rebootRequired(false) , _increment(std::numeric_limits::quiet_NaN()) + , _hasControl(true) { } @@ -105,6 +107,7 @@ FactMetaData::FactMetaData(ValueType_t type, QObject* parent) , _cookedTranslator(_defaultTranslator) , _rebootRequired(false) , _increment(std::numeric_limits::quiet_NaN()) + , _hasControl(true) { } @@ -115,6 +118,27 @@ FactMetaData::FactMetaData(const FactMetaData& other, QObject* parent) *this = other; } +FactMetaData::FactMetaData(ValueType_t type, const QString name, QObject* parent) + : QObject(parent) + , _type(type) + , _decimalPlaces(unknownDecimalPlaces) + , _rawDefaultValue(0) + , _defaultValueAvailable(false) + , _group("*Default Group") + , _rawMax(_maxForType()) + , _maxIsDefaultForType(true) + , _rawMin(_minForType()) + , _minIsDefaultForType(true) + , _name(name) + , _rawTranslator(_defaultTranslator) + , _cookedTranslator(_defaultTranslator) + , _rebootRequired(false) + , _increment(std::numeric_limits::quiet_NaN()) + , _hasControl(true) +{ + +} + const FactMetaData& FactMetaData::operator=(const FactMetaData& other) { _decimalPlaces = other._decimalPlaces; @@ -139,6 +163,7 @@ const FactMetaData& FactMetaData::operator=(const FactMetaData& other) _cookedTranslator = other._cookedTranslator; _rebootRequired = other._rebootRequired; _increment = other._increment; + _hasControl = other._hasControl; return *this; } @@ -817,8 +842,8 @@ FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObjec // Validate key types QStringList keys; QList types; - keys << _nameJsonKey << _decimalPlacesJsonKey << _typeJsonKey << _shortDescriptionJsonKey << _longDescriptionJsonKey << _unitsJsonKey << _minJsonKey << _maxJsonKey; - types << QJsonValue::String << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Double; + keys << _nameJsonKey << _decimalPlacesJsonKey << _typeJsonKey << _shortDescriptionJsonKey << _longDescriptionJsonKey << _unitsJsonKey << _minJsonKey << _maxJsonKey << _hasControlJsonKey; + types << QJsonValue::String << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Double << QJsonValue::Bool; if (!JsonHelper::validateKeyTypes(json, keys, types, errorString)) { qWarning() << errorString; return new FactMetaData(valueTypeUint32, metaDataParent); @@ -836,7 +861,7 @@ FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObjec metaData->_name = json[_nameJsonKey].toString(); QStringList enumValues, enumStrings; - if (JsonHelper::parseEnum(json, enumStrings, enumValues, errorString)) { + if (JsonHelper::parseEnum(json, enumStrings, enumValues, errorString, metaData->name())) { for (int i=0; iconvertAndValidateRaw(json[_maxJsonKey].toVariant(), true /* convertOnly */, typedValue, errorString); metaData->setRawMax(typedValue); } + if (json.contains(_hasControlJsonKey)) { + metaData->setHasControl(json[_hasControlJsonKey].toBool()); + } else { + metaData->setHasControl(true); + } return metaData; } @@ -905,23 +935,27 @@ QMap FactMetaData::createMapFromJsonFile(const QString& } QJsonArray jsonArray = doc.array(); - for (int i=0; i FactMetaData::createMapFromJsonArray(const QJsonArray jsonArray, QObject* metaDataParent) +{ + QMap metaDataMap; + for (int i=0; iname())) { qWarning() << QStringLiteral("Duplicate fact name:") << metaData->name(); + delete metaData; } else { metaDataMap[metaData->name()] = metaData; } } - return metaDataMap; } diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index f4f88d5f1..b1daa7e57 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -47,9 +47,11 @@ public: FactMetaData(QObject* parent = NULL); FactMetaData(ValueType_t type, QObject* parent = NULL); + FactMetaData(ValueType_t type, const QString name, QObject* parent = NULL); FactMetaData(const FactMetaData& other, QObject* parent = NULL); static QMap createMapFromJsonFile(const QString& jsonFilename, QObject* metaDataParent); + static QMap createMapFromJsonArray(const QJsonArray jsonArray, QObject* metaDataParent); static FactMetaData* createFromJsonObject(const QJsonObject& json, QObject* metaDataParent); @@ -95,6 +97,7 @@ public: QString rawUnits (void) const { return _rawUnits; } QString cookedUnits (void) const { return _cookedUnits; } bool rebootRequired (void) const { return _rebootRequired; } + bool hasControl (void) const { return _hasControl; } /// Amount to increment value when used in controls such as spin button or slider with detents. /// NaN for no increment available. @@ -122,6 +125,7 @@ public: void setRawUnits (const QString& rawUnits); void setRebootRequired (bool rebootRequired) { _rebootRequired = rebootRequired; } void setIncrement (double increment) { _increment = increment; } + void setHasControl (bool bValue) { _hasControl = bValue; } void setTranslators(Translator rawTranslator, Translator cookedTranslator); @@ -216,6 +220,7 @@ private: Translator _cookedTranslator; bool _rebootRequired; double _increment; + bool _hasControl; // Exact conversion constants static const struct UnitConsts_s { @@ -247,6 +252,7 @@ private: static const char* _defaultValueJsonKey; static const char* _minJsonKey; static const char* _maxJsonKey; + static const char* _hasControlJsonKey; }; #endif diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 47b3e7475..683c863c5 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -517,3 +517,20 @@ bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitc yawSupported = false; return false; } + +QGCCameraManager* FirmwarePlugin::cameraManager(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + return NULL; +} + +QGCCameraControl* FirmwarePlugin::createCameraControl(const mavlink_camera_information_t *info, Vehicle *vehicle, int compID, QObject* parent) +{ + Q_UNUSED(info); + Q_UNUSED(vehicle); + Q_UNUSED(compID); + Q_UNUSED(parent); + return NULL; +} + + diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 0aae9367f..98e054cda 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -25,6 +25,8 @@ #include class Vehicle; +class QGCCameraControl; +class QGCCameraManager; /// This is the base class for Firmware specific plugins /// @@ -267,8 +269,15 @@ public: virtual const QVariantList& toolBarIndicators(const Vehicle* vehicle); /// Returns a list of CameraMetaData objects for available cameras on the vehicle. + /// TODO: This should go into QGCCameraManager virtual const QVariantList& cameraList(const Vehicle* vehicle); + /// Vehicle camera manager. Returns NULL if not supported. + virtual QGCCameraManager* cameraManager(Vehicle *vehicle); + + /// Camera control. Returns NULL if not supported. + virtual QGCCameraControl* createCameraControl(const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL); + /// Returns a pointer to a dictionary of firmware-specific FactGroups virtual QMap* factGroups(void); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index d04eb8682..0453c587d 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -21,6 +21,7 @@ #include "SensorsComponentController.h" #include "PowerComponentController.h" #include "RadioComponentController.h" +#include "QGCCameraManager.h" #include @@ -34,7 +35,8 @@ PX4FirmwarePluginInstanceData::PX4FirmwarePluginInstanceData(QObject* parent) } PX4FirmwarePlugin::PX4FirmwarePlugin(void) - : _manualFlightMode(tr("Manual")) + : _cameraManager(NULL) + , _manualFlightMode(tr("Manual")) , _acroFlightMode(tr("Acro")) , _stabilizedFlightMode(tr("Stabilized")) , _rattitudeFlightMode(tr("Rattitude")) @@ -124,6 +126,12 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void) } } +PX4FirmwarePlugin::~PX4FirmwarePlugin() +{ + if(_cameraManager) + delete _cameraManager; +} + AutoPilotPlugin* PX4FirmwarePlugin::autopilotPlugin(Vehicle* vehicle) { return new PX4AutoPilotPlugin(vehicle, vehicle); @@ -545,3 +553,18 @@ bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicl } return true; } + +QGCCameraManager* PX4FirmwarePlugin::cameraManager(Vehicle* vehicle) +{ + if(!_cameraManager) { + _cameraManager = new QGCCameraManager(vehicle); + } + return _cameraManager; +} + +QGCCameraControl* PX4FirmwarePlugin::createCameraControl(const mavlink_camera_information_t* info, Vehicle *vehicle, int compID, QObject* parent) +{ + return new QGCCameraControl(info, vehicle, compID, parent); +} + + diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index bda3803a9..d508931ad 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -25,6 +25,7 @@ class PX4FirmwarePlugin : public FirmwarePlugin public: PX4FirmwarePlugin(void); + ~PX4FirmwarePlugin(); // Overrides from FirmwarePlugin @@ -68,6 +69,8 @@ public: QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); } bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const override; QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("COM_DISARM_LAND"); } + QGCCameraManager* cameraManager (Vehicle* vehicle) override; + QGCCameraControl* createCameraControl (const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL) override; protected: typedef struct { @@ -80,6 +83,7 @@ protected: } FlightModeInfo_t; QList _flightModeInfoList; + QGCCameraManager* _cameraManager; // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the // names may change. diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 0a72adb67..ae6dcb71c 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -52,6 +52,8 @@ QGCView { property alias _altitudeSlider: altitudeSlider + readonly property var _dynamicCameras: _activeVehicle ? _activeVehicle.dynamicCameras : null + readonly property bool _isCamera: _dynamicCameras ? _dynamicCameras.cameras.count > 0 : false readonly property bool isBackgroundDark: _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true readonly property real _defaultRoll: 0 readonly property real _defaultPitch: 0 @@ -370,7 +372,7 @@ QGCView { anchors.right: _flightVideo.right height: ScreenTools.defaultFontPixelHeight * 2 width: height - visible: QGroundControl.videoManager.videoReceiver.videoRunning && QGroundControl.settingsManager.videoSettings.showRecControl.rawValue + visible: QGroundControl.videoManager.videoReceiver.videoRunning && QGroundControl.settingsManager.videoSettings.showRecControl.rawValue && !_isCamera opacity: 0.75 Rectangle { diff --git a/src/FlightDisplay/FlightDisplayViewVideo.qml b/src/FlightDisplay/FlightDisplayViewVideo.qml index 41afb34b9..d502a355c 100644 --- a/src/FlightDisplay/FlightDisplayViewVideo.qml +++ b/src/FlightDisplay/FlightDisplayViewVideo.qml @@ -8,23 +8,25 @@ ****************************************************************************/ -import QtQuick 2.3 -import QtQuick.Controls 1.2 - -import QGroundControl 1.0 -import QGroundControl.FlightDisplay 1.0 -import QGroundControl.FlightMap 1.0 -import QGroundControl.ScreenTools 1.0 -import QGroundControl.Controls 1.0 -import QGroundControl.Palette 1.0 -import QGroundControl.Vehicle 1.0 -import QGroundControl.Controllers 1.0 +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QGroundControl 1.0 +import QGroundControl.FlightDisplay 1.0 +import QGroundControl.FlightMap 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controllers 1.0 Item { id: root - property double _ar: QGroundControl.settingsManager.videoSettings.aspectRatio.rawValue - property bool _showGrid: QGroundControl.settingsManager.videoSettings.gridLines.rawValue > 0 + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property double _ar: QGroundControl.settingsManager.videoSettings.aspectRatio.rawValue + property bool _showGrid: QGroundControl.settingsManager.videoSettings.gridLines.rawValue > 0 + property var _dynamicCameras: _activeVehicle ? _activeVehicle.dynamicCameras : null + property bool _connected: _activeVehicle ? !_activeVehicle.connectionLost : false Rectangle { id: noVideo anchors.fill: parent @@ -90,4 +92,13 @@ Item { } } } + //-- Camera Controller + Loader { + source: _dynamicCameras ? _dynamicCameras.controllerSource : "" + visible: !_mainIsMap && _dynamicCameras && _dynamicCameras.cameras.count && _connected + anchors.right: parent.right + anchors.rightMargin: ScreenTools.defaultFontPixelWidth + anchors.bottom: parent.bottom + anchors.bottomMargin: ScreenTools.defaultFontPixelHeight * 2 + } } diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 05b9c9d1a..5d959145a 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -119,7 +119,7 @@ Item { property bool __flightMode: _flightMode function _outputState() { - console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode)) + //console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode)) } Component.onCompleted: _outputState() diff --git a/src/JsonHelper.cc b/src/JsonHelper.cc index f7dcc7608..e7d47a618 100644 --- a/src/JsonHelper.cc +++ b/src/JsonHelper.cc @@ -111,13 +111,36 @@ bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringLi return true; } -bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString) +bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName) { - enumStrings = jsonObject.value(_enumStringsJsonKey).toString().split(",", QString::SkipEmptyParts); - enumValues = jsonObject.value(_enumValuesJsonKey).toString().split(",", QString::SkipEmptyParts); + if(jsonObject.value(_enumStringsJsonKey).isArray()) { + // "enumStrings": ["Auto" , "Manual", "Shutter Priority", "Aperture Priority"], + QJsonArray jArray = jsonObject.value(_enumStringsJsonKey).toArray(); + for(int i = 0; i < jArray.count(); ++i) { + enumStrings << jArray.at(i).toString(); + } + } else { + // "enumStrings": "Auto,Manual,Shutter Priority,Aperture Priority", + enumStrings = jsonObject.value(_enumStringsJsonKey).toString().split(",", QString::SkipEmptyParts); + } + + if(jsonObject.value(_enumValuesJsonKey).isArray()) { + // "enumValues": [0, 1, 2, 3, 4, 5], + QJsonArray jArray = jsonObject.value(_enumValuesJsonKey).toArray(); + // This should probably be a variant list and not a string list. + for(int i = 0; i < jArray.count(); ++i) { + if(jArray.at(i).isString()) + enumValues << jArray.at(i).toString(); + else + enumValues << QString::number(jArray.at(i).toDouble()); + } + } else { + // "enumValues": "0,1,2,3,4,5", + enumValues = jsonObject.value(_enumValuesJsonKey).toString().split(",", QString::SkipEmptyParts); + } if (enumStrings.count() != enumValues.count()) { - errorString = QObject::tr("enum strings/values count mismatch strings:values %1:%2").arg(enumStrings.count()).arg(enumValues.count()); + errorString = QObject::tr("enum strings/values count mismatch in %3 strings:values %1:%2").arg(enumStrings.count()).arg(enumValues.count()).arg(valueName); return false; } diff --git a/src/JsonHelper.h b/src/JsonHelper.h index 8f88dceab..955392bd0 100644 --- a/src/JsonHelper.h +++ b/src/JsonHelper.h @@ -104,7 +104,7 @@ public: static void savePolygon(QmlObjectListModel& list, ///< List which contains vertices QJsonArray& polygonArray); ///< Array to save into - static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString); + static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName = QString()); /// Returns NaN if the value is null, or it not the double value static double possibleNaNJsonValue(const QJsonValue& value); diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index ec559b5c0..f7889c507 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -121,8 +121,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss MAV_FRAME_MISSION, 0, // camera id, all cameras _cameraModeFact.rawValue().toDouble(), - NAN, // Audio off/on - NAN, NAN, NAN, NAN, // param 4-7 reserved + NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved true, // autoContinue false, // isCurrentItem missionItemParent); diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index db6f521f8..e30a9a6bb 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -77,8 +77,7 @@ void CameraSectionTest::init(void) MAV_FRAME_MISSION, 0, // camera id = 0, all cameras CameraSection::CameraModePhoto, - NAN, // Audio off/on - NAN, NAN, NAN, NAN, // param 4-7 reserved + NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved true, // autocontinue false), // isCurrentItem this); @@ -88,8 +87,7 @@ void CameraSectionTest::init(void) MAV_FRAME_MISSION, 0, // camera id = 0, all cameras CameraSection::CameraModeVideo, - NAN, // Audio off/on - NAN, NAN, NAN, NAN, // param 4-7 reserved + NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved true, // autocontinue false), // isCurrentItem this); @@ -670,13 +668,12 @@ void CameraSectionTest::_testScanForCameraModeSection(void) MAV_CMD_SET_CAMERA_MODE Mission Param #1 Camera ID (0 for all cameras, 1 for first, 2 for second, etc.) Mission Param #2 Camera mode (0: photo mode, 1: video mode) - Mission Param #3 Audio recording enabled (0: off 1: on) - Mission Param #4 Reserved (all remaining params) + Mission Param #3 Reserved (all remaining params) */ // Mode command but incorrect settings SimpleMissionItem invalidSimpleItem(_offlineVehicle, _validCameraPhotoModeItem->missionItem()); - invalidSimpleItem.missionItem().setParam3(1); // Audio should be NaN + invalidSimpleItem.missionItem().setParam3(1); // Param3 should be NaN visualItems.append(&invalidSimpleItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); QCOMPARE(visualItems.count(), 1); diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 589cf5786..96f1596e7 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -983,12 +983,6 @@ "enumStrings": "Take photos,Record video", "enumValues": "0,1", "default": 0 - }, - "param3": { - "label": "Audio", - "enumStrings": "Recording disabled,Recording enabled", - "enumValues": "0,1", - "default": 0 } }, { diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index e5fed5d01..53b6f3e1e 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -30,15 +30,15 @@ Rectangle { property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 property var _cameraList: [ qsTr("Manual Grid (no camera specs)"), qsTr("Custom Camera Grid") ] property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle - property var _vehicleCameraList: _vehicle.cameraList + property var _vehicleCameraList: _vehicle.staticCcameraList readonly property int _gridTypeManual: 0 readonly property int _gridTypeCustomCamera: 1 readonly property int _gridTypeCamera: 2 Component.onCompleted: { - for (var i=0; i<_vehicle.cameraList.length; i++) { - _cameraList.push(_vehicle.cameraList[i].name) + for (var i=0; i<_vehicle.staticCcameraList.length; i++) { + _cameraList.push(_vehicle.staticCcameraList[i].name) } gridTypeCombo.model = _cameraList if (missionItem.manualGrid.value) { diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 7abc033aa..1991cddee 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -80,6 +80,7 @@ #include "ParameterManager.h" #include "SettingsManager.h" #include "QGCCorePlugin.h" +#include "QGCCameraManager.h" #ifndef NO_SERIAL_LINK #include "SerialLink.h" @@ -344,9 +345,9 @@ void QGCApplication::_initCommon(void) qmlRegisterType ("QGroundControl.Palette", 1, 0, "QGCPalette"); qmlRegisterType ("QGroundControl.Palette", 1, 0, "QGCMapPalette"); - qmlRegisterUncreatableType ("QGroundControl", 1, 0, "CoordinateVector", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QmlObjectListModel", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl", 1, 0, "MissionCommandTree", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "CoordinateVector", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QmlObjectListModel", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "MissionCommandTree", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "AutoPilotPlugin", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Reference only"); @@ -354,13 +355,15 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionItem", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "ParameterManager", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.FlightMap", 1, 0, "QGCMapPolygon", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "MissionController", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Controllers", 1, 0, "GeoFenceController", "Reference only"); - qmlRegisterUncreatableType("QGroundControl.Controllers", 1, 0, "RallyPointController", "Reference only"); + qmlRegisterUncreatableType("QGroundControl.Controllers", 1, 0, "RallyPointController", "Reference only"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ESP8266ComponentController"); diff --git a/src/QmlControls/ScreenTools.qml b/src/QmlControls/ScreenTools.qml index 5e3ba7702..2dccb42b8 100644 --- a/src/QmlControls/ScreenTools.qml +++ b/src/QmlControls/ScreenTools.qml @@ -114,7 +114,7 @@ Item { minTouchPixels = defaultFontPixelHeight * 3 } - console.log(minTouchPixels / Screen.height) + //console.log(minTouchPixels / Screen.height) toolbarHeight = isMobile ? minTouchPixels : defaultFontPixelHeight * 3 } diff --git a/src/QmlControls/SliderSwitch.qml b/src/QmlControls/SliderSwitch.qml index 2b5dd585b..3aef291bb 100644 --- a/src/QmlControls/SliderSwitch.qml +++ b/src/QmlControls/SliderSwitch.qml @@ -71,7 +71,7 @@ Rectangle { property bool dragActive: drag.active property real _dragOffset: 1 - Component.onCompleted: console.log(height, ScreenTools.minTouchPixels) + //Component.onCompleted: console.log(height, ScreenTools.minTouchPixels) onPressed: { mouse.x diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index df6497b0b..e0f3e4870 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -30,6 +30,7 @@ #include "QGroundControlQmlGlobal.h" #include "SettingsManager.h" #include "QGCQGeoCoordinate.h" +#include "QGCCameraManager.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -113,6 +114,7 @@ Vehicle::Vehicle(LinkInterface* link, , _telemetryRNoise(0) , _vehicleCapabilitiesKnown(false) , _supportsMissionItemInt(false) + , _cameras(NULL) , _connectionLost(false) , _connectionLostEnabled(true) , _initialPlanRequestComplete(false) @@ -225,6 +227,10 @@ Vehicle::Vehicle(LinkInterface* link, _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints); connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint); + + // Create camera manager instance + _cameras = _firmwarePlugin->cameraManager(this); + emit dynamicCamerasChanged(); } // Disconnected Vehicle for offline editing @@ -271,6 +277,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) , _vehicleCapabilitiesKnown(true) , _supportsMissionItemInt(false) + , _cameras(NULL) , _connectionLost(false) , _connectionLostEnabled(true) , _initialPlanRequestComplete(false) @@ -2576,7 +2583,7 @@ const QVariantList& Vehicle::toolBarIndicators() return emptyList; } -const QVariantList& Vehicle::cameraList(void) const +const QVariantList& Vehicle::staticCcameraList(void) const { if (_firmwarePlugin) { return _firmwarePlugin->cameraList(this); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index faddcdf14..692a3d5d8 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -38,6 +38,7 @@ class ParameterManager; class JoystickManager; class UASMessage; class SettingsManager; +class QGCCameraManager; Q_DECLARE_LOGGING_CATEGORY(VehicleLog) @@ -310,7 +311,8 @@ public: Q_PROPERTY(int telemetryLNoise READ telemetryLNoise NOTIFY telemetryLNoiseChanged) Q_PROPERTY(int telemetryRNoise READ telemetryRNoise NOTIFY telemetryRNoiseChanged) Q_PROPERTY(QVariantList toolBarIndicators READ toolBarIndicators CONSTANT) - Q_PROPERTY(QVariantList cameraList READ cameraList CONSTANT) + Q_PROPERTY(QVariantList staticCcameraList READ staticCcameraList CONSTANT) + Q_PROPERTY(QGCCameraManager* dynamicCameras READ dynamicCameras NOTIFY dynamicCamerasChanged) /// true: Vehicle is flying, false: Vehicle is on ground Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) @@ -670,8 +672,10 @@ public: QString vehicleImageOutline () const; QString vehicleImageCompass () const; - const QVariantList& toolBarIndicators (); - const QVariantList& cameraList (void) const; + const QVariantList& toolBarIndicators (); + const QVariantList& staticCcameraList (void) const; + + QGCCameraManager* dynamicCameras () { return _cameras; } bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; } @@ -712,6 +716,7 @@ signals: void defaultHoverSpeedChanged(double hoverSpeed); void firmwareTypeChanged(void); void vehicleTypeChanged(void); + void dynamicCamerasChanged(); void messagesReceivedChanged (); void messagesSentChanged (); @@ -907,6 +912,8 @@ private: bool _vehicleCapabilitiesKnown; bool _supportsMissionItemInt; + QGCCameraManager* _cameras; + typedef struct { int component; MAV_CMD command; diff --git a/src/api/QGCCorePlugin.h b/src/api/QGCCorePlugin.h index 9e996f722..4bdd09eb0 100644 --- a/src/api/QGCCorePlugin.h +++ b/src/api/QGCCorePlugin.h @@ -36,16 +36,16 @@ public: QGCCorePlugin(QGCApplication* app, QGCToolbox* toolbox); ~QGCCorePlugin(); - Q_PROPERTY(QVariantList settingsPages READ settingsPages NOTIFY settingsPagesChanged) - Q_PROPERTY(int defaultSettings READ defaultSettings CONSTANT) - Q_PROPERTY(QGCOptions* options READ options CONSTANT) + Q_PROPERTY(QVariantList settingsPages READ settingsPages NOTIFY settingsPagesChanged) + Q_PROPERTY(int defaultSettings READ defaultSettings CONSTANT) + Q_PROPERTY(QGCOptions* options READ options CONSTANT) - Q_PROPERTY(bool showTouchAreas READ showTouchAreas WRITE setShowTouchAreas NOTIFY showTouchAreasChanged) - Q_PROPERTY(bool showAdvancedUI READ showAdvancedUI WRITE setShowAdvancedUI NOTIFY showAdvancedUIChanged) - Q_PROPERTY(QString showAdvancedUIMessage READ showAdvancedUIMessage CONSTANT) + Q_PROPERTY(bool showTouchAreas READ showTouchAreas WRITE setShowTouchAreas NOTIFY showTouchAreasChanged) + Q_PROPERTY(bool showAdvancedUI READ showAdvancedUI WRITE setShowAdvancedUI NOTIFY showAdvancedUIChanged) + Q_PROPERTY(QString showAdvancedUIMessage READ showAdvancedUIMessage CONSTANT) - Q_PROPERTY(QString brandImageIndoor READ brandImageIndoor CONSTANT) - Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor CONSTANT) + Q_PROPERTY(QString brandImageIndoor READ brandImageIndoor CONSTANT) + Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor CONSTANT) /// The list of settings under the Settings Menu /// @return A list of QGCSettings diff --git a/src/ui/MainWindowInner.qml b/src/ui/MainWindowInner.qml index ae4f0316f..c0841eff6 100644 --- a/src/ui/MainWindowInner.qml +++ b/src/ui/MainWindowInner.qml @@ -246,8 +246,8 @@ Item { currentPopUp.close() } if(oldIndicator !== dropItem) { - console.log(oldIndicator) - console.log(dropItem) + //console.log(oldIndicator) + //console.log(dropItem) indicatorDropdown.centerX = centerX indicatorDropdown.sourceComponent = dropItem indicatorDropdown.visible = true diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index 333cee9e8..75b9af075 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -333,13 +333,13 @@ Rectangle { onActivated: { saveItems(); QGroundControl.mavlinkLogManager.windSpeed = windItems.get(index).value - console.log('Set Wind: ' + windItems.get(index).value) + //console.log('Set Wind: ' + windItems.get(index).value) } Component.onCompleted: { for(var i = 0; i < windItems.count; i++) { if(windItems.get(i).value === QGroundControl.mavlinkLogManager.windSpeed) { windCombo.currentIndex = i; - console.log('Wind: ' + windItems.get(i).value) + //console.log('Wind: ' + windItems.get(i).value) break; } } @@ -370,13 +370,13 @@ Rectangle { onActivated: { saveItems(); QGroundControl.mavlinkLogManager.rating = ratingItems.get(index).value - console.log('Set Rating: ' + ratingItems.get(index).value) + //console.log('Set Rating: ' + ratingItems.get(index).value) } Component.onCompleted: { for(var i = 0; i < ratingItems.count; i++) { if(ratingItems.get(i).value === QGroundControl.mavlinkLogManager.rating) { ratingCombo.currentIndex = i; - console.log('Rating: ' + ratingItems.get(i).value) + //console.log('Rating: ' + ratingItems.get(i).value) break; } } -- 2.22.0