getSignals() seems to work, but causes crash

src/comm/OpalLink.cc

@@ -5,6 +5,8 @@ OpalLink::OpalLink() :
         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)
@@ -17,85 +19,153 @@ OpalLink::OpalLink() :
     LinkManager::instance()->add(this);
 
     // Start heartbeat timer, emitting a heartbeat at the configured rate
-    qDebug() << "OpalLink::OpalLink:: Connect the timer";
     QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat()));
-    heartbeatTimer->start(1000/heartbeatRate);
+
+    QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals()));
 }
 
-void OpalLink::run()
+
+/*
+ *
+  Communication
+ *
+ */
+
+qint64 OpalLink::bytesAvailable()
 {
-    qDebug() << "OpalLink::run():: Starting the thread";
+    return 0;
 }
 
-int OpalLink::getId()
+void OpalLink::writeBytes(const char *bytes, qint64 length)
 {
-    return id;
+
 }
 
-QString OpalLink::getName()
+
+void OpalLink::readBytes(char *bytes, qint64 maxLength)
 {
-    return name;
+
+    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::setName(QString name)
+void OpalLink::receiveMessage(mavlink_message_t message)
 {
-    this->name = name;
-    emit nameChanged(this->name);
-}
 
-bool OpalLink::isConnected() {
-    //qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
-    return connectState;
+    // 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);
+    }
+
 }
 
-qint64 OpalLink::getNominalDataRate()
+void OpalLink::heartbeat()
 {
-    return 0; //unknown
+
+    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);
+    }
+
 }
 
-int OpalLink::getLinkQuality()
+void OpalLink::getSignals()
 {
-    return -1; //not supported
+    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;
+
 }
 
-qint64 OpalLink::getTotalUpstream()
+/*
+ *
+  Administrative
+ *
+ */
+void OpalLink::run()
 {
-    statisticsMutex.lock();
-    qint64 totalUpstream =  bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
-    statisticsMutex.unlock();
-    return totalUpstream;
+    qDebug() << "OpalLink::run():: Starting the thread";
 }
 
-qint64 OpalLink::getTotalDownstream() {
-    statisticsMutex.lock();
-    qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
-    statisticsMutex.unlock();
-    return totalDownstream;
+int OpalLink::getId()
+{
+    return id;
 }
 
-qint64 OpalLink::getCurrentUpstream()
+QString OpalLink::getName()
 {
-    return 0; //unknown
+    return name;
 }
 
-qint64 OpalLink::getMaxUpstream()
+void OpalLink::setName(QString name)
 {
-    return 0; //unknown
+    this->name = name;
+    emit nameChanged(this->name);
 }
 
-qint64 OpalLink::getBitsSent() {
-    return bitsSentTotal;
+bool OpalLink::isConnected() {
+    //qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
+    return connectState;
 }
 
-qint64 OpalLink::getBitsReceived() {
-    return bitsReceivedTotal;
-}
 
 
-bool OpalLink::isFullDuplex()
-{
-    return false;
-}
 
 bool OpalLink::connect()
 {
@@ -105,22 +175,17 @@ bool OpalLink::connect()
     if (OpalConnect(101, false, &modelState) == EOK)
     {
         connectState = true;
+        emit connected();
+        heartbeatTimer->start(1000/heartbeatRate);
+        getSignalsTimer->start(getSignalsPeriod);
     }
     else
     {
         connectState = false;
-        setLastErrorMsg();
-        QMessageBox msgBox;
-        msgBox.setIcon(QMessageBox::Critical);
-        msgBox.setText(lastErrorMsg);
-        msgBox.exec();
+        displayErrorMsg();
     }
 
     emit connected(connectState);
-    if (connectState)
-    {
-        emit connected();
-    }
     return connectState;
 }
 
@@ -129,6 +194,15 @@ 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];
@@ -138,64 +212,58 @@ void OpalLink::setLastErrorMsg()
     lastErrorMsg.append(buf);
 }
 
-qint64 OpalLink::bytesAvailable()
+
+/*
+ *
+  Statisctics
+ *
+ */
+
+qint64 OpalLink::getNominalDataRate()
 {
-    return 0;
+    return 0; //unknown
 }
 
-void OpalLink::writeBytes(const char *bytes, qint64 length)
+int OpalLink::getLinkQuality()
 {
-
+    return -1; //not supported
 }
 
-
-void OpalLink::readBytes(char *bytes, qint64 maxLength)
+qint64 OpalLink::getTotalUpstream()
 {
+    statisticsMutex.lock();
+    qint64 totalUpstream =  bitsSentTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
+    statisticsMutex.unlock();
+    return totalUpstream;
+}
 
-    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();
-
+qint64 OpalLink::getTotalDownstream() {
+    statisticsMutex.lock();
+    qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
+    statisticsMutex.unlock();
+    return totalDownstream;
 }
 
-void OpalLink::heartbeat()
+qint64 OpalLink::getCurrentUpstream()
 {
+    return 0; //unknown
+}
 
-    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);
-    }
+qint64 OpalLink::getMaxUpstream()
+{
+    return 0; //unknown
+}
 
+qint64 OpalLink::getBitsSent() {
+    return bitsSentTotal;
 }
 
-void OpalLink::receiveMessage(mavlink_message_t message)
-{
+qint64 OpalLink::getBitsReceived() {
+    return bitsReceivedTotal;
+}
 
-    // 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);
-    }
 
+bool OpalLink::isFullDuplex()
+{
+    return false;
 }

src/comm/OpalLink.h

@@ -26,6 +26,12 @@
 #include "OpalApi.h"
 #include "string.h"
 
+/*
+  Configuration info for the model
+ */
+
+#define NUM_OUTPUT_SIGNALS 6
+
 class OpalLink : public LinkInterface
 {
     Q_OBJECT
@@ -72,6 +78,8 @@ public slots:
 
     void heartbeat();
 
+    void getSignals();
+
 protected slots:
 
     void receiveMessage(mavlink_message_t message);
@@ -95,12 +103,17 @@ protected:
     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;
 

src/comm/UDPLink.cc

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