diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index e4dc52056d27a54c9e0b37a8db3820c91209ba6a..0db01398ca72b69c6ce14a2587d9e1c2eafd1383 100644
--- a/src/comm/LinkManager.cc
+++ b/src/comm/LinkManager.cc
@@ -94,6 +94,8 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
     {
         // Protocol is new, add
         connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray)));
+        // Add status
+        connect(link, SIGNAL(connected(bool)), protocol, SLOT(linkStatusChanged(bool)));
         // Store the connection information in the protocol links map
         protocolLinks.insertMulti(protocol, link);
     }
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index e14d6bdd60cf55e23d28e8527f2c701ec1830256..36f3f0aee3c7f07aad1f9f3e8fb34b3503b9377c 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -174,6 +174,33 @@ QString MAVLinkProtocol::getLogfileName()
     }
 }
 
+void MAVLinkProtocol::linkStatusChanged(bool connected)
+{
+    LinkInterface* link = qobject_cast<LinkInterface*>(QObject::sender());
+
+    if (link) {
+        if (connected) {
+            // Send command to start MAVLink
+            // XXX hacky but safe
+            // Start NSH
+            const char init[] = {0x0d, 0x0d, 0x0d};
+            link->writeBytes(init, 1);
+            QGC::SLEEP::msleep(500);
+
+            // Stop any running mavlink instance
+            char* cmd = "mavlink stop\n";
+            link->writeBytes(cmd, strlen(cmd));
+            link->writeBytes(init, 2);
+            cmd = "uorb start";
+            link->writeBytes(cmd, strlen(cmd));
+            link->writeBytes(init, 2);
+            cmd = "sh /etc/init.d/rc.usb\n";
+            link->writeBytes(cmd, strlen(cmd));
+            link->writeBytes(init, 4);
+        }
+    }
+}
+
 /**
  * The bytes are copied by calling the LinkInterface::readBytes() method.
  * This method parses all incoming bytes and constructs a MAVLink packet.
diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h
index 03d8bfe14ceaed33676bb08c3eb7da33eb4b14eb..829b8e6ac46d556d2e5b379012328f49370365fc 100644
--- a/src/comm/MAVLinkProtocol.h
+++ b/src/comm/MAVLinkProtocol.h
@@ -128,6 +128,7 @@ public:
 public slots:
     /** @brief Receive bytes from a communication interface */
     void receiveBytes(LinkInterface* link, QByteArray b);
+    void linkStatusChanged(bool connected);
     /** @brief Send MAVLink message through serial interface */
     void sendMessage(mavlink_message_t message);
     /** @brief Send MAVLink message */
diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h
index 6f0e5839d86949900690d48948caeb98f258f5e7..758a0a79bcfe0cb7a44ac5bea445395d11735416 100644
--- a/src/comm/ProtocolInterface.h
+++ b/src/comm/ProtocolInterface.h
@@ -55,6 +55,7 @@ public:
 
 public slots:
     virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0;
+    virtual void linkStatusChanged(bool connected) = 0;
 
 signals:
     /** @brief Update the packet loss from one system */