MAVLinkProtocol.cc 27.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>
19
#include <QtEndian>
pixhawk's avatar
pixhawk committed
20 21 22 23 24 25

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

35 36 37 38
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif

39

pixhawk's avatar
pixhawk committed
40 41 42 43 44
/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
    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
60
{
61
    m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
62 63
    loadSettings();
    //start(QThread::LowPriority);
pixhawk's avatar
pixhawk committed
64 65 66
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);
67 68 69 70 71

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

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

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

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

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

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

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

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

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

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

189 190 191 192 193 194 195 196 197 198
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
199
            link->writeBytes(init, sizeof(init));
200
            const char* cmd = "sh /etc/init.d/rc.usb\n";
201 202 203 204 205 206
            link->writeBytes(cmd, strlen(cmd));
            link->writeBytes(init, 4);
        }
    }
}

pixhawk's avatar
pixhawk committed
207 208 209 210 211 212 213
/**
 * 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
 **/
214
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
215
{
216
//    receiveMutex.lock();
pixhawk's avatar
pixhawk committed
217 218
    mavlink_message_t message;
    mavlink_status_t status;
219

220 221 222
    // Cache the link ID for common use.
    int linkId = link->getId();

223
    static int mavlink09Count = 0;
224
    static int nonmavlinkCount = 0;
225
    static bool decodedFirstPacket = false;
226
    static bool warnedUser = false;
227 228
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
229

230
    // FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
231
    for (int position = 0; position < b.size(); position++) {
232
        unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
233 234

        if ((uint8_t)b[position] == 0x55) mavlink09Count++;
235
        if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
236
        {
237
            warnedUser = true;
238 239
            // Obviously the user tries to use a 0.9 autopilot
            // with QGroundControl built for version 1.0
240
            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.");
241
        }
pixhawk's avatar
pixhawk committed
242

243 244 245
        if (decodeState == 0 && !decodedFirstPacket)
        {
            nonmavlinkCount++;
246
            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
247
            {
248
                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
249 250 251 252 253 254 255 256 257 258 259 260
                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.");
                }
            }
        }
261 262
        if (decodeState == 1)
        {
263
            decodedFirstPacket = true;
264 265 266 267 268 269 270 271 272 273 274 275 276 277

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

278
#if defined(QGC_PROTOBUF_ENABLED)
LM's avatar
LM committed
279

280 281
            if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
            {
282 283 284 285 286 287
                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
288

289 290
                memcpy(&extended_message.extended_payload_len, payload + 3, 4);

LM's avatar
LM committed
291 292 293 294 295 296 297 298 299
                // 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;
                }

300 301 302 303 304
                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);

305 306
#if defined(QGC_USE_PIXHAWK_MESSAGES)

307 308 309 310 311 312
                if (protobufManager.cacheFragment(extended_message))
                {
                    std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;

                    if (protobufManager.getMessage(protobuf_msg))
                    {
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 342 343 344 345 346 347 348
                        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);
                        }
349 350
                    }
                }
351
#endif
352 353 354 355

                position += extended_message.extended_payload_len;

                continue;
356 357 358
            }
#endif

pixhawk's avatar
pixhawk committed
359
            // Log data
360 361
            if (m_loggingEnabled && m_logfile)
            {
362 363 364 365 366 367 368 369 370 371 372 373 374 375 376
                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.
377
                QByteArray b((const char*)buf, len);
Lorenz Meier's avatar
Lorenz Meier committed
378
                if(m_logfile->write(b) != len)
379
                {
380
                    // If there's an error logging data, raise an alert and stop logging.
381 382 383
                    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
384 385
            }

pixhawk's avatar
pixhawk committed
386 387 388
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
389

pixhawk's avatar
pixhawk committed
390 391 392
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
393 394
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
pixhawk's avatar
pixhawk committed
395 396 397 398 399 400
                // 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
401
                // Check if the UAS has the same id like this system
402 403
                if (message.sysid == getSystemId())
                {
404
                    emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
405 406
                }

407 408 409 410
                // 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
411
                // First create new UAS object
412 413
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
pixhawk's avatar
pixhawk committed
414 415
                // Reset version field to 0
                heartbeat.mavlink_version = 0;
416
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
pixhawk's avatar
pixhawk committed
417 418

                // Check if the UAS has a different protocol version
419 420
                if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
                {
pixhawk's avatar
pixhawk committed
421
                    // Bring up dialog to inform user
422 423
                    if (!versionMismatchIgnore)
                    {
424
                        emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
425
                                                   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));
426 427
                        versionMismatchIgnore = true;
                    }
pixhawk's avatar
pixhawk committed
428 429 430 431 432

                    // Ignore this message and continue gracefully
                    continue;
                }

433 434
                // Create a new UAS object
                uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
435

pixhawk's avatar
pixhawk committed
436
            }
437 438

            // Only count message if UAS exists for this message
439 440
            if (uas != NULL)
            {
441

442
                // Increase receive counter
443 444
                totalReceiveCounter[linkId]++;
                currReceiveCounter[linkId]++;
445 446 447

                // Update last message sequence ID
                uint8_t expectedIndex;
448
                if (lastIndex[message.sysid][message.compid] == -1)
449
                {
450
                    lastIndex[message.sysid][message.compid] = message.seq;
451
                    expectedIndex = message.seq;
452
                }
453 454
                else
                {
455
                    // NOTE: Using uint8_t here auto-wraps the number around to 0.
456
                    expectedIndex = lastIndex[message.sysid][message.compid] + 1;
457 458 459
                }

                // Make some noise if a message was skipped
460
                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
461 462 463
                if (message.seq != expectedIndex)
                {
                    // Determine how many messages were skipped accounting for 0-wraparound
464
                    int16_t lostMessages = message.seq - expectedIndex;
465 466 467 468 469 470 471
                    if (lostMessages < 0)
                    {
                        // Usually, this happens in the case of an out-of order packet
                        lostMessages = 0;
                    }
                    else
                    {
LM's avatar
LM committed
472 473
                        // 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);
474
                    }
475 476
                    totalLossCounter[linkId] += lostMessages;
                    currLossCounter[linkId] += lostMessages;
477
                }
478 479 480

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

482
                // Update on every 32th packet
483
                if (totalReceiveCounter[linkId] % 32 == 0)
484
                {
485 486
                    // Calculate new loss ratio
                    // Receive loss
487
                    float receiveLoss = (double)currLossCounter[linkId]/(double)(currReceiveCounter[linkId]+currLossCounter[linkId]);
488
                    receiveLoss *= 100.0f;
489 490
                    currLossCounter[linkId] = 0;
                    currReceiveCounter[linkId] = 0;
pixhawk's avatar
pixhawk committed
491
                    emit receiveLossChanged(message.sysid, receiveLoss);
lm's avatar
lm committed
492 493
                }

494 495 496 497
                // 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);
498 499

                // Multiplex message if enabled
500 501
                if (m_multiplexingEnabled)
                {
502 503 504 505
                    // Get all links connected to this unit
                    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

                    // Emit message on all links that are currently connected
506 507
                    foreach (LinkInterface* currLink, links)
                    {
508 509
                        // Only forward this message to the other links,
                        // not the link the message was received on
Lorenz Meier's avatar
Lorenz Meier committed
510
                        if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
511 512
                    }
                }
513
            }
pixhawk's avatar
pixhawk committed
514 515 516 517 518 519 520 521 522 523 524 525
        }
    }
}

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

526
/** @return System id of this application */
527
int MAVLinkProtocol::getSystemId()
528
{
529 530 531 532 533 534
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
535 536 537
}

/** @return Component id of this application */
538
int MAVLinkProtocol::getComponentId()
539
{
540
    return QGC::defaultComponentId;
541 542
}

pixhawk's avatar
pixhawk committed
543 544 545 546 547 548 549 550 551 552
/**
 * @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;
553 554
    for (i = links.begin(); i != links.end(); ++i)
    {
pixhawk's avatar
pixhawk committed
555
        sendMessage(*i, message);
556
//        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
557 558 559 560 561 562 563 564 565 566
    }
}

/**
 * @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
567
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
568
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
569 570
    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
571
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
572
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
573
    // If link is connected
574 575
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
576 577 578 579 580
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603
/**
 * @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
604 605 606 607 608 609 610
/**
 * 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
611 612
    if (m_heartbeatsEnabled)
    {
pixhawk's avatar
pixhawk committed
613
        mavlink_message_t beat;
lm's avatar
lm committed
614
        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
615 616
        sendMessage(beat);
    }
Lorenz Meier's avatar
Lorenz Meier committed
617 618
    if (m_authEnabled)
    {
James Goppert's avatar
James Goppert committed
619 620
        mavlink_message_t msg;
        mavlink_auth_key_t auth;
621
        memset(&auth, 0, sizeof(auth));
622
        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
623 624 625
        mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth);
        sendMessage(msg);
    }
pixhawk's avatar
pixhawk committed
626 627 628 629 630 631 632 633 634
}

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

635 636 637 638 639 640 641 642 643
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
    bool changed = false;
    if (enabled != m_multiplexingEnabled) changed = true;

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

644 645 646 647
void MAVLinkProtocol::enableAuth(bool enable)
{
    bool changed = false;
    m_authEnabled = enable;
648
    if (m_authEnabled != enable) {
649 650 651 652 653
        changed = true;
    }
    if (changed) emit authChanged(m_authEnabled);
}

654 655
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
656
    if (enabled != m_paramGuardEnabled) {
657 658 659 660 661
        m_paramGuardEnabled = enabled;
        emit paramGuardChanged(m_paramGuardEnabled);
    }
}

662 663
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
664
    if (enabled != m_actionGuardEnabled) {
665 666 667 668 669
        m_actionGuardEnabled = enabled;
        emit actionGuardChanged(m_actionGuardEnabled);
    }
}

670 671
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
672
    if (ms != m_paramRetransmissionTimeout) {
673 674 675 676 677 678 679
        m_paramRetransmissionTimeout = ms;
        emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
    }
}

void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
680
    if (ms != m_paramRewriteTimeout) {
681 682 683 684 685
        m_paramRewriteTimeout = ms;
        emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
    }
}

686 687
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
688
    if (ms != m_actionRetransmissionTimeout) {
689 690 691 692 693
        m_actionRetransmissionTimeout = ms;
        emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
    }
}

lm's avatar
lm committed
694 695
void MAVLinkProtocol::enableLogging(bool enabled)
{
696 697 698
    bool changed = false;
    if (enabled != m_loggingEnabled) changed = true;

699 700 701 702
    if (enabled)
    {
        if (m_logfile && m_logfile->isOpen())
        {
703 704 705
            m_logfile->flush();
            m_logfile->close();
        }
706 707 708 709 710

        if (m_logfile)
        {
            if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
            {
711 712
                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;
713
            }
714
        }
715 716 717 718 719 720 721 722 723 724 725
        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())
            {
726 727 728
                m_logfile->flush();
                m_logfile->close();
            }
729
        }
lm's avatar
lm committed
730 731
    }
    m_loggingEnabled = enabled;
732
    if (changed) emit loggingChanged(enabled);
733 734 735 736
}

void MAVLinkProtocol::setLogfileName(const QString& filename)
{
737 738
    if (!m_logfile)
    {
739
        m_logfile = new QFile(filename);
740 741 742
    }
    else
    {
743 744 745
        m_logfile->flush();
        m_logfile->close();
    }
746
    m_logfile->setFileName(filename);
747
    enableLogging(m_loggingEnabled);
lm's avatar
lm committed
748 749
}

lm's avatar
lm committed
750 751 752
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
753
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
754 755
}

pixhawk's avatar
pixhawk committed
756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771
/**
 * 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;
}