From e784c17ea1be75f434f7dbffb1199f55d4e88213 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 16 Apr 2013 14:07:09 +0200
Subject: [PATCH] Ask mavlink app to start on PX4 boards if connected via NSH

---
 src/comm/LinkManager.cc      |  2 ++
 src/comm/MAVLinkProtocol.cc  | 27 +++++++++++++++++++++++++++
 src/comm/MAVLinkProtocol.h   |  1 +
 src/comm/ProtocolInterface.h |  1 +
 4 files changed, 31 insertions(+)

diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index e4dc52056..0db01398c 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 e14d6bdd6..36f3f0aee 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 03d8bfe14..829b8e6ac 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 6f0e5839d..758a0a79b 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 */
-- 
2.22.0