MAVLinkProtocol.cc 15.8 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 56
    , m_enable_version_check(true)
    , versionMismatchIgnore(false)
57
    , systemId(255)
58 59
    , _logSuspendError(false)
    , _logSuspendReplay(false)
60
    , _vehicleWasArmed(false)
61 62 63
    , _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension))
    , _linkMgr(NULL)
    , _multiVehicleManager(NULL)
pixhawk's avatar
pixhawk committed
64
{
65 66 67 68 69
    memset(&totalReceiveCounter, 0, sizeof(totalReceiveCounter));
    memset(&totalLossCounter, 0, sizeof(totalLossCounter));
    memset(&totalErrorCounter, 0, sizeof(totalErrorCounter));
    memset(&currReceiveCounter, 0, sizeof(currReceiveCounter));
    memset(&currLossCounter, 0, sizeof(currLossCounter));
pixhawk's avatar
pixhawk committed
70 71
}

72 73 74 75 76 77
MAVLinkProtocol::~MAVLinkProtocol()
{
    storeSettings();
    _closeLogFile();
}

78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100
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().

   // Initialize the list for tracking dropped messages to invalid.
   for (int i = 0; i < 256; i++)
   {
       for (int j = 0; j < 256; j++)
       {
           lastIndex[i][j] = -1;
       }
   }

101 102 103
   connect(this, &MAVLinkProtocol::protocolStatusMessage,   _app, &QGCApplication::criticalMessageBoxOnMainThread);
   connect(this, &MAVLinkProtocol::saveTelemetryLog,        _app, &QGCApplication::saveTelemetryLogOnMainThread);
   connect(this, &MAVLinkProtocol::checkTelemetrySavePath,  _app, &QGCApplication::checkTelemetrySavePathOnMainThread);
104

105 106
   connect(_multiVehicleManager, &MultiVehicleManager::vehicleAdded, this, &MAVLinkProtocol::_vehicleCountChanged);
   connect(_multiVehicleManager, &MultiVehicleManager::vehicleRemoved, this, &MAVLinkProtocol::_vehicleCountChanged);
107

108 109 110
   emit versionCheckChanged(m_enable_version_check);
}

111 112 113 114 115
void MAVLinkProtocol::loadSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
116
    enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool());
117 118 119

    // Only set system id if it was valid
    int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt();
120 121
    if (temp > 0 && temp < 256)
    {
122 123 124 125 126 127 128 129 130
        systemId = temp;
    }
}

void MAVLinkProtocol::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
131
    settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
132
    settings.setValue("GCS_SYSTEM_ID", systemId);
133
    // Parameter interface settings
134 135
}

136 137
void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
{
138
    int channel = link->mavlinkChannel();
139 140 141 142 143
    totalReceiveCounter[channel] = 0;
    totalLossCounter[channel] = 0;
    totalErrorCounter[channel] = 0;
    currReceiveCounter[channel] = 0;
    currLossCounter[channel] = 0;
144 145
}

pixhawk's avatar
pixhawk committed
146 147 148 149 150 151 152
/**
 * 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
 **/
153
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
154
{
Don Gagne's avatar
Don Gagne committed
155 156 157
    // 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.
158
    if (!_linkMgr->containsLink(link)) {
Don Gagne's avatar
Don Gagne committed
159 160
        return;
    }
Gus Grubba's avatar
Gus Grubba committed
161

162
//    receiveMutex.lock();
pixhawk's avatar
pixhawk committed
163 164
    mavlink_message_t message;
    mavlink_status_t status;
165

166
    int mavlinkChannel = link->mavlinkChannel();
167

168 169 170
    static int nonmavlinkCount = 0;
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
171

172
    for (int position = 0; position < b.size(); position++) {
173
        unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status);
174

175
        if (decodeState == 0 && !link->decodedFirstMavlinkPacket())
176 177
        {
            nonmavlinkCount++;
178
            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
179
            {
180
                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
181 182 183 184 185 186 187 188
                if (!checkedUserNonMavlink)
                {
                    link->requestReset();
                    checkedUserNonMavlink = true;
                }
                else
                {
                    warnedUserNonMavlink = true;
189
                    emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
190
                                                                          "Please check if the baud rates of %1 and your autopilot are the same.").arg(qgcApp()->applicationName()));
191 192 193
                }
            }
        }
194 195
        if (decodeState == 1)
        {
196
            if (!link->decodedFirstMavlinkPacket()) {
197 198
                mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
                if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
199
                    qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
200 201
                    mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
                }
202
                link->setDecodedFirstMavlinkPacket(true);
Don Gagne's avatar
Don Gagne committed
203 204
            }

pixhawk's avatar
pixhawk committed
205
            // Log data
Don Gagne's avatar
Don Gagne committed
206
            if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
207 208 209 210 211 212 213 214 215 216 217 218 219 220 221
                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.
                quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000;
                qToBigEndian(time, buf);

                // Then write the message to the buffer
                int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message);

                // 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.
222
                QByteArray b((const char*)buf, len);
Don Gagne's avatar
Don Gagne committed
223
                if(_tempLogFile.write(b) != len)
224
                {
225
                    // If there's an error logging data, raise an alert and stop logging.
226
                    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
227 228
                    _stopLogging();
                    _logSuspendError = true;
229
                }
Gus Grubba's avatar
Gus Grubba committed
230

Don Gagne's avatar
Don Gagne committed
231
                // Check for the vehicle arming going by. This is used to trigger log save.
232
                if (!_vehicleWasArmed && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
Don Gagne's avatar
Don Gagne committed
233 234 235
                    mavlink_heartbeat_t state;
                    mavlink_msg_heartbeat_decode(&message, &state);
                    if (state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
236
                        _vehicleWasArmed = true;
Don Gagne's avatar
Don Gagne committed
237 238
                    }
                }
pixhawk's avatar
pixhawk committed
239 240
            }

241
            if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
Don Gagne's avatar
Don Gagne committed
242 243
                // Start loggin on first heartbeat
                _startLogging();
244 245
                mavlink_heartbeat_t heartbeat;
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
246
                emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type);
pixhawk's avatar
pixhawk committed
247
            }
248

249
            // Increase receive counter
250 251
            totalReceiveCounter[mavlinkChannel]++;
            currReceiveCounter[mavlinkChannel]++;
252

253 254 255 256
            // Determine what the next expected sequence number is, accounting for
            // never having seen a message for this system/component pair.
            int lastSeq = lastIndex[message.sysid][message.compid];
            int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
257

258 259 260
            // And if we didn't encounter that sequence number, record the error
            if (message.seq != expectedSeq)
            {
261

262 263
                // Determine how many messages were skipped
                int lostMessages = message.seq - expectedSeq;
264

265 266 267 268 269
                // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
                if (lostMessages < 0)
                {
                    lostMessages = 0;
                }
270

271
                // And log how many were lost for all time and just this timestep
272 273
                totalLossCounter[mavlinkChannel] += lostMessages;
                currLossCounter[mavlinkChannel] += lostMessages;
274
            }
275

276 277
            // And update the last sequence number for this system/component pair
            lastIndex[message.sysid][message.compid] = expectedSeq;
278

279
            // Update on every 32th packet
280
            if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0)
281 282 283
            {
                // Calculate new loss ratio
                // Receive loss
284 285
                float receiveLossPercent = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]);
                receiveLossPercent *= 100.0f;
286 287
                currLossCounter[mavlinkChannel] = 0;
                currReceiveCounter[mavlinkChannel] = 0;
288 289
                emit receiveLossPercentChanged(message.sysid, receiveLossPercent);
                emit receiveLossTotalChanged(message.sysid, totalLossCounter[mavlinkChannel]);
290
            }
291

292 293 294 295
            // 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
            emit messageReceived(link, message);
pixhawk's avatar
pixhawk committed
296 297 298 299 300 301 302 303 304 305 306 307
        }
    }
}

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

308
/** @return System id of this application */
309
int MAVLinkProtocol::getSystemId()
310
{
311 312 313 314 315 316
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
317 318 319
}

/** @return Component id of this application */
320
int MAVLinkProtocol::getComponentId()
321
{
322
    return 0;
323 324
}

lm's avatar
lm committed
325 326 327
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
328
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
329 330
}

331
void MAVLinkProtocol::_vehicleCountChanged(void)
Don Gagne's avatar
Don Gagne committed
332
{
333
    int count = _multiVehicleManager->vehicles()->count();
Don Gagne's avatar
Don Gagne committed
334 335 336 337 338 339
    if (count == 0) {
        // Last vehicle is gone, close out logging
        _stopLogging();
    }
}

Don Gagne's avatar
Don Gagne committed
340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358
/// @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)
{
359
    //-- Are we supposed to write logs?
360 361 362
    if (qgcApp()->runningUnitTests()) {
        return;
    }
363 364 365 366 367 368 369 370
#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
371 372 373 374 375 376 377 378 379 380
    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;
            }

381
            qDebug() << "Temp log" << _tempLogFile.fileName();
382
            emit checkTelemetrySavePath();
383

Don Gagne's avatar
Don Gagne committed
384
            _logSuspendError = false;
Don Gagne's avatar
Don Gagne committed
385 386 387 388 389 390
        }
    }
}

void MAVLinkProtocol::_stopLogging(void)
{
391 392 393 394 395 396 397 398
    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());
            }
399
        }
Don Gagne's avatar
Don Gagne committed
400
    }
401
    _vehicleWasArmed = false;
Don Gagne's avatar
Don Gagne committed
402 403 404 405 406
}

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

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

Don Gagne's avatar
Don Gagne committed
415 416 417 418 419 420 421
    foreach(const QFileInfo fileInfo, fileInfoList) {
        qDebug() << "Orphaned log file" << fileInfo.filePath();
        if (fileInfo.size() == 0) {
            // Delete all zero length files
            QFile::remove(fileInfo.filePath());
            continue;
        }
422
        emit saveTelemetryLog(fileInfo.filePath());
Don Gagne's avatar
Don Gagne committed
423 424 425 426 427 428 429
    }
}

void MAVLinkProtocol::suspendLogForReplay(bool suspend)
{
    _logSuspendReplay = suspend;
}
430 431 432 433

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

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

438 439 440 441
    foreach(const QFileInfo fileInfo, fileInfoList) {
        QFile::remove(fileInfo.filePath());
    }
}
442