Merge branch 'master' of git://github.com/pixhawk/qgroundcontrol into checkRemote

doc/Doxyfile

@@ -297,7 +297,7 @@ SYMBOL_CACHE_SIZE      = 0
 # Private class members and static file members will be hidden unless
 # the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
 
-EXTRACT_ALL            = NO
+EXTRACT_ALL            = YES
 
 # If the EXTRACT_PRIVATE tag is set to YES all private members of a class
 # will be included in the documentation.
@@ -976,7 +976,7 @@ FORMULA_FONTSIZE       = 10
 # there is already a search function so this one should typically
 # be disabled.
 
-SEARCHENGINE           = NO
+SEARCHENGINE           = YES
 
 #---------------------------------------------------------------------------
 # configuration options related to the LaTeX output
@@ -1501,7 +1501,7 @@ DOT_TRANSPARENT        = YES
 # makes dot run faster, but since only newer versions of dot (>1.8.10)
 # support this, this feature is disabled by default.
 
-DOT_MULTI_TARGETS      = NO
+DOT_MULTI_TARGETS      = YES
 
 # If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will
 # generate a legend page explaining the meaning of the various boxes and

qgroundcontrol.pro

@@ -216,3 +216,18 @@ SOURCES += src/main.cc \
     src/ui/QGCDataPlot2D.cc \
     src/ui/linechart/IncrementalPlot.cc
 RESOURCES = mavground.qrc
+
+# Include RT-LAB Library
+win32 {
+    LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin -lOpalApi
+    INCLUDEPATH += src/lib/opalrt
+    SOURCES += src/comm/OpalLink.cc
+    HEADERS += src/comm/OpalLink.h
+    DEFINES += OPAL_RT
+}
+
+macx {
+    SOURCES += src/comm/OpalLink.cc
+    HEADERS += src/comm/OpalLink.h
+    DEFINES += OPAL_RT
+}

src/Core.cc

@@ -46,6 +46,9 @@ This file is part of the PIXHAWK project
 #include "MainWindow.h"
 #include "GAudioOutput.h"
 
+#ifdef OPAL_RT
+#include "OpalLink.h"
+#endif
 #include "UDPLink.h"
 #include "MAVLinkSimulationLink.h"
 
@@ -132,6 +135,13 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
         }
     }
 
+#ifdef OPAL_RT
+    // Add OpalRT Link, but do not connect
+    OpalLink* opalLink = new OpalLink();
+    mainWindow->addLink(opalLink);
+    opalLink->connect();
+#warning OPAL LINK NOW AUTO CONNECTING IN CORE.CC
+#endif
     // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt");
     MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt");
     mainWindow->addLink(simulationLink);

src/Core.h

@@ -39,7 +39,11 @@ This file is part of the PIXHAWK project
 #include "UASManager.h"
 #include "LinkManager.h"
 /*#include "ViconTarsusProtocol.h" */
+#ifdef OPAL_RT
 
+#include "OpalLink.h"
+
+#endif
 /**
  * @brief The main application and management class.
  *

src/comm/AS4Protocol.cc

@@ -99,13 +99,13 @@ void AS4Protocol::receiveBytes(LinkInterface* link)
 {
 //    receiveMutex.lock();
     // Prepare buffer
-    static const int maxlen = 4096 * 100;
-    static char buffer[maxlen];
+    //static const int maxlen = 4096 * 100;
+    //static char buffer[maxlen];
 
     qint64 bytesToRead = link->bytesAvailable();
 
     // Get all data at once, let link read the bytes in the buffer array
-    link->readBytes(buffer, maxlen);
+    //link->readBytes(buffer, maxlen);
 //
 //    /*
 //    // Debug output

src/comm/LinkInterface.h

@@ -6,28 +6,28 @@ PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
 
 This file is part of the PIXHAWK project
 
-    PIXHAWK is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
+PIXHAWK is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
 
-    PIXHAWK is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
+PIXHAWK is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
 
-    You should have received a copy of the GNU General Public License
-    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+You should have received a copy of the GNU General Public License
+along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/
 
 /**
- * @file
- *   @brief Brief Description
- *
- *   @author Lorenz Meier <mavteam@student.ethz.ch>
- *
- */
+* @file
+*   @brief Brief Description
+*
+*   @author Lorenz Meier <mavteam@student.ethz.ch>
+*
+*/
 
 #ifndef _LINKINTERFACE_H_
 #define _LINKINTERFACE_H_
@@ -35,10 +35,10 @@ This file is part of the PIXHAWK project
 #include <QThread>
 
 /**
- * The link interface defines the interface for all links used to communicate
- * with the groundstation application.
- *
- **/
+* The link interface defines the interface for all links used to communicate
+* with the groundstation application.
+*
+**/
 class LinkInterface : public QThread {
     Q_OBJECT
 public:
@@ -193,26 +193,15 @@ public slots:
      **/
     virtual void writeBytes(const char *bytes, qint64 length) = 0;
 
-	/**
-	 * @brief Read a number of bytes from the interface.
-	 *
-	 * @param bytes The pointer to write the bytes to
-	 * @param maxLength The maximum length which can be written
-	 **/
-	virtual void readBytes(char *bytes, qint64 maxLength) = 0;
-
 signals:
-	/**
-	 * @brief This signal is emitted when new data is ready to read in the queue
-	 *
-	 * @param link The Interface to read from
-	 **/
-	void bytesReady(LinkInterface* link);
 
     /**
      * @brief New data arrived
      *
-         * The use of this function is discouraged for high-performance links.
+     * The new data is contained in the QByteArray data. The data is copied for each
+     * receiving protocol. For high-speed links like image transmission this might
+     * affect performance, for control links it is however desirable to directly
+     * forward the link data.
      *
      * @param data the new bytes
      */
@@ -245,6 +234,16 @@ protected:
         return nextId++;
     }
 
+protected slots:
+
+/**
+ * @brief Read a number of bytes from the interface.
+ *
+ * @param bytes The pointer to write the bytes to
+ * @param maxLength The maximum length which can be written
+ **/
+virtual void readBytes() = 0;
+
 };
 
 /* Declare C++ interface as Qt interface */

src/comm/LinkManager.cc

@@ -56,7 +56,6 @@ LinkManager::LinkManager()
 {
         links = QList<LinkInterface*>();
         protocolLinks = QMap<ProtocolInterface*, LinkInterface*>();
-	start(QThread::LowPriority);
 }
 
 LinkManager::~LinkManager()
@@ -64,11 +63,6 @@ LinkManager::~LinkManager()
     disconnectAll();
 }
 
-
-void LinkManager::run()
-{
-}
-
 void LinkManager::add(LinkInterface* link)
 {
     links.append(link);
@@ -79,7 +73,7 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
 {
     // Connect link to protocol
     // the protocol will receive new bytes from the link
-    connect(link, SIGNAL(bytesReady(LinkInterface*)), protocol, SLOT(receiveBytes(LinkInterface*)));
+    connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray)));
     // Store the connection information in the protocol links map
     protocolLinks.insertMulti(protocol, link);
     //qDebug() << __FILE__ << __LINE__ << "ADDED LINK TO PROTOCOL" << link->getName() << protocol->getName() << "NEW SIZE OF LINK LIST:" << protocolLinks.size();

src/comm/LinkManager.h

@@ -44,7 +44,8 @@ This file is part of the PIXHAWK project
  * protocol instance to transport the link data into the application.
  *
  **/
-class LinkManager : public QThread {
+class LinkManager : public QObject
+{
     Q_OBJECT
 
 public:

src/comm/MAVLinkLightProtocol.cc

@@ -86,51 +86,53 @@ void MAVLinkLightProtocol::sendMessage(LinkInterface* link, mavlink_light_messag
  * @param link The interface to read from
  * @see LinkInterface
  **/
-void MAVLinkLightProtocol::receiveBytes(LinkInterface* link)
+void MAVLinkLightProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 {
-    receiveMutex.lock();
-    // Prepare buffer
-    static const int maxlen = 4096 * 100;
-    static char buffer[maxlen];
-    qint64 bytesToRead = link->bytesAvailable();
-
-    // Get all data at once, let link read the bytes in the buffer array
-    link->readBytes(buffer, maxlen);
-
-    // Run through all bytes
-    for (int position = 0; position < bytesToRead; position++)
-    {
-        mavlink_light_message_t msg;
-        // FIXME PARSE
-        if (1 == 0/* parsing returned a message */)
-        {
-
-            int sysid = 0; // system id from message, or always null if only one MAV is supported
-            UASInterface* uas = UASManager::instance()->getUASForId(sysid);
-
-            // Check and (if necessary) create UAS object
-            if (uas == NULL)
-            {
-                ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists
-                // 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;
-                // Make UAS aware that this link can be used to communicate with the actual robot
-                uas->addLink(link);
-                // Now add UAS to "official" list, which makes the whole application aware of it
-                UASManager::instance()->addUAS(uas);
-            }
-
-            // 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, msg);
-        }
-    }
-    receiveMutex.unlock();
+    Q_UNUSED(link);
+    Q_UNUSED(b);
+//    receiveMutex.lock();
+//    // Prepare buffer
+//    static const int maxlen = 4096 * 100;
+//    static char buffer[maxlen];
+//    qint64 bytesToRead = link->bytesAvailable();
+//
+//    // Get all data at once, let link read the bytes in the buffer array
+//    link->readBytes(buffer, maxlen);
+//
+//    // Run through all bytes
+//    for (int position = 0; position < bytesToRead; position++)
+//    {
+//        mavlink_light_message_t msg;
+//        // FIXME PARSE
+//        if (1 == 0/* parsing returned a message */)
+//        {
+//
+//            int sysid = 0; // system id from message, or always null if only one MAV is supported
+//            UASInterface* uas = UASManager::instance()->getUASForId(sysid);
+//
+//            // Check and (if necessary) create UAS object
+//            if (uas == NULL)
+//            {
+//                ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists
+//                // 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;
+//                // Make UAS aware that this link can be used to communicate with the actual robot
+//                uas->addLink(link);
+//                // Now add UAS to "official" list, which makes the whole application aware of it
+//                UASManager::instance()->addUAS(uas);
+//            }
+//
+//            // 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, msg);
+//        }
+//    }
+//    receiveMutex.unlock();
 }
 
 /**

src/comm/MAVLinkLightProtocol.h

@@ -31,6 +31,7 @@ This file is part of the QGROUNDCONTROL project
 #define MAVLINKLIGHTPROTOCOL_H
 
 #include <inttypes.h>
+#include <QByteArray>
 #include <MAVLinkProtocol.h>
 
 #define MAVLINK_LIGHT_MAX_PAYLOAD_LEN 50
@@ -59,7 +60,7 @@ signals:
 public slots:
     void sendMessage(mavlink_light_message_t message);
     void sendMessage(LinkInterface* link, mavlink_light_message_t message);
-    void receiveBytes(LinkInterface* link);
+    void receiveBytes(LinkInterface* link, QByteArray b);
 
 };
 

src/comm/MAVLinkProtocol.cc

@@ -108,21 +108,14 @@ QString MAVLinkProtocol::getLogfileName()
  * @param link The interface to read from
  * @see LinkInterface
  **/
-void MAVLinkProtocol::receiveBytes(LinkInterface* link)
+void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 {
     receiveMutex.lock();
-    // Prepare buffer
-    static const int maxlen = 4096 * 100;
-    static char buffer[maxlen];
-    qint64 bytesToRead = link->bytesAvailable();
-
-    // Get all data at once, let link read the bytes in the buffer array
-    link->readBytes(buffer, maxlen);
     mavlink_message_t message;
     mavlink_status_t status;
-    for (int position = 0; position < bytesToRead; position++)
+    for (int position = 0; position < b.size(); position++)
     {
-        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)*(buffer + position), &message, &status);
+        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
 
         if (decodeState == 1)
         {

src/comm/MAVLinkProtocol.h

@@ -73,7 +73,7 @@ public:
 
 public slots:
     /** @brief Receive bytes from a communication interface */
-    void receiveBytes(LinkInterface* link);
+    void receiveBytes(LinkInterface* link, QByteArray b);
     /** @brief Send MAVLink message through serial interface */
     void sendMessage(mavlink_message_t message);
     /** @brief Send MAVLink message through serial interface */

src/comm/MAVLinkSimulationLink.cc

@@ -142,13 +142,7 @@ void MAVLinkSimulationLink::run()
 //                }
 //                readyBufferMutex.unlock();
 
-
-
-
-
-
-
-                emit bytesReady(this);
+                readBytes();
             }
             last = MG::TIME::getGroundTimeNow();
         }
@@ -752,9 +746,11 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
 }
 
 
-void MAVLinkSimulationLink::readBytes(char* const data, qint64 maxLength) {
+void MAVLinkSimulationLink::readBytes() {
     // Lock concurrent resource readyBuffer
     readyBufferMutex.lock();
+    const qint64 maxLength = 2048;
+    char data[maxLength];
     qint64 len = maxLength;
     if (maxLength > readyBuffer.size()) len = readyBuffer.size();
 

src/comm/MAVLinkSimulationLink.h

@@ -84,7 +84,7 @@ public:
 
 public slots:
     void writeBytes(const char* data, qint64 size);
-    void readBytes(char* const data, qint64 maxLength);
+    void readBytes();
     void mainloop();
     bool connectLink(bool connect);
 

src/comm/OpalLink.cc

+#include "OpalLink.h"
+
+OpalLink::OpalLink() :
+        connectState(false),
+        heartbeatTimer(new QTimer(this)),
+        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
+        m_heartbeatsEnabled(true),
+        getSignalsTimer(new QTimer(this)),
+        getSignalsPeriod(1000),
+        receiveBuffer(new QQueue<QByteArray>()),
+        systemID(1),
+        componentID(1)
+{
+    start(QThread::LowPriority);
+
+    // Set unique ID and add link to the list of links
+    this->id = getNextLinkId();
+    this->name = tr("OpalRT link ") + QString::number(getId());
+    LinkManager::instance()->add(this);
+
+    // Start heartbeat timer, emitting a heartbeat at the configured rate
+    QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat()));
+
+    QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals()));
+}
+
+
+/*
+ *
+  Communication
+ *
+ */
+
+qint64 OpalLink::bytesAvailable()
+{
+    return 0;
+}
+
+void OpalLink::writeBytes(const char *bytes, qint64 length)
+{
+
+}
+
+
+void OpalLink::readBytes()
+{
+    receiveDataMutex.lock();
+    const qint64 maxLength = 2048;
+    char bytes[maxLength];
+    qDebug() << "OpalLink::readBytes(): Reading a message.  size of buffer: " << receiveBuffer->count();
+    QByteArray message = receiveBuffer->dequeue();
+    if (maxLength < message.size())
+    {
+        qDebug() << "OpalLink::readBytes:: Buffer Overflow";
+
+        memcpy(bytes, message.data(), maxLength);
+    }
+    else
+    {
+        memcpy(bytes, message.data(), message.size());
+    }
+
+    emit bytesReceived(this, message);
+    receiveDataMutex.unlock();
+
+}
+
+void OpalLink::receiveMessage(mavlink_message_t message)
+{
+
+    // Create buffer
+    char buffer[MAVLINK_MAX_PACKET_LEN];
+    // Write message into buffer, prepending start sign
+    int len = mavlink_msg_to_send_buffer((uint8_t*)(buffer), &message);
+    // If link is connected
+    if (isConnected())
+    {
+        receiveBuffer->enqueue(QByteArray(buffer, len));
+        readBytes();
+    }
+
+}
+
+void OpalLink::heartbeat()
+{
+
+    if (m_heartbeatsEnabled)
+    {
+        qDebug() << "OpalLink::heartbeat(): Generate a heartbeat";
+        mavlink_message_t beat;
+        mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC);
+        receiveMessage(beat);
+    }
+
+}
+
+void OpalLink::getSignals()
+{
+//    qDebug() <<  "OpalLink::getSignals(): Attempting to acquire signals";
+//
+//
+//    unsigned long  timeout = 0;
+//    unsigned short acqGroup = 0; //this is actually group 1 in the model
+//    unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS;
+//    unsigned short *numSignals = new unsigned short(0);
+//    double *timestep = new double(0);
+//    double values[NUM_OUTPUT_SIGNALS] = {};
+//    unsigned short *lastValues = new unsigned short(false);
+//    unsigned short *decimation = new unsigned short(0);
+//
+//    int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep,
+//                                   values, lastValues, decimation);
+//
+//    if (returnVal == EOK )
+//    {
+//        qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues);
+//    }
+//    else if (returnVal == EAGAIN)
+//    {
+//        qDebug() << "OpalLink::getSignals: Data was not ready";
+//    }
+//    // if returnVal == EAGAIN => data just wasn't ready
+//    else if (returnVal != EAGAIN)
+//    {
+//        getSignalsTimer->stop();
+//        displayErrorMsg();
+//    }
+//    /* deallocate used memory */
+//
+//    delete timestep;
+//    delete lastValues;
+//    delete lastValues;
+//    delete decimation;
+
+}
+
+/*
+ *
+  Administrative
+ *
+ */
+void OpalLink::run()
+{
+    qDebug() << "OpalLink::run():: Starting the thread";
+}
+
+int OpalLink::getId()
+{
+    return id;
+}
+
+QString OpalLink::getName()
+{
+    return name;
+}
+
+void OpalLink::setName(QString name)
+{
+    this->name = name;
+    emit nameChanged(this->name);
+}
+
+bool OpalLink::isConnected() {
+    //qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
+    return connectState;
+}
+
+
+
+
+bool OpalLink::connect()
+{
+    short modelState;
+
+    /// \todo allow configuration of instid in window
+//    if (OpalConnect(101, false, &modelState) == EOK)
+//    {
+        connectState = true;
+        emit connected();
+        heartbeatTimer->start(1000/heartbeatRate);
+        getSignalsTimer->start(getSignalsPeriod);
+//    }
+//    else
+//    {
+//        connectState = false;
+//        displayErrorMsg();
+//    }
+
+    emit connected(connectState);
+    return connectState;
+}
+
+bool OpalLink::disconnect()
+{
+    return false;
+}
+
+void OpalLink::displayErrorMsg()
+{
+    setLastErrorMsg();
+    QMessageBox msgBox;
+    msgBox.setIcon(QMessageBox::Critical);
+    msgBox.setText(lastErrorMsg);
+    msgBox.exec();
+}
+
+void OpalLink::setLastErrorMsg()
+{
+//    char buf[512];
+//    unsigned short len;
+//    OpalGetLastErrMsg(buf, sizeof(buf), &len);
+//    lastErrorMsg.clear();
+//    lastErrorMsg.append(buf);
+}
+
+
+/*
+ *
+  Statisctics
+ *
+ */
+
+qint64 OpalLink::getNominalDataRate()
+{
+    return 0; //unknown
+}
+
+int OpalLink::getLinkQuality()
+{
+    return -1; //not supported
+}
+
+qint64 OpalLink::getTotalUpstream()
+{
+    statisticsMutex.lock();
+    qint64 totalUpstream =  bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
+    statisticsMutex.unlock();
+    return totalUpstream;
+}
+
+qint64 OpalLink::getTotalDownstream() {
+    statisticsMutex.lock();
+    qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
+    statisticsMutex.unlock();
+    return totalDownstream;
+}
+
+qint64 OpalLink::getCurrentUpstream()
+{
+    return 0; //unknown
+}
+
+qint64 OpalLink::getMaxUpstream()
+{
+    return 0; //unknown
+}
+
+qint64 OpalLink::getBitsSent() {
+    return bitsSentTotal;
+}
+
+qint64 OpalLink::getBitsReceived() {
+    return bitsReceivedTotal;
+}
+
+
+bool OpalLink::isFullDuplex()
+{
+    return false;
+}

src/comm/OpalLink.h

+#ifndef OPALLINK_H
+#define OPALLINK_H
+/**
+  Connection to OpalRT.  This class receives MAVLink packets as if it is a true link, but it
+  interprets the packets internally, and calls the appropriate api functions.
+
+  \author Bryan Godbolt <godbolt@ualberta.ca>
+*/
+
+#include <QMutex>
+#include <QDebug>
+#include <QMessageBox>
+#include <QTimer>
+#include <QQueue>
+#include <QByteArray>
+#include <QObject>
+
+#include "LinkInterface.h"
+#include "LinkManager.h"
+#include "MG.h"
+#include "mavlink.h"
+#include "mavlink_types.h"
+#include "configuration.h"
+
+#include "errno.h"
+
+
+
+
+// FIXME
+//#include "OpalApi.h"
+
+
+
+
+
+#include "string.h"
+
+/*
+  Configuration info for the model
+ */
+
+#define NUM_OUTPUT_SIGNALS 6
+
+class OpalLink : public LinkInterface
+{
+    Q_OBJECT
+
+public:
+    OpalLink();
+    /* Connection management */
+
+    int getId();
+    QString getName();
+    bool isConnected();
+
+    /* Connection characteristics */
+
+
+    qint64 getNominalDataRate();
+    bool isFullDuplex();
+    int getLinkQuality();
+    qint64 getTotalUpstream();
+    qint64 getTotalDownstream();
+    qint64 getCurrentUpstream();
+    qint64 getMaxUpstream();
+    qint64 getBitsSent();
+    qint64 getBitsReceived();
+
+
+    bool connect();
+
+
+    bool disconnect();
+
+
+    qint64 bytesAvailable();
+
+    void run();
+
+public slots:
+
+
+    void writeBytes(const char *bytes, qint64 length);
+
+
+    void readBytes();
+
+    void heartbeat();
+
+    void getSignals();
+
+protected slots:
+
+    void receiveMessage(mavlink_message_t message);
+
+
+
+protected:
+    QString name;
+    int id;
+    bool connectState;
+
+    quint64 bitsSentTotal;
+    quint64 bitsSentCurrent;
+    quint64 bitsSentMax;
+    quint64 bitsReceivedTotal;
+    quint64 bitsReceivedCurrent;
+    quint64 bitsReceivedMax;
+    quint64 connectionStartTime;
+
+    QMutex statisticsMutex;
+    QMutex receiveDataMutex;
+    QString lastErrorMsg;
+    void setLastErrorMsg();
+    void displayErrorMsg();
+
+    void setName(QString name);
+
+    QTimer* heartbeatTimer;    ///< Timer to emit heartbeats
+    int heartbeatRate;         ///< Heartbeat rate, controls the timer interval
+    bool m_heartbeatsEnabled;  ///< Enabled/disable heartbeat emission
+
+    QTimer* getSignalsTimer;
+    int getSignalsPeriod;
+
+    QQueue<QByteArray>* receiveBuffer;
+    QByteArray* sendBuffer;
+
+    const int systemID;
+    const int componentID;
+
+
+};
+
+#endif // OPALLINK_H

src/comm/ProtocolInterface.h

@@ -34,6 +34,7 @@ This file is part of the PIXHAWK project
 
 #include <QThread>
 #include <QString>
+#include <QByteArray>
 #include "LinkInterface.h"
 
 /**
@@ -53,7 +54,7 @@ public:
     virtual QString getName() = 0;
 
 public slots:
-    virtual void receiveBytes(LinkInterface *link) = 0;
+    virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0;
 
 signals:
     /** @brief Update the packet loss from one system */

src/comm/SerialLink.cc

@@ -114,13 +114,14 @@ SerialLink::~SerialLink()
  * @brief Runs the thread
  *
  **/
-void SerialLink::run() {
-
+void SerialLink::run()
+{
     // Initialize the connection
     hardwareConnect();
 
     // Qt way to make clear what a while(1) loop does
-    forever {
+    forever
+    {
         // Check if new bytes have arrived, if yes, emit the notification signal
         checkForBytes();
         /* Serial data isn't arriving that fast normally, this saves the thread
@@ -128,30 +129,33 @@ void SerialLink::run() {
                  */
         MG::SLEEP::msleep(SerialLink::poll_interval);
     }
-
 }
 
 
-void SerialLink::checkForBytes() {
+void SerialLink::checkForBytes()
+{
     /* Check if bytes are available */
-    if(port->isOpen()) {
+    if(port->isOpen())
+    {
         dataMutex.lock();
         qint64 available = port->bytesAvailable();
         dataMutex.unlock();
 
-        if(available > 0) {
-            emit bytesReady(this);
-            //qDebug() << "Bytes available" << available << "connected:" << port->isOpen();
+        if(available > 0)
+        {
+            readBytes();
+        }
     }
-    } else {
+    else
+    {
         emit disconnected();
     }
 
 }
 
 
-void SerialLink::writeBytes(const char* data, qint64 size) {
-
+void SerialLink::writeBytes(const char* data, qint64 size)
+{
     if(port->isOpen())
     {
         int b = port->write(data, size);
@@ -176,17 +180,20 @@ void SerialLink::writeBytes(const char* data, qint64 size) {
  * @param data Pointer to the data byte array to write the bytes to
  * @param maxLength The maximum number of bytes to write
  **/
-void SerialLink::readBytes(char* data, qint64 maxLength) {
+void SerialLink::readBytes()
+{
     dataMutex.lock();
-    if(port->isOpen()) {
+    if(port->isOpen())
+    {
+        const qint64 maxLength = 2048;
+        char data[maxLength];
         qint64 numBytes = port->bytesAvailable();
-        if(numBytes > 0) {
+        if(numBytes > 0)
+        {
             /* Read as much data in buffer as possible without overflow */
             if(maxLength < numBytes) numBytes = maxLength;
 
             port->read(data, numBytes);
-
-            // FIXME TODO Check if this method should be preferred over manual read by process
             QByteArray b(data, numBytes);
             emit bytesReceived(this, b);
 
@@ -210,23 +217,18 @@ void SerialLink::readBytes(char* data, qint64 maxLength) {
  *
  * @return The number of bytes to read
  **/
-qint64 SerialLink::bytesAvailable() {
+qint64 SerialLink::bytesAvailable()
+{
     return port->bytesAvailable();
 }
 
-/**
- * @brief Convenience method to emit the bytesReady signal
- **/
-void SerialLink::emitBytesReady() {
-    emit bytesReady(this);
-}
-
 /**
  * @brief Disconnect the connection.
  *
  * @return True if connection has been disconnected, false if connection couldn't be disconnected.
  **/
-bool SerialLink::disconnect() {
+bool SerialLink::disconnect()
+{
     //#if !defined _WIN32 || !defined _WIN64
     /* Block the thread until it returns from run() */
     //#endif
@@ -276,10 +278,8 @@ bool SerialLink::connect()
  * @return True if the connection could be established, false otherwise
  * @see connect() For the right function to establish the connection.
  **/
-bool SerialLink::hardwareConnect() {
-
-    qDebug() << "Opening serial port" << porthandle;
-
+bool SerialLink::hardwareConnect()
+{
     QObject::connect(port, SIGNAL(aboutToClose()), this, SIGNAL(disconnected()));
 
     port->open(QIODevice::ReadWrite);
@@ -300,12 +300,15 @@ bool SerialLink::hardwareConnect() {
 
     return connectionUp;
 }
+
+
 /**
  * @brief Check if connection is active.
  *
  * @return True if link is connected, false otherwise.
  **/
-bool SerialLink::isConnected() {
+bool SerialLink::isConnected()
+{
     return port->isOpen();
 }
 
@@ -326,7 +329,8 @@ void SerialLink::setName(QString name)
 }
 
 
-qint64 SerialLink::getNominalDataRate() {
+qint64 SerialLink::getNominalDataRate()
+{
     qint64 dataRate = 0;
     switch (baudrate) {
     case BAUD50:
@@ -399,77 +403,94 @@ qint64 SerialLink::getNominalDataRate() {
     return dataRate;
 }
 
-qint64 SerialLink::getTotalUpstream() {
+qint64 SerialLink::getTotalUpstream()
+{
     statisticsMutex.lock();
     return bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
     statisticsMutex.unlock();
 }
 
-qint64 SerialLink::getCurrentUpstream() {
+qint64 SerialLink::getCurrentUpstream()
+{
     return 0; // TODO
 }
 
-qint64 SerialLink::getMaxUpstream() {
+qint64 SerialLink::getMaxUpstream()
+{
     return 0; // TODO
 }
 
-qint64 SerialLink::getBitsSent() {
+qint64 SerialLink::getBitsSent()
+{
     return bitsSentTotal;
 }
 
-qint64 SerialLink::getBitsReceived() {
+qint64 SerialLink::getBitsReceived()
+{
     return bitsReceivedTotal;
 }
 
-qint64 SerialLink::getTotalDownstream() {
+qint64 SerialLink::getTotalDownstream()
+{
     statisticsMutex.lock();
     return bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
     statisticsMutex.unlock();
 }
 
-qint64 SerialLink::getCurrentDownstream() {
+qint64 SerialLink::getCurrentDownstream()
+{
     return 0; // TODO
 }
 
-qint64 SerialLink::getMaxDownstream() {
+qint64 SerialLink::getMaxDownstream()
+{
     return 0; // TODO
 }
 
-bool SerialLink::isFullDuplex() {
+bool SerialLink::isFullDuplex()
+{
     /* Serial connections are always half duplex */
     return false;
 }
 
-int SerialLink::getLinkQuality() {
+int SerialLink::getLinkQuality()
+{
     /* This feature is not supported with this interface */
     return -1;
 }
 
-QString SerialLink::getPortName() {
+QString SerialLink::getPortName()
+{
     return porthandle;
 }
 
-int SerialLink::getBaudRate() {
+int SerialLink::getBaudRate()
+{
     return getNominalDataRate();
 }
 
-int SerialLink::getBaudRateType() {
+int SerialLink::getBaudRateType()
+{
     return baudrate;
 }
 
-int SerialLink::getFlowType() {
+int SerialLink::getFlowType()
+{
     return flow;
 }
 
-int SerialLink::getParityType() {
+int SerialLink::getParityType()
+{
     return parity;
 }
 
-int SerialLink::getDataBitsType() {
+int SerialLink::getDataBitsType()
+{
     return dataBits;
 }
 
-int SerialLink::getStopBitsType() {
+int SerialLink::getStopBitsType()
+{
     return stopBits;
 }
 
@@ -689,7 +710,8 @@ bool SerialLink::setBaudRate(int rate)
     return accepted;
 }
 
-bool SerialLink::setFlowType(int flow) {
+bool SerialLink::setFlowType(int flow)
+{
     bool reconnect = false;
     bool accepted = true;
     if(isConnected()) {
@@ -717,7 +739,8 @@ bool SerialLink::setFlowType(int flow) {
     return accepted;
 }
 
-bool SerialLink::setParityType(int parity) {
+bool SerialLink::setParityType(int parity)
+{
     bool reconnect = false;
     bool accepted = true;
     if(isConnected()) {
@@ -752,7 +775,8 @@ bool SerialLink::setParityType(int parity) {
     return accepted;
 }
 
-bool SerialLink::setDataBitsType(int dataBits) {
+bool SerialLink::setDataBitsType(int dataBits)
+{
     bool accepted = true;
 
     switch (dataBits) {
@@ -783,7 +807,8 @@ bool SerialLink::setDataBitsType(int dataBits) {
     return accepted;
 }
 
-bool SerialLink::setStopBitsType(int stopBits) {
+bool SerialLink::setStopBitsType(int stopBits)
+{
     bool reconnect = false;
     bool accepted = true;
     if(isConnected()) {

src/comm/SerialLink.h

@@ -106,7 +106,7 @@ public slots:
     bool setDataBitsType(int dataBits);
     bool setStopBitsType(int stopBits);
 
-    void readBytes(char* data, qint64 maxLength);
+    void readBytes();
     /**
      * @brief Write a number of bytes to the interface.
      *
@@ -118,7 +118,6 @@ public slots:
     bool disconnect();
 
 protected slots:
-    void emitBytesReady();
     void checkForBytes();
 
 protected:

src/comm/SerialSimulationLink.h

@@ -95,7 +95,7 @@ public slots:
 
 	void readLine();
         void writeBytes(char *bytes, qint64 length);
-	void readBytes(char *bytes, qint64 maxLength);
+        void readBytes();
 
 	bool connect();
 	bool disconnect();

src/comm/UDPLink.cc

@@ -120,8 +120,10 @@ void UDPLink::writeBytes(const char* data, qint64 size)
  * @param data Pointer to the data byte array to write the bytes to
  * @param maxLength The maximum number of bytes to write
  **/
-void UDPLink::readBytes(char* data, qint64 maxLength)
+void UDPLink::readBytes()
 {
+    const qint64 maxLength = 2048;
+    char data[maxLength];
     QHostAddress sender;
     quint16 senderPort;
 
@@ -169,14 +171,6 @@ qint64 UDPLink::bytesAvailable() {
     return socket->pendingDatagramSize();
 }
 
-/**
- * @brief Convenience method to emit the bytesReady signal
- **/
-void UDPLink::emitBytesReady()
-{
-    emit bytesReady(this);
-}
-
 /**
  * @brief Disconnect the connection.
  *
@@ -204,7 +198,7 @@ bool UDPLink::connect()
     socket = new QUdpSocket(this);
 
     //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
-    QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(emitBytesReady()));
+    QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
     connectState = socket->bind(host, port);
 

src/comm/UDPLink.h

@@ -48,8 +48,6 @@ public:
     UDPLink(QHostAddress host = QHostAddress::Any, quint16 port = 14550);
     ~UDPLink();
 
-    static const int poll_interval = UDP_POLL_INTERVAL; ///< Polling interval, defined in configuration.h
-
     bool isConnected();
     qint64 bytesAvailable();
 
@@ -86,7 +84,7 @@ public slots:
     void setPort(quint16 port);
     //    void readPendingDatagrams();
 
-    void readBytes(char* data, qint64 maxLength);
+    void readBytes();
     /**
      * @brief Write a number of bytes to the interface.
      *
@@ -97,10 +95,6 @@ public slots:
     bool connect();
     bool disconnect();
 
-protected slots:
-    void emitBytesReady();
-    //void checkForBytes();
-
 protected:
     QString name;
     QHostAddress host;

src/configuration.h

@@ -2,9 +2,7 @@
 #define CONFIGURATION_H
 
 /** @brief Polling interval in ms */
-#define SERIAL_POLL_INTERVAL 1
-/** @brief UDP polling interval in ms */
-#define UDP_POLL_INTERVAL 1
+#define SERIAL_POLL_INTERVAL 2
 
 /** @brief Heartbeat emission rate, in Hertz (times per second) */
 #define MAVLINK_HEARTBEAT_DEFAULT_RATE 1
@@ -12,6 +10,6 @@
 #define WITH_TEXT_TO_SPEECH 1
 
 #define QGC_APPLICATION_NAME "QGroundControl"
-#define QGC_APPLICATION_VERSION "v. 0.7.6 (Beta)"
+#define QGC_APPLICATION_VERSION "v. 0.7.7 (Beta)"
 
 #endif // CONFIGURATION_H

src/uas/UASWaypointManager.cc

@@ -386,6 +386,16 @@ void UASWaypointManager::localLoadWaypoints(const QString &loadFile)
     emit waypointListChanged();
 }
 
+void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
+{
+
+}
+
+int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
+{
+    return 0;
+}
+
 void UASWaypointManager::clearWaypointList()
 {
     if(current_state == WP_IDLE)

src/uas/UASWaypointManager.h

@@ -93,6 +93,13 @@ public:
     void localLoadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
     /*@}*/
 
+    /** @name Global waypoint list operations */
+    /*@{*/
+    const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
+    void globalAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
+    int globalRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
+    /*@}*/
+
 private:
     /** @name Message send functions */
     /*@{*/

src/ui/CommConfigurationWindow.cc

@@ -39,8 +39,12 @@ This file is part of the PIXHAWK project
 
 #include "CommConfigurationWindow.h"
 #include "SerialConfigurationWindow.h"
-#include "SerialLinkInterface.h"
+#include "SerialLink.h"
 #include "UDPLink.h"
+#include "MAVLinkSimulationLink.h"
+#ifdef OPAL_RT
+#include "OpalLink.h"
+#endif
 #include "MAVLinkProtocol.h"
 #include "MAVLinkSettingsWidget.h"
 
@@ -86,7 +90,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
     // TODO Move these calls to each link so that dynamic casts vanish
 
     // Open details pane for serial link if necessary
-    SerialLinkInterface* serial = dynamic_cast<SerialLinkInterface*>(link);
+    SerialLink* serial = dynamic_cast<SerialLink*>(link);
     if(serial != 0)
     {
         QWidget* conf = new SerialConfigurationWindow(serial, this);
@@ -97,11 +101,28 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
         //ui.linkGroupBox->setTitle(link->getName());
         //connect(link, SIGNAL(nameChanged(QString)), ui.linkGroupBox, SLOT(setTitle(QString)));
     }
-    else if (dynamic_cast<UDPLink*>(link) != 0)
+    UDPLink* udp = dynamic_cast<UDPLink*>(link);
+    if (udp != 0)
     {
         ui.linkGroupBox->setTitle(tr("UDP Link"));
     }
-    else
+    MAVLinkSimulationLink* sim = dynamic_cast<MAVLinkSimulationLink*>(link);
+    if (sim != 0)
+    {
+        ui.linkGroupBox->setTitle(tr("MAVLink Simulation Link"));
+    }
+#ifdef OPAL_RT
+    OpalLink* opal = dynamic_cast<OpalLink*>(link);
+    if (opal != 0)
+    {
+        ui.linkGroupBox->setTitle(tr("Opal-RT Link"));
+    }
+#endif
+    if (serial == 0 && udp == 0 && sim == 0
+#ifdef OPAL_RT
+        && opal == 0
+#endif
+        )
     {
         qDebug() << "Link is NOT a known link, can't open configuration window";
     }

src/ui/CommConfigurationWindow.h

@@ -40,6 +40,10 @@ This file is part of the PIXHAWK project
 #include "ProtocolInterface.h"
 #include "ui_CommSettings.h"
 
+#ifdef OPAL_RT
+#include "OpalLink.h"
+#endif
+
 class CommConfigurationWindow : public QWidget
 {
     Q_OBJECT

src/ui/HUD.cc

@@ -574,7 +574,7 @@ void HUD::paintHUD()
 
     if ((yawTrans < 5.0) && (yawTrans > -5.0)) yawTrans = 0;
 
-    qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw;
+    //qDebug() << "yaw translation" << yawTrans << "integral" << yawInt << "difference" << yawDiff << "yaw" << yaw;
 
     // Update scaling factor
     // adjust scaling to fit both horizontally and vertically

src/ui/MainWindow.cc

@@ -374,9 +374,9 @@ void MainWindow::addLink()
 
 void MainWindow::addLink(LinkInterface *link)
 {
+    LinkManager::instance()->addProtocol(link, mavlink);
     CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this);
     ui.menuNetwork->addAction(commWidget->getAction());
-    LinkManager::instance()->addProtocol(link, mavlink);
 
     // Special case for simulationlink
     MAVLinkSimulationLink* sim = dynamic_cast<MAVLinkSimulationLink*>(link);

src/ui/QGCDataPlot2D.cc

@@ -116,7 +116,7 @@ void QGCDataPlot2D::print()
     QPrintDialog dialog(&printer);
     if ( dialog.exec() )
     {
-        plot->setStyleSheet("QWidget { background-color: #FFFFFF; color: #000000; background-clip: border; font-size: 11pt;}");
+        plot->setStyleSheet("QWidget { background-color: #FFFFFF; color: #000000; background-clip: border; font-size: 10pt;}");
         plot->setCanvasBackground(Qt::white);
         QwtPlotPrintFilter filter;
         filter.color(Qt::white, QwtPlotPrintFilter::CanvasBackground);
@@ -132,6 +132,8 @@ void QGCDataPlot2D::print()
             filter.setOptions(options);
         }
         plot->print(printer, filter);
+        plot->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; font-size: 11pt;}");
+        //plot->setCanvasBackground(QColor(5, 5, 8));
     }
 }