2424#include < QtCore/QMetaType>
2525#include < QtCore/QSettings>
2626#include < QtCore/QStandardPaths>
27+ #include < QtCore/QTimer>
2728
2829QGC_LOGGING_CATEGORY (MAVLinkProtocolLog, " Comms.MAVLinkProtocol" )
2930
@@ -59,16 +60,6 @@ void MAVLinkProtocol::init()
5960 _initialized = true ;
6061}
6162
62- void MAVLinkProtocol::setVersion (unsigned version)
63- {
64- const QList<SharedLinkInterfacePtr> sharedLinks = LinkManager::instance ()->links ();
65- for (const SharedLinkInterfacePtr &interface : sharedLinks) {
66- mavlink_set_proto_version (interface.get ()->mavlinkChannel (), version / 100 );
67- }
68-
69- _currentVersion = version;
70- }
71-
7263void MAVLinkProtocol::resetMetadataForLink (LinkInterface *link)
7364{
7465 const uint8_t channel = link->mavlinkChannel ();
@@ -119,7 +110,11 @@ void MAVLinkProtocol::receiveBytes(LinkInterface *link, const QByteArray &data)
119110 continue ;
120111 }
121112
122- _updateVersion (link, mavlinkChannel);
113+ if (status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
114+ link->reportMavlinkV1Traffic ();
115+ continue ;
116+ }
117+
123118 _updateCounters (mavlinkChannel, message);
124119 if (!linkPtr->linkConfiguration ()->isForwarding ()) {
125120 _forward (message);
@@ -133,25 +128,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface *link, const QByteArray &data)
133128 }
134129}
135130
136- void MAVLinkProtocol::_updateVersion (LinkInterface *link, uint8_t mavlinkChannel)
137- {
138- if (link->decodedFirstMavlinkPacket ()) {
139- return ;
140- }
141-
142- link->setDecodedFirstMavlinkPacket (true );
143- const mavlink_status_t *const mavlinkStatus = mavlink_get_channel_status (mavlinkChannel);
144-
145- if (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
146- return ;
147- }
148-
149- if (mavlink_get_proto_version (mavlinkChannel) == 1 ) {
150- qCDebug (MAVLinkProtocolLog) << " Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkChannel;
151- setVersion (200 );
152- }
153- }
154-
155131void MAVLinkProtocol::_updateCounters (uint8_t mavlinkChannel, const mavlink_message_t &message)
156132{
157133 _totalReceiveCounter[mavlinkChannel]++;
0 commit comments