Merge branch 'dev' of http://github.com/godbolt/qgroundcontrol

qgroundcontrol.pro

@@ -216,3 +216,12 @@ 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
+}

src/Core.cc

@@ -132,6 +132,11 @@ 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);
+#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/OpalLink.cc

@@ -0,0 +1,269 @@
+#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(char *bytes, qint64 maxLength)
+{
+
+    receiveDataMutex.lock();
+    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));
+        emit bytesReady(this);
+    }
+
+}
+
+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

@@ -0,0 +1,126 @@
+#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"
+#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(char *bytes, qint64 maxLength);
+
+    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/UDPLink.cc

@@ -244,7 +244,6 @@ void UDPLink::setName(QString name)
     emit nameChanged(this->name);
 }
 
-
 qint64 UDPLink::getNominalDataRate() {
     return 54000000; // 54 Mbit
 }

src/ui/CommConfigurationWindow.cc

@@ -101,6 +101,12 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
     {
         ui.linkGroupBox->setTitle(tr("UDP Link"));
     }
+#ifdef OPAL_RT
+    else if (dynamic_cast<OpalLink*>(link) != 0)
+    {
+        ui.linkGroupBox->setTitle(tr("Opal-RT Link"));
+    }
+#endif
     else
     {
         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