diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index abb9845..7948de0 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -59,6 +59,15 @@ MAVLinkProtocol::MAVLinkProtocol() : // Start heartbeat timer, emitting a heartbeat at the configured rate connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat())); heartbeatTimer->start(1000/heartbeatRate); + totalReceiveCounter = 0; + totalLossCounter = 0; + for (int i = 0; i < 256; i++) + { + for (int j = 0; j < 256; j++) + { + lastIndex[i][j] = -1; + } + } } MAVLinkProtocol::~MAVLinkProtocol() @@ -120,6 +129,40 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); } + // Increase receive counter + totalReceiveCounter++; + qint64 lastLoss = totalLossCounter; + // Update last packet index + if (lastIndex[message.sysid][message.compid] == -1) + { + lastIndex[message.sysid][message.compid] = message.seq; + } + else + { + int safeguard = 0; + while(lastIndex[message.sysid][message.compid]+1 != message.seq && safeguard < 100) + { + lastIndex[message.sysid][message.compid] += 1; + totalLossCounter++; + safeguard++; + } + } +// if (lastIndex.contains(message.sysid)) +// { +// QMap* lastCompIndex = lastIndex.value(message.sysid); +// if (lastCompIndex->contains(message.compid)) +// while (lastCompIndex->value(message.compid, 0)+1 ) +// } + //if () + if (lastLoss != totalLossCounter) + { + // Calculate new loss ratio + // Receive loss + float receiveLoss = (double)totalLossCounter/(double)(totalReceiveCounter+totalLossCounter); + receiveLoss *= 100.0f; + emit receiveLossChanged(receiveLoss); + } + // The packet is emitted as a whole, as it is only 255 - 261 bytes short // kind of inefficient, but no issue for a groundstation pc. // It buys as reentrancy for the whole code over all threads diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index d9dd2dc..d27b3af 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -37,6 +37,7 @@ This file is part of the PIXHAWK project #include #include #include +#include #include #include "ProtocolInterface.h" #include "LinkInterface.h" @@ -86,6 +87,9 @@ protected: int heartbeatRate; ///< Heartbeat rate, controls the timer interval bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission QMutex receiveMutex; ///< Mutex to protect receiveBytes function + qint64 lastIndex[256][256]; + int totalReceiveCounter; + int totalLossCounter; signals: /** @brief Message received and directly copied via signal */ diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h index ac92f78..c2e998c 100644 --- a/src/comm/ProtocolInterface.h +++ b/src/comm/ProtocolInterface.h @@ -1,26 +1,26 @@ /*===================================================================== - + PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - + (c) 2009, 2010 PIXHAWK PROJECT - + This file is part of the PIXHAWK project - + PIXHAWK is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - + PIXHAWK is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with PIXHAWK. If not, see . - + ======================================================================*/ - + /** * @file * @brief Interface class for protocols @@ -47,14 +47,17 @@ This file is part of the PIXHAWK project **/ class ProtocolInterface : public QThread { -Q_OBJECT + Q_OBJECT public: - //virtual ~ProtocolInterface() {}; - virtual QString getName() = 0; - + //virtual ~ProtocolInterface() {}; + virtual QString getName() = 0; + public slots: - virtual void receiveBytes(LinkInterface *link) = 0; - + virtual void receiveBytes(LinkInterface *link) = 0; + +signals: + void receiveLossChanged(float loss); + }; #endif // _PROTOCOLINTERFACE_H_ diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 88856b6..3fb2a7e 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -164,6 +164,9 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) // Adjust the size adjustSize(); + + // + connect(mavlink, SIGNAL(receiveLossChanged(float)), info, SLOT(updateReceiveLoss(float))); } MainWindow::~MainWindow() diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index a39b49c..89aecad 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -115,6 +115,12 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load) } } +void UASInfoWidget::updateReceiveLoss(float receiveLoss) +{ + ui.receiveLossBar->setValue(receiveLoss); + ui.receiveLossLabel->setText(QString::number(receiveLoss,'f', 2)); +} + void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop) { Q_UNUSED(sysId); diff --git a/src/ui/uas/UASInfoWidget.h b/src/ui/uas/UASInfoWidget.h index 9727210..bb96590 100644 --- a/src/ui/uas/UASInfoWidget.h +++ b/src/ui/uas/UASInfoWidget.h @@ -56,6 +56,7 @@ public slots: void updateBattery(UASInterface* uas, double voltage, double percent, int seconds); void updateCPULoad(UASInterface* uas, double load); + void updateReceiveLoss(float receiveLoss); void updateDropRate(int sysId, float receiveDrop, float sendDrop); void setVoltage(UASInterface* uas, double voltage);