From 47e9e985f4b51a2527bf440ec17a5e0ad9afc2a9 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Fri, 4 Nov 2016 12:37:46 -0400 Subject: [PATCH] Checking for MAVLink 2.0 when first message is received. --- src/comm/MAVLinkProtocol.cc | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index bf1ce9852..4d9be09c4 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -212,19 +212,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) } if (decodeState == 1) { - decodedFirstPacket = true; - - /* - * Handled in Vehicle.cc now. - 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; - 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; + 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; + 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; } - */ if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { -- 2.22.0