Merge pull request #8492 from DonLakeFlyer/Mavlink2

Added correct handling of mavlink 2 change based on capability bits

src/Vehicle/Vehicle.cc

@@ -146,10 +146,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _telemetryTXBuffer(0)
     , _telemetryLNoise(0)
     , _telemetryRNoise(0)
-    , _mavlinkProtocolRequestComplete(false)
-    , _maxProtoVersion(0)
-    , _vehicleCapabilitiesKnown(false)
-    , _capabilityBits(0)
     , _highLatencyLink(false)
     , _receivingAttitudeQuaternion(false)
     , _cameras(nullptr)
@@ -349,7 +345,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
     , _mavlinkProtocolRequestComplete(true)
     , _maxProtoVersion(200)
-    , _vehicleCapabilitiesKnown(true)
+    , _capabilityBitsKnown(true)
     , _capabilityBits(MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
     , _highLatencyLink(false)
     , _receivingAttitudeQuaternion(false)
@@ -692,6 +688,30 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
         return;
     }
 
+    if (!_capabilityBitsKnown && message.msgid == MAVLINK_MSG_ID_HEARTBEAT && message.compid == MAV_COMP_ID_AUTOPILOT1) {
+        // We want to try to get capabilities as fast as possible so we retry on heartbeats
+        bool foundRequest = false;
+        for (MavCommandQueueEntry_t& queuedCommand : _mavCommandQueue) {
+            if (queuedCommand.command ==  MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
+                foundRequest = true;
+                break;
+            }
+        }
+        if (!foundRequest) {
+            if (++_capabilitiesRetryCount > 5) {
+                qgcApp()->showMessage(QStringLiteral("Vehicle failed to send AUTOPILOT_VERSION"));
+                _handleUnsupportedRequestAutopilotCapabilities();
+            } else {
+                // Vehicle never sent us AUTOPILOT_VERSION response. Try again.
+                qCDebug(VehicleLog) << "Sending MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES again due to no response with AUTOPILOT_VERSION - _capabilitiesRetryCount" << _capabilitiesRetryCount;
+                sendMavCommand(MAV_COMP_ID_ALL,
+                               MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+                               true,                                    // Show error on failure
+                               1);                                      // Request firmware version
+            }
+        }
+    }
+
     switch (message.msgid) {
     case MAVLINK_MSG_ID_HOME_POSITION:
         _handleHomePosition(message);
@@ -1288,7 +1308,7 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
 void Vehicle::_setCapabilities(uint64_t capabilityBits)
 {
     _capabilityBits = capabilityBits;
-    _vehicleCapabilitiesKnown = true;
+    _capabilityBitsKnown = true;
     emit capabilitiesKnownChanged(true);
     emit capabilityBitsChanged(_capabilityBits);
 
@@ -1300,6 +1320,8 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits)
     qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_COMMAND_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_COMMAND_INT ? supports : doesNotSupport);
     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);
+
+    _setMaxProtoVersionFromBothSources();
 }
 
 void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
@@ -1362,8 +1384,9 @@ void Vehicle::_handleProtocolVersion(LinkInterface *link, mavlink_message_t& mes
 
     qCDebug(VehicleLog) << "_handleProtocolVersion" << protoVersion.max_version;
 
+    _mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version;
     _mavlinkProtocolRequestComplete = true;
-    _setMaxProtoVersion(protoVersion.max_version);
+    _setMaxProtoVersionFromBothSources();
     _startPlanRequest();
 }
 
@@ -1377,6 +1400,19 @@ void Vehicle::_setMaxProtoVersion(unsigned version) {
     }
 }
 
+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()
 {
     QString uid;
@@ -2600,7 +2636,7 @@ void Vehicle::_startPlanRequest()
     //  - Parameter download is complete
     //  - We know the vehicle capabilities
     //  - We know the max mavlink protocol version
-    if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown && _mavlinkProtocolRequestComplete) {
+    if (_parameterManager->parametersReady() && _capabilityBitsKnown && _mavlinkProtocolRequestComplete) {
         qCDebug(VehicleLog) << "_startPlanRequest";
         _missionManagerInitialRequestSent = true;
         if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
@@ -2619,7 +2655,7 @@ void Vehicle::_startPlanRequest()
     } else {
         if (!_parameterManager->parametersReady()) {
             qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
-        } else if (!_vehicleCapabilitiesKnown) {
+        } else if (!_capabilityBitsKnown) {
             qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
         } else if (!_mavlinkProtocolRequestComplete) {
             qCDebug(VehicleLog) << "Delaying _startPlanRequest due to mavlink protocol request not complete";
@@ -3384,19 +3420,9 @@ void Vehicle::_handleUnsupportedRequestProtocolVersion()
     // We end up here if either the vehicle does not support the MAV_CMD_REQUEST_PROTOCOL_VERSION command or if
     // we never received an Ack back for the command.
 
-    // If the link isn't already running Mavlink V2 fall back to the capability bits to determine whether to use Mavlink V2 or not.
-    if (_maxProtoVersion == 0) {
-        if (capabilitiesKnown()) {
-            unsigned vehicleMaxProtoVersion = capabilityBits() & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? 200 : 100;
-            qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to %1 based on capabilitity bits since not yet set.").arg(vehicleMaxProtoVersion);
-            _setMaxProtoVersion(vehicleMaxProtoVersion);
-        } else {
-            qCDebug(VehicleLog) << "Waiting for capabilities to be known to set _maxProtoVersion";
-        }
-    } else {
-        qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion;
-    }
+    // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown
     _mavlinkProtocolRequestComplete = true;
+    _setMaxProtoVersionFromBothSources();
 
     // Determining protocol version is one of the triggers to possibly start downloading the plan
     _startPlanRequest();
@@ -3407,8 +3433,9 @@ void Vehicle::_protocolVersionTimeOut()
     // The PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC.
     // This means although the vehicle may support mavlink 2, the pipe does not.
     qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message.");
-    _setMaxProtoVersion(100);
+    _mavlinkProtocolRequestMaxProtoVersion = 100;
     _mavlinkProtocolRequestComplete = true;
+    _setMaxProtoVersionFromBothSources();
     _startPlanRequest();
 }
 

src/Vehicle/Vehicle.h

@@ -1078,7 +1078,7 @@ public:
     const QVariantList&         toolBarIndicators   ();
     const QVariantList&         staticCameraList    () const;
 
-    bool capabilitiesKnown      () const { return _vehicleCapabilitiesKnown; }
+    bool capabilitiesKnown      () const { return _capabilityBitsKnown; }
     uint64_t capabilityBits     () const { return _capabilityBits; }    // Change signalled by capabilityBitsChanged
 
     QGCCameraManager*           dynamicCameras      () { return _cameras; }
@@ -1096,7 +1096,8 @@ public:
     void _setFlying(bool flying);
     void _setLanding(bool landing);
     void _setHomePosition(QGeoCoordinate& homeCoord);
-    void _setMaxProtoVersion (unsigned version);
+    void _setMaxProtoVersion(unsigned version);
+    void _setMaxProtoVersionFromBothSources();
 
     /// Vehicle is about to be deleted
     void prepareDelete();
@@ -1406,9 +1407,10 @@ private:
     uint32_t        _telemetryTXBuffer;
     int             _telemetryLNoise;
     int             _telemetryRNoise;
-    bool            _mavlinkProtocolRequestComplete;
-    unsigned        _maxProtoVersion;
-    bool            _vehicleCapabilitiesKnown;
+    bool            _mavlinkProtocolRequestComplete =           false;
+    unsigned        _mavlinkProtocolRequestMaxProtoVersion =    0;
+    unsigned        _maxProtoVersion =                          0;
+    bool            _capabilityBitsKnown =                      false;
     uint64_t        _capabilityBits;
     bool            _highLatencyLink;
     bool            _receivingAttitudeQuaternion;
@@ -1428,8 +1430,9 @@ private:
     QList<MavCommandQueueEntry_t>   _mavCommandQueue;
     QTimer                          _mavCommandAckTimer;
     int                             _mavCommandRetryCount;
-    static const int                _mavCommandMaxRetryCount = 3;
-    static const int                _mavCommandAckTimeoutMSecs = 3000;
+    int                             _capabilitiesRetryCount =               0;
+    static const int                _mavCommandMaxRetryCount =              3;
+    static const int                _mavCommandAckTimeoutMSecs =            3000;
     static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;
 
     QString             _prearmError;