From 78bdc052c3893e7e1f27588c40ac9fad6e415be6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 1 Aug 2015 01:26:45 +0200
Subject: [PATCH] UAS: Connect message sending via buffered signals

---
 src/uas/UAS.cc | 21 +++++++++++++++++++++
 src/uas/UAS.h  |  6 ++++++
 2 files changed, 27 insertions(+)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 1cd89b6..e51de03 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -165,6 +165,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
         componentMulti[i] = false;
     }
 
+    connect(this, &UAS::_sendMessageOnThread, this, &UAS::_sendMessage, Qt::QueuedConnection);
+    connect(this, &UAS::_sendMessageOnThreadLink, this, &UAS::_sendMessageLink, Qt::QueuedConnection);
     connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), &fileManager, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
 
     // Store a list of available actions for this UAS.
@@ -1729,6 +1731,15 @@ void UAS::setModeArm(uint8_t newBaseMode, uint32_t newCustomMode)
 */
 void UAS::sendMessage(mavlink_message_t message)
 {
+    emit _sendMessageOnThread(message);
+}
+
+/**
+* Send a message to every link that is connected.
+* @param message that is to be sent
+*/
+void UAS::_sendMessage(mavlink_message_t message)
+{
     if (!LinkManager::instance())
     {
         qDebug() << "LINKMANAGER NOT AVAILABLE!";
@@ -1757,6 +1768,16 @@ void UAS::sendMessage(mavlink_message_t message)
 */
 void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
 {
+    emit _sendMessageOnThreadLink(link, message);
+}
+
+/**
+* Send a message to the link that is connected.
+* @param link that the message will be sent to
+* @message that is to be sent
+*/
+void UAS::_sendMessageLink(LinkInterface* link, mavlink_message_t message)
+{
     if(!link) return;
     // Create buffer
     uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index a9519ec..85a515f 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -894,6 +894,8 @@ signals:
     void groundSpeedChanged(double val, QString name);
     void airSpeedChanged(double val, QString name);
     void bearingToWaypointChanged(double val,QString name);
+    void _sendMessageOnThread(mavlink_message_t message);
+    void _sendMessageOnThreadLink(LinkInterface* link, mavlink_message_t message);
 protected:
     /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
     quint64 getUnixTime(quint64 time=0);
@@ -924,6 +926,10 @@ protected slots:
     void writeSettings();
     /** @brief Read settings from disk */
     void readSettings();
+    /** @brief Send a message over this link (to this or to all UAS on this link) */
+    void _sendMessageLink(LinkInterface* link, mavlink_message_t message);
+    /** @brief Send a message over all links this UAS can be reached with (!= all links) */
+    void _sendMessage(mavlink_message_t message);
     
 private slots:
     void _linkDisconnected(LinkInterface* link);