diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index f19f1b100c4d53225c30aaa29458f3f46ef961bc..38292b268d85e1f2e7035b68beb4c731a8932f89 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -589,6 +589,14 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message) return; } + if (_vehicle->apmFirmware() && missionAck.type == MAV_MISSION_INVALID_SEQUENCE) { + // ArduPilot sends these Acks which can happen just due to noisy links causing duplicated requests being responded to. + // As far as I'm concerned this is incorrect protocol implementation but we need to deal with it anyway. So we just + // ignore it and if things really go haywire the timeouts will fire to fail the overall transaction. + qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck ArduPilot sending possibly bogus MAV_MISSION_INVALID_SEQUENCE").arg(_planTypeString()) << _planType; + return; + } + // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what // type of a protocol sequence we are in. AckType_t savedExpectedAck = _expectedAck; diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h index caa6173a084ba5e50cdc5f5d9523a457fb0cc0e5..0cfb50905eb78becc8a9ed855ad623c98d9b3ded 100644 --- a/src/MissionManager/PlanManager.h +++ b/src/MissionManager/PlanManager.h @@ -71,7 +71,7 @@ public: // These values are public so the unit test can set appropriate signal wait times // When passively waiting for a mission process, use a longer timeout. - static const int _ackTimeoutMilliseconds = 1000; + static const int _ackTimeoutMilliseconds = 1500; // When actively retrying to request mission items, use a shorter timeout instead. static const int _retryTimeoutMilliseconds = 250; static const int _maxRetryCount = 5;