diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 916c282ee3b971ca355dd75c791df70c0f3198e4..690c2a8ca186a30c2e77b882f404996552239818 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -140,6 +140,7 @@ Vehicle::Vehicle(LinkInterface* link, , _telemetryTXBuffer(0) , _telemetryLNoise(0) , _telemetryRNoise(0) + , _mavlinkProtocolRequestComplete(false) , _maxProtoVersion(0) , _vehicleCapabilitiesKnown(false) , _capabilityBits(0) @@ -219,6 +220,7 @@ Vehicle::Vehicle(LinkInterface* link, connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings); _mavlink = _toolbox->mavlinkProtocol(); + qCDebug(VehicleLog) << "Link started with Mavlink " << (_mavlink->getCurrentVersion() >= 200 ? "V2" : "V1"); connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); connect(_mavlink, &MAVLinkProtocol::mavlinkMessageStatus, this, &Vehicle::_mavlinkMessageStatus); @@ -267,17 +269,14 @@ Vehicle::Vehicle(LinkInterface* link, _geoFenceManagerInitialRequestSent = true; _rallyPointManagerInitialRequestSent = true; } else { - // 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. + sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, false, // No error shown if fails 1); // Request firmware version + sendMavCommand(MAV_COMP_ID_ALL, + MAV_CMD_REQUEST_PROTOCOL_VERSION, + false, // No error shown if fails + 1); // Request protocol version } _firmwarePlugin->initializeVehicle(this); @@ -629,12 +628,11 @@ void Vehicle::resetCounters() void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) { - // if the minimum supported version of MAVLink is already 2.0 - // set our max proto version to it. + // If the link is already running at Mavlink V2 set our max proto version to it. unsigned mavlinkVersion = _mavlink->getCurrentVersion(); if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) { - _maxProtoVersion = _mavlink->getCurrentVersion(); - qCDebug(VehicleLog) << "Vehicle::_mavlinkMessageReceived setting _maxProtoVersion" << _maxProtoVersion; + _maxProtoVersion = mavlinkVersion; + qCDebug(VehicleLog) << "Vehicle::_mavlinkMessageReceived Link already running Mavlink v2. Setting _maxProtoVersion" << _maxProtoVersion; } if (message.sysid != _id && message.sysid != 0) { @@ -1276,13 +1274,6 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) emit capabilitiesKnownChanged(true); emit capabilityBitsChanged(_capabilityBits); - // This should potentially be turned into a user-facing warning - // if the general experience after deployment is that users want MAVLink 2 - // but forget to upgrade their radio firmware - if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 && maxProtoVersion() < 200) { - qCDebug(VehicleLog) << QString("Vehicle does support MAVLink 2 but the link does not allow for it."); - } - QString supports("supports"); QString doesNotSupport("does not support"); @@ -1350,7 +1341,11 @@ void Vehicle::_handleProtocolVersion(LinkInterface *link, mavlink_message_t& mes mavlink_protocol_version_t protoVersion; mavlink_msg_protocol_version_decode(&message, &protoVersion); + qCDebug(VehicleLog) << "_handleProtocolVersion" << protoVersion.max_version; + + _mavlinkProtocolRequestComplete = true; _setMaxProtoVersion(protoVersion.max_version); + _startPlanRequest(); } void Vehicle::_setMaxProtoVersion(unsigned version) { @@ -1360,11 +1355,6 @@ void Vehicle::_setMaxProtoVersion(unsigned version) { qCDebug(VehicleLog) << "_setMaxProtoVersion before:after" << _maxProtoVersion << version; _maxProtoVersion = version; emit requestProtocolVersion(_maxProtoVersion); - - // Now that the protocol version is known, the mission load is safe - // as it will use the right MAVLink version to enable all features - // the vehicle supports - _startPlanRequest(); } } @@ -2591,10 +2581,15 @@ void Vehicle::_mapTrajectoryStop() void Vehicle::_startPlanRequest(void) { if (_missionManagerInitialRequestSent) { + // We have already started (or possibly completed) the sequence of requesting the plan for the first time return; } - if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) { + // We don't start the Plan request until the following things are satisfied: + // - Parameter download is complete + // - We know the vehicle capabilities + // - We know the max mavlink protocol version + if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown && _mavlinkProtocolRequestComplete) { qCDebug(VehicleLog) << "_startPlanRequest"; _missionManagerInitialRequestSent = true; if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) { @@ -2615,6 +2610,8 @@ void Vehicle::_startPlanRequest(void) qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready"; } else if (!_vehicleCapabilitiesKnown) { qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known"; + } else if (!_mavlinkProtocolRequestComplete) { + qCDebug(VehicleLog) << "Delaying _startPlanRequest due to mavlink protocol request not complete"; } } } @@ -2660,6 +2657,7 @@ void Vehicle::_rallyPointLoadComplete(void) void Vehicle::_parametersReady(bool parametersReady) { + qDebug() << "_parametersReady" << parametersReady; // Try to set current unix time to the vehicle _sendQGCTimeToVehicle(); // Send time twice, more likely to get to the vehicle on a noisy link @@ -2787,12 +2785,11 @@ void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID } else if (!active && !_connectionLost) { _updatePriorityLink(false /* updateActive */, false /* sendCommand */); - // check if another active link has been found if (link == _priorityLink) { + // No other link to switch to was found, notify user of comm lossf _connectionLost = true; communicationLost = true; _heardFrom = false; - _maxProtoVersion = 0; emit connectionLostChanged(true); if (_autoDisconnect) { @@ -3197,22 +3194,13 @@ void Vehicle::_sendMavCommandAgain(void) if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { - // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items - qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES. Setting no capabilities. Starting Plan request."; - _setCapabilities(0); - _startPlanRequest(); + qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES."; + _handleUnsupportedRequestAutopilotCapabilities(); } 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. - // If the max protocol version is uninitialized, fall back to v1. - qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_PROTOCOL_VERSION. Starting Plan request."; - if (_maxProtoVersion == 0) { - qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set."; - _setMaxProtoVersion(100); - } else { - qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion; - } + qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_PROTOCOL_VERSION."; + _handleUnsupportedRequestProtocolVersion(); } emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); @@ -3225,25 +3213,10 @@ void Vehicle::_sendMavCommandAgain(void) } if (_mavCommandRetryCount > 1) { - // We always let AUTOPILOT_CAPABILITIES go through multiple times even if we don't get acks. This is because - // we really need to get capabilities and version info back over a lossy link. - if (queuedCommand.command != MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { - if (px4Firmware()) { - // Older PX4 firmwares are inconsistent with repect to sending back an Ack from a COMMAND_LONG, hence we can't support retry logic for it. - if (_firmwareMajorVersion != versionNotSetValue) { - // If no version set assume lastest master dev build, so acks are suppored - if (_firmwareMajorVersion <= 1 && _firmwareMinorVersion <= 5 && _firmwarePatchVersion <= 3) { - // Acks not supported in this version - return; - } - } - } else { - if (queuedCommand.command == MAV_CMD_START_RX_PAIR) { - // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares - // we aren't really sure whether they are correct or not. - return; - } - } + if (!px4Firmware() && queuedCommand.command == MAV_CMD_START_RX_PAIR) { + // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares + // we aren't really sure whether they are correct or not. + return; } qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount; } @@ -3304,6 +3277,49 @@ void Vehicle::_sendNextQueuedMavCommand(void) } } +void Vehicle::_handleUnsupportedRequestProtocolVersion(void) +{ + // 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; + } + _mavlinkProtocolRequestComplete = true; + + // Determining protocol version is one of the triggers to possibly start downloading the plan + _startPlanRequest(); +} + +void Vehicle::_protocolVersionTimeOut(void) +{ + // 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); + _mavlinkProtocolRequestComplete = true; + _startPlanRequest(); +} + +void Vehicle::_handleUnsupportedRequestAutopilotCapabilities(void) +{ + // We end up here if either the vehicle does not support the MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES command or if + // we never received an Ack back for the command. + + _setCapabilities(0); + + // Determining vehicle capabilities is one of the triggers to possibly start downloading the plan + _startPlanRequest(); +} void Vehicle::_handleCommandAck(mavlink_message_t& message) { @@ -3313,23 +3329,21 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) mavlink_msg_command_ack_decode(&message, &ack); if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) { - // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items - qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error(%1). Setting no capabilities. Starting Plan request.").arg(ack.result); - _setCapabilities(0); + qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error(%1). Setting no capabilities.").arg(ack.result); + _handleUnsupportedRequestAutopilotCapabilities(); } - 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 - qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_PROTOCOL_VERSION with error(%1).").arg(ack.result); - if (_maxProtoVersion == 0) { - qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set."; - _setMaxProtoVersion(100); + if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { + if (ack.result == MAV_RESULT_ACCEPTED) { + // The vehicle should be sending a PROTOCOL_VERSION message in a mavlink 2 packet. This may or may not make it through the pipe. + // So we wait for it to come and timeout if it doesn't. + if (!_mavlinkProtocolRequestComplete) { + QTimer::singleShot(1000, this, &Vehicle::_protocolVersionTimeOut); + } } else { - qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion; + qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_PROTOCOL_VERSION with error(%1).").arg(ack.result); + _handleUnsupportedRequestProtocolVersion(); } - // FIXME: Is this missing here. I believe it is a bug. Debug to verify. May need to go into Stable. - //_startPlanRequest(); } #if !defined(NO_ARDUPILOT_DIALECT) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 83bb46fe697a089659e81d84d09fbf91004283fa..0b6aea31909bdc3141a12196ade3e0d9181b13c0 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1232,6 +1232,7 @@ private slots: void _trafficUpdate (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); void _adsbTimerTimeout (); void _orbitTelemetryTimeout (void); + void _protocolVersionTimeOut(void); private: bool _containsLink(LinkInterface* link); @@ -1299,6 +1300,8 @@ private: void _updateArmed(bool armed); bool _apmArmingNotRequired(void); void _pidTuningAdjustRates(bool setRatesForTuning); + void _handleUnsupportedRequestAutopilotCapabilities(void); + void _handleUnsupportedRequestProtocolVersion(void); int _id; ///< Mavlink system id int _defaultComponentId; @@ -1356,6 +1359,7 @@ private: uint32_t _telemetryTXBuffer; int _telemetryLNoise; int _telemetryRNoise; + bool _mavlinkProtocolRequestComplete; unsigned _maxProtoVersion; bool _vehicleCapabilitiesKnown; uint64_t _capabilityBits;