From edcb532f2642541186e70a4c220ce6cab2d7e17d Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Tue, 12 Apr 2016 18:45:28 -0700
Subject: [PATCH] Auto-Disconnect on cable pull

---
 src/comm/LinkInterface.h |  3 +++
 src/comm/LinkManager.cc  | 18 +++++++++++++++---
 src/comm/LinkManager.h   |  1 +
 src/comm/SerialLink.cc   | 13 ++++++++-----
 4 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h
index bd0b48e2f..8e11d13d2 100644
--- a/src/comm/LinkInterface.h
+++ b/src/comm/LinkInterface.h
@@ -170,6 +170,9 @@ signals:
     void activeChanged(bool active);
     void _invokeWriteBytes(QByteArray);
 
+    /// Signalled when a link suddenly goes away due to it being removed by for example pulling the cable to the connection.
+    void connectionRemoved(LinkInterface* link);
+
     /**
      * @brief New data arrived
      *
diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index 0712826c3..f7be0fb18 100644
--- a/src/comm/LinkManager.cc
+++ b/src/comm/LinkManager.cc
@@ -214,8 +214,12 @@ void LinkManager::_addLink(LinkInterface* link)
 
     _mavlinkProtocol->resetMetadataForLink(link);
 
-    connect(link, &LinkInterface::connected,    this, &LinkManager::_linkConnected);
-    connect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected);
+    connect(link, &LinkInterface::connected,            this, &LinkManager::_linkConnected);
+    connect(link, &LinkInterface::disconnected,         this, &LinkManager::_linkDisconnected);
+
+    // This connection is queued since it will cloe the link. So we want the link emitter to return otherwise we would
+    // close the link our from under itself.
+    connect(link, &LinkInterface::connectionRemoved,    this, &LinkManager::_linkConnectionRemoved, Qt::QueuedConnection);
 }
 
 void LinkManager::disconnectAll(void)
@@ -239,7 +243,9 @@ bool LinkManager::connectLink(LinkInterface* link)
 
 void LinkManager::disconnectLink(LinkInterface* link)
 {
-    Q_ASSERT(link);
+    if (!link || !_links.contains(link)) {
+        return;
+    }
 
     link->_disconnect();
     LinkConfiguration* config = link->getLinkConfiguration();
@@ -306,6 +312,12 @@ void LinkManager::_linkDisconnected(void)
     emit linkDisconnected((LinkInterface*)sender());
 }
 
+void LinkManager::_linkConnectionRemoved(LinkInterface* link)
+{
+    // Link has been removed from system, disconnect it automatically
+    disconnectLink(link);
+}
+
 void LinkManager::suspendConfigurationUpdates(bool suspend)
 {
     _configUpdateSuspended = suspend;
diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h
index 8c11ff2cb..2ef786374 100644
--- a/src/comm/LinkManager.h
+++ b/src/comm/LinkManager.h
@@ -202,6 +202,7 @@ signals:
 private slots:
     void _linkConnected(void);
     void _linkDisconnected(void);
+    void _linkConnectionRemoved(LinkInterface* link);
 #ifndef __ios__
     void _activeLinkCheck(void);
 #endif
diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
index ae702b5fb..65f2cfc17 100644
--- a/src/comm/SerialLink.cc
+++ b/src/comm/SerialLink.cc
@@ -182,10 +182,6 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
     }
 
     _port = new QSerialPort(_config->portName());
-    if (!_port) {
-        emit communicationUpdate(getName(),"Error opening port: " + _config->portName());
-        return false; // couldn't create serial port.
-    }
 
     QObject::connect(_port, static_cast<void (QSerialPort::*)(QSerialPort::SerialPortError)>(&QSerialPort::error),
                      this, &SerialLink::linkError);
@@ -249,12 +245,19 @@ void SerialLink::_readBytes(void)
 
 void SerialLink::linkError(QSerialPort::SerialPortError error)
 {
-    if (error != QSerialPort::NoError) {
+    switch (error) {
+    case QSerialPort::NoError:
+        break;
+    case QSerialPort::ResourceError:
+        emit connectionRemoved(this);
+        break;
+    default:
         // You can use the following qDebug output as needed during development. Make sure to comment it back out
         // when you are done. The reason for this is that this signal is very noisy. For example if you try to
         // connect to a PixHawk before it is ready to accept the connection it will output a continuous stream
         // of errors until the Pixhawk responds.
         //qCDebug(SerialLinkLog) << "SerialLink::linkError" << error;
+        break;
     }
 }
 
-- 
2.22.0