From 53c54b15a3c0c84f37d5895ccabf2fc6e75e0bb7 Mon Sep 17 00:00:00 2001 From: lm Date: Tue, 15 Feb 2011 10:48:58 +0100 Subject: [PATCH] Added write retransmission --- src/QGC.cc | 9 ++ src/QGC.h | 2 + src/comm/MAVLinkSimulationLink.cc | 4 +- src/ui/QGCParamWidget.cc | 183 +++++++++++++++++++++++------- src/ui/QGCParamWidget.h | 5 +- 5 files changed, 159 insertions(+), 44 deletions(-) diff --git a/src/QGC.cc b/src/QGC.cc index eaef76797..d55b7208b 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -37,6 +37,15 @@ quint64 groundTimeUsecs() return static_cast(microseconds + (time.time().msec()*1000)); } +quint64 groundTimeMilliseconds() +{ + QDateTime time = QDateTime::currentDateTime(); + time = time.toUTC(); + /* Return seconds and milliseconds, in milliseconds unit */ + quint64 seconds = time.toTime_t() * static_cast(1000); + return static_cast(seconds + (time.time().msec())); +} + float limitAngleToPMPIf(float angle) { while (angle > ((float)M_PI+FLT_EPSILON)) diff --git a/src/QGC.h b/src/QGC.h index 0f4d05808..e5b87e288 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -23,6 +23,8 @@ namespace QGC /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); + /** @brief Get the current ground time in milliseconds */ + quint64 groundTimeMilliseconds(); /** @brief Returns the angle limited to -pi - pi */ float limitAngleToPMPIf(float angle); /** @brief Returns the angle limited to -pi - pi */ diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index a2391e756..565426325 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -884,6 +884,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer+=bufferlength; + //qDebug() << "Sending PARAM" << key; } else if (read.param_index < onboardParams.size()) { @@ -891,12 +892,13 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer+=bufferlength; + //qDebug() << "Sending PARAM #ID" << (read.param_index) << "KEY:" << key; } } break; diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index d826b9cf3..5e91cf2bb 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -44,7 +44,6 @@ This file is part of the QGROUNDCONTROL project */ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : QWidget(parent), - mav(uas), components(new QMap()), paramGroups(), @@ -52,9 +51,10 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : parameters(), transmissionListMode(false), transmissionActive(false), - transmissionStarted(0), + transmissionTimeout(0), retransmissionTimeout(350), - rewriteTimeout(500) + rewriteTimeout(500), + retransmissionBurstRequestSize(2) { // Load settings loadSettings(); @@ -210,6 +210,12 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa { addParameter(uas, component, parameterName, value); + // Missing packets list has to be instantiated for all components + if (!transmissionMissingPackets.contains(component)) + { + transmissionMissingPackets.insert(component, new QList()); + } + // List mode is different from single parameter transfers if (transmissionListMode) { @@ -217,13 +223,8 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa // each component if (!transmissionListSizeKnown.contains(component)) { - // If this was the first packet to announce the list size - // set all packets not received yet as missing to enforce - // retransmission until full list is received - if (!transmissionMissingPackets.contains(component)) - { - transmissionMissingPackets.insert(component, new QList()); - } + // Mark list size as known + transmissionListSizeKnown.insert(component, true); // Mark all parameters as missing for (int i = 0; i < paramCount; ++i) @@ -234,8 +235,14 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa } } - // Mark list size as known - transmissionListSizeKnown.insert(component, true); + // There is only one transmission timeout for all components + // since components do not manage their transmission, + // the longest timeout is safe for all components. + quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount/retransmissionBurstRequestSize+5)*retransmissionTimeout); + if (thisTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = thisTransmissionTimeout; + } } // Start retransmission guard @@ -243,18 +250,44 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa setRetransmissionGuardEnabled(true); } - // Mark this parameter as received + // Mark this parameter as received in read list int index = transmissionMissingPackets.value(component)->indexOf(paramId); // If the MAV sent the parameter without request, it wont be in missing list if (index != -1) transmissionMissingPackets.value(component)->removeAt(index); + bool justWritten = false; + bool writeMismatch = false; + // Mark this parameter as received in write ACK list + QMap* map = transmissionMissingWriteAckPackets.value(component); + if (map && map->contains(parameterName)) + { + justWritten = true; + if (map->value(parameterName) != value) + { + writeMismatch = true; + } + map->remove(parameterName); + } + int missCount = 0; foreach (int key, transmissionMissingPackets.keys()) { missCount += transmissionMissingPackets.value(key)->count(); } - statusLabel->setText(tr("Got Param (#%1 of %5) %2: %3 (%4 missing)").arg(paramId+1).arg(parameterName).arg(value).arg(missCount).arg(paramCount)); + if (justWritten && !writeMismatch) + { + statusLabel->setText(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(parameterName).arg(value).arg(paramCount)); + } + else if (justWritten && writeMismatch) + { + // Mismatch, tell user + statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName), value)); + } + else + { + statusLabel->setText(tr("Got %2 (#%1/%5): %3 (%4 missing)").arg(paramId+1).arg(parameterName).arg(value).arg(missCount).arg(paramCount)); + } // Check if last parameter was received if (missCount == 0) @@ -417,7 +450,13 @@ void QGCParamWidget::requestParameterList() transmissionMissingPackets.value(key)->clear(); } transmissionActive = true; - transmissionStarted = QGC::groundTimeUsecs(); + + // Set status text + statusLabel->setText(tr("Requested param list.. waiting")); + + // Request twice as mean of forward error correction + mav->requestParameters(); + QGC::SLEEP::msleep(10); mav->requestParameters(); } @@ -456,8 +495,8 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) // Check if the value was numerically changed if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, 0.0f) != value) { - current->setBackground(0, QBrush(QColor(QGC::colorGreen))); - current->setBackground(1, QBrush(QColor(QGC::colorGreen))); + current->setBackground(0, QBrush(QColor(QGC::colorOrange))); + current->setBackground(1, QBrush(QColor(QGC::colorOrange))); } // All parameters list @@ -593,27 +632,54 @@ void QGCParamWidget::retransmissionGuardTick() if (transmissionActive) { qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS.."; - // Re-request at maximum five parameters at once - // to prevent link flooding + // Check for timeout + // stop retransmission attempts on timeout + if (QGC::groundTimeMilliseconds() > transmissionTimeout) + { + setRetransmissionGuardEnabled(false); + transmissionActive = false; + + // Empty read retransmission list + // Empty write retransmission list + int missingReadCount = 0; + QList readKeys = transmissionMissingPackets.keys(); + foreach (int component, readKeys) + { + missingReadCount += transmissionMissingPackets.value(component)->count(); + transmissionMissingPackets.value(component)->clear(); + } + + // Empty write retransmission list + int missingWriteCount = 0; + QList writeKeys = transmissionMissingWriteAckPackets.keys(); + foreach (int component, writeKeys) + { + missingWriteCount += transmissionMissingWriteAckPackets.value(component)->count(); + transmissionMissingWriteAckPackets.value(component)->clear(); + } + statusLabel->setText(tr("TIMEOUT! MISSING: %1 read, %2 write.").arg(missingReadCount).arg(missingWriteCount)); + } + + // Re-request at maximum retransmissionBurstRequestSize parameters at once + // to prevent link flooding QMap*>::iterator i; for (i = parameters.begin(); i != parameters.end(); ++i) { // Iterate through the parameters of the component int component = i.key(); - // Request five parameters from this component (at maximum) + // Request n parameters from this component (at maximum) QList * paramList = transmissionMissingPackets.value(component, NULL); if (paramList) { int count = 0; - int maxCount = 1; foreach (int id, *paramList) { - if (count < maxCount) + if (count < retransmissionBurstRequestSize) { qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component; emit requestParameter(component, id); - statusLabel->setText(tr("Requ. retransmission of #%1").arg(id)); + statusLabel->setText(tr("Requested retransmission of #%1").arg(id+1)); count++; } else @@ -623,6 +689,30 @@ void QGCParamWidget::retransmissionGuardTick() } } } + + // Re-request at maximum retransmissionBurstRequestSize parameters at once + // to prevent write-request link flooding + // Empty write retransmission list + QList writeKeys = transmissionMissingWriteAckPackets.keys(); + foreach (int component, writeKeys) + { + int count = 0; + QMap * missingParams = transmissionMissingWriteAckPackets.value(component); + foreach (QString key, missingParams->keys()) + { + if (count < retransmissionBurstRequestSize) + { + // Re-request write operation + emit parameterChanged(component, key, missingParams->value(key)); + statusLabel->setText(tr("Re-requested write: %1: %2").arg(key).arg(missingParams->value(key))); + count++; + } + else + { + break; + } + } + } } else { @@ -640,23 +730,25 @@ void QGCParamWidget::retransmissionGuardTick() void QGCParamWidget::setParameter(int component, QString parameterName, float value) { emit parameterChanged(component, parameterName, value); -// // Wait for parameter to be written back -// // mark it therefore as missing -// if (!transmissionMissingPackets.contains(component)) -// { -// transmissionMissingPackets.insert(component, new QList()); -// } - -// for (int i = 0; i < paramCount; ++i) -// { -// if (!transmissionMissingPackets.value(component)->contains(i)) -// { -// transmissionMissingPackets.value(component)->append(i); -// } -// } -// transmissionActive = true; -// transmissionStarted = QGC::groundTimeUsecs(); -// setRetransmissionGuardEnabled(true); + // Wait for parameter to be written back + // mark it therefore as missing + if (!transmissionMissingWriteAckPackets.contains(component)) + { + transmissionMissingWriteAckPackets.insert(component, new QMap()); + } + + // Insert it in missing write ACK list + transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value); + + // Set timeouts + transmissionActive = true; + quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + 5*rewriteTimeout; + if (newTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = newTransmissionTimeout; + } + // Enable guard / reset timeouts + setRetransmissionGuardEnabled(true); } /** @@ -682,7 +774,7 @@ void QGCParamWidget::setParameters() } } - // Update stats label + // Change transmission status if necessary if (parametersSent == 0) { statusLabel->setText(tr("No transmission: No changed values.")); @@ -690,6 +782,15 @@ void QGCParamWidget::setParameters() else { statusLabel->setText(tr("Transmitting %1 parameters.").arg(parametersSent)); + // Set timeouts + transmissionActive = true; + quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + (parametersSent/retransmissionBurstRequestSize+5)*rewriteTimeout; + if (newTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = newTransmissionTimeout; + } + // Enable guard + setRetransmissionGuardEnabled(true); } changedValues.clear(); diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index 2a427855c..c12b08925 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -94,14 +94,15 @@ protected: QMap* > parameters; ///< All parameters QVector received; ///< Successfully received parameters QMap* > transmissionMissingPackets; ///< Missing packets - QMap* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets + QMap* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets bool transmissionListMode; ///< Currently requesting list QMap transmissionListSizeKnown; ///< List size initialized? bool transmissionActive; ///< Missing packets, working on list? - quint64 transmissionStarted; ///< Timeout + quint64 transmissionTimeout; ///< Timeout QTimer retransmissionTimer; ///< Timer handling parameter retransmission int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds int rewriteTimeout; ///< Write request timeout, in milliseconds + int retransmissionBurstRequestSize; ///< Number of packets requested for retransmission per burst /** @brief Activate / deactivate parameter retransmission */ void setRetransmissionGuardEnabled(bool enabled); -- 2.22.0