diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index a7c721df2cca10ad3b2ea23a96efc204f56abc50..aeb4612ccc40739fcd4c1e6b856eb05483b6ef4a 100644 --- a/qgroundcontrol.pro +++ b/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 +} diff --git a/src/Core.cc b/src/Core.cc index 41a099fab6593ff5167122ec3acae44e5e7956b7..653df01fb4e433dc883746de76e53f0ae99a8e08 100644 --- a/src/Core.cc +++ b/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); diff --git a/src/Core.h b/src/Core.h index 2277a30685d91bc09de6df889ff4f86ea5dc3fba..78bb5f63905f9b1657f7bfea1cab01a2441caedb 100644 --- a/src/Core.h +++ b/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. * diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc new file mode 100644 index 0000000000000000000000000000000000000000..f9f3b87fcb3a4d718b735b1898b64133e20bf7da --- /dev/null +++ b/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()), + 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; +} diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h new file mode 100644 index 0000000000000000000000000000000000000000..1c9a40864dbf1de05cb2d57899a27e0f11315db4 --- /dev/null +++ b/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 +*/ + +#include +#include +#include +#include +#include +#include +#include + +#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* receiveBuffer; + QByteArray* sendBuffer; + + const int systemID; + const int componentID; + + +}; + +#endif // OPALLINK_H diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 61c1ed5170f53465baa46da113cc32e843b0a5e2..ea0b82bcbd242a13e68c9d1ab88dd78a69b18198 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -244,7 +244,6 @@ void UDPLink::setName(QString name) emit nameChanged(this->name); } - qint64 UDPLink::getNominalDataRate() { return 54000000; // 54 Mbit } diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 1fecb1b6e87c4c395b75db50ab73918173f4cc1d..53d672585e759cc21603aa5ef40772ff60b6db01 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/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(link) != 0) + { + ui.linkGroupBox->setTitle(tr("Opal-RT Link")); + } +#endif else { qDebug() << "Link is NOT a known link, can't open configuration window"; diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index 7d3c3d9020ce69754abd6eb9eb2d49867e91b96d..ab0e5100b3de9d209bbb23865d69e67cedaba02b 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/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