zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #8492 from DonLakeFlyer/Mavlink2 

Added correct handling of mavlink 2 change based on capability bits
QGC4.4
Don Gagne 5 years ago committed by GitHub
parent
9f664587a9 f227844cf3
commit
ef759624fa
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 59 additions and 29 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 71
      src/Vehicle/Vehicle.cc
  2. 17
      src/Vehicle/Vehicle.h

71
src/Vehicle/Vehicle.cc
Unescape View File

@ -146,10 +146,6 @@ Vehicle::Vehicle(LinkInterface* link, @@ -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, @@ -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 @@ -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) @@ -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) @@ -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 @@ -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) { @@ -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() @@ -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() @@ -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() @@ -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() @@ -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();
}

17
src/Vehicle/Vehicle.h
Unescape View File

@ -1078,7 +1078,7 @@ public: @@ -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: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save