Renamed getNominalDataRate to getConnectionSpeed. Also added upstream…

…/downstream data rate functions to all Link classes.

src/comm/LinkInterface.h

@@ -73,15 +73,35 @@ class LinkInterface : public QThread
     /* Connection characteristics */
 
     /**
-     * @Brief Get the nominal data rate of the interface.
+     * @Brief Get the maximum connection speed for this interface.
      *
      * The nominal data rate is the theoretical maximum data rate of the
      * interface. For 100Base-T Ethernet this would be 100 Mbit/s (100'000'000
      * Bit/s, NOT 104'857'600 Bit/s).
      *
      * @return The nominal data rate of the interface in bit per second, 0 if unknown
      **/
-    virtual qint64 getNominalDataRate() const = 0;
+    virtual qint64 getConnectionSpeed() const = 0;
+
+    /**
+     * @Brief Get the current incoming data rate.
+     *
+     * This should be over a short timespan, something like 100ms. A precise value isn't necessary,
+     * and this can be filtered, but should be a reasonable estimate of current data rate.
+     *
+     * @return The data rate of the interface in bits per second, 0 if unknown
+     **/
+    virtual qint64 getCurrentInDataRate() const = 0;
+
+    /**
+     * @Brief Get the current outgoing data rate.
+     *
+     * This should be over a short timespan, something like 100ms. A precise value isn't necessary,
+     * and this can be filtered, but should be a reasonable estimate of current data rate.
+     *
+     * @return The data rate of the interface in bits per second, 0 if unknown
+     **/
+    virtual qint64 getCurrentOutDataRate() const = 0;
 
     /**
      * @brief Connect this interface logically

src/comm/MAVLinkSimulationLink.cc

@@ -1000,8 +1000,18 @@ QString MAVLinkSimulationLink::getName() const
     return name;
 }
 
-qint64 MAVLinkSimulationLink::getNominalDataRate() const
+qint64 MAVLinkSimulationLink::getConnectionSpeed() const
 {
     /* 100 Mbit is reasonable fast and sufficient for all embedded applications */
     return 100000000;
-}
+}
+
+qint64 MAVLinkSimulationLink::getCurrentInDataRate() const
+{
+    return 0;
+}
+
+qint64 MAVLinkSimulationLink::getCurrentOutDataRate() const
+{
+    return 0;
+}

src/comm/MAVLinkSimulationLink.h

@@ -58,8 +58,10 @@ class MAVLinkSimulationLink : public LinkInterface
     bool connect();
     bool disconnect();
 
-    /* Extensive statistics for scientific purposes */
-    qint64 getNominalDataRate() const;
+    // Extensive statistics for scientific purposes
+    qint64 getConnectionSpeed() const;
+    qint64 getCurrentInDataRate() const;
+    qint64 getCurrentOutDataRate() const;
 
     QString getName() const;
     int getId() const;

src/comm/OpalLink.cc

@@ -496,16 +496,18 @@ bool OpalLink::disconnect()
     return true;
 }
 
+// Data rate functions
+qint64 OpalLink::getConnectionSpeed() const
+{
+    return 0; //unknown
+}
 
+qint64 OpalLink::getCurrentInDataRate() const
+{
+    return 0;
+}
 
-
-/*
- *
-  Statisctics
- *
- */
-
-qint64 OpalLink::getNominalDataRate() const
+qint64 OpalLink::getCurrentOutDataRate() const
 {
-    return 0; //unknown
-}
+    return 0;
+}

src/comm/OpalLink.h

@@ -75,10 +75,9 @@ class OpalLink : public LinkInterface
     QString getName() const;
     bool isConnected() const;
 
-    /* Connection characteristics */
-
-
-    qint64 getNominalDataRate() const;
+    qint64 getConnectionSpeed() const;
+    qint64 getCurrentInDataRate() const;
+    qint64 getCurrentOutDataRate() const;
 
     bool connect();
 

src/comm/SerialLink.cc

@@ -19,18 +19,26 @@
 #include "QGC.h"
 #include <MG.h>
 
-
-
 SerialLink::SerialLink(QString portname, int baudRate, bool hardwareFlowControl, bool parity,
                        int dataBits, int stopBits) :
     m_bytesRead(0),
     m_port(NULL),
+    inDataIndex(0),
+    outDataIndex(0),
     m_stopp(false),
     m_reqReset(false)
 {
-    // Setup settings
-    m_portName = portname.trimmed();
+    // Initialize our arrays manually, cause C++<03 is dumb.
+    for (int i = 0; i < buffer_size; ++i)
+    {
+        inDataWriteAmounts[i] = 0;
+        inDataWriteTimes[i] = 0;
+        outDataWriteAmounts[i] = 0;
+        outDataWriteTimes[i] = 0;
+    }
 
+    // Get the name of the current port in use.
+    m_portName = portname.trimmed();
     if (m_portName == "" && getCurrentPorts().size() > 0)
     {
         m_portName = m_ports.first().trimmed();
@@ -69,6 +77,7 @@ SerialLink::SerialLink(QString portname, int baudRate, bool hardwareFlowControl,
 
     LinkManager::instance()->add(this);
 }
+
 void SerialLink::requestReset()
 {
     QMutexLocker locker(&this->m_stoppMutex);
@@ -184,7 +193,7 @@ void SerialLink::run()
 
         // Write all our buffered data out the serial port.
         if (m_transmitBuffer.count() > 0) {
-            QMutexLocker writeLocker(&m_writeMutex);
+            m_writeMutex.lock();
             int numWritten = m_port->write(m_transmitBuffer);
             bool txSuccess = m_port->waitForBytesWritten(5);
             if (!txSuccess || (numWritten != m_transmitBuffer.count())) {
@@ -196,25 +205,41 @@ void SerialLink::run()
                 // Since we were successful, reset out error counter.
                 linkErrorCount = 0;
             }
-            m_transmitBuffer =  m_transmitBuffer.remove(0, numWritten);
+
+            // Now that we transmit all of the data in the transmit buffer, flush it.
+            m_transmitBuffer = m_transmitBuffer.remove(0, numWritten);
+            m_writeMutex.unlock();
+
+            // Log this written data for this timestep. If this value ends up being 0 due to
+            // write() failing, that's what we want as well.
+            QMutexLocker statsLocker(&m_statisticsMutex);
+            WriteDataStatsBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, numWritten, QDateTime::currentMSecsSinceEpoch());
         }
 
         //wait n msecs for data to be ready
         //[TODO][BB] lower to SerialLink::poll_interval?
+        m_dataMutex.lock();
         bool success = m_port->waitForReadyRead(10);
 
         if (success) {
             QByteArray readData = m_port->readAll();
             while (m_port->waitForReadyRead(10))
                 readData += m_port->readAll();
+            m_dataMutex.unlock();
             if (readData.length() > 0) {
                 emit bytesReceived(this, readData);
 
+                // Log this data reception for this timestep
+                QMutexLocker statsLocker(&m_statisticsMutex);
+                WriteDataStatsBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, readData.length(), QDateTime::currentMSecsSinceEpoch());
+
+                // Track the total amount of data read.
                 m_bytesRead += readData.length();
                 linkErrorCount = 0;
             }
         }
         else {
+            m_dataMutex.unlock();
             linkErrorCount++;
         }
 
@@ -268,18 +293,31 @@ void SerialLink::run()
     }
 }
 
+void SerialLink::WriteDataStatsBuffer(quint64 *bytesBuffer, qint64 *timeBuffer, int *writeIndex, quint64 bytes, qint64 time)
+{
+    int i = *writeIndex;
+
+    // Now write into the buffer, if there's no room, we just overwrite the first data point.
+    bytesBuffer[i] = bytes;
+    timeBuffer[i] = time;
+
+    // Increment and wrap the write index
+    ++i;
+    if (i == buffer_size)
+    {
+        i = 0;
+    }
+    *writeIndex = i;
+}
+
 void SerialLink::writeBytes(const char* data, qint64 size)
 {
     if(m_port && m_port->isOpen()) {
 
         QByteArray byteArray(data, size);
-        {
-            QMutexLocker writeLocker(&m_writeMutex);
-            m_transmitBuffer.append(byteArray);
-        }
-
-        // Extra debug logging
-//            qDebug() << byteArray->toHex();
+        m_writeMutex.lock();
+        m_transmitBuffer.append(byteArray);
+        m_writeMutex.unlock();
     } else {
         disconnect();
         // Error occured
@@ -295,10 +333,10 @@ void SerialLink::writeBytes(const char* data, qint64 size)
  **/
 void SerialLink::readBytes()
 {
-    m_dataMutex.lock();
     if(m_port && m_port->isOpen()) {
         const qint64 maxLength = 2048;
         char data[maxLength];
+        m_dataMutex.lock();
         qint64 numBytes = m_port->bytesAvailable();
 
         if(numBytes > 0) {
@@ -309,8 +347,8 @@ void SerialLink::readBytes()
             QByteArray b(data, numBytes);
             emit bytesReceived(this, b);
         }
+        m_dataMutex.unlock();
     }
-    m_dataMutex.unlock();
 }
 
 
@@ -473,7 +511,7 @@ QString SerialLink::getName() const
   * This function maps baud rate constants to numerical equivalents.
   * It relies on the mapping given in qportsettings.h from the QSerialPort library.
   */
-qint64 SerialLink::getNominalDataRate() const
+qint64 SerialLink::getConnectionSpeed() const
 {
     int baudRate;
     if (m_port) {
@@ -517,6 +555,94 @@ qint64 SerialLink::getNominalDataRate() const
     return dataRate;
 }
 
+qint64 SerialLink::getCurrentOutDataRate() const
+{
+    const qint64 now = QDateTime::currentMSecsSinceEpoch();
+
+    // Limit the time we calculate to the recent past
+    const qint64 cutoff = now - stats_timespan;
+
+    // Grab the mutex for working with the stats variables
+    QMutexLocker statsLocker(&m_statisticsMutex);
+
+    // Now iterate through the buffer of all received data packets adding up all values
+    // within now and our cutof.
+    int index = outDataIndex;
+    qint64 totalBytes = 0;
+    qint64 totalTime = 0;
+    qint64 lastTime = 0;
+    int size = buffer_size;
+    while (size-- > 0)
+    {
+        // If this data is within our cutoff time, include it in our calculations.
+        // This also accounts for when the buffer is empty and filled with 0-times.
+        if (outDataWriteTimes[index] > cutoff && lastTime > 0) {
+            // Track the total time, using the previous time as our timeperiod.
+            totalTime += outDataWriteTimes[index] - lastTime;
+            totalBytes += outDataWriteAmounts[index];
+        }
+
+        // Track the last time sample for doing timespan calculations
+        lastTime = outDataWriteTimes[index];
+
+        // Increment and wrap the index if necessary.
+        if (++index == buffer_size)
+        {
+            index = 0;
+        }
+    }
+
+    // Return the final calculated value in bits / s, converted from bytes/ms.
+    qint64 dataRate = (totalTime != 0)?(qint64)((float)totalBytes * 8.0f / ((float)totalTime / 1000.0f)):0;
+
+    // Finally return our calculated data rate.
+    return dataRate;
+}
+
+qint64 SerialLink::getCurrentInDataRate() const
+{
+    const qint64 now = QDateTime::currentMSecsSinceEpoch();
+
+    // Limit the time we calculate to the recent past
+    const qint64 cutoff = now - stats_timespan;
+
+    // Grab the mutex for working with the stats variables
+    QMutexLocker statsLocker(&m_statisticsMutex);
+
+    // Now iterate through the buffer of all received data packets adding up all values
+    // within now and our cutof.
+    int index = inDataIndex;
+    qint64 totalBytes = 0;
+    qint64 totalTime = 0;
+    qint64 lastTime = 0;
+    int size = buffer_size;
+    while (size-- > 0)
+    {
+        // If this data is within our cutoff time, include it in our calculations.
+        // This also accounts for when the buffer is empty and filled with 0-times.
+        if (inDataWriteTimes[index] > cutoff && lastTime > 0) {
+            // Track the total time, using the previous time as our timeperiod.
+            totalTime += inDataWriteTimes[index] - lastTime;
+            totalBytes += inDataWriteAmounts[index];
+        }
+
+        // Track the last time sample for doing timespan calculations
+        lastTime = inDataWriteTimes[index];
+
+        // Increment and wrap the index if necessary.
+        if (++index == buffer_size)
+        {
+            index = 0;
+        }
+    }
+
+    // Return the final calculated value in bits / s, converted from bytes/ms.
+    qint64 dataRate = (totalTime != 0)?(qint64)((float)totalBytes * 8.0f / ((float)totalTime / 1000.0f)):0;
+
+    // Finally return our calculated data rate.
+    return dataRate;
+}
+
 QString SerialLink::getPortName() const
 {
     return m_portName;
@@ -526,7 +652,7 @@ QString SerialLink::getPortName() const
 
 int SerialLink::getBaudRate() const
 {
-    return getNominalDataRate();
+    return getConnectionSpeed();
 }
 
 int SerialLink::getBaudRateType() const