Skip to content

Q
qgroundcontrol
Unverified Commit c246f688 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #7533 from DonLakeFlyer/MavlinkNegotiate 

Mavlink protocol version negotiation fixes
parents fac6111e 0f3a711f
Hide whitespace changes
Inline Side-by-side
Showing with 90 additions and 72 deletions
src/Vehicle/Vehicle.cc
View file @ c246f688
......@@ -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)
......
src/Vehicle/Vehicle.h
View file @ c246f688
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment