Skip to content

Q
qgroundcontrol
Commit f39e88c8 authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

parent d6225972
Hide whitespace changes
Inline Side-by-side
Showing with 115 additions and 57 deletions
src/MissionManager/PlanManager.cc
View file @ f39e88c8
...@@ -494,14 +494,25 @@ void PlanManager::_clearMissionItems(void) ...@@ -494,14 +494,25 @@ void PlanManager::_clearMissionItems(void)
void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt) void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
{ {
mavlink_mission_request_t missionRequest; MAV_MISSION_TYPE missionRequestMissionType;
uint16_t missionRequestSeq;
mavlink_msg_mission_request_decode(&message, &missionRequest);
if (missionItemInt) {
mavlink_mission_request_int_t missionRequest;
mavlink_msg_mission_request_int_decode(&message, &missionRequest);
missionRequestMissionType = static_cast<MAV_MISSION_TYPE>(missionRequest.mission_type);
missionRequestSeq = missionRequest.seq;
} else {
mavlink_mission_request_t missionRequest;
mavlink_msg_mission_request_decode(&message, &missionRequest);
missionRequestMissionType = static_cast<MAV_MISSION_TYPE>(missionRequest.mission_type);
missionRequestSeq = missionRequest.seq;
}
if (missionRequest.mission_type != _planType) { if (missionRequestMissionType != _planType) {
// if there was a previous transaction with a different mission_type, it can happen that we receive // if there was a previous transaction with a different mission_type, it can happen that we receive
// a stale message here, for example when the MAV ran into a timeout and sent a message twice // a stale message here, for example when the MAV ran into a timeout and sent a message twice
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionRequest.mission_type; qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionRequestMissionType;
return; return;
} }
...@@ -509,38 +520,38 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m ...@@ -509,38 +520,38 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m
return; return;
} }
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber").arg(_planTypeString()) << missionRequest.seq; qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber").arg(_planTypeString()) << missionRequestSeq;
if (missionRequest.seq > _writeMissionItems.count() - 1) { if (missionRequestSeq > _writeMissionItems.count() - 1) {
_sendError(RequestRangeError, tr("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq)); _sendError(RequestRangeError, tr("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequestSeq));
_finishTransaction(false); _finishTransaction(false);
return; return;
} }
emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count()); emit progressPct((double)missionRequestSeq / (double)_writeMissionItems.count());
_lastMissionRequest = missionRequest.seq; _lastMissionRequest = missionRequestSeq;
if (!_itemIndicesToWrite.contains(missionRequest.seq)) { if (!_itemIndicesToWrite.contains(missionRequestSeq)) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequest.seq; qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequestSeq;
} else { } else {
_itemIndicesToWrite.removeOne(missionRequest.seq); _itemIndicesToWrite.removeOne(missionRequestSeq);
} }
MissionItem* item = _writeMissionItems[missionRequest.seq]; MissionItem* item = _writeMissionItems[missionRequestSeq];
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequest.seq << item->command(); qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequestSeq << item->command();
mavlink_message_t messageOut; mavlink_message_t messageOut;
if (missionItemInt) { if (missionItemInt || _vehicle->apmFirmware() /* ArduPilot always expects to get MISSION_ITEM_INT no matter what */) {
mavlink_msg_mission_item_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), mavlink_msg_mission_item_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(), _dedicatedLink->mavlinkChannel(),
&messageOut, &messageOut,
_vehicle->id(), _vehicle->id(),
MAV_COMP_ID_AUTOPILOT1, MAV_COMP_ID_AUTOPILOT1,
missionRequest.seq, missionRequestSeq,
item->frame(), item->frame(),
item->command(), item->command(),
missionRequest.seq == 0, missionRequestSeq == 0,
item->autoContinue(), item->autoContinue(),
item->param1(), item->param1(),
item->param2(), item->param2(),
...@@ -557,10 +568,10 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m ...@@ -557,10 +568,10 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m
&messageOut, &messageOut,
_vehicle->id(), _vehicle->id(),
MAV_COMP_ID_AUTOPILOT1, MAV_COMP_ID_AUTOPILOT1,
missionRequest.seq, missionRequestSeq,
item->frame(), item->frame(),
item->command(), item->command(),
missionRequest.seq == 0, missionRequestSeq == 0,
item->autoContinue(), item->autoContinue(),
item->param1(), item->param1(),
item->param2(), item->param2(),
......
src/comm/MockLinkMissionItemHandler.cc
View file @ f39e88c8
...@@ -54,7 +54,11 @@ bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) ...@@ -54,7 +54,11 @@ bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
break; break;
case MAVLINK_MSG_ID_MISSION_ITEM: case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg); _handleMissionItem(msg, false);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(msg, true);
break; break;
case MAVLINK_MSG_ID_MISSION_COUNT: case MAVLINK_MSG_ID_MISSION_COUNT:
...@@ -198,45 +202,63 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& ...@@ -198,45 +202,63 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(MAV_MISSION_ERROR); _sendAck(MAV_MISSION_ERROR);
} else { } else {
mavlink_mission_item_t item; MissionItemBoth_t missionItemBoth;
mavlink_message_t responseMsg;
switch (request.mission_type) { switch (request.mission_type) {
case MAV_MISSION_TYPE_MISSION: case MAV_MISSION_TYPE_MISSION:
if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) { if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; memset(&missionItemBoth, 0, sizeof(missionItemBoth));
item.command = MAV_CMD_NAV_WAYPOINT; missionItemBoth.missionItem.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
item.current = false; missionItemBoth.missionItem.command = MAV_CMD_NAV_WAYPOINT;
item.autocontinue = true; missionItemBoth.missionItem.autocontinue = true;
item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
} else { } else {
item = _missionItems[request.seq]; missionItemBoth = _missionItems[request.seq];
} }
break; break;
case MAV_MISSION_TYPE_FENCE: case MAV_MISSION_TYPE_FENCE:
item = _fenceItems[request.seq]; missionItemBoth = _fenceItems[request.seq];
break; break;
case MAV_MISSION_TYPE_RALLY: case MAV_MISSION_TYPE_RALLY:
item = _rallyItems[request.seq]; missionItemBoth = _rallyItems[request.seq];
break; break;
default: default:
Q_ASSERT(false); Q_ASSERT(false);
} }
mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(), mavlink_message_t responseMsg;
MAV_COMP_ID_AUTOPILOT1, if (missionItemBoth.isIntItem) {
_mockLink->mavlinkChannel(), mavlink_mission_item_int_t& item = missionItemBoth.missionItemInt;
&responseMsg, // Outgoing message mavlink_msg_mission_item_int_pack_chan(_mockLink->vehicleId(),
msg.sysid, // Target is original sender MAV_COMP_ID_AUTOPILOT1,
msg.compid, // Target is original sender _mockLink->mavlinkChannel(),
request.seq, // Index of mission item being sent &responseMsg, // Outgoing message
item.frame, msg.sysid, // Target is original sender
item.command, msg.compid, // Target is original sender
item.current, request.seq, // Index of mission item being sent
item.autocontinue, item.frame,
item.param1, item.param2, item.param3, item.param4, item.command,
item.x, item.y, item.z, item.current,
_requestType); item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z,
_requestType);
} else {
mavlink_mission_item_t& item = missionItemBoth.missionItem;
mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
request.seq, // Index of mission item being sent
item.frame,
item.command,
item.current,
item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z,
_requestType);
}
_mockLink->respondWithMavlinkMessage(responseMsg); _mockLink->respondWithMavlinkMessage(responseMsg);
} }
} }
...@@ -342,27 +364,44 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) ...@@ -342,27 +364,44 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
_mockLink->respondWithMavlinkMessage(message); _mockLink->respondWithMavlinkMessage(message);
} }
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg) void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg, bool missionItemInt)
{ {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence"; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
_missionItemResponseTimer->stop(); _missionItemResponseTimer->stop();
mavlink_mission_item_t missionItem; MAV_MISSION_TYPE missionType;
uint16_t seq;
mavlink_msg_mission_item_decode(&msg, &missionItem); MissionItemBoth_t missionItemBoth;
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId()); missionItemBoth.isIntItem = missionItemInt;
if (missionItemInt) {
mavlink_mission_item_int_t missionItem;
mavlink_msg_mission_item_int_decode(&msg, &missionItem);
missionItemBoth.missionItemInt = missionItem;
missionType = static_cast<MAV_MISSION_TYPE>(missionItem.mission_type);
seq = missionItem.seq;
} else {
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&msg, &missionItem);
missionItemBoth.missionItem = missionItem;
missionType = static_cast<MAV_MISSION_TYPE>(missionItem.mission_type);
seq = missionItem.seq;
}
switch (missionItem.mission_type) { switch (missionType) {
case MAV_MISSION_TYPE_MISSION: case MAV_MISSION_TYPE_MISSION:
_missionItems[missionItem.seq] = missionItem; _missionItems[seq] = missionItemBoth;
break; break;
case MAV_MISSION_TYPE_FENCE: case MAV_MISSION_TYPE_FENCE:
_fenceItems[missionItem.seq] = missionItem; _fenceItems[seq] = missionItemBoth;
break; break;
case MAV_MISSION_TYPE_RALLY: case MAV_MISSION_TYPE_RALLY:
_rallyItems[missionItem.seq] = missionItem; _rallyItems[seq] = missionItemBoth;
break;
case MAV_MISSION_TYPE_ENUM_END:
case MAV_MISSION_TYPE_ALL:
qWarning() << "Internal error";
break; break;
} }
......
src/comm/MockLinkMissionItemHandler.h
View file @ f39e88c8
...@@ -85,7 +85,7 @@ private slots: ...@@ -85,7 +85,7 @@ private slots:
private: private:
void _handleMissionRequestList(const mavlink_message_t& msg); void _handleMissionRequestList(const mavlink_message_t& msg);
void _handleMissionRequest(const mavlink_message_t& msg); void _handleMissionRequest(const mavlink_message_t& msg);
void _handleMissionItem(const mavlink_message_t& msg); void _handleMissionItem(const mavlink_message_t& msg, bool missionItemInt);
void _handleMissionCount(const mavlink_message_t& msg); void _handleMissionCount(const mavlink_message_t& msg);
void _handleMissionClearAll(const mavlink_message_t& msg); void _handleMissionClearAll(const mavlink_message_t& msg);
void _requestNextMissionItem(int sequenceNumber); void _requestNextMissionItem(int sequenceNumber);
...@@ -97,8 +97,16 @@ private: ...@@ -97,8 +97,16 @@ private:
int _writeSequenceCount; ///< Numbers of items about to be written int _writeSequenceCount; ///< Numbers of items about to be written
int _writeSequenceIndex; ///< Current index being reqested int _writeSequenceIndex; ///< Current index being reqested
typedef struct {
bool isIntItem;
union {
mavlink_mission_item_t missionItem;
mavlink_mission_item_int_t missionItemInt;
};
} MissionItemBoth_t;
typedef QMap<uint16_t, mavlink_mission_item_t> MissionItemList_t; typedef QMap<uint16_t, MissionItemBoth_t> MissionItemList_t;
MAV_MISSION_TYPE _requestType; MAV_MISSION_TYPE _requestType;
MissionItemList_t _missionItems; MissionItemList_t _missionItems;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment