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

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

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

#include <QDebug>
#include <QTime>
lm's avatar
lm committed
15
#include <QApplication>
pixhawk's avatar
pixhawk committed
16
#include <QMessageBox>
17 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

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

217
    static int mavlink09Count = 0;
218
    static int nonmavlinkCount = 0;
219
    static bool decodedFirstPacket = false;
220
    static bool warnedUser = false;
221 222
    static bool checkedUserNonMavlink = false;
    static bool warnedUserNonMavlink = false;
223

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

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

236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253
        if (decodeState == 0 && !decodedFirstPacket)
        {
            nonmavlinkCount++;
            if (nonmavlinkCount > 500 && !warnedUserNonMavlink)
            {
                //500 bytes with no mavlink message. Are we connected to a mavlink capable device?
                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.");
                }
            }
        }
254 255
        if (decodeState == 1)
        {
256
            decodedFirstPacket = true;
257
#if defined(QGC_PROTOBUF_ENABLED)
LM's avatar
LM committed
258

259 260
            if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
            {
261 262 263 264 265 266
                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
267

268 269
                memcpy(&extended_message.extended_payload_len, payload + 3, 4);

LM's avatar
LM committed
270 271 272 273 274 275 276 277 278
                // 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;
                }

279 280 281 282 283
                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);

284 285
#if defined(QGC_USE_PIXHAWK_MESSAGES)

286 287 288 289 290 291
                if (protobufManager.cacheFragment(extended_message))
                {
                    std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;

                    if (protobufManager.getMessage(protobuf_msg))
                    {
292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327
                        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);
                        }
328 329
                    }
                }
330
#endif
331 332 333 334

                position += extended_message.extended_payload_len;

                continue;
335 336 337
            }
#endif

pixhawk's avatar
pixhawk committed
338
            // Log data
339 340
            if (m_loggingEnabled && m_logfile)
            {
341
                uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)] = {0};
342
                quint64 time = QGC::groundTimeUsecs();
343
                memcpy(buf, (void*)&time, sizeof(quint64));
344
                // Write message to buffer
345 346 347 348
                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);
349
                QByteArray b((const char*)buf, len);
Lorenz Meier's avatar
Lorenz Meier committed
350
                if(m_logfile->write(b) != len)
351
                {
352 353 354 355
                    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
356 357
            }

pixhawk's avatar
pixhawk committed
358 359 360
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
361

pixhawk's avatar
pixhawk committed
362 363 364
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
365 366
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
            {
pixhawk's avatar
pixhawk committed
367 368 369 370 371 372
                // 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
373
                // Check if the UAS has the same id like this system
374 375
                if (message.sysid == getSystemId())
                {
376
                    emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
377 378
                }

379 380 381 382
                // 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
383
                // First create new UAS object
384 385
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
pixhawk's avatar
pixhawk committed
386 387
                // Reset version field to 0
                heartbeat.mavlink_version = 0;
388
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
pixhawk's avatar
pixhawk committed
389 390

                // Check if the UAS has a different protocol version
391 392
                if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
                {
pixhawk's avatar
pixhawk committed
393
                    // Bring up dialog to inform user
394 395
                    if (!versionMismatchIgnore)
                    {
396
                        emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
397
                                                   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));
398 399
                        versionMismatchIgnore = true;
                    }
pixhawk's avatar
pixhawk committed
400 401 402 403 404

                    // Ignore this message and continue gracefully
                    continue;
                }

405 406
                // Create a new UAS object
                uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
407

pixhawk's avatar
pixhawk committed
408
            }
409 410

            // Only count message if UAS exists for this message
411 412
            if (uas != NULL)
            {
413

414 415 416
                // Increase receive counter
                totalReceiveCounter++;
                currReceiveCounter++;
417 418 419

                // Update last message sequence ID
                uint8_t expectedIndex;
420
                if (lastIndex[message.sysid][message.compid] == -1)
421
                {
422
                    lastIndex[message.sysid][message.compid] = message.seq;
423
                    expectedIndex = message.seq;
424
                }
425 426
                else
                {
427
                    // NOTE: Using uint8_t here auto-wraps the number around to 0.
428
                    expectedIndex = lastIndex[message.sysid][message.compid] + 1;
429 430 431
                }

                // Make some noise if a message was skipped
432
                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
433 434 435 436 437 438 439 440 441 442 443
                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
444 445
                        // 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);
446 447 448 449
                    }
                    totalLossCounter += lostMessages;
                    currLossCounter += lostMessages;
                }
450 451 452

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

454 455
                // Update on every 32th packet
                if (totalReceiveCounter % 32 == 0)
456
                {
457 458 459 460 461 462
                    // 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
463
                    emit receiveLossChanged(message.sysid, receiveLoss);
lm's avatar
lm committed
464 465
                }

466 467 468 469
                // 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);
470 471

                // Multiplex message if enabled
472 473
                if (m_multiplexingEnabled)
                {
474 475 476 477
                    // Get all links connected to this unit
                    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

                    // Emit message on all links that are currently connected
478 479
                    foreach (LinkInterface* currLink, links)
                    {
480 481
                        // Only forward this message to the other links,
                        // not the link the message was received on
Lorenz Meier's avatar
Lorenz Meier committed
482
                        if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
483 484
                    }
                }
485
            }
pixhawk's avatar
pixhawk committed
486 487 488 489 490 491 492 493 494 495 496 497
        }
    }
}

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

498
/** @return System id of this application */
499
int MAVLinkProtocol::getSystemId()
500
{
501 502 503 504 505 506
    return systemId;
}

void MAVLinkProtocol::setSystemId(int id)
{
    systemId = id;
507 508 509
}

/** @return Component id of this application */
510
int MAVLinkProtocol::getComponentId()
511
{
512
    return QGC::defaultComponentId;
513 514
}

pixhawk's avatar
pixhawk committed
515 516 517 518 519 520 521 522 523 524
/**
 * @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;
525 526
    for (i = links.begin(); i != links.end(); ++i)
    {
pixhawk's avatar
pixhawk committed
527
        sendMessage(*i, message);
Lorenz Meier's avatar
Lorenz Meier committed
528
        qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
529 530 531 532 533 534 535 536 537 538
    }
}

/**
 * @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
539
    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
540
    // Rewriting header to ensure correct link ID is set
lm's avatar
lm committed
541 542
    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
543
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
544
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
545
    // If link is connected
546 547
    if (link->isConnected())
    {
pixhawk's avatar
pixhawk committed
548 549 550 551 552
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575
/**
 * @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
576 577 578 579 580 581 582
/**
 * 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
583 584
    if (m_heartbeatsEnabled)
    {
pixhawk's avatar
pixhawk committed
585
        mavlink_message_t beat;
lm's avatar
lm committed
586
        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
587 588
        sendMessage(beat);
    }
Lorenz Meier's avatar
Lorenz Meier committed
589 590
    if (m_authEnabled)
    {
James Goppert's avatar
James Goppert committed
591 592
        mavlink_message_t msg;
        mavlink_auth_key_t auth;
593
        memset(&auth, 0, sizeof(auth));
594
        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
595 596 597
        mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth);
        sendMessage(msg);
    }
pixhawk's avatar
pixhawk committed
598 599 600 601 602 603 604 605 606
}

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

607 608 609 610 611 612 613 614 615
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
    bool changed = false;
    if (enabled != m_multiplexingEnabled) changed = true;

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

616 617 618 619
void MAVLinkProtocol::enableAuth(bool enable)
{
    bool changed = false;
    m_authEnabled = enable;
620
    if (m_authEnabled != enable) {
621 622 623 624 625
        changed = true;
    }
    if (changed) emit authChanged(m_authEnabled);
}

626 627
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
628
    if (enabled != m_paramGuardEnabled) {
629 630 631 632 633
        m_paramGuardEnabled = enabled;
        emit paramGuardChanged(m_paramGuardEnabled);
    }
}

634 635
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
636
    if (enabled != m_actionGuardEnabled) {
637 638 639 640 641
        m_actionGuardEnabled = enabled;
        emit actionGuardChanged(m_actionGuardEnabled);
    }
}

642 643
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
644
    if (ms != m_paramRetransmissionTimeout) {
645 646 647 648 649 650 651
        m_paramRetransmissionTimeout = ms;
        emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
    }
}

void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
652
    if (ms != m_paramRewriteTimeout) {
653 654 655 656 657
        m_paramRewriteTimeout = ms;
        emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
    }
}

658 659
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
660
    if (ms != m_actionRetransmissionTimeout) {
661 662 663 664 665
        m_actionRetransmissionTimeout = ms;
        emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
    }
}

lm's avatar
lm committed
666 667
void MAVLinkProtocol::enableLogging(bool enabled)
{
668 669 670
    bool changed = false;
    if (enabled != m_loggingEnabled) changed = true;

671 672 673 674
    if (enabled)
    {
        if (m_logfile && m_logfile->isOpen())
        {
675 676 677
            m_logfile->flush();
            m_logfile->close();
        }
678 679 680 681 682

        if (m_logfile)
        {
            if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
            {
683 684
                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;
685
            }
686
        }
687 688 689 690 691 692 693 694 695 696 697
        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())
            {
698 699 700
                m_logfile->flush();
                m_logfile->close();
            }
701
        }
lm's avatar
lm committed
702 703
    }
    m_loggingEnabled = enabled;
704
    if (changed) emit loggingChanged(enabled);
705 706 707 708
}

void MAVLinkProtocol::setLogfileName(const QString& filename)
{
709 710
    if (!m_logfile)
    {
711
        m_logfile = new QFile(filename);
712 713 714
    }
    else
    {
715 716 717
        m_logfile->flush();
        m_logfile->close();
    }
718
    m_logfile->setFileName(filename);
719
    enableLogging(m_loggingEnabled);
lm's avatar
lm committed
720 721
}

lm's avatar
lm committed
722 723 724
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
725
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
726 727
}

pixhawk's avatar
pixhawk committed
728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743
/**
 * 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;
}