Skip to content

Commit 3c75fe8

Browse files
committed
Remove support for MAVLink v1 message protocol
Notify user if v1 traffic is detected on link
1 parent 9fd3fd9 commit 3c75fe8

22 files changed

+30
-251
lines changed

src/Comms/LinkInterface.cc

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -140,3 +140,19 @@ void LinkInterface::setSigningSignatureFailure(bool failure)
140140
}
141141
}
142142
}
143+
144+
void LinkInterface::reportMavlinkV1Traffic()
145+
{
146+
if (!_mavlinkV1TrafficReported) {
147+
_mavlinkV1TrafficReported = true;
148+
149+
const SharedLinkConfigurationPtr linkConfig = linkConfiguration();
150+
const QString linkName = linkConfig ? linkConfig->name() : QStringLiteral("unknown");
151+
qCWarning(LinkInterfaceLog) << "MAVLink v1 traffic detected on link" << linkName;
152+
const QString message = tr("MAVLink v1 traffic detected on link '%1'. "
153+
"%2 only supports MAVLink v2. "
154+
"Please ensure your vehicle is configured to use MAVLink v2.")
155+
.arg(linkName).arg(qgcApp()->applicationName());
156+
qgcApp()->showAppMessage(message);
157+
}
158+
}

src/Comms/LinkInterface.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class LinkInterface : public QObject
4646
void removeVehicleReference();
4747
bool initMavlinkSigning();
4848
void setSigningSignatureFailure(bool failure);
49+
void reportMavlinkV1Traffic();
4950

5051
signals:
5152
void bytesReceived(LinkInterface *link, const QByteArray &data);
@@ -80,6 +81,7 @@ private slots:
8081
bool _decodedFirstMavlinkPacket = false;
8182
int _vehicleReferenceCount = 0;
8283
bool _signingSignatureFailure = false;
84+
bool _mavlinkV1TrafficReported = false;
8385
};
8486

8587
typedef std::shared_ptr<LinkInterface> SharedLinkInterfacePtr;

src/Comms/LinkManager.cc

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,6 @@ bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr &config)
169169
(void) connect(link.get(), &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected);
170170

171171
MAVLinkProtocol::instance()->resetMetadataForLink(link.get());
172-
MAVLinkProtocol::instance()->setVersion(MAVLinkProtocol::instance()->getCurrentVersion());
173172

174173
// Try to connect before adding to active links list
175174
if (!link->_connect()) {

src/Comms/MAVLinkProtocol.cc

Lines changed: 6 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include <QtCore/QMetaType>
2525
#include <QtCore/QSettings>
2626
#include <QtCore/QStandardPaths>
27+
#include <QtCore/QTimer>
2728

2829
QGC_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-
7263
void 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-
155131
void MAVLinkProtocol::_updateCounters(uint8_t mavlinkChannel, const mavlink_message_t &message)
156132
{
157133
_totalReceiveCounter[mavlinkChannel]++;

src/Comms/MAVLinkProtocol.h

Lines changed: 0 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -51,21 +51,12 @@ class MAVLinkProtocol : public QObject
5151
/// Get the component id of this application
5252
static int getComponentId() { return MAV_COMP_ID_MISSIONPLANNER; }
5353

54-
/// Get the protocol version
55-
static int getVersion() { return MAVLINK_VERSION; }
56-
57-
/// Get the currently configured protocol version
58-
unsigned getCurrentVersion() const { return _currentVersion; }
59-
6054
/// Reset the counters for all metadata for this link.
6155
void resetMetadataForLink(LinkInterface *link);
6256

6357
/// Suspend/Restart logging during replay.
6458
void suspendLogForReplay(bool suspend) { _logSuspendReplay = suspend; }
6559

66-
/// Set protocol version
67-
void setVersion(unsigned version);
68-
6960
/// Checks the temp directory for log files which may have been left there.
7061
/// This could happen if QGC crashes without the temp log file being saved.
7162
/// Give the user an option to save these orphaned files.
@@ -107,7 +98,6 @@ private slots:
10798

10899
void _updateCounters(uint8_t mavlinkChannel, const mavlink_message_t &message);
109100
bool _updateStatus(LinkInterface *link, const SharedLinkInterfacePtr linkPtr, uint8_t mavlinkChannel, const mavlink_message_t &message);
110-
void _updateVersion(LinkInterface *link, uint8_t mavlinkChannel);
111101

112102
void _saveTelemetryLog(const QString &tempLogfile);
113103
bool _checkTelemetrySavePath();
@@ -124,7 +114,6 @@ private slots:
124114
uint64_t _totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]{}; ///< Total messages lost during transmission.
125115
float _runningLossPercent[MAVLINK_COMM_NUM_BUFFERS]{}; ///< Loss rate
126116

127-
unsigned _currentVersion = 100;
128117
bool _initialized = false;
129118

130119
static constexpr const char *_tempLogFileTemplate = "FlightDataXXXXXX"; ///< Template for temporary log file

src/Comms/MockLink/MockConfiguration.h

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,6 @@ class MockConfiguration : public LinkConfiguration
5959
FailMissingParamOnAllRequests, // Not all params are sent on initial request, QGC retries will fail as well
6060
FailInitialConnectRequestMessageAutopilotVersionFailure, // REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure
6161
FailInitialConnectRequestMessageAutopilotVersionLost, // REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent
62-
FailInitialConnectRequestMessageProtocolVersionFailure, // REQUEST_MESSAGE:PROTOCOL_VERSION returns failure
63-
FailInitialConnectRequestMessageProtocolVersionLost, // REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent
6462
};
6563
FailureMode_t failureMode() const { return _failureMode; }
6664
void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; }

src/Comms/MockLink/MockLink.cc

Lines changed: 0 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -1818,39 +1818,6 @@ void MockLink::_handleRequestMessageAutopilotVersion(const mavlink_command_long_
18181818
_respondWithAutopilotVersion();
18191819
}
18201820

1821-
void MockLink::_handleRequestMessageProtocolVersion(const mavlink_command_long_t &request, bool &accepted)
1822-
{
1823-
accepted = true;
1824-
1825-
switch (_failureMode) {
1826-
case MockConfiguration::FailNone:
1827-
break;
1828-
case MockConfiguration::FailInitialConnectRequestMessageProtocolVersionFailure:
1829-
accepted = false;
1830-
return;
1831-
case MockConfiguration::FailInitialConnectRequestMessageProtocolVersionLost:
1832-
accepted = true;
1833-
return;
1834-
default:
1835-
break;
1836-
}
1837-
1838-
const uint8_t nullHash[8]{};
1839-
mavlink_message_t responseMsg{};
1840-
(void) mavlink_msg_protocol_version_pack_chan(
1841-
_vehicleSystemId,
1842-
_vehicleComponentId,
1843-
mavlinkChannel(),
1844-
&responseMsg,
1845-
200,
1846-
100,
1847-
200,
1848-
nullHash,
1849-
nullHash
1850-
);
1851-
respondWithMavlinkMessage(responseMsg);
1852-
}
1853-
18541821
void MockLink::_handleRequestMessageDebug(const mavlink_command_long_t &request, bool &accepted, bool &noAck)
18551822
{
18561823
accepted = true;
@@ -1946,9 +1913,6 @@ void MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool
19461913
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
19471914
_handleRequestMessageAutopilotVersion(request, accepted);
19481915
break;
1949-
case MAVLINK_MSG_ID_PROTOCOL_VERSION:
1950-
_handleRequestMessageProtocolVersion(request, accepted);
1951-
break;
19521916
case MAVLINK_MSG_ID_COMPONENT_METADATA:
19531917
if (_firmwareType == MAV_AUTOPILOT_PX4) {
19541918
_sendGeneralMetaData();

src/Comms/MockLink/MockLink.h

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -186,7 +186,6 @@ private slots:
186186
void _handleParamMapRC(const mavlink_message_t &msg);
187187
void _handleRequestMessage(const mavlink_command_long_t &request, bool &accepted, bool &noAck);
188188
void _handleRequestMessageAutopilotVersion(const mavlink_command_long_t &request, bool &accepted);
189-
void _handleRequestMessageProtocolVersion(const mavlink_command_long_t &request, bool &accepted);
190189
void _handleRequestMessageDebug(const mavlink_command_long_t &request, bool &accepted, bool &noAck);
191190
void _handleRequestMessageAvailableModes(const mavlink_command_long_t &request, bool &accepted);
192191
void _handleRequestMessageGimbalManagerInformation(const mavlink_command_long_t &request, bool &accepted);

src/MissionManager/GeoFenceController.cc

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -105,17 +105,11 @@ void GeoFenceController::_managerVehicleChanged(Vehicle* managerVehicle)
105105
connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete);
106106
connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
107107

108-
//-- GeoFenceController::supported() tests both the capability bit AND the protocol version.
109108
(void) connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, [this](uint64_t capabilityBits) {
110109
Q_UNUSED(capabilityBits);
111110
emit supportedChanged(supported());
112111
});
113112

114-
(void) connect(_managerVehicle, &Vehicle::requestProtocolVersion, this, [this](unsigned version) {
115-
Q_UNUSED(version);
116-
emit supportedChanged(supported());
117-
});
118-
119113
connect(_managerVehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &GeoFenceController::_parametersReady);
120114
_parametersReady();
121115

@@ -489,7 +483,7 @@ void GeoFenceController::clearAllInteractive(void)
489483

490484
bool GeoFenceController::supported(void) const
491485
{
492-
return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200);
486+
return _managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
493487
}
494488

495489
/* Returns the radius of the "paramCircularFence"

src/MissionManager/GeoFenceManager.cc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -193,5 +193,5 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested)
193193

194194
bool GeoFenceManager::supported(void) const
195195
{
196-
return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_vehicle->maxProtoVersion() >= 200);
196+
return _vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
197197
}

0 commit comments

Comments
 (0)