working for test

qgroundcontrol.pro

@@ -153,7 +153,8 @@ HEADERS += src/MG.h \
     src/ui/QGCPxImuFirmwareUpdate.h \
     src/comm/MAVLinkLightProtocol.h \
     src/ui/QGCDataPlot2D.h \
-    src/ui/linechart/IncrementalPlot.h
+    src/ui/linechart/IncrementalPlot.h \
+    src/comm/OpalRT.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -219,7 +220,8 @@ RESOURCES = mavground.qrc
 
 # Include RT-LAB Library
 win32 { 
-    LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin -lOpalApi
+    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

src/Core.cc

@@ -138,7 +138,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
     // Add OpalRT Link, but do not connect
     OpalLink* opalLink = new OpalLink();
     mainWindow->addLink(opalLink);
-#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");

src/comm/OpalLink.cc

@@ -35,7 +35,7 @@ OpalLink::OpalLink() :
         heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
         m_heartbeatsEnabled(true),
         getSignalsTimer(new QTimer(this)),
-        getSignalsPeriod(1000),
+        getSignalsPeriod(10),
         receiveBuffer(new QQueue<QByteArray>()),
         systemID(1),
         componentID(1)
@@ -74,22 +74,11 @@ 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";
+//    qDebug() << "OpalLink::readBytes(): Reading a message.  size of buffer: " << receiveBuffer->count();
+//    QByteArray message = receiveBuffer->dequeue();
 
-        memcpy(bytes, message.data(), maxLength);
-    }
-    else
-    {
-        memcpy(bytes, message.data(), message.size());
-    }
 
-    emit bytesReceived(this, message);
+    emit bytesReceived(this, receiveBuffer->dequeue());
     receiveDataMutex.unlock();
 
 }
@@ -104,7 +93,9 @@ void OpalLink::receiveMessage(mavlink_message_t message)
     // If link is connected
     if (isConnected())
     {
+        receiveDataMutex.lock();
         receiveBuffer->enqueue(QByteArray(buffer, len));
+        receiveDataMutex.unlock();
         readBytes();
     }
 
@@ -115,7 +106,7 @@ void OpalLink::heartbeat()
 
     if (m_heartbeatsEnabled)
     {
-        qDebug() << "OpalLink::heartbeat(): Generate a heartbeat";
+//        qDebug() << "OpalLink::heartbeat(): Generate a heartbeat";
         mavlink_message_t beat;
         mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC);
         receiveMessage(beat);
@@ -125,41 +116,57 @@ void OpalLink::heartbeat()
 
 void OpalLink::getSignals()
 {
+//    getSignalsMutex.lock();
 //    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)
-//    {
+
+
+    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);
+
+    while (!(*lastValues))
+    {
+        int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep,
+                                       values, lastValues, decimation);
+
+        if (returnVal == EOK )
+        {
+            //        qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues);
+            mavlink_message_t local_position;
+            mavlink_msg_local_position_pack(systemID, componentID, &local_position,
+                                            (*timestep)*1000000,
+                                            values[OpalRT::X_POS],
+                                            values[OpalRT::Y_POS],
+                                            values[OpalRT::Z_POS],
+                                            values[OpalRT::X_VEL],
+                                            values[OpalRT::Y_VEL],
+                                            values[OpalRT::Z_VEL]);
+            receiveMessage(local_position);
+        }
+        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;
+        }
+        // if returnVal == EAGAIN => data just wasn't ready
+        else if (returnVal != EAGAIN)
+        {
+            getSignalsTimer->stop();
+            displayErrorMsg();
+        }
+    }
+
+    /* deallocate used memory */
+
+    delete numSignals;
+    delete timestep;
+    delete lastValues;
+    delete decimation;
+//    getSignalsMutex.unlock();
 
 }
 
@@ -170,7 +177,7 @@ void OpalLink::getSignals()
  */
 void OpalLink::run()
 {
-    qDebug() << "OpalLink::run():: Starting the thread";
+//    qDebug() << "OpalLink::run():: Starting the thread";
 }
 
 int OpalLink::getId()
@@ -202,18 +209,18 @@ bool OpalLink::connect()
     short modelState;
 
     /// \todo allow configuration of instid in window
-//    if (OpalConnect(101, false, &modelState) == EOK)
-//    {
+    if (OpalConnect(101, false, &modelState) == EOK)
+    {
         connectState = true;
         emit connected();
         heartbeatTimer->start(1000/heartbeatRate);
         getSignalsTimer->start(getSignalsPeriod);
-//    }
-//    else
-//    {
-//        connectState = false;
-//        displayErrorMsg();
-//    }
+    }
+    else
+    {
+        connectState = false;
+        displayErrorMsg();
+    }
 
     emit connected(connectState);
     return connectState;
@@ -235,11 +242,11 @@ void OpalLink::displayErrorMsg()
 
 void OpalLink::setLastErrorMsg()
 {
-//    char buf[512];
-//    unsigned short len;
-//    OpalGetLastErrMsg(buf, sizeof(buf), &len);
-//    lastErrorMsg.clear();
-//    lastErrorMsg.append(buf);
+    char buf[512];
+    unsigned short len;
+    OpalGetLastErrMsg(buf, sizeof(buf), &len);
+    lastErrorMsg.clear();
+    lastErrorMsg.append(buf);
 }
 
 

src/comm/OpalLink.h

@@ -44,15 +44,13 @@ This file is part of the QGROUNDCONTROL project
 #include "mavlink.h"
 #include "mavlink_types.h"
 #include "configuration.h"
+#include "OpalRT.h"
 
 #include "errno.h"
 
-
-
-
-// FIXME
-//#include "OpalApi.h"
-
+#ifdef OPAL_RT
+#include "OpalApi.h"
+#endif
 
 
 
@@ -66,9 +64,9 @@ This file is part of the QGROUNDCONTROL project
 #define NUM_OUTPUT_SIGNALS 6
 
 /**
- * @brief Interface to OPAL-RT targets
+ * @brief Interface to OpalRT targets
  *
- * This is an interface to the Opal-RT hardware-in-the-loop (HIL) simulator.
+ * This is an interface to the OpalRT hardware-in-the-loop (HIL) simulator.
  * This class receives MAVLink packets as if it is a true link, but it
  * interprets the packets internally, and calls the appropriate api functions.
  *
@@ -144,6 +142,7 @@ protected:
 
     QMutex statisticsMutex;
     QMutex receiveDataMutex;
+//    QMutex getSignalsMutex;
     QString lastErrorMsg;
     void setLastErrorMsg();
     void displayErrorMsg();