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/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"