Added write retransmission

src/QGC.cc

@@ -37,6 +37,15 @@ quint64 groundTimeUsecs()
     return static_cast<quint64>(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<quint64>(1000);
+    return static_cast<quint64>(seconds + (time.time().msec()));
+}
+
 float limitAngleToPMPIf(float angle)
 {
     while (angle > ((float)M_PI+FLT_EPSILON))

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 */

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;

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<int, QTreeWidgetItem*>()),
         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<int>());
+    }
+
     // 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<int>());
-            }
+            // 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<QString, float>* 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<int> readKeys = transmissionMissingPackets.keys();
+            foreach (int component, readKeys)
+            {
+                missingReadCount += transmissionMissingPackets.value(component)->count();
+                transmissionMissingPackets.value(component)->clear();
+            }
+
+            // Empty write retransmission list
+            int missingWriteCount = 0;
+            QList<int> 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<int, QMap<QString, float>*>::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<int> * 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<int> writeKeys = transmissionMissingWriteAckPackets.keys();
+        foreach (int component, writeKeys)
+        {
+            int count = 0;
+            QMap <QString, float>* 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<int>());
-//    }
-
-//    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<QString, float>());
+    }
+
+    // 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();

src/ui/QGCParamWidget.h

@@ -94,14 +94,15 @@ protected:
     QMap<int, QMap<QString, float>* > parameters; ///< All parameters
     QVector<bool> received; ///< Successfully received parameters
     QMap<int, QList<int>* > transmissionMissingPackets; ///< Missing packets
-    QMap<int, QList<float>* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets
+    QMap<int, QMap<QString, float>* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets
     bool transmissionListMode;       ///< Currently requesting list
     QMap<int, bool> 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);