diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc
index c41aed3..2aa7ad8 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 7e3ce21..c898d3b 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 b21c049..4bd35c4 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 7de40fe..50fc6de 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;