MAVLinkProtocol.cc 24.1 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>
pixhawk's avatar
pixhawk committed
16
#include <QMessageBox>
17
#include <QSettings>
18
#include <QStandardPaths>
19
#include <QtEndian>
20
#include <QMetaType>
pixhawk's avatar
pixhawk committed
21 22 23 24 25 26

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

34
Q_DECLARE_METATYPE(mavlink_message_t)
35

pixhawk's avatar
pixhawk committed
36 37 38 39 40
/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
41
    heartbeatTimer(NULL),
42
    heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
43
    m_heartbeatsEnabled(true),
44 45 46 47 48 49 50 51 52 53 54
    m_multiplexingEnabled(false),
    m_authEnabled(false),
    m_loggingEnabled(false),
    m_logfile(NULL),
    m_enable_version_check(true),
    m_paramRetransmissionTimeout(350),
    m_paramRewriteTimeout(500),
    m_paramGuardEnabled(true),
    m_actionGuardEnabled(false),
    m_actionRetransmissionTimeout(100),
    versionMismatchIgnore(false),
55 56
    systemId(QGC::defaultSystemId),
    _should_exit(false)
pixhawk's avatar
pixhawk committed
57
{
58 59
    qRegisterMetaType<mavlink_message_t>("mavlink_message_t");

60
    m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
61
    loadSettings();
Lorenz Meier's avatar
Lorenz Meier committed
62
    moveToThread(this);
63 64 65 66 67

    // 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.
68 69 70 71
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
lm's avatar
lm committed
72 73 74
            lastIndex[i][j] = -1;
        }
    }
lm's avatar
lm committed
75

Lorenz Meier's avatar
Lorenz Meier committed
76 77
    start(QThread::HighPriority);

lm's avatar
lm committed
78
    emit versionCheckChanged(m_enable_version_check);
pixhawk's avatar
pixhawk committed
79 80
}

81 82 83 84 85 86
void MAVLinkProtocol::loadSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.sync();
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
87
    enableHeartbeats(settings.value("HEARTBEATS_ENABLED", m_heartbeatsEnabled).toBool());
88 89
    enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool());
    enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool());
90 91

    // Only set logfile if there is a name present in settings
92 93
    if (settings.contains("LOGFILE_NAME") && m_logfile == NULL)
    {
94
        m_logfile = new QFile(settings.value("LOGFILE_NAME").toString());
95 96 97
    }
    else if (m_logfile == NULL)
    {
98
        m_logfile = new QFile(QStandardPaths::writableLocation(QStandardPaths::HomeLocation) + "/qgroundcontrol_packetlog.mavlink");
99
    }
100 101
    // Enable logging
    enableLogging(settings.value("LOGGING_ENABLED", m_loggingEnabled).toBool());
102 103 104

    // Only set system id if it was valid
    int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt();
105 106
    if (temp > 0 && temp < 256)
    {
107 108
        systemId = temp;
    }
109

110 111 112 113
    // Set auth key
    m_authKey = settings.value("GCS_AUTH_KEY", m_authKey).toString();
    enableAuth(settings.value("GCS_AUTH_ENABLED", m_authEnabled).toBool());

114 115 116 117 118 119 120
    // 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();
121 122 123 124 125 126 127 128 129 130
    settings.endGroup();
}

void MAVLinkProtocol::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
    settings.setValue("HEARTBEATS_ENABLED", m_heartbeatsEnabled);
    settings.setValue("LOGGING_ENABLED", m_loggingEnabled);
131 132
    settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
    settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled);
133
    settings.setValue("GCS_SYSTEM_ID", systemId);
134 135
    settings.setValue("GCS_AUTH_KEY", m_authKey);
    settings.setValue("GCS_AUTH_ENABLED", m_authEnabled);
136 137
    if (m_logfile)
    {
138 139 140
        // Logfile exists, store the name
        settings.setValue("LOGFILE_NAME", m_logfile->fileName());
    }
141 142 143 144
    // 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);
145 146 147 148 149
    settings.endGroup();
    settings.sync();
    //qDebug() << "Storing settings!";
}

pixhawk's avatar
pixhawk committed
150 151
MAVLinkProtocol::~MAVLinkProtocol()
{
152
    storeSettings();
153 154 155 156
    if (m_logfile)
    {
        if (m_logfile->isOpen())
        {
157 158 159
            m_logfile->flush();
            m_logfile->close();
        }
pixhawk's avatar
pixhawk committed
160
        delete m_logfile;
161
        m_logfile = NULL;
pixhawk's avatar
pixhawk committed
162
    }
Lorenz Meier's avatar
Lorenz Meier committed
163 164

    // Tell the thread to exit
165
    _should_exit = true;
Lorenz Meier's avatar
Lorenz Meier committed
166 167 168 169 170 171 172 173 174 175
    // Wait for it to exit
    wait();
}

/**
 * @brief Runs the thread
 *
 **/
void MAVLinkProtocol::run()
{
176 177 178 179 180 181 182 183 184
    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()) {
185
            delete heartbeatTimer;
186 187 188 189 190 191 192
            qDebug() << "MAVLINK WORKER DONE!";
            return;
        }

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

pixhawk's avatar
pixhawk committed
195 196
QString MAVLinkProtocol::getLogfileName()
{
197 198
    if (m_logfile)
    {
199
        return m_logfile->fileName();
200 201 202
    }
    else
    {
203
        return QStandardPaths::writableLocation(QStandardPaths::HomeLocation) + "/qgroundcontrol_packetlog.mavlink";
204
    }
pixhawk's avatar
pixhawk committed
205 206
}

207 208 209 210 211 212 213 214 215 216
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;
}

217 218 219 220 221 222 223 224 225 226
void MAVLinkProtocol::linkStatusChanged(bool connected)
{
    LinkInterface* link = qobject_cast<LinkInterface*>(QObject::sender());

    if (link) {
        if (connected) {
            // Send command to start MAVLink
            // XXX hacky but safe
            // Start NSH
            const char init[] = {0x0d, 0x0d, 0x0d};
Lorenz Meier's avatar
Lorenz Meier committed
227
            link->writeBytes(init, sizeof(init));
228
            const char* cmd = "sh /etc/init.d/rc.usb\n";
229 230 231 232 233 234
            link->writeBytes(cmd, strlen(cmd));
            link->writeBytes(init, 4);
        }
    }
}

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

248 249 250
    // Cache the link ID for common use.
    int linkId = link->getId();

251
    static int mavlink09Count = 0;
252
    static int nonmavlinkCount = 0;
253
    static bool decodedFirstPacket = false;
254
    static bool warnedUser = false;
255 256
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
257

258
    // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
259
    for (int position = 0; position < b.size(); position++) {
260
        unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
261 262

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

271 272 273
        if (decodeState == 0 && !decodedFirstPacket)
        {
            nonmavlinkCount++;
274
            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
275
            {
276
                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
277 278 279 280 281 282 283 284 285 286 287 288
                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.");
                }
            }
        }
289 290
        if (decodeState == 1)
        {
291
            decodedFirstPacket = true;
292 293 294 295 296 297 298 299 300 301 302 303 304 305

            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);
                }
            }

306 307 308 309 310 311 312 313 314 315
            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
316
            // Log data
317 318
            if (m_loggingEnabled && m_logfile)
            {
319 320 321 322 323 324 325 326 327 328 329 330 331 332 333
                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.
334
                QByteArray b((const char*)buf, len);
Lorenz Meier's avatar
Lorenz Meier committed
335
                if(m_logfile->write(b) != len)
336
                {
337
                    // If there's an error logging data, raise an alert and stop logging.
338 339 340
                    emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName()));
                    enableLogging(false);
                }
pixhawk's avatar
pixhawk committed
341 342
            }

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

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

            // Check and (if necessary) create UAS object
350 351
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
pixhawk's avatar
pixhawk committed
352 353 354 355 356 357
                // 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
358
                // Check if the UAS has the same id like this system
359 360
                if (message.sysid == getSystemId())
                {
361
                    emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
362 363
                }

364 365 366 367
                // 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
368
                // First create new UAS object
369 370
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
pixhawk's avatar
pixhawk committed
371 372
                // Reset version field to 0
                heartbeat.mavlink_version = 0;
373
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
pixhawk's avatar
pixhawk committed
374 375

                // Check if the UAS has a different protocol version
376 377
                if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
                {
pixhawk's avatar
pixhawk committed
378
                    // Bring up dialog to inform user
379 380
                    if (!versionMismatchIgnore)
                    {
381
                        emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
382
                                                   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));
383 384
                        versionMismatchIgnore = true;
                    }
pixhawk's avatar
pixhawk committed
385 386 387 388 389

                    // Ignore this message and continue gracefully
                    continue;
                }

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

pixhawk's avatar
pixhawk committed
393
            }
394

395 396 397
            // Increase receive counter
            totalReceiveCounter[linkId]++;
            currReceiveCounter[linkId]++;
398

399 400 401 402
            // 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);
403

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

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

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

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

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

425 426 427 428 429 430 431 432 433 434 435
            // 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);
            }
436

437 438 439 440
            // 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
441

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

448 449
                // Emit message on all links that are currently connected
                foreach (LinkInterface* currLink, links)
450
                {
451 452 453
                    // 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);
454
                }
455
            }
pixhawk's avatar
pixhawk committed
456 457 458 459 460 461 462 463 464 465 466 467
        }
    }
}

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

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

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
477 478 479
}

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

pixhawk's avatar
pixhawk committed
485 486 487 488 489 490 491 492 493 494
/**
 * @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;
495 496
    for (i = links.begin(); i != links.end(); ++i)
    {
pixhawk's avatar
pixhawk committed
497
        sendMessage(*i, message);
498
//        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
499 500 501 502 503 504 505 506 507 508
    }
}

/**
 * @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
509
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
510
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
511 512
    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
513
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
514
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
515
    // If link is connected
516 517
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
518 519 520 521 522
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545
/**
 * @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
546 547 548 549 550 551 552
/**
 * 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
553 554
    if (m_heartbeatsEnabled)
    {
pixhawk's avatar
pixhawk committed
555
        mavlink_message_t beat;
lm's avatar
lm committed
556
        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
557 558
        sendMessage(beat);
    }
Lorenz Meier's avatar
Lorenz Meier committed
559 560
    if (m_authEnabled)
    {
James Goppert's avatar
James Goppert committed
561 562
        mavlink_message_t msg;
        mavlink_auth_key_t auth;
563
        memset(&auth, 0, sizeof(auth));
564
        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
565 566 567
        mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth);
        sendMessage(msg);
    }
pixhawk's avatar
pixhawk committed
568 569 570 571 572 573 574 575 576
}

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

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

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

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

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

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

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

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

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

lm's avatar
lm committed
636 637
void MAVLinkProtocol::enableLogging(bool enabled)
{
638 639 640
    bool changed = false;
    if (enabled != m_loggingEnabled) changed = true;

641 642 643 644
    if (enabled)
    {
        if (m_logfile && m_logfile->isOpen())
        {
645 646 647
            m_logfile->flush();
            m_logfile->close();
        }
648 649 650 651 652

        if (m_logfile)
        {
            if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
            {
653 654
                emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file. Stopping logging.").arg(m_logfile->fileName()));
                m_loggingEnabled = false;
655
            }
656
        }
657 658 659 660 661 662 663 664 665 666 667
        else
        {
            emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot start logging, no logfile selected."));
        }
    }
    else if (!enabled)
    {
        if (m_logfile)
        {
            if (m_logfile->isOpen())
            {
668 669 670
                m_logfile->flush();
                m_logfile->close();
            }
671
        }
lm's avatar
lm committed
672 673
    }
    m_loggingEnabled = enabled;
674
    if (changed) emit loggingChanged(enabled);
675 676 677 678
}

void MAVLinkProtocol::setLogfileName(const QString& filename)
{
679 680
    if (!m_logfile)
    {
681
        m_logfile = new QFile(filename);
682 683 684
    }
    else
    {
685 686 687
        m_logfile->flush();
        m_logfile->close();
    }
688
    m_logfile->setFileName(filename);
689
    enableLogging(m_loggingEnabled);
lm's avatar
lm committed
690 691
}

lm's avatar
lm committed
692 693 694
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
695
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
696 697
}

pixhawk's avatar
pixhawk committed
698 699 700 701 702 703 704 705
/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
706
    heartbeatTimer->setInterval(1000/heartbeatRate);
pixhawk's avatar
pixhawk committed
707 708 709 710 711 712 713
}

/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
    return heartbeatRate;
}