Mavlink: Improve Image Transmission Protocol Handling

custom-example/qgroundcontrol.qrc

@@ -76,7 +76,7 @@
 		<file alias="PlanViewSettings.qml">../src/UI/preferences/PlanViewSettings.qml</file>
 		<file alias="PlanViewToolBar.qml">../src/UI/toolbar/PlanViewToolBar.qml</file>
 		<file alias="PreFlightCheckList.qml">../src/FlightDisplay/PreFlightCheckList.qml</file>
-		<file alias="PX4FlowSensor.qml">../src/VehicleSetup/PX4FlowSensor.qml</file>
+		<file alias="OpticalFlowSensor.qml">../src/VehicleSetup/OpticalFlowSensor.qml</file>
 		<file alias="VerticalCompassAttitude.qml">../src/FlightMap/Widgets/VerticalCompassAttitude.qml</file>
 		<file alias="HorizontalCompassAttitude.qml">../src/FlightMap/Widgets/HorizontalCompassAttitude.qml</file>
 		<file alias="QGroundControl/Controls/AnalyzePage.qml">../src/AnalyzeView/AnalyzePage.qml</file>

qgroundcontrol.qrc

@@ -76,7 +76,7 @@
         <file alias="PlanViewSettings.qml">src/UI/preferences/PlanViewSettings.qml</file>
         <file alias="PlanViewToolBar.qml">src/UI/toolbar/PlanViewToolBar.qml</file>
         <file alias="PreFlightCheckList.qml">src/FlightDisplay/PreFlightCheckList.qml</file>
-        <file alias="PX4FlowSensor.qml">src/VehicleSetup/PX4FlowSensor.qml</file>
+        <file alias="OpticalFlowSensor.qml">src/VehicleSetup/OpticalFlowSensor.qml</file>
         <file alias="VerticalCompassAttitude.qml">src/FlightMap/Widgets/VerticalCompassAttitude.qml</file>
         <file alias="HorizontalCompassAttitude.qml">src/FlightMap/Widgets/HorizontalCompassAttitude.qml</file>
         <file alias="QGroundControl/Controls/AnalyzePage.qml">src/AnalyzeView/AnalyzePage.qml</file>

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,74 @@ 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<int>(_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());
+
+            _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;

src/MAVLink/ImageProtocolManager.h

@@ -11,30 +11,40 @@
 
 #include "MAVLinkLib.h"
 
-#include <QtCore/QObject>
 #include <QtCore/QByteArray>
 #include <QtCore/QLoggingCategory>
+#include <QtCore/QObject>
 #include <QtGui/QImage>
 
 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;
 };

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;
 }

src/QmlControls/QGCImageProvider.h

@@ -10,23 +10,20 @@
 #pragma once
 
 #include <QtCore/QObject>
+#include <QtGui/QImage>
 #include <QtQuick/QQuickImageProvider>
 
-// 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<uint8_t, QImage> _images;
+    QImage _dummy;
 };

src/Vehicle/MultiVehicleManager.cc

@@ -72,14 +72,14 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle
     // Special case PX4 Flow since depending on firmware it can have different settings. We force to the PX4 Firmware settings.
     if (link->isPX4Flow()) {
         vehicleId = 81;
-        componentId = 50;//MAV_COMP_ID_AUTOPILOT1;
+        componentId = MAV_COMP_ID_USER26;
         vehicleFirmwareType = MAV_AUTOPILOT_GENERIC;
-        vehicleType = 0;
+        vehicleType = MAV_TYPE_GENERIC;
     }
 
     if (componentId != MAV_COMP_ID_AUTOPILOT1) {
         // Special case for PX4 Flow
-        if (vehicleId != 81 || componentId != 50) {
+        if (vehicleId != 81 || componentId != MAV_COMP_ID_USER26) {
             // Don't create vehicles for components other than the autopilot
             qCDebug(MultiVehicleManagerLog()) << "Ignoring heartbeat from unknown component port:vehicleId:componentId:fwType:vehicleType"
                                               << link->linkConfiguration()->name()