MAVLinkProtocol.cc 18.7 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);
292
            if (_message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309
                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."));
                // Ensure the warning can't get stuck
                _radio_version_mismatch_count++;
                // 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;
            }

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

315 316 317
            // 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
318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339
            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
340 341 342 343 344 345 346 347 348 349 350 351
        }
    }
}

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

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

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

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

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

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

Don Gagne's avatar
Don Gagne committed
385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403
/// @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)
{
404
    //-- Are we supposed to write logs?
405 406 407
    if (qgcApp()->runningUnitTests()) {
        return;
    }
408 409 410 411 412 413 414 415
#ifdef __mobile__
    //-- Mobile build don't write to /tmp unless told to do so
    if (!_app->toolbox()->settingsManager()->appSettings()->telemetrySave()->rawValue().toBool()) {
        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
416 417 418 419 420 421 422 423 424 425
    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;
            }

426
            qDebug() << "Temp log" << _tempLogFile.fileName();
427
            emit checkTelemetrySavePath();
428

Don Gagne's avatar
Don Gagne committed
429
            _logSuspendError = false;
Don Gagne's avatar
Don Gagne committed
430 431 432 433 434 435
        }
    }
}

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

/// @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.
452
void MAVLinkProtocol::checkForLostLogFiles(void)
Don Gagne's avatar
Don Gagne committed
453 454
{
    QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
Gus Grubba's avatar
Gus Grubba committed
455

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

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

void MAVLinkProtocol::suspendLogForReplay(bool suspend)
{
    _logSuspendReplay = suspend;
}
475 476 477 478

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

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

483
    for(const QFileInfo fileInfo: fileInfoList) {
484 485 486
        QFile::remove(fileInfo.filePath());
    }
}
487