MAVLinkProtocol.cc

Go to the documentation of this file.

#include "MAVLinkProtocol.h"
 
#include <QtCore/QApplicationStatic>
#include <QtCore/QDir>
#include <QtCore/QFile>
#include <QtCore/QFileInfo>
#include <QtCore/QMetaType>
#include <QtCore/QSettings>
#include <QtCore/QStandardPaths>
#include <QtCore/QTimer>
#include <cstring>
 
#include "AppMessages.h"
#include "AppSettings.h"
#include "LinkManager.h"
#include "MAVLinkLib.h"
#include "LinkInterface.h"
#include "MAVLinkSigning.h"
#include "SigningController.h"
#include "MavlinkSettings.h"
#include "MultiVehicleManager.h"
#include "QGCFileHelper.h"
#include "QGCLoggingCategory.h"
#include "QmlObjectListModel.h"
#include "SettingsManager.h"
 
QGC_LOGGING_CATEGORY(MAVLinkProtocolLog, "Comms.MAVLinkProtocol")
 
Q_APPLICATION_STATIC(MAVLinkProtocol, _mavlinkProtocolInstance);
 
MAVLinkProtocol::MAVLinkProtocol(QObject* parent) : QObject(parent), _tempLogFile(new QFile(this))
{
    qCDebug(MAVLinkProtocolLog) << this;
}
 
MAVLinkProtocol::~MAVLinkProtocol()
{
    _closeLogFile();
 
    qCDebug(MAVLinkProtocolLog) << this;
}
 
MAVLinkProtocol* MAVLinkProtocol::instance()
{
    return _mavlinkProtocolInstance();
}
 
void MAVLinkProtocol::init()
{
    if (_initialized) {
        return;
    }
 
    (void)connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this,
                  &MAVLinkProtocol::_vehicleCountChanged);
 
    _initialized = true;
}
 
void MAVLinkProtocol::resetMetadataForLink(LinkInterface* link)
{
    const uint8_t channel = link->mavlinkChannel();
    _totalReceiveCounter[channel] = 0;
    _totalLossCounter[channel] = 0;
    _runningLossPercent[channel] = 0.f;
 
    link->setDecodedFirstMavlinkPacket(false);
}
 
void MAVLinkProtocol::resetSequenceTracking(LinkInterface* link)
{
    // Clear per-(sysid,compid) sequence state so next packet isn't counted as a gap.
    const uint8_t channel = link->mavlinkChannel();
    _firstMessageSeen[channel].clear();
    std::memset(_lastIndex[channel], 0, sizeof(_lastIndex[channel]));
}
 
void MAVLinkProtocol::logSentBytes(const LinkInterface* link, const QByteArray& data)
{
    Q_UNUSED(link);
 
    if (_logSuspendError || _logSuspendReplay || !_tempLogFile->isOpen()) {
        return;
    }
 
    const quint64 time = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
    uint8_t bytes_time[sizeof(quint64)]{};
    qToBigEndian(time, bytes_time);
 
    QByteArray logData = data;
    QByteArray timeData = QByteArray::fromRawData(reinterpret_cast<const char*>(bytes_time), sizeof(bytes_time));
    (void)logData.prepend(timeData);
    if (_tempLogFile->write(logData) != logData.length()) {
        const QString message = QStringLiteral("MAVLink Logging failed. Could not write to file %1, logging disabled.")
                                    .arg(_tempLogFile->fileName());
        QGC::showAppMessage(message, getName());
        _stopLogging();
        _logSuspendError = true;
    }
}
 
void MAVLinkProtocol::receiveBytes(LinkInterface* link, const QByteArray& data)
{
    const SharedLinkInterfacePtr linkPtr = LinkManager::instance()->sharedLinkInterfacePointerForLink(link);
    if (!linkPtr) {
        qCDebug(MAVLinkProtocolLog) << "receiveBytes: link gone!" << data.size() << "bytes arrived too late";
        return;
    }
 
    for (uint8_t byte : data) {
        const uint8_t mavlinkChannel = link->mavlinkChannel();
        mavlink_message_t message{};
        mavlink_status_t status{};
 
        const uint8_t framing = mavlink_parse_char(mavlinkChannel, byte, &message, &status);
        if (framing == MAVLINK_FRAMING_OK || framing == MAVLINK_FRAMING_BAD_SIGNATURE) {
            if (SigningController* const sigCtrl = link->signing()) {
                // Auto-detected key: reset sequence tracking so the key-install gap isn't counted as loss.
                if (sigCtrl->processFrame(framing == MAVLINK_FRAMING_OK, message)) {
                    resetSequenceTracking(link);
                }
            }
        }
        if (framing != MAVLINK_FRAMING_OK) {
            continue;
        }
 
        // v1/v2 share per-(sysid,compid) sequence counters; counting v1 makes every v2 appear lost. Skip v1 non-heartbeats.
        const bool isV1 = (status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1);
        if (isV1 && message.msgid != MAVLINK_MSG_ID_HEARTBEAT) {
            link->reportMavlinkV1Traffic();
            continue;
        }
 
        if (!isV1) {
            _updateCounters(mavlinkChannel, message);
        }
        if (!linkPtr->linkConfiguration()->isForwarding()) {
            _forward(message);
            _forwardSupport(message);
        }
        _logData(link, message);
 
        if (!_updateStatus(link, linkPtr, mavlinkChannel, message)) {
            break;
        }
    }
}
 
void MAVLinkProtocol::_updateCounters(uint8_t mavlinkChannel, const mavlink_message_t& message)
{
    _totalReceiveCounter[mavlinkChannel]++;
 
    uint8_t& lastSeq = _lastIndex[mavlinkChannel][message.sysid][message.compid];
 
    const QPair<uint8_t, uint8_t> key(message.sysid, message.compid);
    uint8_t expectedSeq;
    if (!_firstMessageSeen[mavlinkChannel].contains(key)) {
        _firstMessageSeen[mavlinkChannel].insert(key);
        expectedSeq = message.seq;
    } else if (message.seq == lastSeq) {
        // v1/v2 of the same message share sequence numbers — duplicate seq isn't loss.
        return;
    } else {
        expectedSeq = lastSeq + 1;
    }
 
    uint64_t lostMessages;
    if (message.seq >= expectedSeq) {
        lostMessages = message.seq - expectedSeq;
    } else {
        lostMessages = static_cast<uint64_t>(message.seq) + 256ULL - expectedSeq;
    }
    _totalLossCounter[mavlinkChannel] += lostMessages;
 
    lastSeq = message.seq;
 
    const uint64_t totalSent = _totalReceiveCounter[mavlinkChannel] + _totalLossCounter[mavlinkChannel];
    const float currentLossPercent = (static_cast<double>(_totalLossCounter[mavlinkChannel]) / totalSent) * 100.0f;
    _runningLossPercent[mavlinkChannel] = (currentLossPercent + _runningLossPercent[mavlinkChannel]) * 0.5f;
}
 
void MAVLinkProtocol::_forward(const mavlink_message_t& message)
{
    if (message.msgid == MAVLINK_MSG_ID_SETUP_SIGNING) {
        return;
    }
 
    if (!SettingsManager::instance()->mavlinkSettings()->forwardMavlink()->rawValue().toBool()) {
        return;
    }
 
    SharedLinkInterfacePtr forwardingLink = LinkManager::instance()->mavlinkForwardingLink();
    if (!forwardingLink) {
        return;
    }
 
    // Strip signature on forward: foreign key would BAD_SIGNATURE on downstream signing-aware parsers.
    const QByteArray bytes = MAVLinkSigning::serializeUnsignedCopy(message);
    (void)forwardingLink->writeBytesThreadSafe(bytes.constData(), bytes.size());
}
 
void MAVLinkProtocol::_forwardSupport(const mavlink_message_t& message)
{
    if (message.msgid == MAVLINK_MSG_ID_SETUP_SIGNING) {
        return;
    }
 
    if (!LinkManager::instance()->mavlinkSupportForwardingEnabled()) {
        return;
    }
 
    SharedLinkInterfacePtr forwardingSupportLink = LinkManager::instance()->mavlinkForwardingSupportLink();
    if (!forwardingSupportLink) {
        return;
    }
 
    const QByteArray bytes = MAVLinkSigning::serializeUnsignedCopy(message);
    (void)forwardingSupportLink->writeBytesThreadSafe(bytes.constData(), bytes.size());
}
 
void MAVLinkProtocol::_logData(LinkInterface* link, const mavlink_message_t& message)
{
    if (!_logSuspendError && !_logSuspendReplay && _tempLogFile->isOpen()) {
        // MAVLink spec §Logging: omit SETUP_SIGNING (contains secret key)
        if (message.msgid != MAVLINK_MSG_ID_SETUP_SIGNING) {
            // MAVLink spec §Logging: strip signature block from logged packets.
            const QByteArray msgBytes = MAVLinkSigning::serializeUnsignedCopy(message);
            const quint64 timestamp = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
            QByteArray log_data;
            log_data.resize(static_cast<qsizetype>(sizeof(timestamp)) + msgBytes.size());
            qToBigEndian(timestamp, reinterpret_cast<uint8_t*>(log_data.data()));
            std::memcpy(log_data.data() + sizeof(timestamp), msgBytes.constData(), msgBytes.size());
            if (_tempLogFile->write(log_data) != log_data.size()) {
                const QString logErrorMessage =
                    QStringLiteral("MAVLink Logging failed. Could not write to file %1, logging disabled.")
                        .arg(_tempLogFile->fileName());
                QGC::showAppMessage(logErrorMessage, getName());
                _stopLogging();
                _logSuspendError = true;
            }
        }
 
        if ((message.msgid == MAVLINK_MSG_ID_HEARTBEAT) && !_vehicleWasArmed) {
            if (mavlink_msg_heartbeat_get_base_mode(&message) & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
                _vehicleWasArmed = true;
            }
        }
    }
 
    switch (message.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT: {
            _startLogging();
            mavlink_heartbeat_t heartbeat{};
            mavlink_msg_heartbeat_decode(&message, &heartbeat);
            emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.autopilot, heartbeat.type);
            break;
        }
        case MAVLINK_MSG_ID_HIGH_LATENCY: {
            _startLogging();
            mavlink_high_latency_t highLatency{};
            mavlink_msg_high_latency_decode(&message, &highLatency);
            // HIGH_LATENCY does not provide autopilot or type information, generic is our safest bet
            emit vehicleHeartbeatInfo(link, message.sysid, message.compid, MAV_AUTOPILOT_GENERIC, MAV_TYPE_GENERIC);
            break;
        }
        case MAVLINK_MSG_ID_HIGH_LATENCY2: {
            _startLogging();
            mavlink_high_latency2_t highLatency2{};
            mavlink_msg_high_latency2_decode(&message, &highLatency2);
            emit vehicleHeartbeatInfo(link, message.sysid, message.compid, highLatency2.autopilot, highLatency2.type);
            break;
        }
        default:
            break;
    }
}
 
bool MAVLinkProtocol::_updateStatus(LinkInterface* link, const SharedLinkInterfacePtr linkPtr, uint8_t mavlinkChannel,
                                    const mavlink_message_t& message)
{
    if ((_totalReceiveCounter[mavlinkChannel] % 31) == 0) {
        const uint64_t totalSent = _totalReceiveCounter[mavlinkChannel] + _totalLossCounter[mavlinkChannel];
        emit mavlinkMessageStatus(message.sysid, totalSent, _totalReceiveCounter[mavlinkChannel],
                                  _totalLossCounter[mavlinkChannel], _runningLossPercent[mavlinkChannel]);
    }
 
    emit messageReceived(link, message);
 
    if (linkPtr.use_count() == 1) {
        return false;
    }
 
    return true;
}
 
bool MAVLinkProtocol::_closeLogFile()
{
    if (!_tempLogFile->isOpen()) {
        return false;
    }
 
    if (_tempLogFile->size() == 0) {
        (void)_tempLogFile->remove();
        return false;
    }
 
    (void)_tempLogFile->flush();
    _tempLogFile->close();
    return true;
}
 
void MAVLinkProtocol::_startLogging()
{
    if (QGC::runningUnitTests()) {
        return;
    }
 
    AppSettings* const appSettings = SettingsManager::instance()->appSettings();
    if (appSettings->disableAllPersistence()->rawValue().toBool()) {
        return;
    }
 
#if defined(Q_OS_ANDROID) || defined(Q_OS_IOS)
    if (!SettingsManager::instance()->mavlinkSettings()->telemetrySave()->rawValue().toBool()) {
        return;
    }
#endif
 
    if (_tempLogFile->isOpen()) {
        return;
    }
 
    if (_logSuspendReplay) {
        return;
    }
 
    // Generate unique temp file path for this logging session
    const QString logPath =
        QGCFileHelper::uniqueTempPath(QStringLiteral("%1.%2").arg(_tempLogFileTemplate, _logFileExtension));
    if (logPath.isEmpty()) {
        qCWarning(MAVLinkProtocolLog) << "Failed to generate temp log path";
        _logSuspendError = true;
        return;
    }
 
    _tempLogFile->setFileName(logPath);
    if (!_tempLogFile->open(QIODevice::WriteOnly)) {
        const QString message = QStringLiteral(
                                    "Opening Flight Data file for writing failed. "
                                    "Unable to write to %1. Please choose a different file location.")
                                    .arg(_tempLogFile->fileName());
        QGC::showAppMessage(message, getName());
        _closeLogFile();
        _logSuspendError = true;
        return;
    }
 
    qCDebug(MAVLinkProtocolLog) << "Temp log" << _tempLogFile->fileName();
    (void)_checkTelemetrySavePath();
 
    _logSuspendError = false;
}
 
void MAVLinkProtocol::_stopLogging()
{
    if (_tempLogFile->isOpen() && _closeLogFile()) {
        auto appSettings = SettingsManager::instance()->appSettings();
        auto mavlinkSettings = SettingsManager::instance()->mavlinkSettings();
        if ((_vehicleWasArmed || mavlinkSettings->telemetrySaveNotArmed()->rawValue().toBool()) &&
            mavlinkSettings->telemetrySave()->rawValue().toBool() &&
            !appSettings->disableAllPersistence()->rawValue().toBool()) {
            _saveTelemetryLog(_tempLogFile->fileName());
        } else {
            (void)QFile::remove(_tempLogFile->fileName());
        }
    }
 
    _vehicleWasArmed = false;
}
 
void MAVLinkProtocol::checkForLostLogFiles()
{
    static const QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
    static const QString filter(QStringLiteral("*.%1").arg(_logFileExtension));
    static const QStringList filterList(filter);
 
    const QFileInfoList fileInfoList = tempDir.entryInfoList(filterList, QDir::Files);
    qCDebug(MAVLinkProtocolLog) << "Orphaned log file count" << fileInfoList.count();
 
    for (const QFileInfo& fileInfo : fileInfoList) {
        qCDebug(MAVLinkProtocolLog) << "Orphaned log file" << fileInfo.filePath();
        if (fileInfo.size() == 0) {
            (void)QFile::remove(fileInfo.filePath());
            continue;
        }
        _saveTelemetryLog(fileInfo.filePath());
    }
}
 
void MAVLinkProtocol::deleteTempLogFiles()
{
    static const QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
    static const QString filter(QStringLiteral("*.%1").arg(_logFileExtension));
 
    const QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
    qCDebug(MAVLinkProtocolLog) << "Temp log file count" << fileInfoList.count();
 
    for (const QFileInfo& fileInfo : fileInfoList) {
        qCDebug(MAVLinkProtocolLog) << "Temp log file" << fileInfo.filePath();
        (void)QFile::remove(fileInfo.filePath());
    }
}
 
void MAVLinkProtocol::_saveTelemetryLog(const QString& tempLogfile)
{
    if (_checkTelemetrySavePath()) {
        const QString saveDirPath = SettingsManager::instance()->appSettings()->telemetrySavePath();
        const QDir saveDir(saveDirPath);
 
        const QString nameFormat("%1%2.%3");
        const QString dtFormat("yyyy-MM-dd hh-mm-ss");
 
        int tryIndex = 1;
        QString saveFileName = nameFormat.arg(QDateTime::currentDateTime().toString(dtFormat), QString(),
                                              AppSettings::telemetryFileExtension);
        while (saveDir.exists(saveFileName)) {
            saveFileName = nameFormat.arg(QDateTime::currentDateTime().toString(dtFormat),
                                          QStringLiteral(".%1").arg(tryIndex++), AppSettings::telemetryFileExtension);
        }
 
        const QString saveFilePath = saveDir.absoluteFilePath(saveFileName);
 
        QFile in(tempLogfile);
        if (!in.open(QIODevice::ReadOnly)) {
            const QString error =
                tr("Unable to save telemetry log. Error opening source '%1': '%2'.").arg(tempLogfile, in.errorString());
            QGC::showAppMessage(error);
            (void)QFile::remove(tempLogfile);
            return;
        }
 
        QSaveFile out(saveFilePath);
        out.setDirectWriteFallback(true);  // allows non-atomic fallback where rename isn’t possible
 
        if (!out.open(QIODevice::WriteOnly)) {
            const QString error = tr("Unable to save telemetry log. Error opening destination '%1': '%2'.")
                                      .arg(saveFilePath, out.errorString());
            QGC::showAppMessage(error);
            (void)QFile::remove(tempLogfile);
            return;
        }
 
        // Stream copy to avoid large allocations.
        QByteArray buffer;
        constexpr int bufferSize = 256 * 1024;  // 256 KiB
        buffer.resize(bufferSize);
        while (true) {
            const qint64 n = in.read(buffer.data(), buffer.size());
            if (n == 0) {
                break;
            }
            if (n < 0) {
                const QString error = tr("Unable to save telemetry log. Error reading source '%1': '%2'.")
                                          .arg(tempLogfile, in.errorString());
                QGC::showAppMessage(error);
                out.cancelWriting();
                (void)QFile::remove(tempLogfile);
                return;
            }
            if (out.write(buffer.constData(), n) != n) {
                const QString error = tr("Unable to save telemetry log. Error writing destination '%1': '%2'.")
                                          .arg(saveFilePath, out.errorString());
                QGC::showAppMessage(error);
                out.cancelWriting();
                (void)QFile::remove(tempLogfile);
                return;
            }
        }
 
        if (!out.commit()) {
            const QString error =
                tr("Unable to finalize telemetry log '%1': '%2'.").arg(saveFilePath, out.errorString());
            QGC::showAppMessage(error);
            (void)QFile::remove(tempLogfile);
            return;
        }
 
        constexpr QFileDevice::Permissions perms =
            QFileDevice::ReadOwner | QFileDevice::WriteOwner | QFileDevice::ReadGroup | QFileDevice::ReadOther;
        (void)out.setPermissions(perms);
    }
 
    (void)QFile::remove(tempLogfile);
}
 
bool MAVLinkProtocol::_checkTelemetrySavePath()
{
    const QString saveDirPath = SettingsManager::instance()->appSettings()->telemetrySavePath();
    if (saveDirPath.isEmpty()) {
        const QString error = tr("Unable to save telemetry log. Application save directory is not set.");
        QGC::showAppMessage(error);
        return false;
    }
 
    const QDir saveDir(saveDirPath);
    if (!saveDir.exists()) {
        const QString error =
            tr("Unable to save telemetry log. Telemetry save directory \"%1\" does not exist.").arg(saveDirPath);
        QGC::showAppMessage(error);
        return false;
    }
 
    return true;
}
 
void MAVLinkProtocol::_vehicleCountChanged()
{
    if (MultiVehicleManager::instance()->vehicles()->count() == 0) {
        _stopLogging();
    }
}
 
int MAVLinkProtocol::getSystemId() const
{
    return SettingsManager::instance()->mavlinkSettings()->gcsMavlinkSystemID()->rawValue().toInt();
}