Browse Source

Merge pull request #4194 from dogmaphobic/MAVLink2Test

Checking for MAVLink 2.0 when first message is received.
QGC4.4
Gus Grubba 9 years ago committed by GitHub
parent
commit
750072dc38
  1. 8
      src/comm/MAVLinkProtocol.cc

8
src/comm/MAVLinkProtocol.cc

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

Loading…
Cancel
Save