MAVLinkProtocol.cc 26.7 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);
66 67 68 69 70

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

    emit versionCheckChanged(m_enable_version_check);
pixhawk's avatar
pixhawk committed
80 81
}

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

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

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

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

115 116 117 118 119 120 121
    // 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();
122 123 124 125 126 127 128 129 130 131
    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);
132 133
    settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
    settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled);
134
    settings.setValue("GCS_SYSTEM_ID", systemId);
135 136
    settings.setValue("GCS_AUTH_KEY", m_authKey);
    settings.setValue("GCS_AUTH_ENABLED", m_authEnabled);
137 138
    if (m_logfile)
    {
139 140 141
        // Logfile exists, store the name
        settings.setValue("LOGFILE_NAME", m_logfile->fileName());
    }
142 143 144 145
    // 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);
146 147 148 149 150
    settings.endGroup();
    settings.sync();
    //qDebug() << "Storing settings!";
}

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

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

178 179 180 181 182 183 184 185 186 187
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;
}

188 189 190 191 192 193 194 195 196 197
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
198
            link->writeBytes(init, sizeof(init));
199 200

            // Stop any running mavlink instance
Bill Bonney's avatar
Bill Bonney committed
201
            const char* cmd = "mavlink stop\n";
202 203 204 205 206 207 208 209 210 211 212 213
            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
214 215 216 217 218 219 220
/**
 * 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
 **/
221
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
222
{
223
//    receiveMutex.lock();
pixhawk's avatar
pixhawk committed
224 225
    mavlink_message_t message;
    mavlink_status_t status;
226

227 228 229
    // Cache the link ID for common use.
    int linkId = link->getId();

230
    static int mavlink09Count = 0;
231
    static int nonmavlinkCount = 0;
232
    static bool decodedFirstPacket = false;
233
    static bool warnedUser = false;
234 235
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
236

237
    // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
238
    for (int position = 0; position < b.size(); position++) {
239
        unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
240 241

        if ((uint8_t)b[position] == 0x55) mavlink09Count++;
242
        if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
243
        {
244
            warnedUser = true;
245 246
            // Obviously the user tries to use a 0.9 autopilot
            // with QGroundControl built for version 1.0
247
            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.");
248
        }
pixhawk's avatar
pixhawk committed
249

250 251 252
        if (decodeState == 0 && !decodedFirstPacket)
        {
            nonmavlinkCount++;
253
            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
254
            {
255
                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
256 257 258 259 260 261 262 263 264 265 266 267
                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.");
                }
            }
        }
268 269
        if (decodeState == 1)
        {
270
            decodedFirstPacket = true;
271
#if defined(QGC_PROTOBUF_ENABLED)
LM's avatar
LM committed
272

273 274
            if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
            {
275 276 277 278 279 280
                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
281

282 283
                memcpy(&extended_message.extended_payload_len, payload + 3, 4);

LM's avatar
LM committed
284 285 286 287 288 289 290 291 292
                // 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;
                }

293 294 295 296 297
                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);

298 299
#if defined(QGC_USE_PIXHAWK_MESSAGES)

300 301 302 303 304 305
                if (protobufManager.cacheFragment(extended_message))
                {
                    std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;

                    if (protobufManager.getMessage(protobuf_msg))
                    {
306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341
                        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);
                        }
342 343
                    }
                }
344
#endif
345 346 347 348

                position += extended_message.extended_payload_len;

                continue;
349 350 351
            }
#endif

pixhawk's avatar
pixhawk committed
352
            // Log data
353 354
            if (m_loggingEnabled && m_logfile)
            {
355
                uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)] = {0};
356
                quint64 time = QGC::groundTimeUsecs();
357
                memcpy(buf, (void*)&time, sizeof(quint64));
358
                // Write message to buffer
359 360 361 362
                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);
363
                QByteArray b((const char*)buf, len);
Lorenz Meier's avatar
Lorenz Meier committed
364
                if(m_logfile->write(b) != len)
365
                {
366 367 368 369
                    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
370 371
            }

pixhawk's avatar
pixhawk committed
372 373 374
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
375

pixhawk's avatar
pixhawk committed
376 377 378
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
379 380
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
pixhawk's avatar
pixhawk committed
381 382 383 384 385 386
                // 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
387
                // Check if the UAS has the same id like this system
388 389
                if (message.sysid == getSystemId())
                {
390
                    emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
391 392
                }

393 394 395 396
                // 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
397
                // First create new UAS object
398 399
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
pixhawk's avatar
pixhawk committed
400 401
                // Reset version field to 0
                heartbeat.mavlink_version = 0;
402
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
pixhawk's avatar
pixhawk committed
403 404

                // Check if the UAS has a different protocol version
405 406
                if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
                {
pixhawk's avatar
pixhawk committed
407
                    // Bring up dialog to inform user
408 409
                    if (!versionMismatchIgnore)
                    {
410
                        emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
411
                                                   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));
412 413
                        versionMismatchIgnore = true;
                    }
pixhawk's avatar
pixhawk committed
414 415 416 417 418

                    // Ignore this message and continue gracefully
                    continue;
                }

419 420
                // Create a new UAS object
                uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
421

pixhawk's avatar
pixhawk committed
422
            }
423 424

            // Only count message if UAS exists for this message
425 426
            if (uas != NULL)
            {
427

428
                // Increase receive counter
429 430
                totalReceiveCounter[linkId]++;
                currReceiveCounter[linkId]++;
431 432 433

                // Update last message sequence ID
                uint8_t expectedIndex;
434
                if (lastIndex[message.sysid][message.compid] == -1)
435
                {
436
                    lastIndex[message.sysid][message.compid] = message.seq;
437
                    expectedIndex = message.seq;
438
                }
439 440
                else
                {
441
                    // NOTE: Using uint8_t here auto-wraps the number around to 0.
442
                    expectedIndex = lastIndex[message.sysid][message.compid] + 1;
443 444 445
                }

                // Make some noise if a message was skipped
446
                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
447 448 449
                if (message.seq != expectedIndex)
                {
                    // Determine how many messages were skipped accounting for 0-wraparound
450
                    int16_t lostMessages = message.seq - expectedIndex;
451 452 453 454 455 456 457
                    if (lostMessages < 0)
                    {
                        // Usually, this happens in the case of an out-of order packet
                        lostMessages = 0;
                    }
                    else
                    {
LM's avatar
LM committed
458 459
                        // 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);
460
                    }
461 462
                    totalLossCounter[linkId] += lostMessages;
                    currLossCounter[linkId] += lostMessages;
463
                }
464 465 466

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

468
                // Update on every 32th packet
469
                if (totalReceiveCounter[linkId] % 32 == 0)
470
                {
471 472
                    // Calculate new loss ratio
                    // Receive loss
473
                    float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]);
474
                    receiveLoss *= 100.0f;
475 476
                    currLossCounter[linkId] = 0;
                    currReceiveCounter[linkId] = 0;
pixhawk's avatar
pixhawk committed
477
                    emit receiveLossChanged(message.sysid, receiveLoss);
lm's avatar
lm committed
478 479
                }

480 481 482 483
                // 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);
484 485

                // Multiplex message if enabled
486 487
                if (m_multiplexingEnabled)
                {
488 489 490 491
                    // Get all links connected to this unit
                    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

                    // Emit message on all links that are currently connected
492 493
                    foreach (LinkInterface* currLink, links)
                    {
494 495
                        // Only forward this message to the other links,
                        // not the link the message was received on
Lorenz Meier's avatar
Lorenz Meier committed
496
                        if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
497 498
                    }
                }
499
            }
pixhawk's avatar
pixhawk committed
500 501 502 503 504 505 506 507 508 509 510 511
        }
    }
}

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

512
/** @return System id of this application */
513
int MAVLinkProtocol::getSystemId()
514
{
515 516 517 518 519 520
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
521 522 523
}

/** @return Component id of this application */
524
int MAVLinkProtocol::getComponentId()
525
{
526
    return QGC::defaultComponentId;
527 528
}

pixhawk's avatar
pixhawk committed
529 530 531 532 533 534 535 536 537 538
/**
 * @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;
539 540
    for (i = links.begin(); i != links.end(); ++i)
    {
pixhawk's avatar
pixhawk committed
541
        sendMessage(*i, message);
Lorenz Meier's avatar
Lorenz Meier committed
542
        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
543 544 545 546 547 548 549 550 551 552
    }
}

/**
 * @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
553
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
554
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
555 556
    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
557
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
558
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
559
    // If link is connected
560 561
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
562 563 564 565 566
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589
/**
 * @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
590 591 592 593 594 595 596
/**
 * 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
597 598
    if (m_heartbeatsEnabled)
    {
pixhawk's avatar
pixhawk committed
599
        mavlink_message_t beat;
lm's avatar
lm committed
600
        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
601 602
        sendMessage(beat);
    }
Lorenz Meier's avatar
Lorenz Meier committed
603 604
    if (m_authEnabled)
    {
James Goppert's avatar
James Goppert committed
605 606
        mavlink_message_t msg;
        mavlink_auth_key_t auth;
607
        memset(&auth, 0, sizeof(auth));
608
        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
609 610 611
        mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth);
        sendMessage(msg);
    }
pixhawk's avatar
pixhawk committed
612 613 614 615 616 617 618 619 620
}

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

621 622 623 624 625 626 627 628 629
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
    bool changed = false;
    if (enabled != m_multiplexingEnabled) changed = true;

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

630 631 632 633
void MAVLinkProtocol::enableAuth(bool enable)
{
    bool changed = false;
    m_authEnabled = enable;
634
    if (m_authEnabled != enable) {
635 636 637 638 639
        changed = true;
    }
    if (changed) emit authChanged(m_authEnabled);
}

640 641
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
642
    if (enabled != m_paramGuardEnabled) {
643 644 645 646 647
        m_paramGuardEnabled = enabled;
        emit paramGuardChanged(m_paramGuardEnabled);
    }
}

648 649
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
650
    if (enabled != m_actionGuardEnabled) {
651 652 653 654 655
        m_actionGuardEnabled = enabled;
        emit actionGuardChanged(m_actionGuardEnabled);
    }
}

656 657
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
658
    if (ms != m_paramRetransmissionTimeout) {
659 660 661 662 663 664 665
        m_paramRetransmissionTimeout = ms;
        emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
    }
}

void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
666
    if (ms != m_paramRewriteTimeout) {
667 668 669 670 671
        m_paramRewriteTimeout = ms;
        emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
    }
}

672 673
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
674
    if (ms != m_actionRetransmissionTimeout) {
675 676 677 678 679
        m_actionRetransmissionTimeout = ms;
        emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
    }
}

lm's avatar
lm committed
680 681
void MAVLinkProtocol::enableLogging(bool enabled)
{
682 683 684
    bool changed = false;
    if (enabled != m_loggingEnabled) changed = true;

685 686 687 688
    if (enabled)
    {
        if (m_logfile && m_logfile->isOpen())
        {
689 690 691
            m_logfile->flush();
            m_logfile->close();
        }
692 693 694 695 696

        if (m_logfile)
        {
            if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
            {
697 698
                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;
699
            }
700
        }
701 702 703 704 705 706 707 708 709 710 711
        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())
            {
712 713 714
                m_logfile->flush();
                m_logfile->close();
            }
715
        }
lm's avatar
lm committed
716 717
    }
    m_loggingEnabled = enabled;
718
    if (changed) emit loggingChanged(enabled);
719 720 721 722
}

void MAVLinkProtocol::setLogfileName(const QString& filename)
{
723 724
    if (!m_logfile)
    {
725
        m_logfile = new QFile(filename);
726 727 728
    }
    else
    {
729 730 731
        m_logfile->flush();
        m_logfile->close();
    }
732
    m_logfile->setFileName(filename);
733
    enableLogging(m_loggingEnabled);
lm's avatar
lm committed
734 735
}

lm's avatar
lm committed
736 737 738
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
739
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
740 741
}

pixhawk's avatar
pixhawk committed
742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757
/**
 * 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;
}