From 393f1533c5b782f20cd7946653cf1ca5785cddd2 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 10 Apr 2016 10:11:57 -0700 Subject: [PATCH] Show mavlink sequence number packet loss in UI --- src/comm/MAVLinkProtocol.cc | 13 +- src/comm/MAVLinkProtocol.h | 5 +- src/ui/UASInfo.ui | 249 +++++++++++++++++++++++++++++++----- src/ui/uas/UASInfoWidget.cc | 62 ++++++--- src/ui/uas/UASInfoWidget.h | 17 ++- 5 files changed, 283 insertions(+), 63 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index ebde1f123..44779f7ef 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -63,7 +63,11 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app) , _linkMgr(NULL) , _multiVehicleManager(NULL) { - + memset(&totalReceiveCounter, 0, sizeof(totalReceiveCounter)); + memset(&totalLossCounter, 0, sizeof(totalLossCounter)); + memset(&totalErrorCounter, 0, sizeof(totalErrorCounter)); + memset(&currReceiveCounter, 0, sizeof(currReceiveCounter)); + memset(&currLossCounter, 0, sizeof(currLossCounter)); } MAVLinkProtocol::~MAVLinkProtocol() @@ -357,11 +361,12 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { // Calculate new loss ratio // Receive loss - float receiveLoss = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]); - receiveLoss *= 100.0f; + float receiveLossPercent = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]); + receiveLossPercent *= 100.0f; currLossCounter[mavlinkChannel] = 0; currReceiveCounter[mavlinkChannel] = 0; - emit receiveLossChanged(message.sysid, receiveLoss); + emit receiveLossPercentChanged(message.sysid, receiveLossPercent); + emit receiveLossTotalChanged(message.sysid, totalLossCounter[mavlinkChannel]); } // The packet is emitted as a whole, as it is only 255 - 261 bytes short diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index a0c092b9e..3dae4f827 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -242,8 +242,9 @@ signals: void actionGuardChanged(bool enabled); /** @brief Emitted if action request timeout changed */ void actionRetransmissionTimeoutChanged(int ms); - /** @brief Update the packet loss from one system */ - void receiveLossChanged(int uasId, float loss); + + void receiveLossPercentChanged(int uasId, float lossPercent); + void receiveLossTotalChanged(int uasId, int totalLoss); /** * @brief Emitted if a new radio status packet received diff --git a/src/ui/UASInfo.ui b/src/ui/UASInfo.ui index 7f9503f9b..8eda19d64 100644 --- a/src/ui/UASInfo.ui +++ b/src/ui/UASInfo.ui @@ -6,7 +6,7 @@ 0 0 - 230 + 243 161 @@ -16,8 +16,17 @@ - - + + + 3 + + + 3 + + + 3 + + 3 @@ -288,14 +297,14 @@ - + - MCU Load + MAV Seq Loss - + Qt::Horizontal @@ -311,7 +320,7 @@ - + 0 @@ -324,7 +333,7 @@ - + % @@ -337,7 +346,7 @@ - + 10 @@ -373,44 +382,199 @@ - - - - Qt::Horizontal + + + + Seq Loss Cnt + + + + + + + Qt::Horizontal + + + QSizePolicy::Maximum + + + + 13 + 15 + + + + + + + + 0 + + + Qt::AutoText + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + Qt::AutoText + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + 10 + 18 + + + + + 100 + 0 + + + + + 16777215 + 20 + + + + + 0 + 18 + + + + 0 + + + true + + + %p% + + + + + + + MCU Load - - + + - Qt::Vertical + Qt::Horizontal - QSizePolicy::Expanding + QSizePolicy::Maximum - 0 - 0 + 13 + 15 - - + + - No error status received yet + 0 + + + Qt::AutoText + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + % + + + Qt::AutoText + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + 10 + 18 + + + + + 100 + 0 + + + + + 16777215 + 20 + + + + + 0 + 18 + + + + 0 + + + true + + + %p% + + + + CPU Load - + + + + Qt::Horizontal + + + + 13 + 15 + + + + + 0 @@ -420,14 +584,14 @@ - + % - + @@ -446,18 +610,35 @@ - - + + Qt::Horizontal - - - 13 - 15 - + + + + + + No error status received yet - + + + + + + Qt::Vertical + + + QSizePolicy::Expanding + + + + 0 + 0 + + + diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 8df20fea8..2e0a658c7 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -46,10 +46,12 @@ This file is part of the PIXHAWK project UASInfoWidget::UASInfoWidget(const QString& title, QAction* action, QWidget *parent, QString name) : QGCDockWidget(title, action, parent) + , _activeUAS(NULL) + , _seqLossPercent(0) + , _seqLossTotal(0) { ui.setupUi(this); this->name = name; - activeUAS = NULL; connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &UASInfoWidget::_activeVehicleChanged); _activeVehicleChanged(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()); @@ -76,6 +78,9 @@ UASInfoWidget::UASInfoWidget(const QString& title, QAction* action, QWidget *par this->setVisible(false); loadSettings(); + + connect(qgcApp()->toolbox()->mavlinkProtocol(), &MAVLinkProtocol::receiveLossPercentChanged, this, &UASInfoWidget::updateSeqLossPercent); + connect(qgcApp()->toolbox()->mavlinkProtocol(), &MAVLinkProtocol::receiveLossTotalChanged, this, &UASInfoWidget::updateSeqLossTotal); } UASInfoWidget::~UASInfoWidget() @@ -101,20 +106,20 @@ void UASInfoWidget::hideEvent(QHideEvent* event) void UASInfoWidget::_activeVehicleChanged(Vehicle* vehicle) { - if (activeUAS) { - disconnect(activeUAS, &UASInterface::batteryChanged, this, &UASInfoWidget::updateBattery); - disconnect(activeUAS, &UASInterface::dropRateChanged, this, &UASInfoWidget::updateReceiveLoss); - disconnect(static_cast(activeUAS), &UAS::loadChanged, this, &UASInfoWidget::updateCPULoad); - disconnect(activeUAS, &UASInterface::errCountChanged, this, &UASInfoWidget::updateErrorCount); - activeUAS = NULL; + if (_activeUAS) { + disconnect(_activeUAS, &UASInterface::batteryChanged, this, &UASInfoWidget::updateBattery); + disconnect(_activeUAS, &UASInterface::dropRateChanged, this, &UASInfoWidget::updateReceiveLoss); + disconnect(static_cast(_activeUAS), &UAS::loadChanged, this, &UASInfoWidget::updateCPULoad); + disconnect(_activeUAS, &UASInterface::errCountChanged, this, &UASInfoWidget::updateErrorCount); + _activeUAS = NULL; } if (vehicle) { - activeUAS = vehicle->uas(); - connect(activeUAS, &UASInterface::batteryChanged, this, &UASInfoWidget::updateBattery); - connect(activeUAS, &UASInterface::dropRateChanged, this, &UASInfoWidget::updateReceiveLoss); - connect(static_cast(activeUAS), &UAS::loadChanged, this, &UASInfoWidget::updateCPULoad); - connect(activeUAS, &UASInterface::errCountChanged, this, &UASInfoWidget::updateErrorCount); + _activeUAS = vehicle->uas(); + connect(_activeUAS, &UASInterface::batteryChanged, this, &UASInfoWidget::updateBattery); + connect(_activeUAS, &UASInterface::dropRateChanged, this, &UASInfoWidget::updateReceiveLoss); + connect(static_cast(_activeUAS), &UAS::loadChanged, this, &UASInfoWidget::updateCPULoad); + connect(_activeUAS, &UASInterface::errCountChanged, this, &UASInfoWidget::updateErrorCount); } } @@ -128,8 +133,8 @@ void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double curr void UASInfoWidget::updateErrorCount(int uasid, QString component, QString device, int count) { - //qDebug() << __FILE__ << __LINE__ << activeUAS->getUASID() << "=" << uasid; - if (activeUAS->getUASID() == uasid) { + //qDebug() << __FILE__ << __LINE__ << _activeUAS->getUASID() << "=" << uasid; + if (_activeUAS->getUASID() == uasid) { errors.remove(component + ":" + device); errors.insert(component + ":" + device, count); } @@ -140,7 +145,7 @@ void UASInfoWidget::updateErrorCount(int uasid, QString component, QString devic */ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load) { - if (activeUAS == uas) { + if (_activeUAS == uas) { this->load = load; } } @@ -151,6 +156,24 @@ void UASInfoWidget::updateReceiveLoss(int uasId, float receiveLoss) this->receiveLoss = this->receiveLoss * 0.8f + receiveLoss * 0.2f; } +void UASInfoWidget::updateSeqLossPercent(int uasId, float seqLossPercent) +{ + if (_activeUAS && _activeUAS->getUASID() == uasId) { + _seqLossPercent = _seqLossPercent * 0.8f + seqLossPercent * 0.2f; + } else { + _seqLossPercent = 0; + } +} + +void UASInfoWidget::updateSeqLossTotal(int uasId, int seqLossTotal) +{ + if (_activeUAS && _activeUAS->getUASID() == uasId) { + _seqLossTotal = seqLossTotal; + } else { + _seqLossTotal = 0; + } +} + /** The send loss is typically calculated on the GCS based on packets that were received scrambled from the MAV @@ -169,14 +192,14 @@ void UASInfoWidget::setVoltage(UASInterface* uas, double voltage) void UASInfoWidget::setChargeLevel(UASInterface* uas, double chargeLevel) { - if (activeUAS == uas) { + if (_activeUAS == uas) { this->chargeLevel = chargeLevel; } } void UASInfoWidget::setTimeRemaining(UASInterface* uas, double seconds) { - if (activeUAS == uas) { + if (_activeUAS == uas) { this->timeRemaining = seconds; } } @@ -192,6 +215,11 @@ void UASInfoWidget::refresh() ui.receiveLossBar->setValue(qMax(0, qMin(static_cast(receiveLoss), 100))); ui.receiveLossLabel->setText(QString::number(receiveLoss, 'f', 2)); + ui.seqLossBar->setValue(qMax(0, qMin(static_cast(_seqLossPercent), 100))); + ui.seqLossLabel->setText(QString::number(_seqLossPercent, 'f', 2)); + + ui.seqcntLossLabel->setText(QString::number(_seqLossTotal)); + 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 a7bd66616..d70148b07 100644 --- a/src/ui/uas/UASInfoWidget.h +++ b/src/ui/uas/UASInfoWidget.h @@ -54,13 +54,16 @@ public: public slots: void updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds); void updateCPULoad(UASInterface* uas, double load); - /** - * @brief Set the loss rate of packets received by the MAV. - * @param uasId UNUSED - * @param receiveLoss A percentage value (0-100) of how many message the UAS has failed to receive. - */ + /** + * @brief Set the loss rate of packets received by the MAV. + * @param uasId UNUSED + * @param receiveLoss A percentage value (0-100) of how many message the UAS has failed to receive. + */ void updateReceiveLoss(int uasId, float receiveLoss); + void updateSeqLossPercent(int uasId, float seqLoss); + void updateSeqLossTotal(int uasId, int seqLossTotal); + /** * @brief Set the loss rate of packets sent from the MAV * @param uasId UNUSED @@ -79,7 +82,6 @@ public slots: protected: - UASInterface* activeUAS; // Configuration variables int voltageDecimals; @@ -110,6 +112,9 @@ private slots: private: Ui::uasInfo ui; + UASInterface* _activeUAS; + float _seqLossPercent; + int _seqLossTotal; }; #endif // _UASINFOWIDGET_H_ -- 2.22.0