From f227844cf3e1409a8aab91f22b58f8c4f59885a7 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 5 Mar 2020 08:43:06 -0800 Subject: [PATCH] Added correct handling of mavlink 2 change based on capability bits --- src/Vehicle/Vehicle.cc | 71 +++++++++++++++++++++++++++++------------- src/Vehicle/Vehicle.h | 17 +++++----- 2 files changed, 59 insertions(+), 29 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f49043374..9803371e3 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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(); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7b56c0778..2ce4a9db6 100644 --- a/src/Vehicle/Vehicle.h +++ b/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 _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; -- 2.22.0