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

@@ -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<Vehicle*>(_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)

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);

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)));

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;
 

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<LinkInterface*> 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);

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