MAVLinkProtocol.cc 16.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>
pixhawk's avatar
pixhawk committed
17 18 19 20 21 22 23

#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
24 25
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
26
#include "ArduPilotMegaMAV.h"
pixhawk's avatar
pixhawk committed
27 28
#include "configuration.h"
#include "LinkManager.h"
29
//#include "MainWindow.h"
pixhawk's avatar
pixhawk committed
30
#include <QGCMAVLink.h>
31
#include "QGC.h"
pixhawk's avatar
pixhawk committed
32 33 34 35 36 37 38 39

/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
        heartbeatTimer(new QTimer(this)),
        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
lm's avatar
lm committed
40 41
        m_heartbeatsEnabled(false),
        m_loggingEnabled(false),
42
        m_logfile(new QFile(QCoreApplication::applicationDirPath()+"/mavlink_packetlog.mavlink")),
lm's avatar
lm committed
43
        m_enable_version_check(true),
44
        versionMismatchIgnore(false)
pixhawk's avatar
pixhawk committed
45 46 47 48 49
{
    start(QThread::LowPriority);
    // 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
50 51
    totalReceiveCounter = 0;
    totalLossCounter = 0;
52 53
    currReceiveCounter = 0;
    currLossCounter = 0;
lm's avatar
lm committed
54 55 56 57 58 59 60
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
            lastIndex[i][j] = -1;
        }
    }
lm's avatar
lm committed
61 62

    emit versionCheckChanged(m_enable_version_check);
pixhawk's avatar
pixhawk committed
63 64 65 66
}

MAVLinkProtocol::~MAVLinkProtocol()
{
pixhawk's avatar
pixhawk committed
67 68
    if (m_logfile)
    {
69
        m_logfile->flush();
pixhawk's avatar
pixhawk committed
70 71 72
        m_logfile->close();
        delete m_logfile;
    }
pixhawk's avatar
pixhawk committed
73 74 75 76 77 78
}



void MAVLinkProtocol::run()
{
79 80 81 82
    forever
    {
        QGC::SLEEP::msleep(5000);
    }
pixhawk's avatar
pixhawk committed
83 84
}

pixhawk's avatar
pixhawk committed
85 86
QString MAVLinkProtocol::getLogfileName()
{
87
    return m_logfile->fileName();
pixhawk's avatar
pixhawk committed
88 89
}

pixhawk's avatar
pixhawk committed
90 91 92 93 94 95 96 97
/**
 * 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
 **/
98
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
pixhawk's avatar
pixhawk committed
99 100 101 102
{
    receiveMutex.lock();
    mavlink_message_t message;
    mavlink_status_t status;
103
    for (int position = 0; position < b.size(); position++)
pixhawk's avatar
pixhawk committed
104
    {
105
        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
pixhawk's avatar
pixhawk committed
106 107 108

        if (decodeState == 1)
        {
pixhawk's avatar
pixhawk committed
109 110 111
            // Log data
            if (m_loggingEnabled)
            {
lm's avatar
lm committed
112
                const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64);
113 114
                uint8_t buf[len];
                quint64 time = QGC::groundTimeUsecs();
115
                memcpy(buf, (void*)&time, sizeof(quint64));
116 117 118
                //                int packetlen =
//                quint64 checktime = *((quint64*)buf);
//                qDebug() << "TIME" << time << "CHECKTIME:" << checktime;
119
                mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
120 121 122 123
                QByteArray b((const char*)buf, len);
                //int packetlen =
                if(m_logfile->write(b) < MAVLINK_MAX_PACKET_LEN+sizeof(quint64)) qDebug() << "WRITING TO LOG FAILED!";
                //qDebug() << "WROTE LOGFILE";
pixhawk's avatar
pixhawk committed
124 125
            }

pixhawk's avatar
pixhawk committed
126 127 128 129 130 131
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
132
            if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
pixhawk's avatar
pixhawk committed
133 134 135 136 137 138 139
            {
                // 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
140
                // Check if the UAS has the same id like this system
141 142
                if (message.sysid == getSystemId())
                {
143
                    emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
144 145
                }

146 147 148 149
                // 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
150
                // First create new UAS object
151 152
                // Decode heartbeat message
                mavlink_heartbeat_t heartbeat;
pixhawk's avatar
pixhawk committed
153 154
                // Reset version field to 0
                heartbeat.mavlink_version = 0;
155
                mavlink_msg_heartbeat_decode(&message, &heartbeat);
pixhawk's avatar
pixhawk committed
156 157

                // Check if the UAS has a different protocol version
lm's avatar
lm committed
158
                if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
pixhawk's avatar
pixhawk committed
159 160
                {
                    // Bring up dialog to inform user
161 162 163
                    if (!versionMismatchIgnore)
                    {
                        emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
164
                                                   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));
165 166
                        versionMismatchIgnore = true;
                    }
pixhawk's avatar
pixhawk committed
167 168 169 170 171

                    // Ignore this message and continue gracefully
                    continue;
                }

172 173 174
                switch (heartbeat.autopilot)
                {
                case MAV_AUTOPILOT_GENERIC:
175

176
                    uas = new UAS(this, message.sysid);
177

178 179
                    // Connect this robot to the UAS object
                    connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
180 181
                    break;
                case MAV_AUTOPILOT_PIXHAWK:
182
                    {
183 184 185 186 187 188 189 190 191
                        // Fixme differentiate between quadrotor and coaxial here
                        PxQuadMAV* mav = new PxQuadMAV(this, message.sysid);
                        // Connect this robot to the UAS object
                        // it is IMPORTANT here to use the right object type,
                        // else the slot of the parent object is called (and thus the special
                        // packets never reach their goal)
                        connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                        uas = mav;
                    }
192 193
                    break;
                case MAV_AUTOPILOT_SLUGS:
194
                    {
195 196 197 198 199 200 201 202
                        SlugsMAV* mav = new SlugsMAV(this, message.sysid);
                        // Connect this robot to the UAS object
                        // it is IMPORTANT here to use the right object type,
                        // else the slot of the parent object is called (and thus the special
                        // packets never reach their goal)
                        connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                        uas = mav;
                    }
203
                    break;
204
                case MAV_AUTOPILOT_ARDUPILOTMEGA:
205
                    {
206 207 208 209 210 211 212 213
                        ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid);
                        // Connect this robot to the UAS object
                        // it is IMPORTANT here to use the right object type,
                        // else the slot of the parent object is called (and thus the special
                        // packets never reach their goal)
                        connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                        uas = mav;
                    }
214 215 216 217 218
                    break;
                default:
                    uas = new UAS(this, message.sysid);
                    break;
                }
219

220 221
                // Set the autopilot type
                uas->setAutopilotType((int)heartbeat.autopilot);
222

pixhawk's avatar
pixhawk committed
223 224
                // Make UAS aware that this link can be used to communicate with the actual robot
                uas->addLink(link);
225

pixhawk's avatar
pixhawk committed
226 227
                // Now add UAS to "official" list, which makes the whole application aware of it
                UASManager::instance()->addUAS(uas);
228

pixhawk's avatar
pixhawk committed
229
            }
230 231 232

            // Only count message if UAS exists for this message
            if (uas != NULL)
lm's avatar
lm committed
233
            {
234 235 236 237 238 239
                // Increase receive counter
                totalReceiveCounter++;
                currReceiveCounter++;
                qint64 lastLoss = totalLossCounter;
                // Update last packet index
                if (lastIndex[message.sysid][message.compid] == -1)
lm's avatar
lm committed
240
                {
241
                    lastIndex[message.sysid][message.compid] = message.seq;
lm's avatar
lm committed
242 243
                }
                else
lm's avatar
lm committed
244
                {
Mariano Lizarraga's avatar
Mariano Lizarraga committed
245
                    // TODO: This if-else block can (should) be greatly simplified
lm's avatar
lm committed
246 247 248 249 250 251 252 253
                    if (lastIndex[message.sysid][message.compid] == 255)
                    {
                        lastIndex[message.sysid][message.compid] = 0;
                    }
                    else
                    {
                        lastIndex[message.sysid][message.compid]++;
                    }
254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280

                    int safeguard = 0;
                    //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
                    while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255)
                    {
                        if (lastIndex[message.sysid][message.compid] == 255)
                        {
                            lastIndex[message.sysid][message.compid] = 0;
                        }
                        else
                        {
                            lastIndex[message.sysid][message.compid]++;
                        }
                        totalLossCounter++;
                        currLossCounter++;
                        safeguard++;
                    }
                }
                //            if (lastIndex.contains(message.sysid))
                //            {
                //                QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
                //                if (lastCompIndex->contains(message.compid))
                //                while (lastCompIndex->value(message.compid, 0)+1 )
                //            }
                //if ()

                // If a new loss was detected or we just hit one 128th packet step
lm's avatar
lm committed
281
                if (lastLoss != totalLossCounter || (totalReceiveCounter % 64 == 0))
282 283 284 285 286 287 288 289
                {
                    // Calculate new loss ratio
                    // Receive loss
                    float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                    receiveLoss *= 100.0f;
                    // qDebug() << "LOSSCHANGED" << receiveLoss;
                    currLossCounter = 0;
                    currReceiveCounter = 0;
pixhawk's avatar
pixhawk committed
290
                    emit receiveLossChanged(message.sysid, receiveLoss);
lm's avatar
lm committed
291 292
                }

293 294 295 296 297
                // 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);
            }
pixhawk's avatar
pixhawk committed
298 299 300 301 302 303 304 305 306 307 308 309 310
        }
    }
    receiveMutex.unlock();
}

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

311
/** @return System id of this application */
312
int MAVLinkProtocol::getSystemId()
313 314 315 316 317
{
    return MG::SYSTEM::ID;
}

/** @return Component id of this application */
318
int MAVLinkProtocol::getComponentId()
319 320 321 322
{
    return MG::SYSTEM::COMPID;
}

pixhawk's avatar
pixhawk committed
323 324 325 326 327 328 329 330 331 332 333 334 335
/**
 * @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;
    for (i = links.begin(); i != links.end(); ++i)
    {
        sendMessage(*i, message);
336
        //qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
pixhawk's avatar
pixhawk committed
337 338 339 340 341 342 343 344 345 346 347
    }
}

/**
 * @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
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
348 349
    // Rewriting header to ensure correct link ID is set
    if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len);
pixhawk's avatar
pixhawk committed
350
    // Write message into buffer, prepending start sign
pixhawk's avatar
pixhawk committed
351
    int len = mavlink_msg_to_send_buffer(buffer, &message);
pixhawk's avatar
pixhawk committed
352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * 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()
{
    if (m_heartbeatsEnabled)
    {
        mavlink_message_t beat;
lm's avatar
lm committed
370
        mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, OCU, MAV_AUTOPILOT_GENERIC);
pixhawk's avatar
pixhawk committed
371 372 373 374 375 376 377 378 379 380 381
        sendMessage(beat);
    }
}

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

lm's avatar
lm committed
382 383
void MAVLinkProtocol::enableLogging(bool enabled)
{
384 385 386 387
    bool changed = false;
    if (enabled != m_loggingEnabled) changed = true;

    if (enabled)
lm's avatar
lm committed
388
    {
389 390 391 392 393 394 395 396 397 398
        if (m_logfile->isOpen())
        {
            m_logfile->flush();
            m_logfile->close();
        }
        if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
        {
            emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file.").arg(m_logfile->fileName()));
            qDebug() << "OPENING LOGFILE FAILED!";
        }
lm's avatar
lm committed
399
    }
400
    else if (!enabled)
lm's avatar
lm committed
401
    {
402 403
        m_logfile->flush();
        m_logfile->close();
lm's avatar
lm committed
404 405
    }
    m_loggingEnabled = enabled;
406
    if (changed) emit loggingChanged(enabled);
407 408 409 410
}

void MAVLinkProtocol::setLogfileName(const QString& filename)
{
411
    m_logfile->flush();
412 413
    m_logfile->close();
    m_logfile->setFileName(filename);
414
    enableLogging(m_loggingEnabled);
lm's avatar
lm committed
415 416
}

lm's avatar
lm committed
417 418 419
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
    m_enable_version_check = enabled;
420
    emit versionCheckChanged(enabled);
lm's avatar
lm committed
421 422
}

pixhawk's avatar
pixhawk committed
423 424 425 426 427
bool MAVLinkProtocol::heartbeatsEnabled(void)
{
    return m_heartbeatsEnabled;
}

lm's avatar
lm committed
428 429 430 431 432
bool MAVLinkProtocol::loggingEnabled(void)
{
    return m_loggingEnabled;
}

lm's avatar
lm committed
433 434 435 436 437
bool MAVLinkProtocol::versionCheckEnabled(void)
{
    return m_enable_version_check;
}

pixhawk's avatar
pixhawk committed
438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453
/**
 * 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;
}