From 6c69332dd3d09bc52e11fe09271310ac8368d922 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 27 May 2011 20:47:02 +0200 Subject: [PATCH] displaying live images from uas in groundcontrol (merged) --- src/uas/UAS.cc | 22 ++++++++++------------ src/uas/UAS.h | 2 +- src/ui/HUD.cc | 23 +++++++++++++++++++++++ src/ui/HUD.h | 5 +++++ 4 files changed, 39 insertions(+), 13 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index cbd5b8709..76a04112a 100644 --- a/src/uas/UAS.cc +++ b/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? } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 49bee291c..c53f63a1c 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -226,7 +226,7 @@ public: } int getSystemType(); QImage getImage(); - void requestImage(); // ? + void requestImage(); int getAutopilotType() { return autopilot; } diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 09b171bbf..989833276 100644 --- a/src/ui/HUD.cc +++ b/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(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); 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(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; + } +} diff --git a/src/ui/HUD.h b/src/ui/HUD.h index b476fac5d..2a293dd98 100644 --- a/src/ui/HUD.h +++ b/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; }; -- 2.22.0