MAVLink: Accept existing MAVLink 2 connection and accept link disconnect command.

src/Vehicle/Vehicle.cc

@@ -484,6 +484,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
@@ -513,8 +519,11 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     }
 
 
-    // Mark this vehicle as active
-    _connectionActive();
+    // Mark this vehicle as active - but only if the traffic is coming from
+    // the actual vehicle
+    if (message.sysid == _id) {
+        _connectionActive();
+    }
 
     // Give the plugin a change to adjust the message contents
     if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
@@ -569,6 +578,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;
@@ -853,7 +865,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) {
@@ -889,6 +903,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;
@@ -1076,9 +1113,11 @@ void Vehicle::_handleHeartbeat(mavlink_message_t& message)
 
 void Vehicle::_handleRadioStatus(mavlink_message_t& message)
 {
+
     //-- Process telemetry status message
     mavlink_radio_status_t rstatus;
     mavlink_msg_radio_status_decode(&message, &rstatus);
+
     int rssi    = rstatus.rssi;
     int remrssi = rstatus.remrssi;
     int lnoise = (int)(int8_t)rstatus.noise;
@@ -1958,9 +1997,15 @@ void Vehicle::_connectionLostTimeout(void)
     if (_connectionLostEnabled && !_connectionLost) {
         _connectionLost = true;
         _heardFrom = false;
+        _maxProtoVersion = 0;
         emit connectionLostChanged(true);
         _say(QString("%1 communication lost").arg(_vehicleIdSpeech()));
         if (_autoDisconnect) {
+
+            // Reset link state
+            for (int i = 0; i < _links.length(); i++) {
+                _mavlink->resetMetadataForLink(_links.at(i));
+            }
             disconnectInactiveVehicle();
         }
     }
@@ -1973,6 +2018,12 @@ void Vehicle::_connectionActive(void)
         _connectionLost = false;
         emit connectionLostChanged(false);
         _say(QString("%1 communication regained").arg(_vehicleIdSpeech()));
+
+        // Re-negotiate protocol version for the link
+        sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
+                        MAV_CMD_REQUEST_PROTOCOL_VERSION,
+                       false,                                   // No error shown if fails
+                        1);                                     // Request protocol version
     }
 }
 
@@ -2289,8 +2340,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

@@ -843,6 +843,7 @@ private:
     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/LinkManager.cc

@@ -191,6 +191,7 @@ void LinkManager::_addLink(LinkInterface* link)
     connect(link, &LinkInterface::bytesReceived,        _mavlinkProtocol,   &MAVLinkProtocol::receiveBytes);
 
     _mavlinkProtocol->resetMetadataForLink(link);
+    _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion());
 
     connect(link, &LinkInterface::connected,            this, &LinkManager::_linkConnected);
     connect(link, &LinkInterface::disconnected,         this, &LinkManager::_linkDisconnected);

src/comm/MAVLinkProtocol.cc

@@ -56,6 +56,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox)
     , versionMismatchIgnore(false)
     , systemId(255)
     , _current_version(100)
+    , _radio_version_mismatch_count(0)
     , _logSuspendError(false)
     , _logSuspendReplay(false)
     , _vehicleWasArmed(false)
@@ -152,7 +153,7 @@ void MAVLinkProtocol::storeSettings()
     // Parameter interface settings
 }
 
-void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
+void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
 {
     int channel = link->mavlinkChannel();
     totalReceiveCounter[channel] = 0;
@@ -160,6 +161,7 @@ void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
     totalErrorCounter[channel] = 0;
     currReceiveCounter[channel] = 0;
     currLossCounter[channel] = 0;
+    link->setDecodedFirstMavlinkPacket(false);
 }
 
 /**
@@ -194,9 +196,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         if (decodeState == 0 && !link->decodedFirstMavlinkPacket())
         {
             nonmavlinkCount++;
-            if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
+            if (nonmavlinkCount > 500 && !warnedUserNonMavlink)
             {
-                //2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
+                // 500 bytes with no mavlink message. Are we connected to a mavlink capable device?
                 if (!checkedUserNonMavlink)
                 {
                     link->requestReset();
@@ -217,13 +219,15 @@ 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;
+
+                    // Set all links to v2
+                    setVersion(200);
                 }
-                link->setDecodedFirstMavlinkPacket(true);
             }
 
             // Log data
@@ -270,6 +274,26 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                 emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type);
             }
 
+            // Detect if we are talking to an old radio not supporting v2
+            mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
+            if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
+                if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)
+                && !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
+
+                    _radio_version_mismatch_count++;
+                }
+            }
+
+            if (_radio_version_mismatch_count == 5) {
+                // Warn the user if the radio continues to send v1 while the link uses v2
+                emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Detected radio still using MAVLink v1.0 on a link with MAVLink v2.0 enabled. Please upgrade the radio firmware."));
+                // Ensure the warning can't get stuck
+                _radio_version_mismatch_count++;
+                // Flick link back to v1
+                qDebug() << "Switching outbound to mavlink 1.0 due to incoming mavlink 1.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
+                mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+            }
+
             // Increase receive counter
             totalReceiveCounter[mavlinkChannel]++;
             currReceiveCounter[mavlinkChannel]++;
@@ -358,6 +382,9 @@ void MAVLinkProtocol::_vehicleCountChanged(void)
     if (count == 0) {
         // Last vehicle is gone, close out logging
         _stopLogging();
+        // Reset protocol version handling
+        _current_version = 0;
+        _radio_version_mismatch_count = 0;
     }
 }
 

src/comm/MAVLinkProtocol.h

@@ -89,7 +89,7 @@ public:
     /**
      * Reset the counters for all metadata for this link.
      */
-    virtual void resetMetadataForLink(const LinkInterface *link);
+    virtual void resetMetadataForLink(LinkInterface *link);
     
     /// Suspend/Restart logging during replay.
     void suspendLogForReplay(bool suspend);
@@ -133,6 +133,7 @@ protected:
     bool versionMismatchIgnore;
     int systemId;
     unsigned _current_version;
+    unsigned _radio_version_mismatch_count;
 
 signals:
     /// Heartbeat received on link

src/comm/ProtocolInterface.h

@@ -62,7 +62,7 @@ public:
      * when reconnecting a link.
      * @param link The link to reset metadata for.
      */
-    virtual void resetMetadataForLink(const LinkInterface *link) = 0;
+    virtual void resetMetadataForLink(LinkInterface *link) = 0;
 
 public slots:
     virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0;

src/comm/SerialLink.h

@@ -137,6 +137,7 @@ public:
     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.