Merge pull request #4194 from dogmaphobic/MAVLink2Test

Checking for MAVLink 2.0 when first message is received.

src/comm/MAVLinkProtocol.cc

@@ -212,10 +212,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         }
         if (decodeState == 1)
         {
-            decodedFirstPacket = true;
-
-            /*
-             * Handled in Vehicle.cc now.
+            if(!decodedFirstPacket) {
                 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;
@@ -224,7 +221,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                     qDebug() << "switch to mavlink 1.0" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
                     mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
                 }
-            */
+                decodedFirstPacket = true;
+            }
 
             if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
             {