MAVLinkProtocol.cc 19 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

pixhawk's avatar
pixhawk committed
10 11 12

/**
 * @file
pixhawk's avatar
pixhawk committed
13 14
 *   @brief Implementation of class MAVLinkProtocol
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
15 16 17 18 19 20 21
 */

#include <inttypes.h>
#include <iostream>

#include <QDebug>
#include <QTime>
lm's avatar
lm committed
22
#include <QApplication>
23
#include <QSettings>
24
#include <QStandardPaths>
25
#include <QtEndian>
26
#include <QMetaType>
27 28
#include <QDir>
#include <QFileInfo>
pixhawk's avatar
pixhawk committed
29 30 31 32 33 34

#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASInterface.h"
#include "UAS.h"
#include "LinkManager.h"
35
#include "QGCMAVLink.h"
36
#include "QGC.h"
Don Gagne's avatar
Don Gagne committed
37
#include "QGCApplication.h"
38
#include "QGCLoggingCategory.h"
39
#include "MultiVehicleManager.h"
40
#include "SettingsManager.h"
pixhawk's avatar
pixhawk committed
41

42
Q_DECLARE_METATYPE(mavlink_message_t)
43

44
QGC_LOGGING_CATEGORY(MAVLinkProtocolLog, "MAVLinkProtocolLog")
45

Don Gagne's avatar
Don Gagne committed
46 47 48
const char* MAVLinkProtocol::_tempLogFileTemplate = "FlightDataXXXXXX"; ///< Template for temporary log file
const char* MAVLinkProtocol::_logFileExtension = "mavlink";             ///< Extension for log files

pixhawk's avatar
pixhawk committed
49 50 51 52
/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
53 54
MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox)
    : QGCTool(app, toolbox)
55
    , m_enable_version_check(true)
56 57
    , _message({})
    , _status({})
58
    , versionMismatchIgnore(false)
59
    , systemId(255)
60
    , _current_version(100)
61
    , _radio_version_mismatch_count(0)
62 63
    , _logSuspendError(false)
    , _logSuspendReplay(false)
64
    , _vehicleWasArmed(false)
65
    , _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension))
66 67
    , _linkMgr(nullptr)
    , _multiVehicleManager(nullptr)
pixhawk's avatar
pixhawk committed
68
{
69 70 71 72
    memset(totalReceiveCounter, 0, sizeof(totalReceiveCounter));
    memset(totalLossCounter,    0, sizeof(totalLossCounter));
    memset(runningLossPercent,  0, sizeof(runningLossPercent));
    memset(firstMessage,        1, sizeof(firstMessage));
73 74
    memset(&_status,            0, sizeof(_status));
    memset(&_message,           0, sizeof(_message));
pixhawk's avatar
pixhawk committed
75 76
}

77 78 79 80 81 82
MAVLinkProtocol::~MAVLinkProtocol()
{
    storeSettings();
    _closeLogFile();
}

83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100
void MAVLinkProtocol::setVersion(unsigned version)
{
    QList<LinkInterface*> links = _linkMgr->links();

    for (int i = 0; i < links.length(); i++) {
        mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(links[i]->mavlinkChannel());

        // Set flags for version
        if (version < 200) {
            mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
        } else {
            mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
        }
    }

    _current_version = version;
}

101 102 103 104 105 106 107 108 109 110 111 112 113 114
void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox)
{
   QGCTool::setToolbox(toolbox);

   _linkMgr =               _toolbox->linkManager();
   _multiVehicleManager =   _toolbox->multiVehicleManager();

   qRegisterMetaType<mavlink_message_t>("mavlink_message_t");

   loadSettings();

   // All the *Counter variables are not initialized here, as they should be initialized
   // on a per-link basis before those links are used. @see resetMetadataForLink().

115 116 117
   connect(this, &MAVLinkProtocol::protocolStatusMessage,   _app, &QGCApplication::criticalMessageBoxOnMainThread);
   connect(this, &MAVLinkProtocol::saveTelemetryLog,        _app, &QGCApplication::saveTelemetryLogOnMainThread);
   connect(this, &MAVLinkProtocol::checkTelemetrySavePath,  _app, &QGCApplication::checkTelemetrySavePathOnMainThread);
118

119 120
   connect(_multiVehicleManager, &MultiVehicleManager::vehicleAdded, this, &MAVLinkProtocol::_vehicleCountChanged);
   connect(_multiVehicleManager, &MultiVehicleManager::vehicleRemoved, this, &MAVLinkProtocol::_vehicleCountChanged);
121

122 123 124
   emit versionCheckChanged(m_enable_version_check);
}

125 126 127 128 129
void MAVLinkProtocol::loadSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
130
    enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool());
131 132 133

    // Only set system id if it was valid
    int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt();
134 135
    if (temp > 0 && temp < 256)
    {
136 137 138 139 140 141 142 143 144
        systemId = temp;
    }
}

void MAVLinkProtocol::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
145
    settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
146
    settings.setValue("GCS_SYSTEM_ID", systemId);
147
    // Parameter interface settings
148 149
}

150
void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
151
{
152
    int channel = link->mavlinkChannel();
153
    totalReceiveCounter[channel] = 0;
154 155 156 157 158
    totalLossCounter[channel]    = 0;
    runningLossPercent[channel]  = 0.0f;
    for(int i = 0; i < 256; i++) {
        firstMessage[channel][i] =  1;
    }
159
    link->setDecodedFirstMavlinkPacket(false);
160 161
}

pixhawk's avatar
pixhawk committed
162 163 164 165 166 167 168
/**
 * This method parses all incoming bytes and constructs a MAVLink packet.
 * It can handle multiple links in parallel, as each link has it's own buffer/
 * parsing state machine.
 * @param link The interface to read from
 * @see LinkInterface
 **/
169

170
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
171
{
Don Gagne's avatar
Don Gagne committed
172 173 174
    // Since receiveBytes signals cross threads we can end up with signals in the queue
    // that come through after the link is disconnected. For these we just drop the data
    // since the link is closed.
175
    if (!_linkMgr->containsLink(link)) {
Don Gagne's avatar
Don Gagne committed
176 177
        return;
    }
Gus Grubba's avatar
Gus Grubba committed
178

179
    uint8_t mavlinkChannel = link->mavlinkChannel();
180

181
    static int  nonmavlinkCount = 0;
182
    static bool checkedUserNonMavlink = false;
183
    static bool warnedUserNonMavlink  = false;
184

185
    for (int position = 0; position < b.size(); position++) {
186 187
        if (mavlink_parse_char(mavlinkChannel, static_cast<uint8_t>(b[position]), &_message, &_status)) {
            // Got a valid message
188
            if (!link->decodedFirstMavlinkPacket()) {
189
                link->setDecodedFirstMavlinkPacket(true);
190 191
                mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
                if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
192
                    qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
193
                    mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
194 195
                    // Set all links to v2
                    setVersion(200);
196
                }
Don Gagne's avatar
Don Gagne committed
197 198
            }

199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230
            //-----------------------------------------------------------------
            // MAVLink Status
            uint8_t lastSeq = lastIndex[_message.sysid][_message.compid];
            uint8_t expectedSeq = lastSeq + 1;
            // Increase receive counter
            totalReceiveCounter[mavlinkChannel]++;
            // Determine what the next expected sequence number is, accounting for
            // never having seen a message for this system/component pair.
            if(firstMessage[_message.sysid][_message.compid]) {
                firstMessage[_message.sysid][_message.compid] = 0;
                lastSeq     = _message.seq;
                expectedSeq = _message.seq;
            }
            // And if we didn't encounter that sequence number, record the error
            //int foo = 0;
            if (_message.seq != expectedSeq)
            {
                //foo = 1;
                int lostMessages = 0;
                //-- Account for overflow during packet loss
                if(_message.seq < expectedSeq) {
                    lostMessages = (_message.seq + 255) - expectedSeq;
                } else {
                    lostMessages = _message.seq - expectedSeq;
                }
                // Log how many were lost
                totalLossCounter[mavlinkChannel] += static_cast<uint64_t>(lostMessages);
            }

            // And update the last sequence number for this system/component pair
            lastIndex[_message.sysid][_message.compid] = _message.seq;;
            // Calculate new loss ratio
231 232
            uint64_t totalSent = totalReceiveCounter[mavlinkChannel] + totalLossCounter[mavlinkChannel];
            float receiveLossPercent = static_cast<float>(static_cast<double>(totalLossCounter[mavlinkChannel]) / static_cast<double>(totalSent));
233 234 235 236 237 238 239
            receiveLossPercent *= 100.0f;
            receiveLossPercent = (receiveLossPercent * 0.5f) + (runningLossPercent[mavlinkChannel] * 0.5f);
            runningLossPercent[mavlinkChannel] = receiveLossPercent;

            //qDebug() << foo << _message.seq << expectedSeq << lastSeq << totalLossCounter[mavlinkChannel] << totalReceiveCounter[mavlinkChannel] << totalSentCounter[mavlinkChannel] << "(" << _message.sysid << _message.compid << ")";

            //-----------------------------------------------------------------
pixhawk's avatar
pixhawk committed
240
            // Log data
Don Gagne's avatar
Don Gagne committed
241
            if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
242 243 244 245 246
                uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];

                // Write the uint64 time in microseconds in big endian format before the message.
                // This timestamp is saved in UTC time. We are only saving in ms precision because
                // getting more than this isn't possible with Qt without a ton of extra code.
247
                quint64 time = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
248 249 250
                qToBigEndian(time, buf);

                // Then write the message to the buffer
251
                int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &_message);
252 253 254 255 256

                // Determine how many bytes were written by adding the timestamp size to the message size
                len += sizeof(quint64);

                // Now write this timestamp/message pair to the log.
257
                QByteArray b(reinterpret_cast<const char*>(buf), len);
Don Gagne's avatar
Don Gagne committed
258
                if(_tempLogFile.write(b) != len)
259
                {
260
                    // If there's an error logging data, raise an alert and stop logging.
261
                    emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
Don Gagne's avatar
Don Gagne committed
262 263
                    _stopLogging();
                    _logSuspendError = true;
264
                }
Gus Grubba's avatar
Gus Grubba committed
265

Don Gagne's avatar
Don Gagne committed
266
                // Check for the vehicle arming going by. This is used to trigger log save.
267
                if (!_vehicleWasArmed && _message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
Don Gagne's avatar
Don Gagne committed
268
                    mavlink_heartbeat_t state;
269
                    mavlink_msg_heartbeat_decode(&_message, &state);
Don Gagne's avatar
Don Gagne committed
270
                    if (state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
271
                        _vehicleWasArmed = true;
Don Gagne's avatar
Don Gagne committed
272 273
                    }
                }
pixhawk's avatar
pixhawk committed
274 275
            }

276
            if (_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
Don Gagne's avatar
Don Gagne committed
277
                _startLogging();
278
                mavlink_heartbeat_t heartbeat;
279 280
                mavlink_msg_heartbeat_decode(&_message, &heartbeat);
                emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, heartbeat.autopilot, heartbeat.type);
281 282
            }

283
            if (_message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) {
284 285
                _startLogging();
                mavlink_high_latency2_t highLatency2;
286 287
                mavlink_msg_high_latency2_decode(&_message, &highLatency2);
                emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, highLatency2.autopilot, highLatency2.type);
pixhawk's avatar
pixhawk committed
288
            }
289

290 291
            // Detect if we are talking to an old radio not supporting v2
            mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
Don Gagne's avatar
Don Gagne committed
292
            if (_message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _radio_version_mismatch_count != -1) {
293 294 295 296 297 298 299 300 301
                if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)
                && !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
                    _radio_version_mismatch_count++;
                }
            }

            if (_radio_version_mismatch_count == 5) {
                // Warn the user if the radio continues to send v1 while the link uses v2
                emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Detected radio still using MAVLink v1.0 on a link with MAVLink v2.0 enabled. Please upgrade the radio firmware."));
Don Gagne's avatar
Don Gagne committed
302 303
                // Set to flag warning already shown
                _radio_version_mismatch_count = -1;
304 305 306 307 308
                // Flick link back to v1
                qDebug() << "Switching outbound to mavlink 1.0 due to incoming mavlink 1.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
                mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
            }

309 310
            // Update MAVLink status on every 32th packet
            if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0) {
311
                emit mavlinkMessageStatus(_message.sysid, totalSent, totalReceiveCounter[mavlinkChannel], totalLossCounter[mavlinkChannel], receiveLossPercent);
312
            }
313

314 315 316
            // The packet is emitted as a whole, as it is only 255 - 261 bytes short
            // kind of inefficient, but no issue for a groundstation pc.
            // It buys as reentrancy for the whole code over all threads
317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338
            emit messageReceived(link, _message);
            // Reset message parsing
            memset(&_status,  0, sizeof(_status));
            memset(&_message, 0, sizeof(_message));
        } else if (!link->decodedFirstMavlinkPacket()) {
            // No formed message yet
            nonmavlinkCount++;
            if (nonmavlinkCount > 1000 && !warnedUserNonMavlink) {
                // 1000 bytes with no mavlink message. Are we connected to a mavlink capable device?
                if (!checkedUserNonMavlink) {
                    link->requestReset();
                    checkedUserNonMavlink = true;
                } else {
                    warnedUserNonMavlink = true;
                    // Disconnect the link since it's some other device and
                    // QGC clinging on to it and feeding it data might have unintended
                    // side effects (e.g. if its a modem)
                    qDebug() << "disconnected link" << link->getName() << "as it contained no MAVLink data";
                    QMetaObject::invokeMethod(_linkMgr, "disconnectLink", Q_ARG( LinkInterface*, link ) );
                    return;
                }
            }
pixhawk's avatar
pixhawk committed
339 340 341 342 343 344 345 346 347 348 349 350
        }
    }
}

/**
 * @return The name of this protocol
 **/
QString MAVLinkProtocol::getName()
{
    return QString(tr("MAVLink protocol"));
}

351
/** @return System id of this application */
352
int MAVLinkProtocol::getSystemId()
353
{
354 355 356 357 358 359
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
360 361 362
}

/** @return Component id of this application */
363
int MAVLinkProtocol::getComponentId()
364
{
365
    return 0;
366 367
}

lm's avatar
lm committed
368 369 370
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
371
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
372 373
}

374
void MAVLinkProtocol::_vehicleCountChanged(void)
Don Gagne's avatar
Don Gagne committed
375
{
376
    int count = _multiVehicleManager->vehicles()->count();
Don Gagne's avatar
Don Gagne committed
377 378 379
    if (count == 0) {
        // Last vehicle is gone, close out logging
        _stopLogging();
380
        _radio_version_mismatch_count = 0;
Don Gagne's avatar
Don Gagne committed
381 382 383
    }
}

Don Gagne's avatar
Don Gagne committed
384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402
/// @brief Closes the log file if it is open
bool MAVLinkProtocol::_closeLogFile(void)
{
    if (_tempLogFile.isOpen()) {
        if (_tempLogFile.size() == 0) {
            // Don't save zero byte files
            _tempLogFile.remove();
            return false;
        } else {
            _tempLogFile.flush();
            _tempLogFile.close();
            return true;
        }
    }
    return false;
}

void MAVLinkProtocol::_startLogging(void)
{
403
    //-- Are we supposed to write logs?
404 405 406
    if (qgcApp()->runningUnitTests()) {
        return;
    }
407
    AppSettings* appSettings = _app->toolbox()->settingsManager()->appSettings();
408
    if(appSettings->disableAllPersistence()->rawValue().toBool()) {
409 410
        return;
    }
411 412
#ifdef __mobile__
    //-- Mobile build don't write to /tmp unless told to do so
413
    if (!appSettings->telemetrySave()->rawValue().toBool()) {
414 415 416 417 418
        return;
    }
#endif
    //-- Log is always written to a temp file. If later the user decides they want
    //   it, it's all there for them.
Don Gagne's avatar
Don Gagne committed
419 420 421 422 423 424 425 426 427 428
    if (!_tempLogFile.isOpen()) {
        if (!_logSuspendReplay) {
            if (!_tempLogFile.open()) {
                emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Opening Flight Data file for writing failed. "
                                                                      "Unable to write to %1. Please choose a different file location.").arg(_tempLogFile.fileName()));
                _closeLogFile();
                _logSuspendError = true;
                return;
            }

429
            qDebug() << "Temp log" << _tempLogFile.fileName();
430
            emit checkTelemetrySavePath();
431

Don Gagne's avatar
Don Gagne committed
432
            _logSuspendError = false;
Don Gagne's avatar
Don Gagne committed
433 434 435 436 437 438
        }
    }
}

void MAVLinkProtocol::_stopLogging(void)
{
439 440 441
    if (_tempLogFile.isOpen()) {
        if (_closeLogFile()) {
            if ((_vehicleWasArmed || _app->toolbox()->settingsManager()->appSettings()->telemetrySaveNotArmed()->rawValue().toBool()) &&
442
                _app->toolbox()->settingsManager()->appSettings()->telemetrySave()->rawValue().toBool() &&
443
                !_app->toolbox()->settingsManager()->appSettings()->disableAllPersistence()->rawValue().toBool()) {
444 445 446 447
                emit saveTelemetryLog(_tempLogFile.fileName());
            } else {
                QFile::remove(_tempLogFile.fileName());
            }
448
        }
Don Gagne's avatar
Don Gagne committed
449
    }
450
    _vehicleWasArmed = false;
Don Gagne's avatar
Don Gagne committed
451 452 453 454 455
}

/// @brief Checks the temp directory for log files which may have been left there.
///         This could happen if QGC crashes without the temp log file being saved.
///         Give the user an option to save these orphaned files.
456
void MAVLinkProtocol::checkForLostLogFiles(void)
Don Gagne's avatar
Don Gagne committed
457 458
{
    QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
Gus Grubba's avatar
Gus Grubba committed
459

Don Gagne's avatar
Don Gagne committed
460 461
    QString filter(QString("*.%1").arg(_logFileExtension));
    QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
462
    //qDebug() << "Orphaned log file count" << fileInfoList.count();
Gus Grubba's avatar
Gus Grubba committed
463

464
    for(const QFileInfo fileInfo: fileInfoList) {
465
        //qDebug() << "Orphaned log file" << fileInfo.filePath();
Don Gagne's avatar
Don Gagne committed
466 467 468 469 470
        if (fileInfo.size() == 0) {
            // Delete all zero length files
            QFile::remove(fileInfo.filePath());
            continue;
        }
471
        emit saveTelemetryLog(fileInfo.filePath());
Don Gagne's avatar
Don Gagne committed
472 473 474 475 476 477 478
    }
}

void MAVLinkProtocol::suspendLogForReplay(bool suspend)
{
    _logSuspendReplay = suspend;
}
479 480 481 482

void MAVLinkProtocol::deleteTempLogFiles(void)
{
    QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
Gus Grubba's avatar
Gus Grubba committed
483

484 485
    QString filter(QString("*.%1").arg(_logFileExtension));
    QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
Gus Grubba's avatar
Gus Grubba committed
486

487
    for(const QFileInfo fileInfo: fileInfoList) {
488 489 490
        QFile::remove(fileInfo.filePath());
    }
}
491