diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f786ccc80b8a9903320f7045640d0fc60d139897..61e5cab7a37c78b285e10aac469bb44ade733571 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3214,25 +3214,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; } @@ -3319,7 +3304,7 @@ void Vehicle::_handleUnsupportedRequestProtocolVersion(void) 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 1, the pipe does not. + // 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;