diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9803371e3e6e9c956ea37e3f417af5a92c65a05a..cd824d5de598aac55a859951ab17eb84bac1d243 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -698,12 +698,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } } if (!foundRequest) { - if (++_capabilitiesRetryCount > 5) { + _capabilitiesRetryCount++; + if (_capabilitiesRetryCount == 1) { + _capabilitiesRetryElapsed.start(); + } else if (_capabilitiesRetryElapsed.elapsed() > 10000){ + qCDebug(VehicleLog) << "Giving up on getting AUTOPILOT_VERSION after 10 seconds"; 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; + qCDebug(VehicleLog) << QStringLiteral("Sending MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES again due to no response with AUTOPILOT_VERSION - _capabilitiesRetryCount(%1) elapsed(%2)").arg(_capabilitiesRetryCount).arg(_capabilitiesRetryElapsed.elapsed()); sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, true, // Show error on failure diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2ce4a9db661c9bfee975df8f9b1e9cba4c1761f4..7e87789a8813e2288dd669208d71092bdd8344e1 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -12,6 +12,7 @@ #include #include #include +#include #include "FactGroup.h" #include "LinkInterface.h" @@ -1431,6 +1432,7 @@ private: QTimer _mavCommandAckTimer; int _mavCommandRetryCount; int _capabilitiesRetryCount = 0; + QTime _capabilitiesRetryElapsed; static const int _mavCommandMaxRetryCount = 3; static const int _mavCommandAckTimeoutMSecs = 3000; static const int _mavCommandAckTimeoutMSecsHighLatency = 120000;