diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 889942480a57e4e003cc11420693b6a5ceca74fb..18e4b83a6e784a7b6b3a48421328694ef1c55e8b 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -227,7 +227,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) // qDebug() << "LOSSCHANGED" << receiveLoss; currLossCounter = 0; currReceiveCounter = 0; - emit receiveLossChanged(receiveLoss); + emit receiveLossChanged(message.sysid, receiveLoss); } // The packet is emitted as a whole, as it is only 255 - 261 bytes short diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h index c2e998c204c9eae94739de89cb2776a8416d60a8..fc8467501ce47220e9372522dd87a4fc41b23238 100644 --- a/src/comm/ProtocolInterface.h +++ b/src/comm/ProtocolInterface.h @@ -56,7 +56,8 @@ public slots: virtual void receiveBytes(LinkInterface *link) = 0; signals: - void receiveLossChanged(float loss); + /** @brief Update the packet loss from one system */ + void receiveLossChanged(int uasId, float loss); }; diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 3891678478216d93af6eccf97e4f600fa1c1a7e5..fb942987679be87994f36192bac4ecdbb99c4ff3 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -67,7 +67,9 @@ void UDPLink::run() void UDPLink::setAddress(QString address) { - // TODO Implement address + Q_UNUSED(address); + // FIXME TODO Implement address + //socket->setLocalAddress(QHostAddress(address)); } void UDPLink::setPort(quint16 port) diff --git a/src/ui/CameraView.cc b/src/ui/CameraView.cc index 1563ede7f17cfd4f4eaccd0bd19397eff6dc90ea..c19f514bb4dcd12b1ef863f06c527e307578e4fc 100644 --- a/src/ui/CameraView.cc +++ b/src/ui/CameraView.cc @@ -44,8 +44,9 @@ CameraView::CameraView(int width, int height, int depth, int channels, QWidget* //setImageSize(width, height, depth, channels); receivedWidth = width; receivedHeight = height; - receivedDepth = 8; - receivedChannels = 1; + receivedDepth = depth; + receivedChannels = channels; + imageId = -1; // Fill with black background QImage fill = QImage(width, height, QImage::Format_Indexed8); @@ -139,6 +140,7 @@ void CameraView::setImageSize(int width, int height, int depth, int channels) void CameraView::startImage(int imgid, int width, int height, int depth, int channels) { + this->imageId = imgid; //qDebug() << "CameraView: starting image (" << width << "x" << height << ", " << depth << "bits) with " << channels << "channels"; // Copy previous image to screen if it hasn't been finished properly @@ -208,6 +210,10 @@ void CameraView::saveImage() void CameraView::setPixels(int imgid, const unsigned char* imageData, int length, unsigned int startIndex) { + // FIXME imgid can be used to receive and then display multiple images + // the image buffer should be converted into a n image buffer. + Q_UNUSED(imgid); + // qDebug() << "at" << __FILE__ << __LINE__ << ": Received startindex" << startIndex << "and length" << length << "(" << startIndex+length << "of" << rawExpectedBytes << "bytes)"; if (imageStarted) diff --git a/src/ui/CameraView.h b/src/ui/CameraView.h index db9b1e826e60e52ee9c6db8175f3085ac3e41b3a..07b6e56f0d6946f2e6ba709b39a096ce85ca54f6 100644 --- a/src/ui/CameraView.h +++ b/src/ui/CameraView.h @@ -71,6 +71,8 @@ protected: int receivedChannels; int receivedWidth; int receivedHeight; + QMap images; ///< Reference to the received images + int imageId; ///< ID of the currently displayed image void commitRawDataToGL(); }; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index f6c29bf023429574ccc0ebce7250439eae8b3cd4..9e7584b25d2f77383a711608217987c8bf60f288 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -166,7 +166,7 @@ settings() adjustSize(); // - connect(mavlink, SIGNAL(receiveLossChanged(float)), info, SLOT(updateSendLoss(float))); + connect(mavlink, SIGNAL(receiveLossChanged(int, float)), info, SLOT(updateSendLoss(int, float))); } MainWindow::~MainWindow() diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 989be0d448e2ca7d698f43371cdcb0d483d31a10..8a0535a89d4efa42a89277e56986fecb4b8473fa 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -117,13 +117,19 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load) } } -void UASInfoWidget::updateReceiveLoss(float receiveLoss) +void UASInfoWidget::updateReceiveLoss(int uasId, float receiveLoss) { + Q_UNUSED(uasId); this->receiveLoss = this->receiveLoss * 0.8f + receiveLoss * 0.2f; } -void UASInfoWidget::updateSendLoss(float sendLoss) +/** + The send loss is typically calculated on the GCS based on packets + that were received scrambled from the MAV + */ +void UASInfoWidget::updateSendLoss(int uasId, float sendLoss) { + Q_UNUSED(uasId); this->sendLoss = this->sendLoss * 0.8f + sendLoss * 0.2f; } diff --git a/src/ui/uas/UASInfoWidget.h b/src/ui/uas/UASInfoWidget.h index 9eb66d43eec1b1da87880b6ab2dbab7980e97efb..065fa9ba76e5fdcbc03d717c4f91e4a76aac0ad1 100644 --- a/src/ui/uas/UASInfoWidget.h +++ b/src/ui/uas/UASInfoWidget.h @@ -56,8 +56,10 @@ public slots: void updateBattery(UASInterface* uas, double voltage, double percent, int seconds); void updateCPULoad(UASInterface* uas, double load); - void updateReceiveLoss(float receiveLoss); - void updateSendLoss(float sendLoss); + /** @brief Set the loss rate of packets received by the MAV */ + void updateReceiveLoss(int uasId, float receiveLoss); + /** @brief Set the loss rate of packets sent from the MAV */ + void updateSendLoss(int uasId, float sendLoss); void setVoltage(UASInterface* uas, double voltage); void setChargeLevel(UASInterface* uas, double chargeLevel);