MAVLinkProtocol.cc 23.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>
pixhawk's avatar
pixhawk committed
27 28 29 30 31 32

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

39
Q_DECLARE_METATYPE(mavlink_message_t)
40

41
QGC_LOGGING_CATEGORY(MAVLinkProtocolLog, "MAVLinkProtocolLog")
42

Don Gagne's avatar
Don Gagne committed
43
#ifndef __mobile__
Don Gagne's avatar
Don Gagne committed
44 45
const char* MAVLinkProtocol::_tempLogFileTemplate = "FlightDataXXXXXX"; ///< Template for temporary log file
const char* MAVLinkProtocol::_logFileExtension = "mavlink";             ///< Extension for log files
Don Gagne's avatar
Don Gagne committed
46
#endif
Don Gagne's avatar
Don Gagne committed
47

pixhawk's avatar
pixhawk committed
48 49 50 51
/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
52 53 54 55 56 57 58 59 60 61 62
MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app)
    : QGCTool(app)
    , m_multiplexingEnabled(false)
    , m_authEnabled(false)
    , m_enable_version_check(true)
    , m_paramRetransmissionTimeout(350)
    , m_paramRewriteTimeout(500)
    , m_paramGuardEnabled(true)
    , m_actionGuardEnabled(false)
    , m_actionRetransmissionTimeout(100)
    , versionMismatchIgnore(false)
63
    , systemId(255)
Don Gagne's avatar
Don Gagne committed
64
#ifndef __mobile__
65 66
    , _logSuspendError(false)
    , _logSuspendReplay(false)
67
    , _logPromptForSave(false)
68
    , _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension))
Don Gagne's avatar
Don Gagne committed
69
#endif
70 71
    , _linkMgr(NULL)
    , _multiVehicleManager(NULL)
pixhawk's avatar
pixhawk committed
72
{
73 74 75 76 77
    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
78 79
}

80 81 82 83
MAVLinkProtocol::~MAVLinkProtocol()
{
    storeSettings();
    
Don Gagne's avatar
Don Gagne committed
84
#ifndef __mobile__
85
    _closeLogFile();
Don Gagne's avatar
Don Gagne committed
86
#endif
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
void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox)
{
   QGCTool::setToolbox(toolbox);

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

   qRegisterMetaType<mavlink_message_t>("mavlink_message_t");

   m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
   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;
       }
   }

   connect(this, &MAVLinkProtocol::protocolStatusMessage, _app, &QGCApplication::criticalMessageBoxOnMainThread);
114
#ifndef __mobile__
115
   connect(this, &MAVLinkProtocol::saveTempFlightDataLog, _app, &QGCApplication::saveTempFlightDataLogOnMainThread);
116
#endif
117

118 119
   connect(_multiVehicleManager->vehicles(), &QmlObjectListModel::countChanged, this, &MAVLinkProtocol::_vehicleCountChanged);

120 121 122
   emit versionCheckChanged(m_enable_version_check);
}

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

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

138 139 140 141
    // Set auth key
    m_authKey = settings.value("GCS_AUTH_KEY", m_authKey).toString();
    enableAuth(settings.value("GCS_AUTH_ENABLED", m_authEnabled).toBool());

142 143 144 145 146 147 148
    // Parameter interface settings
    bool ok;
    temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout).toInt(&ok);
    if (ok) m_paramRetransmissionTimeout = temp;
    temp = settings.value("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout).toInt(&ok);
    if (ok) m_paramRewriteTimeout = temp;
    m_paramGuardEnabled = settings.value("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled).toBool();
149 150 151 152 153 154 155 156
    settings.endGroup();
}

void MAVLinkProtocol::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
157 158
    settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
    settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled);
159
    settings.setValue("GCS_SYSTEM_ID", systemId);
160 161
    settings.setValue("GCS_AUTH_KEY", m_authKey);
    settings.setValue("GCS_AUTH_ENABLED", m_authEnabled);
162 163 164 165
    // Parameter interface settings
    settings.setValue("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout);
    settings.setValue("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout);
    settings.setValue("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled);
166 167 168
    settings.endGroup();
}

169 170
void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
{
171 172 173 174 175 176
    int channel = link->getMavlinkChannel();
    totalReceiveCounter[channel] = 0;
    totalLossCounter[channel] = 0;
    totalErrorCounter[channel] = 0;
    currReceiveCounter[channel] = 0;
    currLossCounter[channel] = 0;
177 178
}

pixhawk's avatar
pixhawk committed
179 180 181 182 183 184 185
/**
 * 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
 **/
186
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
187
{
Don Gagne's avatar
Don Gagne committed
188 189 190
    // 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.
Don Gagne's avatar
Don Gagne committed
191
    if (!_linkMgr->links()->contains(link)) {
Don Gagne's avatar
Don Gagne committed
192 193 194
        return;
    }
    
195
//    receiveMutex.lock();
pixhawk's avatar
pixhawk committed
196 197
    mavlink_message_t message;
    mavlink_status_t status;
198

199
    int mavlinkChannel = link->getMavlinkChannel();
200

201
    static int mavlink09Count = 0;
202
    static int nonmavlinkCount = 0;
203
    static bool decodedFirstPacket = false;
204
    static bool warnedUser = false;
205 206
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
207

208
    for (int position = 0; position < b.size(); position++) {
209
        unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status);
210 211

        if ((uint8_t)b[position] == 0x55) mavlink09Count++;
212
        if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
213
        {
214
            warnedUser = true;
215 216
            // Obviously the user tries to use a 0.9 autopilot
            // with QGroundControl built for version 1.0
217 218 219 220
            emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
                                                                  "Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. "
                                                                  "Please upgrade the MAVLink version of your autopilot. "
                                                                  "If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."));
221
        }
pixhawk's avatar
pixhawk committed
222

223 224 225
        if (decodeState == 0 && !decodedFirstPacket)
        {
            nonmavlinkCount++;
226
            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
227
            {
228
                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
229 230 231 232 233 234 235 236
                if (!checkedUserNonMavlink)
                {
                    link->requestReset();
                    checkedUserNonMavlink = true;
                }
                else
                {
                    warnedUserNonMavlink = true;
237 238
                    emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
                                                                          "Please check if the baud rates of QGroundControl and your autopilot are the same."));
239 240 241
                }
            }
        }
242 243
        if (decodeState == 1)
        {
244
            decodedFirstPacket = true;
245 246 247 248 249 250 251 252 253 254

            if(message.msgid == MAVLINK_MSG_ID_PING)
            {
                // process ping requests (tgt_system and tgt_comp must be zero)
                mavlink_ping_t ping;
                mavlink_msg_ping_decode(&message, &ping);
                if(!ping.target_system && !ping.target_component)
                {
                    mavlink_message_t msg;
                    mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid);
Don Gagne's avatar
Don Gagne committed
255
                    _sendMessage(msg);
256 257 258
                }
            }

259 260 261 262 263
            if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
            {
                // process telemetry status message
                mavlink_radio_status_t rstatus;
                mavlink_msg_radio_status_decode(&message, &rstatus);
264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279
                int rssi = rstatus.rssi,
                    remrssi = rstatus.remrssi;
                // 3DR Si1k radio needs rssi fields to be converted to dBm
                if (message.sysid == '3' && message.compid == 'D') {
                    /* Per the Si1K datasheet figure 23.25 and SI AN474 code
                     * samples the relationship between the RSSI register
                     * and received power is as follows:
                     *
                     *                       10
                     * inputPower = rssi * ------ 127
                     *                       19
                     *
                     * Additionally limit to the only realistic range [-120,0] dBm
                     */
                    rssi    = qMin(qMax(qRound(static_cast<qreal>(rssi)    / 1.9 - 127.0), - 120), 0);
                    remrssi = qMin(qMax(qRound(static_cast<qreal>(remrssi) / 1.9 - 127.0), - 120), 0);
280 281 282
                } else {
                    rssi = (int8_t) rstatus.rssi;
                    remrssi = (int8_t) rstatus.remrssi;
283
                }
284

285
                emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rssi, remrssi,
286 287 288
                    rstatus.txbuf, rstatus.noise, rstatus.remnoise);
            }

Don Gagne's avatar
Don Gagne committed
289
#ifndef __mobile__
pixhawk's avatar
pixhawk committed
290
            // Log data
Don Gagne's avatar
Don Gagne committed
291
            
Don Gagne's avatar
Don Gagne committed
292
            if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
                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.
308
                QByteArray b((const char*)buf, len);
Don Gagne's avatar
Don Gagne committed
309
                if(_tempLogFile.write(b) != len)
310
                {
311
                    // If there's an error logging data, raise an alert and stop logging.
312
                    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
313 314
                    _stopLogging();
                    _logSuspendError = true;
315
                }
Don Gagne's avatar
Don Gagne committed
316 317
                
                // Check for the vehicle arming going by. This is used to trigger log save.
318
                if (!_logPromptForSave && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
Don Gagne's avatar
Don Gagne committed
319 320 321
                    mavlink_heartbeat_t state;
                    mavlink_msg_heartbeat_decode(&message, &state);
                    if (state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
322
                        _logPromptForSave = true;
Don Gagne's avatar
Don Gagne committed
323 324
                    }
                }
pixhawk's avatar
pixhawk committed
325
            }
Don Gagne's avatar
Don Gagne committed
326
#endif
pixhawk's avatar
pixhawk committed
327

328
            if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
Don Gagne's avatar
Don Gagne committed
329 330 331 332 333
#ifndef __mobile__
                // Start loggin on first heartbeat
                _startLogging();
#endif

334 335
                mavlink_heartbeat_t heartbeat;
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
Don Gagne's avatar
Don Gagne committed
336
                emit vehicleHeartbeatInfo(link, message.sysid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type);
pixhawk's avatar
pixhawk committed
337
            }
338

339
            // Increase receive counter
340 341
            totalReceiveCounter[mavlinkChannel]++;
            currReceiveCounter[mavlinkChannel]++;
342

343 344 345 346
            // 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);
347

348 349 350
            // And if we didn't encounter that sequence number, record the error
            if (message.seq != expectedSeq)
            {
351

352 353
                // Determine how many messages were skipped
                int lostMessages = message.seq - expectedSeq;
354

355 356 357 358 359
                // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
                if (lostMessages < 0)
                {
                    lostMessages = 0;
                }
360

361
                // And log how many were lost for all time and just this timestep
362 363
                totalLossCounter[mavlinkChannel] += lostMessages;
                currLossCounter[mavlinkChannel] += lostMessages;
364
            }
365

366 367
            // And update the last sequence number for this system/component pair
            lastIndex[message.sysid][message.compid] = expectedSeq;
368

369
            // Update on every 32th packet
370
            if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0)
371 372 373
            {
                // Calculate new loss ratio
                // Receive loss
374 375
                float receiveLossPercent = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]);
                receiveLossPercent *= 100.0f;
376 377
                currLossCounter[mavlinkChannel] = 0;
                currReceiveCounter[mavlinkChannel] = 0;
378 379
                emit receiveLossPercentChanged(message.sysid, receiveLossPercent);
                emit receiveLossTotalChanged(message.sysid, totalLossCounter[mavlinkChannel]);
380
            }
381

382 383 384 385
            // 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);
lm's avatar
lm committed
386

387 388 389 390
            // Multiplex message if enabled
            if (m_multiplexingEnabled)
            {
                // Emit message on all links that are currently connected
Don Gagne's avatar
Don Gagne committed
391 392 393
                for (int i=0; i<_linkMgr->links()->count(); i++) {
                    LinkInterface* currLink = _linkMgr->links()->value<LinkInterface*>(i);

394 395
                    // Only forward this message to the other links,
                    // not the link the message was received on
Don Gagne's avatar
Don Gagne committed
396
                    if (currLink && currLink != link) _sendMessage(currLink, message, message.sysid, message.compid);
397
                }
398
            }
pixhawk's avatar
pixhawk committed
399 400 401 402 403 404 405 406 407 408 409 410
        }
    }
}

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

411
/** @return System id of this application */
412
int MAVLinkProtocol::getSystemId()
413
{
414 415 416 417 418 419
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
420 421 422
}

/** @return Component id of this application */
423
int MAVLinkProtocol::getComponentId()
424
{
425
    return 0;
426 427
}

pixhawk's avatar
pixhawk committed
428 429 430
/**
 * @param message message to send
 */
Don Gagne's avatar
Don Gagne committed
431
void MAVLinkProtocol::_sendMessage(mavlink_message_t message)
pixhawk's avatar
pixhawk committed
432
{
Don Gagne's avatar
Don Gagne committed
433 434 435
    for (int i=0; i<_linkMgr->links()->count(); i++) {
        LinkInterface* link = _linkMgr->links()->value<LinkInterface*>(i);
        _sendMessage(link, message);
pixhawk's avatar
pixhawk committed
436 437 438 439 440 441 442
    }
}

/**
 * @param link the link to send the message over
 * @param message message to send
 */
Don Gagne's avatar
Don Gagne committed
443
void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t message)
pixhawk's avatar
pixhawk committed
444 445
{
    // Create buffer
Lorenz Meier's avatar
Lorenz Meier committed
446
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
447
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
448
    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
Lorenz Meier's avatar
Lorenz Meier committed
449
    mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]);
pixhawk's avatar
pixhawk committed
450
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
451
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
452
    // If link is connected
453 454
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
455
        // Send the portion of the buffer now occupied by the message
456
        link->writeBytesSafe((const char*)buffer, len);
pixhawk's avatar
pixhawk committed
457 458 459
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
460 461 462 463 464 465
/**
 * @param link the link to send the message over
 * @param message message to send
 * @param systemid id of the system the message is originating from
 * @param componentid id of the component the message is originating from
 */
Don Gagne's avatar
Don Gagne committed
466
void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid)
Lorenz Meier's avatar
Lorenz Meier committed
467 468 469 470 471
{
    // Create buffer
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Rewriting header to ensure correct link ID is set
    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
Lorenz Meier's avatar
Lorenz Meier committed
472
    mavlink_finalize_message_chan(&message, systemid, componentid, link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]);
Lorenz Meier's avatar
Lorenz Meier committed
473 474 475 476 477 478
    // Write message into buffer, prepending start sign
    int len = mavlink_msg_to_send_buffer(buffer, &message);
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
479
        link->writeBytesSafe((const char*)buffer, len);
Lorenz Meier's avatar
Lorenz Meier committed
480 481 482
    }
}

483 484 485 486 487 488 489 490 491
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
    bool changed = false;
    if (enabled != m_multiplexingEnabled) changed = true;

    m_multiplexingEnabled = enabled;
    if (changed) emit multiplexingChanged(m_multiplexingEnabled);
}

492 493 494 495
void MAVLinkProtocol::enableAuth(bool enable)
{
    bool changed = false;
    m_authEnabled = enable;
496
    if (m_authEnabled != enable) {
497 498 499 500 501
        changed = true;
    }
    if (changed) emit authChanged(m_authEnabled);
}

502 503
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
504
    if (enabled != m_paramGuardEnabled) {
505 506 507 508 509
        m_paramGuardEnabled = enabled;
        emit paramGuardChanged(m_paramGuardEnabled);
    }
}

510 511
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
512
    if (enabled != m_actionGuardEnabled) {
513 514 515 516 517
        m_actionGuardEnabled = enabled;
        emit actionGuardChanged(m_actionGuardEnabled);
    }
}

518 519
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
520
    if (ms != m_paramRetransmissionTimeout) {
521 522 523 524 525 526 527
        m_paramRetransmissionTimeout = ms;
        emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
    }
}

void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
528
    if (ms != m_paramRewriteTimeout) {
529 530 531 532 533
        m_paramRewriteTimeout = ms;
        emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
    }
}

534 535
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
536
    if (ms != m_actionRetransmissionTimeout) {
537 538 539 540 541
        m_actionRetransmissionTimeout = ms;
        emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
    }
}

lm's avatar
lm committed
542 543 544
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
545
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
546 547
}

Don Gagne's avatar
Don Gagne committed
548 549 550 551 552 553 554 555 556 557 558 559
void MAVLinkProtocol::_vehicleCountChanged(int count)
{
#ifndef __mobile__
    if (count == 0) {
        // Last vehicle is gone, close out logging
        _stopLogging();
    }
#else
    Q_UNUSED(count);
#endif
}

Don Gagne's avatar
Don Gagne committed
560
#ifndef __mobile__
Don Gagne's avatar
Don Gagne committed
561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580
/// @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)
{
Don Gagne's avatar
Don Gagne committed
581 582 583 584 585 586 587 588 589 590
    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;
            }

591 592 593 594 595
            if (_app->promptFlightDataSaveNotArmed()) {
                _logPromptForSave = true;
            }

            qDebug() << "Temp log" << _tempLogFile.fileName() << _logPromptForSave;
596

Don Gagne's avatar
Don Gagne committed
597
            _logSuspendError = false;
Don Gagne's avatar
Don Gagne committed
598 599 600 601 602 603 604
        }
    }
}

void MAVLinkProtocol::_stopLogging(void)
{
    if (_closeLogFile()) {
605
        // If the signals are not connected it means we are running a unit test. In that case just delete log files
606
        if (_logPromptForSave && _app->promptFlightDataSave()) {
607 608 609 610
            emit saveTempFlightDataLog(_tempLogFile.fileName());
        } else {
            QFile::remove(_tempLogFile.fileName());
        }
Don Gagne's avatar
Don Gagne committed
611
    }
612
    _logPromptForSave = false;
Don Gagne's avatar
Don Gagne committed
613 614 615 616 617
}

/// @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.
618
void MAVLinkProtocol::checkForLostLogFiles(void)
Don Gagne's avatar
Don Gagne committed
619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646
{
    QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
    
    QString filter(QString("*.%1").arg(_logFileExtension));
    QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
    qDebug() << "Orphaned log file count" << fileInfoList.count();
    
    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;
        }

        // Give the user a chance to save the orphaned log file
        emit protocolStatusMessage(tr("Found unsaved Flight Data"),
                                   tr("This can happen if QGroundControl crashes during Flight Data collection. "
                                      "If you want to save the unsaved Flight Data, select the file you want to save it to. "
                                      "If you do not want to keep the Flight Data, select 'Cancel' on the next dialog."));
        emit saveTempFlightDataLog(fileInfo.filePath());
    }
}

void MAVLinkProtocol::suspendLogForReplay(bool suspend)
{
    _logSuspendReplay = suspend;
}
647 648 649 650 651 652 653 654 655 656 657 658

void MAVLinkProtocol::deleteTempLogFiles(void)
{
    QDir tempDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation));
    
    QString filter(QString("*.%1").arg(_logFileExtension));
    QFileInfoList fileInfoList = tempDir.entryInfoList(QStringList(filter), QDir::Files);
    
    foreach(const QFileInfo fileInfo, fileInfoList) {
        QFile::remove(fileInfo.filePath());
    }
}
Don Gagne's avatar
Don Gagne committed
659
#endif