MAVLinkProtocol.cc 26.4 KB
Newer Older
1
/*===================================================================
pixhawk's avatar
pixhawk committed
2 3 4 5
======================================================================*/

/**
 * @file
pixhawk's avatar
pixhawk committed
6 7
 *   @brief Implementation of class MAVLinkProtocol
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
8 9 10 11 12 13 14
 */

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

#include <QDebug>
#include <QTime>
lm's avatar
lm committed
15
#include <QApplication>
16
#include <QSettings>
17
#include <QStandardPaths>
18
#include <QtEndian>
19
#include <QMetaType>
pixhawk's avatar
pixhawk committed
20 21 22 23 24 25 26 27

#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "configuration.h"
#include "LinkManager.h"
28 29
#include "QGCMAVLink.h"
#include "QGCMAVLinkUASFactory.h"
30
#include "QGC.h"
Don Gagne's avatar
Don Gagne committed
31
#include "QGCApplication.h"
pixhawk's avatar
pixhawk committed
32

33
Q_DECLARE_METATYPE(mavlink_message_t)
34

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

pixhawk's avatar
pixhawk committed
38 39 40 41 42
/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
43
    heartbeatTimer(NULL),
44
    heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
45
    m_heartbeatsEnabled(true),
46 47 48 49 50 51 52 53 54
    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),
55
    systemId(QGC::defaultSystemId),
Don Gagne's avatar
Don Gagne committed
56 57 58
    _should_exit(false),
    _logSuspendError(false),
    _logSuspendReplay(false),
59
    _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension)),
Don Gagne's avatar
Don Gagne committed
60 61 62
    _protocolStatusMessageConnected(false),
    _saveTempFlightDataLogConnected(false)

pixhawk's avatar
pixhawk committed
63
{
64
    qRegisterMetaType<mavlink_message_t>("mavlink_message_t");
Don Gagne's avatar
Don Gagne committed
65
    
66
    m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
67
    loadSettings();
Lorenz Meier's avatar
Lorenz Meier committed
68
    moveToThread(this);
69 70 71 72 73

    // 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.
74 75 76 77
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
lm's avatar
lm committed
78 79 80
            lastIndex[i][j] = -1;
        }
    }
lm's avatar
lm committed
81

Lorenz Meier's avatar
Lorenz Meier committed
82 83
    start(QThread::HighPriority);

lm's avatar
lm committed
84
    emit versionCheckChanged(m_enable_version_check);
pixhawk's avatar
pixhawk committed
85 86
}

87 88 89 90 91
void MAVLinkProtocol::loadSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
92
    enableHeartbeats(settings.value("HEARTBEATS_ENABLED", m_heartbeatsEnabled).toBool());
93 94
    enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool());
    enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool());
95 96 97

    // Only set system id if it was valid
    int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt();
98 99
    if (temp > 0 && temp < 256)
    {
100 101
        systemId = temp;
    }
102

103 104 105 106
    // Set auth key
    m_authKey = settings.value("GCS_AUTH_KEY", m_authKey).toString();
    enableAuth(settings.value("GCS_AUTH_ENABLED", m_authEnabled).toBool());

107 108 109 110 111 112 113
    // 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();
114 115 116 117 118 119 120 121 122
    settings.endGroup();
}

void MAVLinkProtocol::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
    settings.setValue("HEARTBEATS_ENABLED", m_heartbeatsEnabled);
123 124
    settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
    settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled);
125
    settings.setValue("GCS_SYSTEM_ID", systemId);
126 127
    settings.setValue("GCS_AUTH_KEY", m_authKey);
    settings.setValue("GCS_AUTH_ENABLED", m_authEnabled);
128 129 130 131
    // 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);
132 133 134
    settings.endGroup();
}

pixhawk's avatar
pixhawk committed
135 136
MAVLinkProtocol::~MAVLinkProtocol()
{
137
    storeSettings();
Don Gagne's avatar
Don Gagne committed
138 139
    
    _closeLogFile();
Lorenz Meier's avatar
Lorenz Meier committed
140 141

    // Tell the thread to exit
142
    _should_exit = true;
Lorenz Meier's avatar
Lorenz Meier committed
143 144 145 146 147 148 149 150 151 152
    // Wait for it to exit
    wait();
}

/**
 * @brief Runs the thread
 *
 **/
void MAVLinkProtocol::run()
{
153 154 155 156 157 158 159 160 161
    heartbeatTimer = new QTimer();
    heartbeatTimer->moveToThread(this);
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);

    while(!_should_exit) {

        if (isFinished()) {
162
            delete heartbeatTimer;
163 164 165 166 167 168 169
            qDebug() << "MAVLINK WORKER DONE!";
            return;
        }

        QCoreApplication::processEvents();
        QGC::SLEEP::msleep(2);
    }
pixhawk's avatar
pixhawk committed
170 171
}

172 173 174 175 176 177 178 179 180 181
void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
{
    int linkId = link->getId();
    totalReceiveCounter[linkId] = 0;
    totalLossCounter[linkId] = 0;
    totalErrorCounter[linkId] = 0;
    currReceiveCounter[linkId] = 0;
    currLossCounter[linkId] = 0;
}

182 183 184
void MAVLinkProtocol::linkStatusChanged(bool connected)
{
    LinkInterface* link = qobject_cast<LinkInterface*>(QObject::sender());
Don Gagne's avatar
Don Gagne committed
185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220
    
    if (link == NULL) {
        Q_ASSERT(false);
        return;
    }
    
    if (connected) {
        Q_ASSERT(!_connectedLinks.contains(link));
        _connectedLinks.append(link);
        
        if (_connectedLinks.count() == 1) {
            // This is the first link, we need to start logging
            _startLogging();
        }
        
        // Send command to start MAVLink
        // XXX hacky but safe
        // Start NSH
        const char init[] = {0x0d, 0x0d, 0x0d};
        link->writeBytes(init, sizeof(init));
        const char* cmd = "sh /etc/init.d/rc.usb\n";
        link->writeBytes(cmd, strlen(cmd));
        link->writeBytes(init, 4);
    } else {
        Q_ASSERT(_connectedLinks.contains(link));
        _connectedLinks.removeOne(link);
        
        if (_connectedLinks.count() == 0) {
            // Last link is gone, close out logging
            _stopLogging();
        }
    }
    
    // Track the links which are connected to the protocol
    QList<LinkInterface*> _connectedLinks;  ///< List of all links connected to protocol

221
    //qDebug() << "linkStatusChanged" << connected;
Don Gagne's avatar
Don Gagne committed
222
    
223 224 225 226 227 228 229

    if (link) {
        if (connected) {
        }
    }
}

pixhawk's avatar
pixhawk committed
230 231 232 233 234 235 236
/**
 * 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
 **/
237
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
238
{
239
//    receiveMutex.lock();
pixhawk's avatar
pixhawk committed
240 241
    mavlink_message_t message;
    mavlink_status_t status;
242

243 244 245
    // Cache the link ID for common use.
    int linkId = link->getId();

246
    static int mavlink09Count = 0;
247
    static int nonmavlinkCount = 0;
248
    static bool decodedFirstPacket = false;
249
    static bool warnedUser = false;
250 251
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
252

253
    // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
254
    for (int position = 0; position < b.size(); position++) {
255
        unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
256 257

        if ((uint8_t)b[position] == 0x55) mavlink09Count++;
258
        if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
259
        {
260
            warnedUser = true;
261 262
            // Obviously the user tries to use a 0.9 autopilot
            // with QGroundControl built for version 1.0
263
            emit protocolStatusMessage("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.");
264
        }
pixhawk's avatar
pixhawk committed
265

266 267 268
        if (decodeState == 0 && !decodedFirstPacket)
        {
            nonmavlinkCount++;
269
            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
270
            {
271
                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
272 273 274 275 276 277 278 279 280 281 282 283
                if (!checkedUserNonMavlink)
                {
                    link->requestReset();
                    checkedUserNonMavlink = true;
                }
                else
                {
                    warnedUserNonMavlink = true;
                    emit protocolStatusMessage("MAVLink Baud Rate Mismatch", "Please check if the baud rates of QGroundControl and your autopilot are the same.");
                }
            }
        }
284 285
        if (decodeState == 1)
        {
286
            decodedFirstPacket = true;
287 288 289 290 291 292 293 294 295 296 297 298 299 300

            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);
                    sendMessage(msg);
                }
            }

301 302 303 304 305 306 307 308 309 310
            if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
            {
                // process telemetry status message
                mavlink_radio_status_t rstatus;
                mavlink_msg_radio_status_decode(&message, &rstatus);

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

pixhawk's avatar
pixhawk committed
311
            // Log data
Don Gagne's avatar
Don Gagne committed
312 313 314 315
            
            Q_ASSERT(_logSuspendError || _logSuspendReplay || _tempLogFile.isOpen());
            
            if (!_logSuspendError && !_logSuspendReplay) {
316 317 318 319 320 321 322 323 324 325 326 327 328 329 330
                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.
331
                QByteArray b((const char*)buf, len);
Don Gagne's avatar
Don Gagne committed
332
                if(_tempLogFile.write(b) != len)
333
                {
334
                    // If there's an error logging data, raise an alert and stop logging.
Don Gagne's avatar
Don Gagne committed
335 336 337
                    emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
                    _stopLogging();
                    _logSuspendError = true;
338
                }
pixhawk's avatar
pixhawk committed
339 340
            }

pixhawk's avatar
pixhawk committed
341 342 343
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
344

pixhawk's avatar
pixhawk committed
345 346 347
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
348 349
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
pixhawk's avatar
pixhawk committed
350 351 352 353 354 355
                // ORDER MATTERS HERE!
                // The UAS object has first to be created and connected,
                // only then the rest of the application can be made aware
                // of its existence, as it only then can send and receive
                // it's first messages.

pixhawk's avatar
pixhawk committed
356
                // Check if the UAS has the same id like this system
357 358
                if (message.sysid == getSystemId())
                {
359
                    emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
360 361
                }

362 363 364 365
                // Create a new UAS based on the heartbeat received
                // Todo dynamically load plugin at run-time for MAV
                // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION

pixhawk's avatar
pixhawk committed
366
                // First create new UAS object
367 368
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
pixhawk's avatar
pixhawk committed
369 370
                // Reset version field to 0
                heartbeat.mavlink_version = 0;
371
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
pixhawk's avatar
pixhawk committed
372 373

                // Check if the UAS has a different protocol version
374 375
                if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
                {
pixhawk's avatar
pixhawk committed
376
                    // Bring up dialog to inform user
377 378
                    if (!versionMismatchIgnore)
                    {
379
                        emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
380
                                                   tr("It is unsafe to use different MAVLink versions. QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION));
381 382
                        versionMismatchIgnore = true;
                    }
pixhawk's avatar
pixhawk committed
383 384 385 386 387

                    // Ignore this message and continue gracefully
                    continue;
                }

388 389
                // Create a new UAS object
                uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
390

pixhawk's avatar
pixhawk committed
391
            }
392

393 394 395
            // Increase receive counter
            totalReceiveCounter[linkId]++;
            currReceiveCounter[linkId]++;
396

397 398 399 400
            // 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);
401

402 403 404
            // And if we didn't encounter that sequence number, record the error
            if (message.seq != expectedSeq)
            {
405

406 407
                // Determine how many messages were skipped
                int lostMessages = message.seq - expectedSeq;
408

409 410 411 412 413
                // Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
                if (lostMessages < 0)
                {
                    lostMessages = 0;
                }
414

415 416 417 418
                // And log how many were lost for all time and just this timestep
                totalLossCounter[linkId] += lostMessages;
                currLossCounter[linkId] += lostMessages;
            }
419

420 421
            // And update the last sequence number for this system/component pair
            lastIndex[message.sysid][message.compid] = expectedSeq;
422

423 424 425 426 427 428 429 430 431 432 433
            // Update on every 32th packet
            if ((totalReceiveCounter[linkId] & 0x1F) == 0)
            {
                // Calculate new loss ratio
                // Receive loss
                float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]);
                receiveLoss *= 100.0f;
                currLossCounter[linkId] = 0;
                currReceiveCounter[linkId] = 0;
                emit receiveLossChanged(message.sysid, receiveLoss);
            }
434

435 436 437 438
            // 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
439

440 441 442 443 444
            // Multiplex message if enabled
            if (m_multiplexingEnabled)
            {
                // Get all links connected to this unit
                QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
445

446 447
                // Emit message on all links that are currently connected
                foreach (LinkInterface* currLink, links)
448
                {
449 450 451
                    // Only forward this message to the other links,
                    // not the link the message was received on
                    if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
452
                }
453
            }
pixhawk's avatar
pixhawk committed
454 455 456 457 458 459 460 461 462 463 464 465
        }
    }
}

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

466
/** @return System id of this application */
467
int MAVLinkProtocol::getSystemId()
468
{
469 470 471 472 473 474
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
475 476 477
}

/** @return Component id of this application */
478
int MAVLinkProtocol::getComponentId()
479
{
480
    return QGC::defaultComponentId;
481 482
}

pixhawk's avatar
pixhawk committed
483 484 485 486 487 488 489 490 491 492
/**
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
    // Get all links connected to this unit
    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
493 494
    for (i = links.begin(); i != links.end(); ++i)
    {
pixhawk's avatar
pixhawk committed
495
        sendMessage(*i, message);
496
//        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
497 498 499 500 501 502 503 504 505 506
    }
}

/**
 * @param link the link to send the message over
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    // Create buffer
Lorenz Meier's avatar
Lorenz Meier committed
507
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
508
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
509 510
    static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
    if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]);
pixhawk's avatar
pixhawk committed
511
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
512
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
513
    // If link is connected
514 515
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
516 517 518 519 520
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543
/**
 * @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
 */
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid)
{
    // 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;
    if (link->getId() != 0) mavlink_finalize_message_chan(&message, systemid, componentid, link->getId(), message.len, messageKeys[message.msgid]);
    // 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
        link->writeBytes((const char*)buffer, len);
    }
}

pixhawk's avatar
pixhawk committed
544 545 546 547 548 549 550
/**
 * The heartbeat is sent out of order and does not reset the
 * periodic heartbeat emission. It will be just sent in addition.
 * @return mavlink_message_t heartbeat message sent on serial link
 */
void MAVLinkProtocol::sendHeartbeat()
{
Lorenz Meier's avatar
Lorenz Meier committed
551 552
    if (m_heartbeatsEnabled)
    {
pixhawk's avatar
pixhawk committed
553
        mavlink_message_t beat;
lm's avatar
lm committed
554
        mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_MANUAL_ARMED, 0, MAV_STATE_ACTIVE);
pixhawk's avatar
pixhawk committed
555 556
        sendMessage(beat);
    }
Lorenz Meier's avatar
Lorenz Meier committed
557 558
    if (m_authEnabled)
    {
James Goppert's avatar
James Goppert committed
559 560
        mavlink_message_t msg;
        mavlink_auth_key_t auth;
561
        memset(&auth, 0, sizeof(auth));
562
        memcpy(auth.key, m_authKey.toStdString().c_str(), qMin(m_authKey.length(), MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN));
James Goppert's avatar
James Goppert committed
563 564 565
        mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth);
        sendMessage(msg);
    }
pixhawk's avatar
pixhawk committed
566 567 568 569 570 571 572 573 574
}

/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
    m_heartbeatsEnabled = enabled;
    emit heartbeatChanged(enabled);
}

575 576 577 578 579 580 581 582 583
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
    bool changed = false;
    if (enabled != m_multiplexingEnabled) changed = true;

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

584 585 586 587
void MAVLinkProtocol::enableAuth(bool enable)
{
    bool changed = false;
    m_authEnabled = enable;
588
    if (m_authEnabled != enable) {
589 590 591 592 593
        changed = true;
    }
    if (changed) emit authChanged(m_authEnabled);
}

594 595
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
596
    if (enabled != m_paramGuardEnabled) {
597 598 599 600 601
        m_paramGuardEnabled = enabled;
        emit paramGuardChanged(m_paramGuardEnabled);
    }
}

602 603
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
604
    if (enabled != m_actionGuardEnabled) {
605 606 607 608 609
        m_actionGuardEnabled = enabled;
        emit actionGuardChanged(m_actionGuardEnabled);
    }
}

610 611
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
612
    if (ms != m_paramRetransmissionTimeout) {
613 614 615 616 617 618 619
        m_paramRetransmissionTimeout = ms;
        emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
    }
}

void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
620
    if (ms != m_paramRewriteTimeout) {
621 622 623 624 625
        m_paramRewriteTimeout = ms;
        emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
    }
}

626 627
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
628
    if (ms != m_actionRetransmissionTimeout) {
629 630 631 632 633
        m_actionRetransmissionTimeout = ms;
        emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
    }
}

lm's avatar
lm committed
634 635 636
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
637
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
638 639
}

pixhawk's avatar
pixhawk committed
640 641 642 643 644 645 646 647
/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
648
    heartbeatTimer->setInterval(1000/heartbeatRate);
pixhawk's avatar
pixhawk committed
649 650 651 652 653 654 655
}

/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
    return heartbeatRate;
}
Don Gagne's avatar
Don Gagne committed
656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755

/// @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)
{
    Q_ASSERT(!_tempLogFile.isOpen());

    if (!_logSuspendReplay) {
        if (!_tempLogFile.open()) {
            emit protocolStatusMessage(tr("Opening Flight Data file for writing failed"),
                                       tr("Unable to write to %1. Please choose a different file location.").arg(_tempLogFile.fileName()));
            _closeLogFile();
            _logSuspendError = true;
            return;
        }
    
        qDebug() << "Temp log" << _tempLogFile.fileName();
        
        _logSuspendError = false;
    }
}

void MAVLinkProtocol::_stopLogging(void)
{
    if (_closeLogFile()) {
        emit saveTempFlightDataLog(_tempLogFile.fileName());
    }
}

/// @brief We override this virtual from QObject so that we can track when the
///         protocolStatusMessage and saveTempFlightDataLog signals are connected.
///         Once both are connected we can check for lost log files.
void MAVLinkProtocol::connectNotify(const QMetaMethod& signal)
{
    if (signal == QMetaMethod::fromSignal(&MAVLinkProtocol::protocolStatusMessage)) {
        _protocolStatusMessageConnected = true;
        if (_saveTempFlightDataLogConnected) {
            _checkLostLogFiles();
        }
    } else if (signal == QMetaMethod::fromSignal(&MAVLinkProtocol::saveTempFlightDataLog)) {
        _saveTempFlightDataLogConnected = true;
        if (_protocolStatusMessageConnected) {
            _checkLostLogFiles();
        }
    }
}

/// @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.
void MAVLinkProtocol::_checkLostLogFiles(void)
{
    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)
{
    // This must come through a slot so that we handle this on the right thread. This will
    // also cause the signal to be queued behind any bytesReady signals which must be flushed
    // out of the queue before we change suspend state.
    Q_ASSERT(QThread::currentThread() == this);
    
    _logSuspendReplay = suspend;
}