Commit 47e9e985 authored by Gus Grubba's avatar Gus Grubba

Checking for MAVLink 2.0 when first message is received.

parent 8f86bd29
......@@ -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)
{
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment