diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index fa5069f157558ea824e20d2be67a9e749e2ac7b1..51f59ee92586258dc4b0c1e0c00fd9a0c57b28c9 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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; diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index a65d3fb3150000c7e08f0b4257f02a1cd6066b37..9361fb2b77faf1421d56d9db874a19e6d4d61aa7 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -26,6 +26,7 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config) , _mavlinkChannelSet(false) , _active(false) , _enableRateCollection(false) + , _decodedFirstMavlinkPacket(false) { _config->setLink(this); diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 63ce682571b03177e969e6d1cbd214e4e511f31d..477b8a9e49593afcc8d0bb2da52b0fea823058eb 100644 --- a/src/comm/LinkInterface.h +++ b/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); @@ -263,8 +266,9 @@ private: mutable QMutex _dataRateMutex; // Mutex for accessing the data rate member variables - bool _active; ///< true: link is actively receiving mavlink messages + 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 SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index e014464d30c8f228eee59a8e923d4d30ad374818..da028d086a1fec5953fc003f09e2be5e153ba843 100644 --- a/src/comm/LinkManager.cc +++ b/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; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 4fee754823bece8ff6dab81441264a857508ca18..17f1e853c412a4b3142e3633c38a2b2e70fee6f3 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/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)