From 2402fc33f60ddca7e74ee7b596d92964d56127e4 Mon Sep 17 00:00:00 2001
From: Bryan Godbolt <godbolt@ualberta.ca>
Date: Wed, 18 Aug 2010 10:14:42 -0600
Subject: [PATCH] Heartbeats working, but uas not created

---
 src/comm/OpalLink.cc | 66 +++++++++++++++++++++++++++++++++++++++++++-
 src/comm/OpalLink.h  | 33 ++++++++++++++++++++--
 2 files changed, 96 insertions(+), 3 deletions(-)

diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index c74343a04..ad2ef1c07 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -1,12 +1,30 @@
 #include "OpalLink.h"
 
-OpalLink::OpalLink() : connectState(false)
+OpalLink::OpalLink() :
+        connectState(false),
+        heartbeatTimer(new QTimer(this)),
+        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
+        m_heartbeatsEnabled(true),
+        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
+    qDebug() << "OpalLink::OpalLink:: Connect the timer";
+    QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat()));
+    heartbeatTimer->start(1000/heartbeatRate);
+}
+
+void OpalLink::run()
+{
+    qDebug() << "OpalLink::run():: Starting the thread";
 }
 
 int OpalLink::getId()
@@ -134,4 +152,50 @@ 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::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::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);
+    }
+
 }
diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h
index 47dd1c21c..da2ed0abb 100644
--- a/src/comm/OpalLink.h
+++ b/src/comm/OpalLink.h
@@ -10,17 +10,28 @@
 #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"
 
 class OpalLink : public LinkInterface
 {
     Q_OBJECT
+
+public:
+    OpalLink();
     /* Connection management */
 
     int getId();
@@ -49,6 +60,8 @@ class OpalLink : public LinkInterface
 
     qint64 bytesAvailable();
 
+    void run();
+
 public slots:
 
 
@@ -57,9 +70,13 @@ public slots:
 
     void readBytes(char *bytes, qint64 maxLength);
 
+    void heartbeat();
+
+protected slots:
+
+    void receiveMessage(mavlink_message_t message);
+
 
-public:
-    OpalLink();
 
 protected:
     QString name;
@@ -75,10 +92,22 @@ protected:
     quint64 connectionStartTime;
 
     QMutex statisticsMutex;
+    QMutex receiveDataMutex;
     QString lastErrorMsg;
     void setLastErrorMsg();
 
     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
+    QQueue<QByteArray>* receiveBuffer;
+    QByteArray* sendBuffer;
+
+    const int systemID;
+    const int componentID;
+
+
 };
 
 #endif // OPALLINK_H
-- 
2.22.0