LinkInterface.cc

/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "LinkInterface.h"
#include "QGCApplication.h"

bool LinkInterface::active() const
{
    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
    while (iter.hasNext()) {
        iter.next();
        if (iter.value()->getActive()) {
            return true;
        }
    }

    return false;
}

bool LinkInterface::link_active(int vehicle_id) const
{
    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
        return _mavlinkMessagesTimers.value(vehicle_id)->getActive();
    } else {
        return false;
    }
}

/// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only
/// set into the link when it is added to LinkManager
uint8_t LinkInterface::mavlinkChannel(void) const
{
    if (!_mavlinkChannelSet) {
        qWarning() << "Call to LinkInterface::mavlinkChannel with _mavlinkChannelSet == false";
    }
    return _mavlinkChannel;
}
// Links are only created by LinkManager so constructor is not public
LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow)
    : QThread                   (0)
    , _config                   (config)
    , _highLatency              (config->isHighLatency())
    , _mavlinkChannelSet        (false)
    , _enableRateCollection     (false)
    , _decodedFirstMavlinkPacket(false)
    , _isPX4Flow                (isPX4Flow)
{
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);

    _config->setLink(this);

    // Initialize everything for the data rate calculation buffers.
    _inDataIndex  = 0;
    _outDataIndex = 0;

    // Initialize our data rate buffers.
    memset(_inDataWriteAmounts, 0, sizeof(_inDataWriteAmounts));
    memset(_inDataWriteTimes,   0, sizeof(_inDataWriteTimes));
    memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts));
    memset(_outDataWriteTimes,  0, sizeof(_outDataWriteTimes));

    QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes);
    qRegisterMetaType<LinkInterface*>("LinkInterface*");
}

/// This function logs the send times and amounts of datas for input. Data is used for calculating
/// the transmission rate.
///     @param byteCount Number of bytes received
///     @param time Time in ms send occurred
void LinkInterface::_logInputDataRate(quint64 byteCount, qint64 time) {
    if(_enableRateCollection)
        _logDataRateToBuffer(_inDataWriteAmounts, _inDataWriteTimes, &_inDataIndex, byteCount, time);
}

/// This function logs the send times and amounts of datas for output. Data is used for calculating
/// the transmission rate.
///     @param byteCount Number of bytes sent
///     @param time Time in ms receive occurred
void LinkInterface::_logOutputDataRate(quint64 byteCount, qint64 time) {
    if(_enableRateCollection)
        _logDataRateToBuffer(_outDataWriteAmounts, _outDataWriteTimes, &_outDataIndex, byteCount, time);
}

/**
     * @brief logDataRateToBuffer Stores transmission times/amounts for statistics
     *
     * This function logs the send times and amounts of datas to the given circular buffers.
     * This data is used for calculating the transmission rate.
     *
     * @param bytesBuffer[out] The buffer to write the bytes value into.
     * @param timeBuffer[out] The buffer to write the time value into
     * @param writeIndex[out] The write index used for this buffer.
     * @param bytes The amount of bytes transmit.
     * @param time The time (in ms) this transmission occurred.
     */
void LinkInterface::_logDataRateToBuffer(quint64 *bytesBuffer, qint64 *timeBuffer, int *writeIndex, quint64 bytes, qint64 time)
{
    QMutexLocker dataRateLocker(&_dataRateMutex);

    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 == _dataRateBufferSize)
    {
        i = 0;
    }
    *writeIndex = i;
}

/**
     * @brief getCurrentDataRate Get the current data rate given a data rate buffer.
     *
     * This function attempts to use the times and number of bytes transmit into a current data rate
     * estimation. Since it needs to use timestamps to get the timeperiods over when the data was sent,
     * this is effectively a global data rate over the last _dataRateBufferSize - 1 data points. Also note
     * that data points older than NOW - dataRateCurrentTimespan are ignored.
     *
     * @param index The first valid sample in the data rate buffer. Refers to the oldest time sample.
     * @param dataWriteTimes The time, in ms since epoch, that each data sample took place.
     * @param dataWriteAmounts The amount of data (in bits) that was transferred.
     * @return The bits per second of data transferrence of the interface over the last [-statsCurrentTimespan, 0] timespan.
     */
qint64 LinkInterface::_getCurrentDataRate(int index, const qint64 dataWriteTimes[], const quint64 dataWriteAmounts[]) const
{
    const qint64 now = QDateTime::currentMSecsSinceEpoch();

    // Limit the time we calculate to the recent past
    const qint64 cutoff = now - _dataRateCurrentTimespan;

    // Grab the mutex for working with the stats variables
    QMutexLocker dataRateLocker(&_dataRateMutex);

    // Now iterate through the buffer of all received data packets adding up all values
    // within now and our cutof.
    qint64 totalBytes = 0;
    qint64 totalTime = 0;
    qint64 lastTime = 0;
    int size = _dataRateBufferSize;
    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 (dataWriteTimes[index] > cutoff && lastTime > 0) {
            // Track the total time, using the previous time as our timeperiod.
            totalTime += dataWriteTimes[index] - lastTime;
            totalBytes += dataWriteAmounts[index];
        }

        // Track the last time sample for doing timespan calculations
        lastTime = dataWriteTimes[index];

        // Increment and wrap the index if necessary.
        if (++index == _dataRateBufferSize)
        {
            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;
}

/// Sets the mavlink channel to use for this link
void LinkInterface::_setMavlinkChannel(uint8_t channel)
{
    if (_mavlinkChannelSet) {
        qWarning() << "Mavlink channel set multiple times";
    }
    _mavlinkChannelSet = true;
    _mavlinkChannel = channel;
}

void LinkInterface::_activeChanged(bool active, int vehicle_id)
{
    emit activeChanged(this, active, vehicle_id);
}

void LinkInterface::startMavlinkMessagesTimer(int vehicle_id) {
    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
        _mavlinkMessagesTimers.value(vehicle_id)->restartTimer();
    } else {
        _mavlinkMessagesTimers.insert(vehicle_id, new MavlinkMessagesTimer(vehicle_id, _highLatency));
        QObject::connect(_mavlinkMessagesTimers.value(vehicle_id), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
        _mavlinkMessagesTimers.value(vehicle_id)->init();
    }
}

void LinkInterface::stopMavlinkMessagesTimer() {
    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
    while (iter.hasNext()) {
        iter.next();
        QObject::disconnect(iter.value(), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
        _mavlinkMessagesTimers[iter.key()]->deleteLater();
        _mavlinkMessagesTimers[iter.key()] = nullptr;
    }

    _mavlinkMessagesTimers.clear();
}