Merge pull request #4359 from DonLakeFlyer/MAv2Switch

Update MAV 2 switching semantics

src/Vehicle/Vehicle.cc

@@ -563,15 +563,11 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
 
 void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
 {
+    Q_UNUSED(link);
+
     mavlink_autopilot_version_t autopilotVersion;
     mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);
 
-    bool isMavlink2 = (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0;
-    if(isMavlink2) {
-        mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->mavlinkChannel());
-        mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-    }
-
     if (autopilotVersion.flight_sw_version != 0) {
         int majorVersion, minorVersion, patchVersion;
         FIRMWARE_VERSION_TYPE versionType;

src/comm/LinkInterface.cc

@@ -26,6 +26,7 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
     , _mavlinkChannelSet(false)
     , _active(false)
     , _enableRateCollection(false)
+    , _decodedFirstMavlinkPacket(false)
 {
     _config->setLink(this);
 

src/comm/LinkInterface.h

@@ -116,6 +116,9 @@ public:
     /// set into the link when it is added to LinkManager
     uint8_t mavlinkChannel(void) const;
 
+    bool decodedFirstMavlinkPacket(void) const { return _decodedFirstMavlinkPacket; }
+    bool setDecodedFirstMavlinkPacket(bool decodedFirstMavlinkPacket) { return _decodedFirstMavlinkPacket = decodedFirstMavlinkPacket; }
+
     // 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.
     bool connect(void);
@@ -265,6 +268,7 @@ private:
 
     bool _active;                       ///< true: link is actively receiving mavlink messages
     bool _enableRateCollection;
+    bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;

src/comm/LinkManager.cc

@@ -191,7 +191,7 @@ void LinkManager::_addLink(LinkInterface* link)
                 // Start the channel on Mav 1 protocol
                 mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(i);
                 mavlinkStatus->flags = mavlink_get_channel_status(i)->flags | MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-                qDebug() << "LinkManager mavlinkStatus" << mavlinkStatus << i << mavlinkStatus->flags;
+                qDebug() << "LinkManager mavlinkStatus:channel:flags" << mavlinkStatus << i << mavlinkStatus->flags;
                 _mavlinkChannelsUsedBitMask |= 1 << i;
                 channelSet = true;
                 break;

src/comm/MAVLinkProtocol.cc

@@ -169,29 +169,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
 
     int mavlinkChannel = link->mavlinkChannel();
 
-    static int mavlink09Count = 0;
     static int nonmavlinkCount = 0;
-    static bool decodedFirstPacket = false;
-    static bool warnedUser = false;
     static bool checkedUserNonMavlink = false;
     static bool warnedUserNonMavlink = false;
 
     for (int position = 0; position < b.size(); position++) {
         unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status);
 
-        if ((uint8_t)b[position] == 0x55) mavlink09Count++;
-        if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
-        {
-            warnedUser = true;
-            // Obviously the user tries to use a 0.9 autopilot
-            // with QGroundControl built for version 1.0
-            emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
-                                                                  "Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. "
-                                                                  "Please upgrade the MAVLink version of your autopilot. "
-                                                                  "If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."));
-        }
-
-        if (decodeState == 0 && !decodedFirstPacket)
+        if (decodeState == 0 && !link->decodedFirstMavlinkPacket())
         {
             nonmavlinkCount++;
             if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
@@ -212,16 +197,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         }
         if (decodeState == 1)
         {
-            if(!decodedFirstPacket) {
+            if (!link->decodedFirstMavlinkPacket()) {
                 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() << "switch to mavlink 2.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
+                    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;
-                } else if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
-                    qDebug() << "switch to mavlink 1.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
-                    mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
                 }
-                decodedFirstPacket = true;
+                link->setDecodedFirstMavlinkPacket(true);
             }
 
             if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)