diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index c41aed3455c2833eac0c88474e2015aa1aa94285..2aa7ad8cc1ec13d32130b4d92c9e408724e5200b 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -342,50 +342,108 @@ void MissionManager::_requestNextMissionItem(void) qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount; - mavlink_message_t message; - mavlink_mission_request_t missionRequest; - - missionRequest.target_system = _vehicle->id(); - missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionRequest.seq = _itemIndicesToRead[0]; - - mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &message, - &missionRequest); + mavlink_message_t message; + if (_vehicle->supportsMissionItemInt()) { + mavlink_mission_request_int_t missionRequest; + + missionRequest.target_system = _vehicle->id(); + missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionRequest.seq = _itemIndicesToRead[0]; + + mavlink_msg_mission_request_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + &missionRequest); + } else { + mavlink_mission_request_t missionRequest; + + missionRequest.target_system = _vehicle->id(); + missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionRequest.seq = _itemIndicesToRead[0]; + + mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + &missionRequest); + } _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionItem); } -void MissionManager::_handleMissionItem(const mavlink_message_t& message) +void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt) { - mavlink_mission_item_t missionItem; if (!_checkForExpectedAck(AckMissionItem)) { return; } + + MAV_CMD command; + MAV_FRAME frame; + double param1; + double param2; + double param3; + double param4; + double param5; + double param6; + double param7; + bool autoContinue; + bool isCurrentItem; + int seq; + + if (missionItemInt) { + mavlink_mission_item_int_t missionItem; + mavlink_msg_mission_item_int_decode(&message, &missionItem); + + command = (MAV_CMD)missionItem.command, + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; + param2 = missionItem.param2; + param3 = missionItem.param3; + param4 = missionItem.param4; + param5 = (double)missionItem.x / qPow(10.0, 7.0); + param6 = (double)missionItem.y / qPow(10.0, 7.0); + param7 = (double)missionItem.z / qPow(10.0, 7.0); + autoContinue = missionItem.autocontinue; + isCurrentItem = missionItem.current; + seq = missionItem.seq; + } else { + mavlink_mission_item_t missionItem; + mavlink_msg_mission_item_decode(&message, &missionItem); + + command = (MAV_CMD)missionItem.command, + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; + param2 = missionItem.param2; + param3 = missionItem.param3; + param4 = missionItem.param4; + param5 = missionItem.x; + param6 = missionItem.y; + param7 = missionItem.z; + autoContinue = missionItem.autocontinue; + isCurrentItem = missionItem.current; + seq = missionItem.seq; + } - mavlink_msg_mission_item_decode(&message, &missionItem); - - qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq; + qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << seq; - if (_itemIndicesToRead.contains(missionItem.seq)) { - _itemIndicesToRead.removeOne(missionItem.seq); - - MissionItem* item = new MissionItem(missionItem.seq, - (MAV_CMD)missionItem.command, - (MAV_FRAME)missionItem.frame, - missionItem.param1, - missionItem.param2, - missionItem.param3, - missionItem.param4, - missionItem.x, - missionItem.y, - missionItem.z, - missionItem.autocontinue, - missionItem.current, + if (_itemIndicesToRead.contains(seq)) { + _itemIndicesToRead.removeOne(seq); + + MissionItem* item = new MissionItem(seq, + command, + frame, + param1, + param2, + param3, + param4, + param5, + param6, + param7, + autoContinue, + isCurrentItem, this); if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) { @@ -395,7 +453,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) _missionItems.append(item); } else { - qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq; + qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << seq; // We have to put the ack timeout back since it was removed above _startAckTimeout(AckMissionItem); return; @@ -415,7 +473,7 @@ void MissionManager::_clearMissionItems(void) _missionItems.clear(); } -void MissionManager::_handleMissionRequest(const mavlink_message_t& message) +void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt) { mavlink_mission_request_t missionRequest; @@ -440,30 +498,57 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) } mavlink_message_t messageOut; - mavlink_mission_item_t missionItem; - - MissionItem* item = _missionItems[missionRequest.seq]; - - missionItem.target_system = _vehicle->id(); - missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionItem.seq = missionRequest.seq; - missionItem.command = item->command(); - missionItem.param1 = item->param1(); - missionItem.param2 = item->param2(); - missionItem.param3 = item->param3(); - missionItem.param4 = item->param4(); - missionItem.x = item->param5(); - missionItem.y = item->param6(); - missionItem.z = item->param7(); - missionItem.frame = item->frame(); - missionItem.current = missionRequest.seq == 0; - missionItem.autocontinue = item->autoContinue(); - - mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - _dedicatedLink->mavlinkChannel(), - &messageOut, - &missionItem); + if (missionItemInt) { + mavlink_mission_item_int_t missionItem; + + MissionItem* item = _missionItems[missionRequest.seq]; + + missionItem.target_system = _vehicle->id(); + missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionItem.seq = missionRequest.seq; + missionItem.command = item->command(); + missionItem.param1 = item->param1(); + missionItem.param2 = item->param2(); + missionItem.param3 = item->param3(); + missionItem.param4 = item->param4(); + missionItem.x = item->param5() * qPow(10.0, 7.0); + missionItem.y = item->param6() * qPow(10.0, 7.0); + missionItem.z = item->param7() * qPow(10.0, 7.0); + missionItem.frame = item->frame(); + missionItem.current = missionRequest.seq == 0; + missionItem.autocontinue = item->autoContinue(); + + mavlink_msg_mission_item_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &messageOut, + &missionItem); + } else { + mavlink_mission_item_t missionItem; + + MissionItem* item = _missionItems[missionRequest.seq]; + + missionItem.target_system = _vehicle->id(); + missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionItem.seq = missionRequest.seq; + missionItem.command = item->command(); + missionItem.param1 = item->param1(); + missionItem.param2 = item->param2(); + missionItem.param3 = item->param3(); + missionItem.param4 = item->param4(); + missionItem.x = item->param5(); + missionItem.y = item->param6(); + missionItem.z = item->param7(); + missionItem.frame = item->frame(); + missionItem.current = missionRequest.seq == 0; + missionItem.autocontinue = item->autoContinue(); + + mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &messageOut, + &missionItem); + } _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); _startAckTimeout(AckMissionRequest); @@ -535,29 +620,37 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) { switch (message.msgid) { - case MAVLINK_MSG_ID_MISSION_COUNT: - _handleMissionCount(message); - break; + case MAVLINK_MSG_ID_MISSION_COUNT: + _handleMissionCount(message); + break; - case MAVLINK_MSG_ID_MISSION_ITEM: - _handleMissionItem(message); - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: - _handleMissionRequest(message); - break; - - case MAVLINK_MSG_ID_MISSION_ACK: - _handleMissionAck(message); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: - // FIXME: NYI - break; - - case MAVLINK_MSG_ID_MISSION_CURRENT: - _handleMissionCurrent(message); - break; + case MAVLINK_MSG_ID_MISSION_ITEM: + _handleMissionItem(message, false /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + _handleMissionItem(message, true /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + _handleMissionRequest(message, false /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_INT: + _handleMissionRequest(message, true /* missionItemInt */); + break; + + case MAVLINK_MSG_ID_MISSION_ACK: + _handleMissionAck(message); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: + // FIXME: NYI + break; + + case MAVLINK_MSG_ID_MISSION_CURRENT: + _handleMissionCurrent(message); + break; } } diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 7e3ce2171f9dff31409adce137d0ae87db689093..c898d3b0a4455bb7d0bbede8396348acb394a50a 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -88,8 +88,8 @@ private: bool _checkForExpectedAck(AckType_t receivedAck); void _readTransactionComplete(void); void _handleMissionCount(const mavlink_message_t& message); - void _handleMissionItem(const mavlink_message_t& message); - void _handleMissionRequest(const mavlink_message_t& message); + void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt); + void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt); void _handleMissionAck(const mavlink_message_t& message); void _handleMissionCurrent(const mavlink_message_t& message); void _requestNextMissionItem(void); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b21c0494f539f070a4399fe789c805f6c19881cb..4bd35c45abdfc200d9ed51ad58ae7de39621cfe6 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -111,6 +111,8 @@ Vehicle::Vehicle(LinkInterface* link, , _telemetryTXBuffer(0) , _telemetryLNoise(0) , _telemetryRNoise(0) + , _vehicleCapabilitiesKnown(false) + , _supportsMissionItemInt(false) , _connectionLost(false) , _connectionLostEnabled(true) , _missionManager(NULL) @@ -264,6 +266,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _globalPositionIntMessageAvailable(false) , _cruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble()) , _hoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) + , _vehicleCapabilitiesKnown(true) + , _supportsMissionItemInt(false) , _connectionLost(false) , _connectionLostEnabled(true) , _missionManager(NULL) @@ -692,6 +696,13 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me } emit gitHashChanged(_gitHash); } + + if (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { + qCDebug(VehicleLog) << "Vehicle supports MISSION_ITEM_INT"; + _supportsMissionItemInt = true; + _vehicleCapabilitiesKnown = true; + _startMissionRequest(); + } } void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) @@ -725,6 +736,12 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&message, &ack); + if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) { + // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items + qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error. Starting mission request."; + _startMissionRequest(); + } + if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) { _mavCommandAckTimer.stop(); showError = _mavCommandQueue[0].showError; @@ -1634,9 +1651,10 @@ void Vehicle::_mapTrajectoryStop() _mapTrajectoryTimer.stop(); } -void Vehicle::_parametersReady(bool parametersReady) +void Vehicle::_startMissionRequest(void) { - if (parametersReady && !_missionManagerInitialRequestSent) { + if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) { + qCDebug(VehicleLog) << "_startMissionRequest"; _missionManagerInitialRequestSent = true; QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionAutoLoadDir()->rawValue().toString(); if (missionAutoLoadDirPath.isEmpty()) { @@ -1650,6 +1668,19 @@ void Vehicle::_parametersReady(bool parametersReady) MissionController::sendItemsToVehicle(this, visualItems); } } + } else { + if (!_parameterManager->parametersReady()) { + qCDebug(VehicleLog) << "Delaying _startMissionRequest due to parameters not ready"; + } else if (!_vehicleCapabilitiesKnown) { + qCDebug(VehicleLog) << "Delaying _startMissionRequest due to vehicle capabilities not know"; + } + } +} + +void Vehicle::_parametersReady(bool parametersReady) +{ + if (parametersReady) { + _startMissionRequest(); } if (parametersReady) { @@ -2043,6 +2074,11 @@ void Vehicle::_sendMavCommandAgain(void) MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0]; if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { + if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items + _startMissionRequest(); + } + emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); if (queuedCommand.showError) { qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command))); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7de40fe749cfbb2c0e7dc8880c792ad3c7c13c63..50fc6de7eb7890991aa4784802e1f25c37413892 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -658,6 +658,7 @@ public: const QVariantList& toolBarIndicators (); const QVariantList& cameraList (void) const; + bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; } public slots: /// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle @@ -822,6 +823,7 @@ private: void _sendNextQueuedMavCommand(void); void _updatePriorityLink(void); void _commonInit(void); + void _startMissionRequest(void); int _id; ///< Mavlink system id int _defaultComponentId; @@ -879,6 +881,8 @@ private: uint32_t _telemetryTXBuffer; uint32_t _telemetryLNoise; uint32_t _telemetryRNoise; + bool _vehicleCapabilitiesKnown; + bool _supportsMissionItemInt; typedef struct { int component;