MAVLink: Accept existing MAVLink 2 connection and accept link disconn…

…ect command.

src/Vehicle/Vehicle.cc

@@ -480,6 +480,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
 
     if (!_containsLink(link)) {
         _addLink(link);
+
+        // if the minimum supported version of MAVLink is already 2.0
+        // set our max proto version to it.
+        if (_mavlink->getCurrentVersion() >= 200) {
+            _maxProtoVersion = _mavlink->getCurrentVersion();
+        }
     }
 
     //-- Check link status
@@ -560,6 +566,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_COMMAND_ACK:
         _handleCommandAck(message);
         break;
+    case MAVLINK_MSG_ID_COMMAND_LONG:
+        _handleCommandLong(message);
+        break;
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
         _handleAutopilotVersion(link, message);
         break;
@@ -840,7 +849,9 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION && ack.result != MAV_RESULT_ACCEPTED) {
         // The autopilot does not understand the request and consequently is likely handling only
         // MAVLink 1
-        _setMaxProtoVersion(100);
+        if (_maxProtoVersion == 0) {
+            _setMaxProtoVersion(100);
+        }
     }
 
     if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
@@ -876,6 +887,29 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     _sendNextQueuedMavCommand();
 }
 
+void Vehicle::_handleCommandLong(mavlink_message_t& message)
+{
+    mavlink_command_long_t cmd;
+    mavlink_msg_command_long_decode(&message, &cmd);
+
+    switch (cmd.command) {
+        // Other component on the same system
+        // requests that QGC frees up the serial port of the autopilot
+        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+            if (cmd.param6 > 0) {
+                // disconnect the USB link if its a direct connection to a Pixhawk
+                for (int i = 0; i < _links.length(); i++) {
+                    SerialLink *sl = qobject_cast<SerialLink*>(_links.at(i));
+                    if (sl && sl->getSerialConfig()->usbDirect()) {
+                        qDebug() << "Disconnecting:" << _links.at(i)->getName();
+                        qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i));
+                    }
+                }
+            }
+        break;
+    }
+}
+
 void Vehicle::_handleExtendedSysState(mavlink_message_t& message)
 {
     mavlink_extended_sys_state_t extendedState;
@@ -2273,8 +2307,11 @@ void Vehicle::_sendMavCommandAgain(void)
         }
 
         if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
-            // We aren't going to get a response back for the protocol version, so assume v1 is all we can do
-            _setMaxProtoVersion(100);
+            // We aren't going to get a response back for the protocol version, so assume v1 is all we can do.
+            // If the max protocol version is uninitialized, fall back to v1.
+            if (_maxProtoVersion == 0) {
+                _setMaxProtoVersion(100);
+            }
         }
 
         emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);

src/Vehicle/Vehicle.h

@@ -832,6 +832,7 @@ private slots:
     void _handleVibration(mavlink_message_t& message);
     void _handleExtendedSysState(mavlink_message_t& message);
     void _handleCommandAck(mavlink_message_t& message);
+    void _handleCommandLong(mavlink_message_t& message);
     void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message);
     void _handleProtocolVersion(LinkInterface* link, mavlink_message_t& message);
     void _handleHilActuatorControls(mavlink_message_t& message);

src/comm/MAVLinkProtocol.cc

@@ -217,13 +217,34 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         if (decodeState == 1)
         {
             if (!link->decodedFirstMavlinkPacket()) {
+                link->setDecodedFirstMavlinkPacket(true);
                 mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
                 if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
                     qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
                     mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-                    _current_version = 200;
+
+                    // check which links are active and MAVLink 2.0
+                    QList<LinkInterface*> links = _linkMgr->links();
+
+                    for (int i = 0; i < links.length(); i++) {
+                        mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(links[i]->mavlinkChannel());
+
+                        bool active_mavlink1 = false;
+
+                        // if the link is set to MAVLink 1 and it has seen incoming traffic
+                        // flag as MAVLink 1 being active
+                        if (((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0)
+                            && (links[i]->decodedFirstMavlinkPacket())) {
+                            active_mavlink1 = true;
+                        }
+
+                        // there is no active MAVLink 1 connection, so we can
+                        // upgrade to MAVLink 2
+                        if (!active_mavlink1 && _current_version < 200) {
+                            _current_version = 200;
+                        }
+                    }
                 }
-                link->setDecodedFirstMavlinkPacket(true);
             }
 
             // Log data

src/comm/SerialLink.h

@@ -137,6 +137,7 @@ class SerialLink : public LinkInterface
     void    requestReset();
     bool    isConnected() const;
     qint64  getConnectionSpeed() const;
+    SerialConfiguration* getSerialConfig() const { return _serialConfig; }
 
     // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
     // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.