From a41cceec729e257e1bd79da2663fb312f4c19d6c Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 23 Mar 2020 11:42:55 -0700 Subject: [PATCH] ArduPilot: Only force MISSION_ITEM_INT if vehicle support bit is set --- src/MissionManager/PlanManager.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 3ab585b6f..6a08a8822 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -541,8 +541,10 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m MissionItem* item = _writeMissionItems[missionRequestSeq]; qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequestSeq << item->command(); + // ArduPilot always expects to get MISSION_ITEM_INT if possible + bool forceMissionItemInt = (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_INT) && _vehicle->apmFirmware(); mavlink_message_t messageOut; - if (missionItemInt || _vehicle->apmFirmware() /* ArduPilot always expects to get MISSION_ITEM_INT no matter what */) { + if (missionItemInt || forceMissionItemInt) { mavlink_msg_mission_item_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), _dedicatedLink->mavlinkChannel(), -- 2.22.0