diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 2b0203ffc97d8592422ab2c3dd09355365513e9e..a5e70c22c75167af913b0afa6e01a76c6ee9d9a0 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -263,6 +263,16 @@ QGCCameraControl::storageFreeStr() return QGCMapEngine::storageFreeSizeToString(static_cast(_storageFree)); } +//----------------------------------------------------------------------------- +QString +QGCCameraControl::batteryRemainingStr() +{ + if(_batteryRemaining >= 0) { + return QGCMapEngine::numberToString(static_cast(_batteryRemaining)) + " %"; + } + return ""; +} + //----------------------------------------------------------------------------- void QGCCameraControl::setCameraMode(CameraMode mode) @@ -1490,6 +1500,17 @@ QGCCameraControl::handleStorageInfo(const mavlink_storage_information_t& st) } } +//----------------------------------------------------------------------------- +void +QGCCameraControl::handleBatteryStatus(const mavlink_battery_status_t& bs) +{ + qCDebug(CameraControlLog) << "handleBatteryStatus:" << bs.battery_remaining; + if(bs.battery_remaining >= 0 && _batteryRemaining != static_cast(bs.battery_remaining)) { + _batteryRemaining = static_cast(bs.battery_remaining); + emit batteryRemainingChanged(); + } +} + //----------------------------------------------------------------------------- void QGCCameraControl::handleCaptureStatus(const mavlink_camera_capture_status_t& cap) diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h index 95f099118d4f9aa7ad9b0036af86c568b6da685e..abb78479205702151c423a6bb75657509235cda1 100644 --- a/src/Camera/QGCCameraControl.h +++ b/src/Camera/QGCCameraControl.h @@ -155,6 +155,8 @@ public: Q_PROPERTY(quint32 storageFree READ storageFree NOTIFY storageFreeChanged) Q_PROPERTY(QString storageFreeStr READ storageFreeStr NOTIFY storageFreeChanged) Q_PROPERTY(quint32 storageTotal READ storageTotal NOTIFY storageTotalChanged) + Q_PROPERTY(int batteryRemaining READ batteryRemaining NOTIFY batteryRemainingChanged) + Q_PROPERTY(QString batteryRemainingStr READ batteryRemainingStr NOTIFY batteryRemainingChanged) Q_PROPERTY(bool paramComplete READ paramComplete NOTIFY parametersReady) Q_PROPERTY(qreal zoomLevel READ zoomLevel WRITE setZoomLevel NOTIFY zoomLevelChanged) @@ -232,6 +234,8 @@ public: virtual quint32 storageFree () { return _storageFree; } virtual QString storageFreeStr (); virtual quint32 storageTotal () { return _storageTotal; } + virtual int batteryRemaining () { return _batteryRemaining; } + virtual QString batteryRemainingStr (); virtual bool paramComplete () { return _paramComplete; } virtual qreal zoomLevel () { return _zoomLevel; } virtual qreal focusLevel () { return _focusLevel; } @@ -273,6 +277,7 @@ public: virtual void handleParamAck (const mavlink_param_ext_ack_t& ack); virtual void handleParamValue (const mavlink_param_ext_value_t& value); virtual void handleStorageInfo (const mavlink_storage_information_t& st); + virtual void handleBatteryStatus (const mavlink_battery_status_t& bs); virtual void handleVideoInfo (const mavlink_video_stream_information_t *vi); virtual void handleVideoStatus (const mavlink_video_stream_status_t *vs); @@ -303,6 +308,7 @@ signals: void activeSettingsChanged (); void storageFreeChanged (); void storageTotalChanged (); + void batteryRemainingChanged (); void dataReady (QByteArray data); void parametersReady (); void zoomLevelChanged (); @@ -373,6 +379,7 @@ protected: qreal _focusLevel = 0.0; uint32_t _storageFree = 0; uint32_t _storageTotal = 0; + int _batteryRemaining = -1; QNetworkAccessManager* _netManager = nullptr; QString _modelName; QString _vendor; diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index e85a45587ebc03a9dfed658d4a6dc795fde219a9..33410b36158de7cfd4a4c6c67d84ff303845145a 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -97,6 +97,9 @@ QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message) case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS: _handleVideoStreamStatus(message); break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + _handleBatteryStatus(message); + break; } } } @@ -358,6 +361,18 @@ QGCCameraManager::_handleVideoStreamStatus(const mavlink_message_t& message) } } +//----------------------------------------------------------------------------- +void +QGCCameraManager::_handleBatteryStatus(const mavlink_message_t& message) +{ + QGCCameraControl* pCamera = _findCamera(message.compid); + if(pCamera) { + mavlink_battery_status_t batteryStatus; + mavlink_msg_battery_status_decode(&message, &batteryStatus); + pCamera->handleBatteryStatus(batteryStatus); + } +} + //----------------------------------------------------------------------------- void QGCCameraManager::_requestCameraInfo(int compID) diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index e13cf9ab2c55c69e8e224964ab490f17c543fe8b..7641e155bd1516a76f12880830f7135d3e5d5313 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -35,7 +35,7 @@ public: //-- Return a list of cameras provided by this vehicle virtual QmlObjectListModel* cameras () { return &_cameras; } //-- Camera names to show the user (for selection) - virtual QStringList cameraLabels () { return _cameraLabels; } + virtual QStringList cameraLabels () { return _cameraLabels; } //-- Current selected camera virtual int currentCamera () { return _currentCamera; } virtual QGCCameraControl* currentCameraInstance(); @@ -79,6 +79,7 @@ protected: virtual void _handleCaptureStatus (const mavlink_message_t& message); virtual void _handleVideoStreamInfo (const mavlink_message_t& message); virtual void _handleVideoStreamStatus(const mavlink_message_t& message); + virtual void _handleBatteryStatus (const mavlink_message_t& message); protected: diff --git a/src/FlightMap/Widgets/CameraPageWidget.qml b/src/FlightMap/Widgets/CameraPageWidget.qml index 916dca306691e8875a73eab57baf1ef14a0b885f..fe6a50b5cdecfae1e455cb11706696f3a253e18c 100644 --- a/src/FlightMap/Widgets/CameraPageWidget.qml +++ b/src/FlightMap/Widgets/CameraPageWidget.qml @@ -45,6 +45,7 @@ Column { property bool _hasModes: _camera && _camera && _camera.hasModes property bool _videoRecording: _camera && _camera.videoStatus === QGCCameraControl.VIDEO_CAPTURE_STATUS_RUNNING property bool _storageReady: _camera && _camera.storageStatus === QGCCameraControl.STORAGE_READY + property bool _batteryReady: _camera && _camera.batteryRemaining >= 0 property bool _storageIgnored: _camera && _camera.storageStatus === QGCCameraControl.STORAGE_NOT_SUPPORTED property bool _canShoot: !_videoRecording && _cameraPhotoIdle && ((_storageReady && _camera.storageFree > 0) || _storageIgnored) property int _curCameraIndex: _dynamicCameras ? _dynamicCameras.currentCamera : 0 @@ -76,6 +77,12 @@ Column { anchors.horizontalCenter: parent.horizontalCenter visible: _camera && _storageReady } + QGCLabel { + text: _camera ? qsTr("Battery: ") + _camera.batteryRemainingStr : "" + font.pointSize: ScreenTools.smallFontPointSize + anchors.horizontalCenter: parent.horizontalCenter + visible: _camera && _batteryReady + } //-- Camera Mode (visible only if camera has modes) Item { width: 1; height: ScreenTools.defaultFontPixelHeight * 0.75; visible: camMode.visible; } Rectangle {