diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index db35e2fca8bb0797774e246ed2691504eb29a581..8a88bada062df9658350f8e3c8fba2d768711585 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -409,11 +409,6 @@ void MissionManager::_requestNextMissionItem(void) void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt) { - - if (!_checkForExpectedAck(AckMissionItem)) { - return; - } - MAV_CMD command; MAV_FRAME frame; double param1; @@ -468,7 +463,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } - qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem; + + bool ardupilotHomePositionUpdate = false; + if (!_checkForExpectedAck(AckMissionItem)) { + if (_vehicle->apmFirmware() && seq == 0) { + ardupilotHomePositionUpdate = true; + } else { + qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem; + return; + } + } + + qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate; + + if (ardupilotHomePositionUpdate) { + QGeoCoordinate newHomePosition(param5, param6, param7); + _vehicle->_setHomePosition(newHomePosition); + return; + } if (_itemIndicesToRead.contains(seq)) { _itemIndicesToRead.removeOne(seq);