displaying live images from uas in groundcontrol (merged)

src/uas/UAS.cc

@@ -897,11 +897,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             ++imagePacketsArrived;
 
             // emit signal if all packets arrived
-            if ((imagePacketsArrived == imagePackets)) {
-                image.loadFromData(imageRecBuffer);
-                emit imageReady(this);
+            if ((imagePacketsArrived == imagePackets))
+            {
                 // Restart statemachine
                 imagePacketsArrived = 0;
+                emit imageReady(this);
 
                 //this->requestImage();
                 //qDebug() << "SENDING REQUEST TO GET NEW IMAGE FROM SYSTEM" << uasId;
@@ -1325,7 +1325,11 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
 
 QImage UAS::getImage()
 {
+    #ifdef MAVLINK_ENABLED_PIXHAWK
+    image.loadFromData(imageRecBuffer);
     return image;
+    #endif
+
 }
 
 void UAS::requestImage()
@@ -1333,20 +1337,14 @@ void UAS::requestImage()
 #ifdef MAVLINK_ENABLED_PIXHAWK
     qDebug() << "trying to get an image from the uas...";
 
-    if (imagePacketsArrived == 0) {
+    // check if there is already an image transmission going on
+    if (imagePacketsArrived == 0)
+    {
         mavlink_message_t msg;
         mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, DATA_TYPE_JPEG_IMAGE, 0, 0, 0, 50);
         sendMessage(msg);
-    } else if (QGC::groundTimeMilliseconds() - imageStart >= 1000) {
-        // handshake happened more than 1 second ago, packets should have arrived by now
-        // maybe we missed some packets (dropped along the way)
-        image.loadFromData(imageRecBuffer);
-        emit imageReady(this);
-        // Restart statemachine
-        imagePacketsArrived = 0;
     }
 #endif
-    // default else, wait?
 }
 
 

src/uas/UAS.h

@@ -226,7 +226,7 @@ public:
     }
     int getSystemType();
     QImage getImage();
-    void requestImage(); // ?
+    void requestImage();
     int getAutopilotType() {
         return autopilot;
     }

src/ui/HUD.cc

@@ -75,6 +75,7 @@ inline bool isinf(T value)
  */
 HUD::HUD(int width, int height, QWidget* parent)
     : QGLWidget(QGLFormat(QGL::SampleBuffers), parent),
+      u(NULL),
       uas(NULL),
       yawInt(0.0f),
       mode(tr("UNKNOWN MODE")),
@@ -105,6 +106,7 @@ HUD::HUD(int width, int height, QWidget* parent)
       fuelColor(criticalColor),
       warningBlinkRate(5),
       refreshTimer(new QTimer(this)),
+      imageTimer(new QTimer(this)),
       noCamera(true),
       hardwareAcceleration(true),
       strongStrokeWidth(1.5f),
@@ -137,6 +139,7 @@ HUD::HUD(int width, int height, QWidget* parent)
       videoEnabled(false),
       xImageFactor(1.0),
       yImageFactor(1.0)
+      imageRequested(false),
 {
     // Set auto fill to false
     setAutoFillBackground(false);
@@ -162,8 +165,10 @@ HUD::HUD(int width, int height, QWidget* parent)
 
     // Refresh timer
     refreshTimer->setInterval(updateInterval);
+    imageTimer->setInterval(1);
     //connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update()));
     connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD()));
+    connect(imageTimer, SIGNAL(timeout()), this, SLOT(requestNewImage()));
 
     // Resize to correct size and fill with image
     //glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
@@ -202,6 +207,7 @@ HUD::HUD(int width, int height, QWidget* parent)
 HUD::~HUD()
 {
     refreshTimer->stop();
+    imageTimer->stop();
 }
 
 QSize HUD::sizeHint() const
@@ -282,6 +288,7 @@ void HUD::setActiveUAS(UASInterface* uas)
         UAS* u = dynamic_cast<UAS*>(this->uas);
         if (u) {
             disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+            disconnect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(requestNewImage()));
         }
     }
 
@@ -303,10 +310,12 @@ void HUD::setActiveUAS(UASInterface* uas)
         UAS* u = dynamic_cast<UAS*>(uas);
         if (u) {
             connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+            connect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(requestNewImage()));
         }
 
         // Set new UAS
         this->uas = uas;
+        this->u = dynamic_cast<UAS*>(this->uas);
     }
 }
 
@@ -1623,3 +1632,17 @@ void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int s
         //        }
     }
 }
+
+void HUD::requestNewImage()
+{
+    if (!imageRequested)
+    {
+        this->u->requestImage();
+        imageRequested = true;
+    }
+    else
+    {
+        this->glImage = this->u->getImage();
+        imageRequested = false;
+    }
+}

src/ui/HUD.h

@@ -88,6 +88,8 @@ public slots:
     void enableHUDInstruments(bool enabled);
     /** @brief Enable Video */
     void enableVideo(bool enabled);
+    /** @brief Handler for image transmission */
+    void requestNewImage();
 
 
 protected slots:
@@ -174,6 +176,7 @@ protected:
     int warningBlinkRate;      ///< Blink rate of warning messages, will be rounded to the refresh rate
 
     QTimer* refreshTimer;      ///< The main timer, controls the update rate
+    QTimer* imageTimer;        ///< The timer for the image update rate
     QPainter* hudPainter;
     QFont font;                ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts
     QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system)
@@ -216,6 +219,8 @@ protected:
     QAction* selectOfflineDirectoryAction;
     QAction* selectVideoChannelAction;
     void paintEvent(QPaintEvent *event);
+    bool imageRequested;
+    UAS* u;
 
 };