MAVLinkProtocol.cc 25.3 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 18
#include <QSettings>
#include <QDesktopServices>
pixhawk's avatar
pixhawk committed
19 20 21 22 23 24

#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
25 26
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
27
#include "ArduPilotMegaMAV.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 35 36 37
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif

38

pixhawk's avatar
pixhawk committed
39 40 41 42 43
/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
44 45 46 47 48 49 50 51 52 53 54 55 56 57 58
    heartbeatTimer(new QTimer(this)),
    heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
    m_heartbeatsEnabled(false),
    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),
    systemId(QGC::defaultSystemId)
pixhawk's avatar
pixhawk committed
59
{
60
    m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
61 62
    loadSettings();
    //start(QThread::LowPriority);
pixhawk's avatar
pixhawk committed
63 64 65
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);
lm's avatar
lm committed
66 67
    totalReceiveCounter = 0;
    totalLossCounter = 0;
68 69
    currReceiveCounter = 0;
    currLossCounter = 0;
70 71 72 73
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
lm's avatar
lm committed
74 75 76
            lastIndex[i][j] = -1;
        }
    }
lm's avatar
lm committed
77 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 99
        m_logfile = new QFile(QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink");
    }
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
    }
pixhawk's avatar
pixhawk committed
163 164
}

pixhawk's avatar
pixhawk committed
165 166
QString MAVLinkProtocol::getLogfileName()
{
167 168
    if (m_logfile)
    {
169
        return m_logfile->fileName();
170 171 172
    }
    else
    {
173
        return QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink";
174
    }
pixhawk's avatar
pixhawk committed
175 176
}

177 178 179 180 181 182 183 184 185 186
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
187
            link->writeBytes(init, sizeof(init));
188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203
            QGC::SLEEP::msleep(500);

            // Stop any running mavlink instance
            char* cmd = "mavlink stop\n";
            link->writeBytes(cmd, strlen(cmd));
            link->writeBytes(init, 2);
            cmd = "uorb start";
            link->writeBytes(cmd, strlen(cmd));
            link->writeBytes(init, 2);
            cmd = "sh /etc/init.d/rc.usb\n";
            link->writeBytes(cmd, strlen(cmd));
            link->writeBytes(init, 4);
        }
    }
}

pixhawk's avatar
pixhawk committed
204 205 206 207 208 209 210 211
/**
 * The bytes are copied by calling the LinkInterface::readBytes() method.
 * 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
 **/
212
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
213
{
214
//    receiveMutex.lock();
pixhawk's avatar
pixhawk committed
215 216
    mavlink_message_t message;
    mavlink_status_t status;
217

218 219
    static int mavlink09Count = 0;
    static bool decodedFirstPacket = false;
220
    static bool warnedUser = false;
221

222
    for (int position = 0; position < b.size(); position++) {
223 224 225
        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b[position]), &message, &status);

        if ((uint8_t)b[position] == 0x55) mavlink09Count++;
226
        if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
227
        {
228
            warnedUser = true;
229 230
            // Obviously the user tries to use a 0.9 autopilot
            // with QGroundControl built for version 1.0
231
            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.");
232
        }
pixhawk's avatar
pixhawk committed
233

234 235
        if (decodeState == 1)
        {
236
            decodedFirstPacket = true;
237
#if defined(QGC_PROTOBUF_ENABLED)
LM's avatar
LM committed
238

239 240
            if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
            {
241 242 243 244 245 246
                mavlink_extended_message_t extended_message;

                extended_message.base_msg = message;

                // read extended header
                uint8_t* payload = reinterpret_cast<uint8_t*>(message.payload64);
LM's avatar
LM committed
247

248 249
                memcpy(&extended_message.extended_payload_len, payload + 3, 4);

LM's avatar
LM committed
250 251 252 253 254 255 256 257 258
                // Check if message is valid
                if
                 (b.size() != MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_EXTENDED_HEADER_LEN+ extended_message.extended_payload_len)
                {
                    //invalid message
                    qDebug() << "GOT INVALID EXTENDED MESSAGE, ABORTING";
                    return;
                }

259 260 261 262 263
                const uint8_t* extended_payload = reinterpret_cast<const uint8_t*>(b.constData()) + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_EXTENDED_HEADER_LEN;

                // copy extended payload data
                memcpy(extended_message.extended_payload, extended_payload, extended_message.extended_payload_len);

264 265
#if defined(QGC_USE_PIXHAWK_MESSAGES)

266 267 268 269 270 271
                if (protobufManager.cacheFragment(extended_message))
                {
                    std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;

                    if (protobufManager.getMessage(protobuf_msg))
                    {
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
                        const google::protobuf::Descriptor* descriptor = protobuf_msg->GetDescriptor();
                        if (!descriptor)
                        {
                            continue;
                        }

                        const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
                        if (!headerField)
                        {
                            continue;
                        }

                        const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
                        if (!headerDescriptor)
                        {
                            continue;
                        }

                        const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
                        if (!sourceSysIdField)
                        {
                            continue;
                        }

                        const google::protobuf::Reflection* reflection = protobuf_msg->GetReflection();
                        const google::protobuf::Message& headerMsg = reflection->GetMessage(*protobuf_msg, headerField);
                        const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();

                        int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);

                        UASInterface* uas = UASManager::instance()->getUASForId(source_sysid);

                        if (uas != NULL)
                        {
                            emit extendedMessageReceived(link, protobuf_msg);
                        }
308 309
                    }
                }
310
#endif
311 312 313 314

                position += extended_message.extended_payload_len;

                continue;
315 316 317
            }
#endif

pixhawk's avatar
pixhawk committed
318
            // Log data
319 320
            if (m_loggingEnabled && m_logfile)
            {
321
                uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)] = {0};
322
                quint64 time = QGC::groundTimeUsecs();
323
                memcpy(buf, (void*)&time, sizeof(quint64));
324
                // Write message to buffer
325 326 327 328
                mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
                //we need to write the maximum package length for having a
                //consistent file structure and beeing able to parse it again
                int len = MAVLINK_MAX_PACKET_LEN + sizeof(quint64);
329
                QByteArray b((const char*)buf, len);
Lorenz Meier's avatar
Lorenz Meier committed
330
                if(m_logfile->write(b) != len)
331
                {
332 333 334 335
                    emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName()));
                    // Stop logging
                    enableLogging(false);
                }
pixhawk's avatar
pixhawk committed
336 337
            }

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

pixhawk's avatar
pixhawk committed
342 343 344
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

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

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

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

                    // Ignore this message and continue gracefully
                    continue;
                }

385 386
                // Create a new UAS object
                uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
pixhawk's avatar
pixhawk committed
387
            }
388 389

            // Only count message if UAS exists for this message
390 391
            if (uas != NULL)
            {
392

393 394 395
                // Increase receive counter
                totalReceiveCounter++;
                currReceiveCounter++;
396 397 398

                // Update last message sequence ID
                uint8_t expectedIndex;
399
                if (lastIndex[message.sysid][message.compid] == -1)
400
                {
401
                    lastIndex[message.sysid][message.compid] = message.seq;
402
                    expectedIndex = message.seq;
403
                }
404 405
                else
                {
406
                    // NOTE: Using uint8_t here auto-wraps the number around to 0.
407
                    expectedIndex = lastIndex[message.sysid][message.compid] + 1;
408 409 410
                }

                // Make some noise if a message was skipped
411
                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
412 413 414 415 416 417 418 419 420 421 422
                if (message.seq != expectedIndex)
                {
                    // Determine how many messages were skipped accounting for 0-wraparound
                    int16_t lostMessages = message.seq - expectedIndex; 
                    if (lostMessages < 0)
                    {
                        // Usually, this happens in the case of an out-of order packet
                        lostMessages = 0;
                    }
                    else
                    {
LM's avatar
LM committed
423 424
                        // Console generates excessive load at high loss rates, needs better GUI visualization
                        //qDebug() << QString("Lost %1 messages for comp %4: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq).arg(message.compid);
425 426 427 428
                    }
                    totalLossCounter += lostMessages;
                    currLossCounter += lostMessages;
                }
429 430 431

                // Update the last sequence ID
                lastIndex[message.sysid][message.compid] = message.seq;
432

433 434
                // Update on every 32th packet
                if (totalReceiveCounter % 32 == 0)
435
                {
436 437 438 439 440 441
                    // Calculate new loss ratio
                    // Receive loss
                    float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                    receiveLoss *= 100.0f;
                    currLossCounter = 0;
                    currReceiveCounter = 0;
pixhawk's avatar
pixhawk committed
442
                    emit receiveLossChanged(message.sysid, receiveLoss);
lm's avatar
lm committed
443 444
                }

445 446 447 448
                // 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);
449 450

                // Multiplex message if enabled
451 452
                if (m_multiplexingEnabled)
                {
453 454 455 456
                    // Get all links connected to this unit
                    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

                    // Emit message on all links that are currently connected
457 458
                    foreach (LinkInterface* currLink, links)
                    {
459 460
                        // Only forward this message to the other links,
                        // not the link the message was received on
Lorenz Meier's avatar
Lorenz Meier committed
461
                        if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
462 463
                    }
                }
464
            }
pixhawk's avatar
pixhawk committed
465 466 467 468 469 470 471 472 473 474 475 476
        }
    }
}

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

477
/** @return System id of this application */
478
int MAVLinkProtocol::getSystemId()
479
{
480 481 482 483 484 485
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
486 487 488
}

/** @return Component id of this application */
489
int MAVLinkProtocol::getComponentId()
490
{
491
    return QGC::defaultComponentId;
492 493
}

pixhawk's avatar
pixhawk committed
494 495 496 497 498 499 500 501 502 503
/**
 * @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;
504 505
    for (i = links.begin(); i != links.end(); ++i)
    {
pixhawk's avatar
pixhawk committed
506
        sendMessage(*i, message);
Lorenz Meier's avatar
Lorenz Meier committed
507
        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
508 509 510 511 512 513 514 515 516 517
    }
}

/**
 * @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
518
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
519
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
520 521
    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
522
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
523
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
524
    // If link is connected
525 526
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
527 528 529 530 531
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

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

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

586 587 588 589 590 591 592 593 594
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
    bool changed = false;
    if (enabled != m_multiplexingEnabled) changed = true;

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

595 596 597 598
void MAVLinkProtocol::enableAuth(bool enable)
{
    bool changed = false;
    m_authEnabled = enable;
599
    if (m_authEnabled != enable) {
600 601 602 603 604
        changed = true;
    }
    if (changed) emit authChanged(m_authEnabled);
}

605 606
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
607
    if (enabled != m_paramGuardEnabled) {
608 609 610 611 612
        m_paramGuardEnabled = enabled;
        emit paramGuardChanged(m_paramGuardEnabled);
    }
}

613 614
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
615
    if (enabled != m_actionGuardEnabled) {
616 617 618 619 620
        m_actionGuardEnabled = enabled;
        emit actionGuardChanged(m_actionGuardEnabled);
    }
}

621 622
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
623
    if (ms != m_paramRetransmissionTimeout) {
624 625 626 627 628 629 630
        m_paramRetransmissionTimeout = ms;
        emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
    }
}

void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
631
    if (ms != m_paramRewriteTimeout) {
632 633 634 635 636
        m_paramRewriteTimeout = ms;
        emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
    }
}

637 638
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
639
    if (ms != m_actionRetransmissionTimeout) {
640 641 642 643 644
        m_actionRetransmissionTimeout = ms;
        emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
    }
}

lm's avatar
lm committed
645 646
void MAVLinkProtocol::enableLogging(bool enabled)
{
647 648 649
    bool changed = false;
    if (enabled != m_loggingEnabled) changed = true;

650 651 652 653
    if (enabled)
    {
        if (m_logfile && m_logfile->isOpen())
        {
654 655 656
            m_logfile->flush();
            m_logfile->close();
        }
657 658 659 660 661

        if (m_logfile)
        {
            if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
            {
662 663
                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;
664
            }
665
        }
666 667 668 669 670 671 672 673 674 675 676
        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())
            {
677 678 679
                m_logfile->flush();
                m_logfile->close();
            }
680
        }
lm's avatar
lm committed
681 682
    }
    m_loggingEnabled = enabled;
683
    if (changed) emit loggingChanged(enabled);
684 685 686 687
}

void MAVLinkProtocol::setLogfileName(const QString& filename)
{
688 689
    if (!m_logfile)
    {
690
        m_logfile = new QFile(filename);
691 692 693
    }
    else
    {
694 695 696
        m_logfile->flush();
        m_logfile->close();
    }
697
    m_logfile->setFileName(filename);
698
    enableLogging(m_loggingEnabled);
lm's avatar
lm committed
699 700
}

lm's avatar
lm committed
701 702 703
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
704
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
705 706
}

pixhawk's avatar
pixhawk committed
707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722
/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
    heartbeatTimer->setInterval(1000/heartbeatRate);
}

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