LinkInterface.cc 8.07 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10 11 12
 *
 * 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"

13 14
bool LinkInterface::active() const
{
15
    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
acfloria's avatar
acfloria committed
16 17 18
    while (iter.hasNext()) {
        iter.next();
        if (iter.value()->getActive()) {
19 20 21 22 23 24 25
            return true;
        }
    }

    return false;
}

26
bool LinkInterface::link_active(int vehicle_id) const
27
{
28 29
    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
        return _mavlinkMessagesTimers.value(vehicle_id)->getActive();
acfloria's avatar
acfloria committed
30 31
    } else {
        return false;
32 33 34
    }
}

35 36 37 38 39 40 41 42 43 44
/// 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
45
LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4Flow)
46 47
    : QThread                   (0)
    , _config                   (config)
48
    , _highLatency              (config->isHighLatency())
49 50
    , _mavlinkChannelSet        (false)
    , _enableRateCollection     (false)
51
    , _decodedFirstMavlinkPacket(false)
52
    , _isPX4Flow                (isPX4Flow)
53
{
54 55
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);

56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
    _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)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
180 181 182
    if (_mavlinkChannelSet) {
        qWarning() << "Mavlink channel set multiple times";
    }
183 184 185
    _mavlinkChannelSet = true;
    _mavlinkChannel = channel;
}
186

187
void LinkInterface::_activeChanged(bool active, int vehicle_id)
188
{
189
    emit activeChanged(this, active, vehicle_id);
190
}
191

192 193 194
void LinkInterface::startMavlinkMessagesTimer(int vehicle_id) {
    if (_mavlinkMessagesTimers.contains(vehicle_id)) {
        _mavlinkMessagesTimers.value(vehicle_id)->restartTimer();
195
    } else {
196 197 198
        _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();
199 200 201
    }
}

202 203
void LinkInterface::stopMavlinkMessagesTimer() {
    QMapIterator<int /* vehicle id */, MavlinkMessagesTimer*> iter(_mavlinkMessagesTimers);
acfloria's avatar
acfloria committed
204 205
    while (iter.hasNext()) {
        iter.next();
206 207 208
        QObject::disconnect(iter.value(), &MavlinkMessagesTimer::activeChanged, this, &LinkInterface::_activeChanged);
        _mavlinkMessagesTimers[iter.key()]->deleteLater();
        _mavlinkMessagesTimers[iter.key()] = nullptr;
209
    }
210

211
    _mavlinkMessagesTimers.clear();
212
}