From 6603b6399b708d561407a9982e3ece63e36175b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jun 2017 11:33:55 +0200 Subject: [PATCH] MAVLink 2 automatic version handshaking This extension introduces a complete version handshake mechanism which will detect wether the vehicle AND the complete link / routing infrastructure support MAVLink 2. If they do, QGC will switch to the highest protocol version supported by all connected vehicles. If a new vehicle connects, it will re-negotiate the highest possible version. This means that we error on the side of compatibility, which later can be easily changed. --- src/Vehicle/MultiVehicleManager.cc | 25 +++++++++++++++++++++ src/Vehicle/MultiVehicleManager.h | 1 + src/Vehicle/Vehicle.cc | 36 ++++++++++++++++++++++++++++++ src/Vehicle/Vehicle.h | 11 ++++++++- src/comm/MAVLinkProtocol.cc | 19 ++++++++++++++++ src/comm/MAVLinkProtocol.h | 8 +++++++ 6 files changed, 99 insertions(+), 1 deletion(-) diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 69dd5f335..0d82002c7 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -131,6 +131,7 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager); connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1); + connect(vehicle, &Vehicle::requestProtocolVersion, this, &MultiVehicleManager::_requestProtocolVersion); connect(vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &MultiVehicleManager::_vehicleParametersReadyChanged); _vehicles.append(vehicle); @@ -161,6 +162,30 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle } +/// 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 +void MultiVehicleManager::_requestProtocolVersion(unsigned version) +{ + unsigned maxversion = 0; + + if (_vehicles.count() == 0) { + _mavlinkProtocol->setVersion(version); + return; + } + + for (int i=0; i<_vehicles.count(); i++) { + + Vehicle *v = qobject_cast(_vehicles[i]); + if (v && v->maxProtoVersion() > maxversion) { + maxversion = v->maxProtoVersion(); + } + } + + if (_mavlinkProtocol->getCurrentVersion() != maxversion) { + _mavlinkProtocol->setVersion(maxversion); + } +} + /// This slot is connected to the Vehicle::allLinksDestroyed signal such that the Vehicle is deleted /// and all other right things happen when the Vehicle goes away. void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle) diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index 3e209e45c..5b2cfe6ca 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -95,6 +95,7 @@ private slots: void _vehicleParametersReadyChanged(bool parametersReady); void _sendGCSHeartbeat(void); void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); + void _requestProtocolVersion(unsigned version); private: bool _vehicleExists(int vehicleId); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b813c0aa8..472887b23 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -114,6 +114,7 @@ Vehicle::Vehicle(LinkInterface* link, , _telemetryTXBuffer(0) , _telemetryLNoise(0) , _telemetryRNoise(0) + , _maxProtoVersion(100) , _vehicleCapabilitiesKnown(false) , _supportsMissionItemInt(false) , _connectionLost(false) @@ -215,6 +216,12 @@ Vehicle::Vehicle(LinkInterface* link, _loadSettings(); + // Ask the vehicle for protocol version info. + sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. + MAV_CMD_REQUEST_PROTOCOL_VERSION, + false, // No error shown if fails + 1); // Request protocol version + // Ask the vehicle for firmware version info. sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, @@ -565,6 +572,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_AUTOPILOT_VERSION: _handleAutopilotVersion(link, message); break; + case MAVLINK_MSG_ID_PROTOCOL_VERSION: + _handleProtocolVersion(link, message); + break; case MAVLINK_MSG_ID_WIND_COV: _handleWindCov(message); break; @@ -776,6 +786,21 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me _startPlanRequest(); } +void Vehicle::_handleProtocolVersion(LinkInterface *link, mavlink_message_t& message) +{ + Q_UNUSED(link); + + mavlink_protocol_version_t protoVersion; + mavlink_msg_protocol_version_decode(&message, &protoVersion); + + _setMaxProtoVersion(protoVersion.max_version); +} + +void Vehicle::_setMaxProtoVersion(unsigned version) { + _maxProtoVersion = version; + emit requestProtocolVersion(_maxProtoVersion); +} + void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) { mavlink_hil_actuator_controls_t hil; @@ -814,6 +839,12 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) _startPlanRequest(); } + if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION && ack.result != MAV_RESULT_ACCEPTED) { + // The autopilot does not understand the request and consequently is likely handling only + // MAVLink 1 + _setMaxProtoVersion(100); + } + if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) { _mavCommandAckTimer.stop(); showError = _mavCommandQueue[0].showError; @@ -2246,6 +2277,11 @@ void Vehicle::_sendMavCommandAgain(void) _startPlanRequest(); } + if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { + // We aren't going to get a response back for the protocol version, so assume v1 is all we can do + _setMaxProtoVersion(100); + } + emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); if (queuedCommand.showError) { qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command))); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index dd524bcd3..f7343e802 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -605,6 +605,9 @@ public: int telemetryLNoise () { return _telemetryLNoise; } int telemetryRNoise () { return _telemetryRNoise; } bool autoDisarm (); + /// Get the maximum MAVLink protocol version supported + /// @return the maximum version + unsigned maxProtoVersion () const { return _maxProtoVersion; } Fact* roll (void) { return &_rollFact; } Fact* heading (void) { return &_headingFact; } @@ -699,6 +702,7 @@ public: void _setFlying(bool flying); void _setLanding(bool landing); void _setHomePosition(QGeoCoordinate& homeCoord); + void _setMaxProtoVersion (unsigned version); signals: void allLinksInactive(Vehicle* vehicle); @@ -782,9 +786,12 @@ signals: /// @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result void mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle); - // Mavlink Serial Data + // 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); + private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _linkInactiveOrDeleted(LinkInterface* link); @@ -837,6 +844,7 @@ private: void _handleExtendedSysState(mavlink_message_t& message); void _handleCommandAck(mavlink_message_t& message); void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message); + void _handleProtocolVersion(LinkInterface* link, mavlink_message_t& message); void _handleHilActuatorControls(mavlink_message_t& message); void _handleGpsRawInt(mavlink_message_t& message); void _handleGlobalPositionInt(mavlink_message_t& message); @@ -921,6 +929,7 @@ private: uint32_t _telemetryTXBuffer; int _telemetryLNoise; int _telemetryRNoise; + unsigned _maxProtoVersion; bool _vehicleCapabilitiesKnown; bool _supportsMissionItemInt; diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index a8c507b83..679fe763c 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -55,6 +55,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox) , m_enable_version_check(true) , versionMismatchIgnore(false) , systemId(255) + , _current_version(100) , _logSuspendError(false) , _logSuspendReplay(false) , _vehicleWasArmed(false) @@ -75,6 +76,24 @@ MAVLinkProtocol::~MAVLinkProtocol() _closeLogFile(); } +void MAVLinkProtocol::setVersion(unsigned version) +{ + QList links = _linkMgr->links(); + + for (int i = 0; i < links.length(); i++) { + mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(links[i]->mavlinkChannel()); + + // Set flags for version + if (version < 200) { + mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } + } + + _current_version = version; +} + void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox) { QGCTool::setToolbox(toolbox); diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 6e2b1d814..e115c327a 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -61,6 +61,10 @@ public: int getVersion() { return MAVLINK_VERSION; } + /** @brief Get the currently configured protocol version */ + unsigned getCurrentVersion() { + return _current_version; + } /** * Retrieve a total of all successfully parsed packets for the specified link. * @returns -1 if this is not available for this protocol, # of packets otherwise. @@ -90,6 +94,9 @@ public: /// Suspend/Restart logging during replay. void suspendLogForReplay(bool suspend); + /// Set protocol version + void setVersion(unsigned version); + // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); @@ -125,6 +132,7 @@ protected: int currLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Lost messages during this sample time window. Used for calculating loss %. bool versionMismatchIgnore; int systemId; + unsigned _current_version; signals: /// Heartbeat received on link -- 2.22.0