diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 7948de03fea78bbb2f225d8b40d7715a32feea04..df36c80641f609dfc7a04959df2559b8de390acc 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -139,20 +139,37 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) } else { + if (lastIndex[message.sysid][message.compid] == 255) + { + lastIndex[message.sysid][message.compid] = 0; + } + else + { + lastIndex[message.sysid][message.compid]++; + } + int safeguard = 0; - while(lastIndex[message.sysid][message.compid]+1 != message.seq && safeguard < 100) + //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq; + while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 1) { - lastIndex[message.sysid][message.compid] += 1; + if (lastIndex[message.sysid][message.compid] == 255) + { + lastIndex[message.sysid][message.compid] = 0; + } + else + { + lastIndex[message.sysid][message.compid]++; + } 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 (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) { @@ -160,6 +177,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) // Receive loss float receiveLoss = (double)totalLossCounter/(double)(totalReceiveCounter+totalLossCounter); receiveLoss *= 100.0f; + qDebug() << "LOSS" << receiveLoss; emit receiveLossChanged(receiveLoss); } diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index d27b3af6ceb91e50df63c29baebf2726458f471b..5995c3b50593598e64ae83078feb1ffc37b1d1ca 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -87,7 +87,7 @@ 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 lastIndex[256][256]; int totalReceiveCounter; int totalLossCounter; diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 89aecad21e8c26caaee0b72d8e594b45318047c1..d33c9acd992ec1ed4e9ece7ed1c291b671e67b9e 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -67,6 +67,8 @@ UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent) this->voltage = 0; this->chargeLevel = 0; this->load = 0; + receiveLoss = 0; + sendLoss = 0; updateTimer = new QTimer(this); connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); @@ -117,17 +119,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)); + this->receiveLoss = this->receiveLoss * 0.8f + receiveLoss * 0.2f; } -void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop) +void UASInfoWidget::updateSendLoss(float sendLoss) { - Q_UNUSED(sysId); - ui.receiveLossBar->setValue(receiveDrop); - ui.receiveLossLabel->setText(QString::number(receiveDrop) + "%"); - ui.sendLossBar->setValue(sendDrop); - ui.sendLossLabel->setText(QString::number(receiveDrop) + "%"); + this->sendLoss = this->sendLoss * 0.8f + sendLoss * 0.2f; } void UASInfoWidget::setVoltage(UASInterface* uas, double voltage) @@ -159,4 +156,10 @@ void UASInfoWidget::refresh() ui.loadLabel->setText(QString::number(this->load, 'f', loadDecimals)); ui.loadBar->setValue(static_cast(this->load)); + + ui.receiveLossBar->setValue(receiveLoss); + ui.receiveLossLabel->setText(QString::number(receiveLoss,'f', 2)); + + ui.sendLossBar->setValue(sendLoss); + ui.sendLossLabel->setText(QString::number(sendLoss, 'f', 2)); } diff --git a/src/ui/uas/UASInfoWidget.h b/src/ui/uas/UASInfoWidget.h index bb965904a2e0ddb468a7ea4e5401a9dfdee715fa..9eb66d43eec1b1da87880b6ab2dbab7980e97efb 100644 --- a/src/ui/uas/UASInfoWidget.h +++ b/src/ui/uas/UASInfoWidget.h @@ -57,7 +57,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 updateSendLoss(float sendLoss); void setVoltage(UASInterface* uas, double voltage); void setChargeLevel(UASInterface* uas, double chargeLevel); @@ -83,6 +83,8 @@ protected: double chargeLevel; double timeRemaining; double load; + float receiveLoss; + float sendLoss; QTimer* updateTimer; QString name; quint64 startTime;