diff --git a/src/MAVLink/ImageProtocolManager.cc b/src/MAVLink/ImageProtocolManager.cc index 6647e0c0cd9a..f96a58b59566 100644 --- a/src/MAVLink/ImageProtocolManager.cc +++ b/src/MAVLink/ImageProtocolManager.cc @@ -10,27 +10,56 @@ #include "ImageProtocolManager.h" #include "QGCLoggingCategory.h" -QGC_LOGGING_CATEGORY(ImageProtocolManagerLog, "ImageProtocolManagerLog") +QGC_LOGGING_CATEGORY(ImageProtocolManagerLog, "qgc.mavlink.imageprotocolmanager") -ImageProtocolManager::ImageProtocolManager(void) +ImageProtocolManager::ImageProtocolManager(QObject *parent) + : QObject(parent) { - memset(&_imageHandshake, 0, sizeof(_imageHandshake)); + // qCDebug(ImageProtocolManagerLog) << Q_FUNC_INFO << this; } -void ImageProtocolManager::mavlinkMessageReceived(const mavlink_message_t& message) +ImageProtocolManager::~ImageProtocolManager() +{ + // qCDebug(ImageProtocolManagerLog) << Q_FUNC_INFO << this; +} + +bool ImageProtocolManager::requestImage(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t &message) +{ + // Check if there is already an image transmission going on + if (_imageHandshake.packets != 0) { + return false; + } + + constexpr mavlink_data_transmission_handshake_t data = { + 0, 0, 0, 0, + MAVLINK_DATA_STREAM_IMG_JPEG, + 0, + 50 + }; + (void) mavlink_msg_data_transmission_handshake_encode_chan(system_id, component_id, chan, &message, &data); + + return true; +} + +void ImageProtocolManager::cancelRequest(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t &message) +{ + constexpr mavlink_data_transmission_handshake_t data{0}; + (void) mavlink_msg_data_transmission_handshake_encode_chan(system_id, component_id, chan, &message, &data); +} + +void ImageProtocolManager::mavlinkMessageReceived(const mavlink_message_t &message) { switch (message.msgid) { case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: { - if (_imageHandshake.packets) { + if (_imageHandshake.packets > 0) { qCWarning(ImageProtocolManagerLog) << "DATA_TRANSMISSION_HANDSHAKE: Previous image transmission incomplete."; } _imageBytes.clear(); mavlink_msg_data_transmission_handshake_decode(&message, &_imageHandshake); qCDebug(ImageProtocolManagerLog) << QStringLiteral("DATA_TRANSMISSION_HANDSHAKE: type(%1) width(%2) height (%3)").arg(_imageHandshake.type).arg(_imageHandshake.width).arg(_imageHandshake.height); - } break; - + } case MAVLINK_MSG_ID_ENCAPSULATED_DATA: { if (_imageHandshake.packets == 0) { @@ -41,71 +70,75 @@ void ImageProtocolManager::mavlinkMessageReceived(const mavlink_message_t& messa mavlink_encapsulated_data_t encapsulatedData; mavlink_msg_encapsulated_data_decode(&message, &encapsulatedData); - int bytePosition = encapsulatedData.seqnr * _imageHandshake.payload; - if (bytePosition >= static_cast(_imageHandshake.size)) { - qCWarning(ImageProtocolManagerLog) << "ENCAPSULATED_DATA: seqnr is past end of image size. seqnr:" << encapsulatedData.seqnr << "_imageHandshake.size" << _imageHandshake.size; + uint32_t bytePosition = encapsulatedData.seqnr * _imageHandshake.payload; + if (bytePosition >= _imageHandshake.size) { + qCWarning(ImageProtocolManagerLog) << "ENCAPSULATED_DATA: seqnr is past end of image size. seqnr:" << encapsulatedData.seqnr << "_imageHandshake.size:" << _imageHandshake.size; break; } - for (uint8_t i=0; i<_imageHandshake.payload; i++) { + for (uint8_t i = 0; i < _imageHandshake.payload; i++) { _imageBytes[bytePosition] = encapsulatedData.data[i]; bytePosition++; } // We use the packets field to track completion _imageHandshake.packets--; - if (_imageHandshake.packets == 0) { // We have all the packets - emit imageReady(); + emit imageReady(_getImage()); + if ((message.sysid == kPX4FlowSysId) && (message.compid == kPX4FlowCompId)) { + _flowImageIndex++; + emit flowImageIndexChanged(_flowImageIndex); + } } - } break; - + } default: break; } } -QImage ImageProtocolManager::getImage(void) +QImage ImageProtocolManager::_getImage() { QImage image; if (_imageBytes.isEmpty()) { - qCWarning(ImageProtocolManagerLog) << "getImage: Called when no image available"; - } else if (_imageHandshake.packets) { - qCWarning(ImageProtocolManagerLog) << "getImage: Called when image is imcomplete. _imageHandshake.packets:" << _imageHandshake.packets; - } else { - switch (_imageHandshake.type) { - case MAVLINK_DATA_STREAM_IMG_RAW8U: - case MAVLINK_DATA_STREAM_IMG_RAW32U: - { - // Construct PGM header - QString header("P5\n%1 %2\n%3\n"); - header = header.arg(_imageHandshake.width).arg(_imageHandshake.height).arg(255 /* image colors */); - - QByteArray tmpImage(header.toStdString().c_str(), header.length()); - tmpImage.append(_imageBytes); - - if (!image.loadFromData(tmpImage, "PGM")) { - qCWarning(ImageProtocolManagerLog) << "getImage: IMG_RAW8U QImage::loadFromData failed"; - } - } - break; + qCWarning(ImageProtocolManagerLog) << Q_FUNC_INFO << "Called when no image available"; + return image; + } - case MAVLINK_DATA_STREAM_IMG_BMP: - case MAVLINK_DATA_STREAM_IMG_JPEG: - case MAVLINK_DATA_STREAM_IMG_PGM: - case MAVLINK_DATA_STREAM_IMG_PNG: - if (!image.loadFromData(_imageBytes)) { - qCWarning(ImageProtocolManagerLog) << "getImage: Known header QImage::loadFromData failed"; - } - break; + if (_imageHandshake.packets > 0) { + qCWarning(ImageProtocolManagerLog) << Q_FUNC_INFO << "Called when image is imcomplete. _imageHandshake.packets:" << _imageHandshake.packets; + return image; + } - default: - qCWarning(ImageProtocolManagerLog) << "getImage: Unsupported image type:" << _imageHandshake.type; - break; + switch (_imageHandshake.type) { + case MAVLINK_DATA_STREAM_IMG_RAW8U: + case MAVLINK_DATA_STREAM_IMG_RAW32U: + { + // Construct PGM header + const QString header = QStringLiteral("P5\n%1 %2\n255\n").arg(_imageHandshake.width).arg(_imageHandshake.height); + + QByteArray tempImage(header.toStdString().c_str(), header.length()); + (void) tempImage.append(_imageBytes); + + if (!image.loadFromData(tempImage, "PGM")) { + qCWarning(ImageProtocolManagerLog) << Q_FUNC_INFO << "IMG_RAW8U QImage::loadFromData failed"; + } + break; + } + case MAVLINK_DATA_STREAM_IMG_BMP: + case MAVLINK_DATA_STREAM_IMG_JPEG: + case MAVLINK_DATA_STREAM_IMG_PGM: + case MAVLINK_DATA_STREAM_IMG_PNG: + if (!image.loadFromData(_imageBytes)) { + qCWarning(ImageProtocolManagerLog) << Q_FUNC_INFO << "Known header QImage::loadFromData failed"; } + break; + + default: + qCWarning(ImageProtocolManagerLog) << Q_FUNC_INFO << "Unsupported image type:" << _imageHandshake.type; + break; } return image; diff --git a/src/MAVLink/ImageProtocolManager.h b/src/MAVLink/ImageProtocolManager.h index e5658709610f..81fd0c946377 100644 --- a/src/MAVLink/ImageProtocolManager.h +++ b/src/MAVLink/ImageProtocolManager.h @@ -11,30 +11,44 @@ #include "MAVLinkLib.h" -#include #include #include +#include #include Q_DECLARE_LOGGING_CATEGORY(ImageProtocolManagerLog) -// Supports the Mavlink image transmission protocol (https://mavlink.io/en/services/image_transmission.html). -// Mainly used by optical flow cameras. +/// Supports the Mavlink image transmission protocol (https://mavlink.io/en/services/image_transmission.html). +/// Mainly used by optical flow cameras. class ImageProtocolManager : public QObject { Q_OBJECT - + Q_PROPERTY(uint flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) + public: - ImageProtocolManager(void); + ImageProtocolManager(QObject *parent = nullptr); + ~ImageProtocolManager(); - void mavlinkMessageReceived (const mavlink_message_t& message); - QImage getImage (void); + uint32_t flowImageIndex() const { return _flowImageIndex; } + + bool requestImage(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t &message); + void cancelRequest(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t &message); signals: - void imageReady(void); + void imageReady(const QImage &image); + void flowImageIndexChanged(uint32_t index); + +public slots: + void mavlinkMessageReceived(const mavlink_message_t &message); private: - mavlink_data_transmission_handshake_t _imageHandshake; - QByteArray _imageBytes; + QImage _getImage(); + + mavlink_data_transmission_handshake_t _imageHandshake{0}; + QByteArray _imageBytes; + uint32_t _flowImageIndex = 0; + static constexpr uint8_t kPX4FlowSysId = 81; + static constexpr MAV_COMPONENT kPX4FlowCompId = MAV_COMP_ID_USER26; + static constexpr mavlink_system_t kPX4FlowSystem = { kPX4FlowSysId, kPX4FlowCompId }; }; diff --git a/src/QmlControls/QGCImageProvider.cc b/src/QmlControls/QGCImageProvider.cc index 2c95e1277f00..f2844d85ddce 100644 --- a/src/QmlControls/QGCImageProvider.cc +++ b/src/QmlControls/QGCImageProvider.cc @@ -14,54 +14,63 @@ QGCImageProvider::QGCImageProvider(QQmlImageProviderBase::ImageType imageType) : QQuickImageProvider(imageType) + , _dummy(320, 240, QImage::Format_RGBA8888) { - //-- Dummy temporary image until something comes along - m_image = QImage(320, 240, QImage::Format_RGBA8888); - m_image.fill(Qt::black); - QPainter painter(&m_image); - QFont f = painter.font(); - f.setPixelSize(20); - painter.setFont(f); - painter.setPen(Qt::white); - painter.drawText(QRectF(0, 0, 320, 240), Qt::AlignCenter, "Waiting..."); + // qCDebug(ImageProtocolManagerLog) << Q_FUNC_INFO << this; + + Q_ASSERT(imageType == QQmlImageProviderBase::ImageType::Image); + + // Dummy temporary image until something comes along + _dummy.fill(Qt::black); + QPainter painter(&_dummy); + QFont f = painter.font(); + f.setPixelSize(20); + painter.setFont(f); + painter.setPen(Qt::white); + painter.drawText(QRectF(0, 0, _dummy.width(), _dummy.height()), Qt::AlignCenter, QStringLiteral("Waiting...")); + _images[0] = _dummy; } QGCImageProvider::~QGCImageProvider() { - + // qCDebug(ImageProtocolManagerLog) << Q_FUNC_INFO << this; } -QImage QGCImageProvider::requestImage(const QString & /* image url with vehicle id*/, QSize *, const QSize &) +QImage QGCImageProvider::requestImage(const QString &id, QSize *size, const QSize &requestedSize) { -/* - The QML side will request an image using a special URL, which we've registered as QGCImages. - The URL follows this format (or anything you want to make out of it after the "QGCImages" part): + Q_UNUSED(requestedSize); - "image://QGCImages/vvv/iii" + if (id.isEmpty()) { + return _dummy; + } - Where: - vvv: Some vehicle id - iii: An auto incremented index (which forces the Item to reload the image) + if (!id.contains("/")) { + return _dummy; + } - The image index is incremented each time a new image arrives. A signal is emitted and the QML side - updates its contents automatically. + const QStringList url = id.split('/', Qt::SkipEmptyParts); + if (!url.size() == 2) { + return _dummy; + } - Image { - source: "image://QGCImages/" + _activeVehicle.id + "/" + _activeVehicle.flowImageIndex - width: parent.width * 0.5 - height: width * 0.75 - cache: false - anchors.centerIn: parent - fillMode: Image.PreserveAspectFit - } + bool ok = false; + const uint8_t vehicleId = url[0].toUInt(&ok); + if (!ok) { + return _dummy; + } - For now, we don't even look at the URL. This will have to be fixed if we're to support multiple - vehicles transmitting flow images. -*/ - return m_image; -} + const uint8_t index = url[1].toUInt(&ok); + if (!ok) { + return _dummy; + } -void QGCImageProvider::setImage(QImage* pImage, int /* vehicle id*/) -{ - m_image = pImage->mirrored(); + if (!_images.contains(vehicleId)) { + return _dummy; + } + + const QImage image = _images[vehicleId]; + // image->scaled(requestedSize); + *size = image.size(); + + return image; } diff --git a/src/QmlControls/QGCImageProvider.h b/src/QmlControls/QGCImageProvider.h index e81c72ea63c9..6c596baf205a 100644 --- a/src/QmlControls/QGCImageProvider.h +++ b/src/QmlControls/QGCImageProvider.h @@ -10,23 +10,20 @@ #pragma once #include +#include #include -// This is used to expose images from ImageProtocolHandler +/// This is used to expose images from ImageProtocolHandler class QGCImageProvider : public QQuickImageProvider { public: QGCImageProvider(QQmlImageProviderBase::ImageType type = QQmlImageProviderBase::ImageType::Image); ~QGCImageProvider(); - void setImage(QImage* pImage, int id = 0); - - QImage requestImage(const QString& id, QSize* size, const QSize& requestedSize) override; + QImage requestImage(const QString &id, QSize *size, const QSize &requestedSize) final; + void setImage(const QImage &image, uint8_t vehicleId = 0) { _images[vehicleId] = image.mirrored(); } private: - //-- TODO: For now this is holding a single image. If you happen to have two - // or more vehicles with flow, it will not work. To properly manage that condition - // this should be a map between each vehicle and its image. The URL provided - // for the image request would contain the vehicle identification. - QImage m_image; + QMap _images; + QImage _dummy; }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 58441aa861a0..833cbec99a5e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -293,7 +293,7 @@ void Vehicle::_commonInit() _componentInformationManager = new ComponentInformationManager (this); _initialConnectStateMachine = new InitialConnectStateMachine (this); _ftpManager = new FTPManager (this); - _imageProtocolManager = new ImageProtocolManager (); + _vehicleLinkManager = new VehicleLinkManager (this); connect(_standardModes, &StandardModes::modesUpdated, this, &Vehicle::flightModesChanged); @@ -325,8 +325,7 @@ void Vehicle::_commonInit() // Flight modes can differ based on advanced mode connect(_toolbox->corePlugin(), &QGCCorePlugin::showAdvancedUIChanged, this, &Vehicle::flightModesChanged); - connect(_imageProtocolManager, &ImageProtocolManager::imageReady, this, &Vehicle::_imageProtocolImageReady); - + _createImageProtocolManager(); _createStatusTextHandler(); // _addFactGroup(_vehicleFactGroup, _vehicleFactGroupName); @@ -530,7 +529,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } _ftpManager->_mavlinkMessageReceived(message); _parameterManager->mavlinkMessageReceived(message); - _imageProtocolManager->mavlinkMessageReceived(message); + (void) QMetaObject::invokeMethod(_imageProtocolManager, "mavlinkMessageReceived", Qt::AutoConnection, message); _remoteIDManager->mavlinkMessageReceived(message); _waitForMavlinkMessageMessageReceivedHandler(message); @@ -1879,14 +1878,6 @@ void Vehicle::_sendQGCTimeToVehicle() sendMessageOnLinkThreadSafe(sharedLink.get(), msg); } -void Vehicle::_imageProtocolImageReady(void) -{ - QImage img = _imageProtocolManager->getImage(); - qgcApp()->qgcImageProvider()->setImage(&img, _id); - _flowImageIndex++; - emit flowImageIndexChanged(); -} - void Vehicle::_remoteControlRSSIChanged(uint8_t rssi) { //-- 0 <= rssi <= 100 - 255 means "invalid/unknown" @@ -4073,3 +4064,22 @@ void Vehicle::sendSetupSigning() } /*---------------------------------------------------------------------------*/ +/*===========================================================================*/ +/* Image Protocol Manager */ +/*===========================================================================*/ + +void Vehicle::_createImageProtocolManager() +{ + _imageProtocolManager = new ImageProtocolManager(this); + (void) connect(_imageProtocolManager, &ImageProtocolManager::flowImageIndexChanged, this, &Vehicle::flowImageIndexChanged); + (void) connect(_imageProtocolManager, &ImageProtocolManager::imageReady, this, [this](const QImage &image) { + qgcApp()->qgcImageProvider()->setImage(image, _id); + }); +} + +uint32_t Vehicle::flowImageIndex() const +{ + return (_imageProtocolManager ? _imageProtocolManager->flowImageIndex() : 0); +} + +/*---------------------------------------------------------------------------*/ diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ed1aae477430..699ee9c16ede 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -158,7 +158,6 @@ class Vehicle : public VehicleFactGroup Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) - Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) Q_PROPERTY(int rcRSSI READ rcRSSI NOTIFY rcRSSIChanged) Q_PROPERTY(bool px4Firmware READ px4Firmware NOTIFY firmwareTypeChanged) Q_PROPERTY(bool apmFirmware READ apmFirmware NOTIFY firmwareTypeChanged) @@ -512,8 +511,6 @@ class Vehicle : public VehicleFactGroup QmlObjectListModel* cameraTriggerPoints () { return &_cameraTriggerPoints; } - int flowImageIndex() const{ return _flowImageIndex; } - //-- Mavlink Logging void startMavlinkLog(); void stopMavlinkLog(); @@ -841,7 +838,6 @@ public slots: void checkListStateChanged (); void longitudeChanged (); void currentConfigChanged (); - void flowImageIndexChanged (); void rcRSSIChanged (int rcRSSI); void telemetryRRSSIChanged (int value); void telemetryLRSSIChanged (int value); @@ -915,7 +911,6 @@ private slots: void _announceArmedChanged (bool armed); void _offlineCruiseSpeedSettingChanged (QVariant value); void _offlineHoverSpeedSettingChanged (QVariant value); - void _imageProtocolImageReady (void); void _prearmErrorTimeout (); void _firstMissionLoadComplete (); void _firstGeoFenceLoadComplete (); @@ -1100,8 +1095,6 @@ private slots: // Toolbox references - int _flowImageIndex = 0; - bool _allLinksRemovedSent = false; ///< true: allLinkRemoved signal already sent one time uint _messagesReceived = 0; @@ -1276,7 +1269,6 @@ private slots: RallyPointManager* _rallyPointManager = nullptr; VehicleLinkManager* _vehicleLinkManager = nullptr; FTPManager* _ftpManager = nullptr; - ImageProtocolManager* _imageProtocolManager = nullptr; InitialConnectStateMachine* _initialConnectStateMachine = nullptr; Actuators* _actuators = nullptr; RemoteIDManager* _remoteIDManager = nullptr; @@ -1362,6 +1354,23 @@ private slots: StatusTextHandler *m_statusTextHandler = nullptr; /*---------------------------------------------------------------------------*/ +/*===========================================================================*/ +/* Image Protocol Manager */ +/*===========================================================================*/ +private: + Q_PROPERTY(uint flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) + +public: + uint32_t flowImageIndex() const; + +signals: + void flowImageIndexChanged(); + +private: + void _createImageProtocolManager(); + + ImageProtocolManager *_imageProtocolManager = nullptr; }; +/*---------------------------------------------------------------------------*/ Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t) diff --git a/src/VehicleSetup/PX4FlowSensor.qml b/src/VehicleSetup/PX4FlowSensor.qml index 1c0bdf0ecaa5..c07b4b578f9e 100644 --- a/src/VehicleSetup/PX4FlowSensor.qml +++ b/src/VehicleSetup/PX4FlowSensor.qml @@ -18,16 +18,16 @@ import QGroundControl.ScreenTools Item { QGCLabel { - id: titleLabel - text: qsTr("PX4Flow Camera") - font.family: ScreenTools.demiboldFontFamily + text: qsTr("PX4Flow Camera") + font.family: ScreenTools.demiboldFontFamily } + Image { - source: globals.activeVehicle ? "image://QGCImages/" + globals.activeVehicle.id + "/" + globals.activeVehicle.flowImageIndex : "" - width: parent.width * 0.5 - height: width * 0.75 - cache: false - fillMode: Image.PreserveAspectFit + source: globals.activeVehicle ? "image://QGCImages/" + globals.activeVehicle.id + "/" + globals.activeVehicle.flowImageIndex : "" + width: parent.width * 0.5 + height: width * 0.75 + cache: false + fillMode: Image.PreserveAspectFit anchors.centerIn: parent } }