diff --git a/src/Comms/LinkInterface.cc b/src/Comms/LinkInterface.cc index 77d4187847e..7eb7e01c7dd 100644 --- a/src/Comms/LinkInterface.cc +++ b/src/Comms/LinkInterface.cc @@ -88,6 +88,7 @@ bool LinkInterface::_allocateMavlinkChannel() qCDebug(LinkInterfaceLog) << "_allocateMavlinkChannel" << _mavlinkChannel; + mavlink_set_proto_version(_mavlinkChannel, MAVLINK_VERSION); // We only support v2 protcol initMavlinkSigning(); return true; @@ -140,3 +141,19 @@ void LinkInterface::setSigningSignatureFailure(bool failure) } } } + +void LinkInterface::reportMavlinkV1Traffic() +{ + if (!_mavlinkV1TrafficReported) { + _mavlinkV1TrafficReported = true; + + const SharedLinkConfigurationPtr linkConfig = linkConfiguration(); + const QString linkName = linkConfig ? linkConfig->name() : QStringLiteral("unknown"); + qCWarning(LinkInterfaceLog) << "MAVLink v1 traffic detected on link" << linkName; + const QString message = tr("MAVLink v1 traffic detected on link '%1'. " + "%2 only supports MAVLink v2. " + "Please ensure your vehicle is configured to use MAVLink v2.") + .arg(linkName).arg(qgcApp()->applicationName()); + qgcApp()->showAppMessage(message); + } +} diff --git a/src/Comms/LinkInterface.h b/src/Comms/LinkInterface.h index 1af16f73992..1837449c8d2 100644 --- a/src/Comms/LinkInterface.h +++ b/src/Comms/LinkInterface.h @@ -46,6 +46,7 @@ class LinkInterface : public QObject void removeVehicleReference(); bool initMavlinkSigning(); void setSigningSignatureFailure(bool failure); + void reportMavlinkV1Traffic(); signals: void bytesReceived(LinkInterface *link, const QByteArray &data); @@ -80,6 +81,7 @@ private slots: bool _decodedFirstMavlinkPacket = false; int _vehicleReferenceCount = 0; bool _signingSignatureFailure = false; + bool _mavlinkV1TrafficReported = false; }; typedef std::shared_ptr SharedLinkInterfacePtr; diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 3482f8c0c09..187bfe2a39c 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -169,7 +169,6 @@ bool LinkManager::createConnectedLink(SharedLinkConfigurationPtr &config) (void) connect(link.get(), &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected); MAVLinkProtocol::instance()->resetMetadataForLink(link.get()); - MAVLinkProtocol::instance()->setVersion(MAVLinkProtocol::instance()->getCurrentVersion()); // Try to connect before adding to active links list if (!link->_connect()) { diff --git a/src/Comms/MAVLinkProtocol.cc b/src/Comms/MAVLinkProtocol.cc index 84da76a0cc8..320808f6317 100644 --- a/src/Comms/MAVLinkProtocol.cc +++ b/src/Comms/MAVLinkProtocol.cc @@ -24,6 +24,7 @@ #include #include #include +#include QGC_LOGGING_CATEGORY(MAVLinkProtocolLog, "Comms.MAVLinkProtocol") @@ -59,16 +60,6 @@ void MAVLinkProtocol::init() _initialized = true; } -void MAVLinkProtocol::setVersion(unsigned version) -{ - const QList sharedLinks = LinkManager::instance()->links(); - for (const SharedLinkInterfacePtr &interface : sharedLinks) { - mavlink_set_proto_version(interface.get()->mavlinkChannel(), version / 100); - } - - _currentVersion = version; -} - void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) { const uint8_t channel = link->mavlinkChannel(); @@ -119,7 +110,11 @@ void MAVLinkProtocol::receiveBytes(LinkInterface *link, const QByteArray &data) continue; } - _updateVersion(link, mavlinkChannel); + if (status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + link->reportMavlinkV1Traffic(); + continue; + } + _updateCounters(mavlinkChannel, message); if (!linkPtr->linkConfiguration()->isForwarding()) { _forward(message); @@ -133,25 +128,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface *link, const QByteArray &data) } } -void MAVLinkProtocol::_updateVersion(LinkInterface *link, uint8_t mavlinkChannel) -{ - if (link->decodedFirstMavlinkPacket()) { - return; - } - - link->setDecodedFirstMavlinkPacket(true); - const mavlink_status_t *const mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); - - if (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { - return; - } - - if (mavlink_get_proto_version(mavlinkChannel) == 1) { - qCDebug(MAVLinkProtocolLog) << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkChannel; - setVersion(200); - } -} - void MAVLinkProtocol::_updateCounters(uint8_t mavlinkChannel, const mavlink_message_t &message) { _totalReceiveCounter[mavlinkChannel]++; diff --git a/src/Comms/MAVLinkProtocol.h b/src/Comms/MAVLinkProtocol.h index 921f74086a1..b3c7fcbb101 100644 --- a/src/Comms/MAVLinkProtocol.h +++ b/src/Comms/MAVLinkProtocol.h @@ -51,21 +51,12 @@ class MAVLinkProtocol : public QObject /// Get the component id of this application static int getComponentId() { return MAV_COMP_ID_MISSIONPLANNER; } - /// Get the protocol version - static int getVersion() { return MAVLINK_VERSION; } - - /// Get the currently configured protocol version - unsigned getCurrentVersion() const { return _currentVersion; } - /// Reset the counters for all metadata for this link. void resetMetadataForLink(LinkInterface *link); /// Suspend/Restart logging during replay. void suspendLogForReplay(bool suspend) { _logSuspendReplay = suspend; } - /// Set protocol version - void setVersion(unsigned version); - /// Checks the temp directory for log files which may have been left there. /// This could happen if QGC crashes without the temp log file being saved. /// Give the user an option to save these orphaned files. @@ -107,7 +98,6 @@ private slots: void _updateCounters(uint8_t mavlinkChannel, const mavlink_message_t &message); bool _updateStatus(LinkInterface *link, const SharedLinkInterfacePtr linkPtr, uint8_t mavlinkChannel, const mavlink_message_t &message); - void _updateVersion(LinkInterface *link, uint8_t mavlinkChannel); void _saveTelemetryLog(const QString &tempLogfile); bool _checkTelemetrySavePath(); @@ -124,7 +114,6 @@ private slots: uint64_t _totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]{}; ///< Total messages lost during transmission. float _runningLossPercent[MAVLINK_COMM_NUM_BUFFERS]{}; ///< Loss rate - unsigned _currentVersion = 100; bool _initialized = false; static constexpr const char *_tempLogFileTemplate = "FlightDataXXXXXX"; ///< Template for temporary log file diff --git a/src/Comms/MockLink/MockConfiguration.h b/src/Comms/MockLink/MockConfiguration.h index 1d3523df719..a6ef65a786d 100644 --- a/src/Comms/MockLink/MockConfiguration.h +++ b/src/Comms/MockLink/MockConfiguration.h @@ -59,8 +59,6 @@ class MockConfiguration : public LinkConfiguration FailMissingParamOnAllRequests, // Not all params are sent on initial request, QGC retries will fail as well FailInitialConnectRequestMessageAutopilotVersionFailure, // REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure FailInitialConnectRequestMessageAutopilotVersionLost, // REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent - FailInitialConnectRequestMessageProtocolVersionFailure, // REQUEST_MESSAGE:PROTOCOL_VERSION returns failure - FailInitialConnectRequestMessageProtocolVersionLost, // REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent }; FailureMode_t failureMode() const { return _failureMode; } void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index 9a03af0284a..2a580b070c1 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -1818,39 +1818,6 @@ void MockLink::_handleRequestMessageAutopilotVersion(const mavlink_command_long_ _respondWithAutopilotVersion(); } -void MockLink::_handleRequestMessageProtocolVersion(const mavlink_command_long_t &request, bool &accepted) -{ - accepted = true; - - switch (_failureMode) { - case MockConfiguration::FailNone: - break; - case MockConfiguration::FailInitialConnectRequestMessageProtocolVersionFailure: - accepted = false; - return; - case MockConfiguration::FailInitialConnectRequestMessageProtocolVersionLost: - accepted = true; - return; - default: - break; - } - - const uint8_t nullHash[8]{}; - mavlink_message_t responseMsg{}; - (void) mavlink_msg_protocol_version_pack_chan( - _vehicleSystemId, - _vehicleComponentId, - mavlinkChannel(), - &responseMsg, - 200, - 100, - 200, - nullHash, - nullHash - ); - respondWithMavlinkMessage(responseMsg); -} - void MockLink::_handleRequestMessageDebug(const mavlink_command_long_t &request, bool &accepted, bool &noAck) { accepted = true; @@ -1946,9 +1913,6 @@ void MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool case MAVLINK_MSG_ID_AUTOPILOT_VERSION: _handleRequestMessageAutopilotVersion(request, accepted); break; - case MAVLINK_MSG_ID_PROTOCOL_VERSION: - _handleRequestMessageProtocolVersion(request, accepted); - break; case MAVLINK_MSG_ID_COMPONENT_METADATA: if (_firmwareType == MAV_AUTOPILOT_PX4) { _sendGeneralMetaData(); diff --git a/src/Comms/MockLink/MockLink.h b/src/Comms/MockLink/MockLink.h index d51269c6012..2388daf91a0 100644 --- a/src/Comms/MockLink/MockLink.h +++ b/src/Comms/MockLink/MockLink.h @@ -186,7 +186,6 @@ private slots: void _handleParamMapRC(const mavlink_message_t &msg); void _handleRequestMessage(const mavlink_command_long_t &request, bool &accepted, bool &noAck); void _handleRequestMessageAutopilotVersion(const mavlink_command_long_t &request, bool &accepted); - void _handleRequestMessageProtocolVersion(const mavlink_command_long_t &request, bool &accepted); void _handleRequestMessageDebug(const mavlink_command_long_t &request, bool &accepted, bool &noAck); void _handleRequestMessageAvailableModes(const mavlink_command_long_t &request, bool &accepted); void _handleRequestMessageGimbalManagerInformation(const mavlink_command_long_t &request, bool &accepted); diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index e213c1e2360..72390503daa 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -105,17 +105,11 @@ void GeoFenceController::_managerVehicleChanged(Vehicle* managerVehicle) connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete); connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged); - //-- GeoFenceController::supported() tests both the capability bit AND the protocol version. (void) connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, [this](uint64_t capabilityBits) { Q_UNUSED(capabilityBits); emit supportedChanged(supported()); }); - (void) connect(_managerVehicle, &Vehicle::requestProtocolVersion, this, [this](unsigned version) { - Q_UNUSED(version); - emit supportedChanged(supported()); - }); - connect(_managerVehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &GeoFenceController::_parametersReady); _parametersReady(); @@ -489,7 +483,7 @@ void GeoFenceController::clearAllInteractive(void) bool GeoFenceController::supported(void) const { - return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200); + return _managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; } /* Returns the radius of the "paramCircularFence" diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 67024d6a5ee..ed60d944f5f 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -193,5 +193,5 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested) bool GeoFenceManager::supported(void) const { - return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_vehicle->maxProtoVersion() >= 200); + return _vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; } diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index d66fa33f3fa..97c00107180 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -1145,7 +1145,6 @@ { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" }, { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" }, { "id": 512, "rawName": "MAV_CMD_REQUEST_MESSAGE" }, - { "id": 519, "rawName": "MAV_CMD_REQUEST_PROTOCOL_VERSION" }, { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" }, { "id": 530, diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index 03a08acd65c..614d263647b 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -66,17 +66,11 @@ void RallyPointController::_managerVehicleChanged(Vehicle* managerVehicle) connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete); connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged); - //-- RallyPointController::supported() tests both the capability bit AND the protocol version. (void) connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, [this](uint64_t capabilityBits) { Q_UNUSED(capabilityBits); emit supportedChanged(supported()); }); - (void) connect(_managerVehicle, &Vehicle::requestProtocolVersion, this, [this](unsigned version) { - Q_UNUSED(version); - emit supportedChanged(supported()); - }); - emit supportedChanged(supported()); } @@ -261,7 +255,7 @@ void RallyPointController::addPoint(QGeoCoordinate point) bool RallyPointController::supported(void) const { - return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200); + return _managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; } void RallyPointController::removePoint(QObject* rallyPoint) diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index b6553704b96..2e975be6f06 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -71,7 +71,7 @@ void RallyPointManager::removeAll(void) bool RallyPointManager::supported(void) const { - return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200); + return _vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; } void RallyPointManager::_planManagerLoadComplete(bool removeAllRequested) diff --git a/src/Settings/MavlinkSettings.cc b/src/Settings/MavlinkSettings.cc index 92c2363a4e9..ed26eaa1434 100644 --- a/src/Settings/MavlinkSettings.cc +++ b/src/Settings/MavlinkSettings.cc @@ -47,7 +47,6 @@ DECLARE_SETTINGSFACT(MavlinkSettings, forwardMavlinkHostName) DECLARE_SETTINGSFACT(MavlinkSettings, forwardMavlinkAPMSupportHostName) DECLARE_SETTINGSFACT(MavlinkSettings, sendGCSHeartbeat) DECLARE_SETTINGSFACT(MavlinkSettings, gcsMavlinkSystemID) -DECLARE_SETTINGSFACT(MavlinkSettings, requireMatchingMavlinkVersions) DECLARE_SETTINGSFACT_NO_FUNC(MavlinkSettings, mavlink2SigningKey) { diff --git a/src/Settings/MavlinkSettings.h b/src/Settings/MavlinkSettings.h index 473dc3b9879..f554573a72c 100644 --- a/src/Settings/MavlinkSettings.h +++ b/src/Settings/MavlinkSettings.h @@ -33,7 +33,6 @@ class MavlinkSettings : public SettingsGroup DEFINE_SETTINGFACT(mavlink2SigningKey) DEFINE_SETTINGFACT(sendGCSHeartbeat) DEFINE_SETTINGFACT(gcsMavlinkSystemID) - DEFINE_SETTINGFACT(requireMatchingMavlinkVersions) // Although this is a global setting it only affects ArduPilot vehicle since PX4 automatically starts the stream from the vehicle side DEFINE_SETTINGFACT(apmStartMavlinkStreams) diff --git a/src/Vehicle/InitialConnectStateMachine.cc b/src/Vehicle/InitialConnectStateMachine.cc index 050a4605781..2bc35b831f7 100644 --- a/src/Vehicle/InitialConnectStateMachine.cc +++ b/src/Vehicle/InitialConnectStateMachine.cc @@ -174,11 +174,7 @@ void InitialConnectStateMachine::_autopilotVersionRequestMessageHandler(void* re if (failureCode != Vehicle::RequestMessageNoFailure) { qCDebug(InitialConnectStateMachineLog) << "REQUEST_MESSAGE:AUTOPILOT_VERSION failed. Setting no capabilities"; - uint64_t assumedCapabilities = 0; - if (vehicle->_mavlinkProtocolRequestMaxProtoVersion >= 200) { - // Link already running mavlink 2 - assumedCapabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; - } + uint64_t assumedCapabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2; if (vehicle->px4Firmware() || vehicle->apmFirmware()) { // We make some assumptions for known firmware assumedCapabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT | MAV_PROTOCOL_CAPABILITY_COMMAND_INT | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; @@ -189,74 +185,6 @@ void InitialConnectStateMachine::_autopilotVersionRequestMessageHandler(void* re connectMachine->advance(); } -void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stateMachine) -{ - InitialConnectStateMachine* connectMachine = static_cast(stateMachine); - Vehicle* vehicle = connectMachine->_vehicle; - SharedLinkInterfacePtr sharedLink = vehicle->vehicleLinkManager()->primaryLink().lock(); - - if (!sharedLink) { - qCDebug(InitialConnectStateMachineLog) << "Skipping REQUEST_MESSAGE:PROTOCOL_VERSION request due to no primary link"; - connectMachine->advance(); - } else { - if (sharedLink->linkConfiguration()->isHighLatency() || sharedLink->isLogReplay()) { - qCDebug(InitialConnectStateMachineLog) << "Skipping REQUEST_MESSAGE:PROTOCOL_VERSION request due to link type"; - connectMachine->advance(); - } else if (vehicle->apmFirmware()) { - qCDebug(InitialConnectStateMachineLog) << "Skipping REQUEST_MESSAGE:PROTOCOL_VERSION request due to Ardupilot firmware"; - connectMachine->advance(); - } else { - qCDebug(InitialConnectStateMachineLog) << "Sending REQUEST_MESSAGE:PROTOCOL_VERSION"; - vehicle->requestMessage(_protocolVersionRequestMessageHandler, - connectMachine, - MAV_COMP_ID_AUTOPILOT1, - MAVLINK_MSG_ID_PROTOCOL_VERSION); - } - } -} - -void InitialConnectStateMachine::_protocolVersionRequestMessageHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message) -{ - InitialConnectStateMachine* connectMachine = static_cast(resultHandlerData); - Vehicle* vehicle = connectMachine->_vehicle; - - switch (failureCode) { - case Vehicle::RequestMessageNoFailure: - { - mavlink_protocol_version_t protoVersion; - mavlink_msg_protocol_version_decode(&message, &protoVersion); - - qCDebug(InitialConnectStateMachineLog) << "PROTOCOL_VERSION received mav_version:" << protoVersion.max_version; - vehicle->_mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version; - vehicle->_mavlinkProtocolRequestComplete = true; - vehicle->_setMaxProtoVersionFromBothSources(); - } - break; - case Vehicle::RequestMessageFailureCommandError: - qCDebug(InitialConnectStateMachineLog) << QStringLiteral("REQUEST_MESSAGE PROTOCOL_VERSION command error(%1)").arg(commandResult); - break; - case Vehicle::RequestMessageFailureCommandNotAcked: - qCDebug(InitialConnectStateMachineLog) << "REQUEST_MESSAGE PROTOCOL_VERSION command never acked"; - break; - case Vehicle::RequestMessageFailureMessageNotReceived: - qCDebug(InitialConnectStateMachineLog) << "REQUEST_MESSAGE PROTOCOL_VERSION command acked but message never received"; - break; - case Vehicle::RequestMessageFailureDuplicateCommand: - qCDebug(InitialConnectStateMachineLog) << "REQUEST_MESSAGE PROTOCOL_VERSION Internal Error: Duplicate command"; - break; - } - - if (failureCode != Vehicle::RequestMessageNoFailure) { - // Either the PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC because the pipe is mavlink 1. - // Or the PROTOCOL_VERSION message was lost on a noisy connection. Either way the best we can do is fall back to mavlink 1. - qCDebug(InitialConnectStateMachineLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message."); - vehicle->_mavlinkProtocolRequestMaxProtoVersion = 100; - vehicle->_mavlinkProtocolRequestComplete = true; - vehicle->_setMaxProtoVersionFromBothSources(); - } - - connectMachine->advance(); -} void InitialConnectStateMachine::_stateRequestCompInfo(StateMachine* stateMachine) { InitialConnectStateMachine* connectMachine = static_cast(stateMachine); diff --git a/src/Vehicle/InitialConnectStateMachine.h b/src/Vehicle/InitialConnectStateMachine.h index 0a133cecb24..fd71586d8a3 100644 --- a/src/Vehicle/InitialConnectStateMachine.h +++ b/src/Vehicle/InitialConnectStateMachine.h @@ -41,7 +41,6 @@ private slots: private: static void _stateRequestAutopilotVersion (StateMachine* stateMachine); - static void _stateRequestProtocolVersion (StateMachine* stateMachine); static void _stateRequestCompInfo (StateMachine* stateMachine); static void _stateRequestStandardModes (StateMachine* stateMachine); static void _stateRequestCompInfoComplete (void* requestAllCompleteFnData); @@ -52,7 +51,6 @@ private slots: static void _stateSignalInitialConnectComplete (StateMachine* stateMachine); static void _autopilotVersionRequestMessageHandler (void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message); - static void _protocolVersionRequestMessageHandler (void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message); float _progress(float subProgress = 0.f); @@ -62,7 +60,6 @@ private slots: static constexpr const StateMachine::StateFn _rgStates[] = { _stateRequestAutopilotVersion, - _stateRequestProtocolVersion, _stateRequestStandardModes, _stateRequestCompInfo, _stateRequestParameters, @@ -73,8 +70,7 @@ private slots: }; static constexpr const int _rgProgressWeights[] = { - 1, //_stateRequestCapabilities - 1, //_stateRequestProtocolVersion + 1, //_stateRequestAutopilotVersion 1, //_stateRequestStandardModes 5, //_stateRequestCompInfo 5, //_stateRequestParameters diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 72da3fa2ded..c944545ff82 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -127,7 +127,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle } Vehicle *const vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, this); - (void) connect(vehicle, &Vehicle::requestProtocolVersion, this, &MultiVehicleManager::_requestProtocolVersion); (void) connect(vehicle->vehicleLinkManager(), &VehicleLinkManager::allLinksRemoved, this, &MultiVehicleManager::_deleteVehiclePhase1); (void) connect(vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &MultiVehicleManager::_vehicleParametersReadyChanged); @@ -158,26 +157,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle #endif } -void MultiVehicleManager::_requestProtocolVersion(unsigned version) const -{ - if (_vehicles->count() == 0) { - MAVLinkProtocol::instance()->setVersion(version); - return; - } - - unsigned maxversion = 0; - for (int i = 0; i < _vehicles->count(); i++) { - const Vehicle *const vehicle = qobject_cast(_vehicles->get(i)); - if (vehicle && (vehicle->maxProtoVersion() > maxversion)) { - maxversion = vehicle->maxProtoVersion(); - } - } - - if (MAVLinkProtocol::instance()->getCurrentVersion() != maxversion) { - MAVLinkProtocol::instance()->setVersion(maxversion); - } -} - void MultiVehicleManager::_deleteVehiclePhase1(Vehicle *vehicle) { qCDebug(MultiVehicleManagerLog) << Q_FUNC_INFO << vehicle; diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index a9dd80f6d4e..e74840df118 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -71,7 +71,6 @@ private slots: void _vehicleParametersReadyChanged(bool parametersReady); void _sendGCSHeartbeat(); void _vehicleHeartbeatInfo(LinkInterface *link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); - void _requestProtocolVersion(unsigned version) const; /// This slot is connected to the Vehicle::requestProtocolVersion signal such that the vehicle manager tries to switch MAVLink to v2 if all vehicles support it private: bool _vehicleExists(int vehicleId); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 72ca657dbfa..9da9d4a20a8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -120,8 +120,6 @@ Vehicle::Vehicle(LinkInterface* link, connect(JoystickManager::instance(), &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadJoystickSettings); connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &Vehicle::_activeVehicleChanged); - qCDebug(VehicleLog) << "Link started with Mavlink " << (MAVLinkProtocol::instance()->getCurrentVersion() >= 200 ? "V2" : "V1"); - connect(MAVLinkProtocol::instance(), &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); connect(MAVLinkProtocol::instance(), &MAVLinkProtocol::mavlinkMessageStatus, this, &Vehicle::_mavlinkMessageStatus); @@ -200,8 +198,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _vehicleType (vehicleType) , _defaultCruiseSpeed (SettingsManager::instance()->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble()) , _defaultHoverSpeed (SettingsManager::instance()->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) - , _mavlinkProtocolRequestComplete (true) - , _maxProtoVersion (200) , _capabilityBitsKnown (true) , _capabilityBits (MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) , _trajectoryPoints (new TrajectoryPoints(this, this)) @@ -445,13 +441,6 @@ void Vehicle::resetCounters() void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) { - // If the link is already running at Mavlink V2 set our max proto version to it. - unsigned mavlinkVersion = MAVLinkProtocol::instance()->getCurrentVersion(); - if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) { - _maxProtoVersion = mavlinkVersion; - qCDebug(VehicleLog) << "_mavlinkMessageReceived Link already running Mavlink v2. Setting _maxProtoVersion" << _maxProtoVersion; - } - if (message.sysid != _id && message.sysid != 0) { // We allow RADIO_STATUS messages which come from a link the vehicle is using to pass through and be handled if (!(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _vehicleLinkManager->containsLink(link))) { @@ -886,31 +875,6 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 Terrain").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_TERRAIN ? supports : doesNotSupport); - - _setMaxProtoVersionFromBothSources(); -} - -void Vehicle::_setMaxProtoVersion(unsigned version) { - - // Set only once or if we need to reduce the max version - if (_maxProtoVersion == 0 || version < _maxProtoVersion) { - qCDebug(VehicleLog) << "_setMaxProtoVersion before:after" << _maxProtoVersion << version; - _maxProtoVersion = version; - emit requestProtocolVersion(_maxProtoVersion); - } -} - -void Vehicle::_setMaxProtoVersionFromBothSources() -{ - if (_mavlinkProtocolRequestComplete && _capabilityBitsKnown) { - if (_mavlinkProtocolRequestMaxProtoVersion != 0) { - qCDebug(VehicleLog) << "_setMaxProtoVersionFromBothSources using protocol version message"; - _setMaxProtoVersion(_mavlinkProtocolRequestMaxProtoVersion); - } else { - qCDebug(VehicleLog) << "_setMaxProtoVersionFromBothSources using capability bits"; - _setMaxProtoVersion(capabilityBits() & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? 200 : 100); - } - } } QString Vehicle::vehicleUIDStr() @@ -2597,7 +2561,6 @@ bool Vehicle::_sendMavCommandShouldRetry(MAV_CMD command) // links where commands could be lost. Also these commands tend to just be requesting status so if they end up being delayed // there are no safety concerns that could occur. case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - case MAV_CMD_REQUEST_PROTOCOL_VERSION: case MAV_CMD_REQUEST_MESSAGE: case MAV_CMD_PREFLIGHT_STORAGE: case MAV_CMD_RUN_PREARM_CHECKS: diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index f9406b24b4c..b246b605a63 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -574,11 +574,6 @@ class Vehicle : public VehicleFactGroup bool requiresGpsFix () const { return static_cast(_onboardControlSensorsPresent & QGCMAVLink::SysStatusSensorGPS); } bool hilMode () const { return _base_mode & MAV_MODE_FLAG_HIL_ENABLED; } Actuators* actuators () const { return _actuators; } - - /// Get the maximum MAVLink protocol version supported - /// @return the maximum version - unsigned maxProtoVersion () const { return _maxProtoVersion; } - bool mavlinkSigning () const { return _mavlinkSigning; } void startCalibration (QGCMAVLink::CalibrationType calType); @@ -784,8 +779,6 @@ class Vehicle : public VehicleFactGroup void _setFlying(bool flying); void _setLanding(bool landing); void _setHomePosition(QGeoCoordinate& homeCoord); - void _setMaxProtoVersion(unsigned version); - void _setMaxProtoVersionFromBothSources(); /// Vehicle is about to be deleted void prepareDelete(); @@ -897,8 +890,6 @@ public slots: // MAVlink Serial Data void mavlinkSerialControl (uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data); - // MAVLink protocol version - void requestProtocolVersion (unsigned version); void mavlinkStatusChanged (); void mavlinkSigningChanged (); @@ -1043,9 +1034,6 @@ private slots: uint32_t _telemetryTXBuffer = 0; int _telemetryLNoise = 0; int _telemetryRNoise = 0; - bool _mavlinkProtocolRequestComplete = false; - unsigned _mavlinkProtocolRequestMaxProtoVersion = 0; - unsigned _maxProtoVersion = 0; bool _capabilityBitsKnown = false; uint64_t _capabilityBits = 0; CheckList _checkListState = CheckListNotSetup; diff --git a/test/Vehicle/InitialConnectTest.cc b/test/Vehicle/InitialConnectTest.cc index 4dfe40d0efa..c2b8a2a09a8 100644 --- a/test/Vehicle/InitialConnectTest.cc +++ b/test/Vehicle/InitialConnectTest.cc @@ -25,8 +25,6 @@ void InitialConnectTest::_performTestCases(void) { MockConfiguration::FailNone, "No failures" }, { MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionFailure, "REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure" }, { MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionLost, "REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent" }, - { MockConfiguration::FailInitialConnectRequestMessageProtocolVersionFailure, "REQUEST_MESSAGE:PROTOCOL_VERSION returns failure" }, - { MockConfiguration::FailInitialConnectRequestMessageProtocolVersionLost, "REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent" }, }; for (const struct TestCase_s& testCase: rgTestCases) {