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)
void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
{
mavlink_mission_request_t missionRequest;
mavlink_msg_mission_request_decode(&message, &missionRequest);
MAV_MISSION_TYPE missionRequestMissionType;
uint16_t missionRequestSeq;
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
// 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;
}
......@@ -509,38 +520,38 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m
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) {
_sendError(RequestRangeError, tr("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq));
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(missionRequestSeq));
_finishTransaction(false);
return;
}
emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count());
emit progressPct((double)missionRequestSeq / (double)_writeMissionItems.count());
_lastMissionRequest = missionRequest.seq;
if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequest.seq;
_lastMissionRequest = missionRequestSeq;
if (!_itemIndicesToWrite.contains(missionRequestSeq)) {
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequestSeq;
} else {
_itemIndicesToWrite.removeOne(missionRequest.seq);
_itemIndicesToWrite.removeOne(missionRequestSeq);
}
MissionItem* item = _writeMissionItems[missionRequest.seq];
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequest.seq << item->command();
MissionItem* item = _writeMissionItems[missionRequestSeq];
qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequestSeq << item->command();
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(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
_vehicle->id(),
MAV_COMP_ID_AUTOPILOT1,
missionRequest.seq,
missionRequestSeq,
item->frame(),
item->command(),
missionRequest.seq == 0,
missionRequestSeq == 0,
item->autoContinue(),
item->param1(),
item->param2(),
......@@ -557,10 +568,10 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m
&messageOut,
_vehicle->id(),
MAV_COMP_ID_AUTOPILOT1,
missionRequest.seq,
missionRequestSeq,
item->frame(),
item->command(),
missionRequest.seq == 0,
missionRequestSeq == 0,
item->autoContinue(),
item->param1(),
item->param2(),
......
src/comm/MockLinkMissionItemHandler.cc
View file @ f39e88c8
......@@ -54,7 +54,11 @@ bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg);
_handleMissionItem(msg, false);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(msg, true);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
......@@ -198,45 +202,63 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(MAV_MISSION_ERROR);
} else {
mavlink_mission_item_t item;
mavlink_message_t responseMsg;
MissionItemBoth_t missionItemBoth;
switch (request.mission_type) {
case MAV_MISSION_TYPE_MISSION:
if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
item.command = MAV_CMD_NAV_WAYPOINT;
item.current = false;
item.autocontinue = true;
item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
memset(&missionItemBoth, 0, sizeof(missionItemBoth));
missionItemBoth.missionItem.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
missionItemBoth.missionItem.command = MAV_CMD_NAV_WAYPOINT;
missionItemBoth.missionItem.autocontinue = true;
} else {
item = _missionItems[request.seq];
missionItemBoth = _missionItems[request.seq];
}
break;
case MAV_MISSION_TYPE_FENCE:
item = _fenceItems[request.seq];
missionItemBoth = _fenceItems[request.seq];
break;
case MAV_MISSION_TYPE_RALLY:
item = _rallyItems[request.seq];
missionItemBoth = _rallyItems[request.seq];
break;
default:
Q_ASSERT(false);
}
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);
mavlink_message_t responseMsg;
if (missionItemBoth.isIntItem) {
mavlink_mission_item_int_t& item = missionItemBoth.missionItemInt;
mavlink_msg_mission_item_int_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);
} 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);
}
}
......@@ -342,27 +364,44 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
_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";
_missionItemResponseTimer->stop();
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&msg, &missionItem);
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
MAV_MISSION_TYPE missionType;
uint16_t seq;
MissionItemBoth_t missionItemBoth;
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:
_missionItems[missionItem.seq] = missionItem;
_missionItems[seq] = missionItemBoth;
break;
case MAV_MISSION_TYPE_FENCE:
_fenceItems[missionItem.seq] = missionItem;
_fenceItems[seq] = missionItemBoth;
break;
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;
}
......
src/comm/MockLinkMissionItemHandler.h
View file @ f39e88c8
......@@ -85,7 +85,7 @@ private slots:
private:
void _handleMissionRequestList(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 _handleMissionClearAll(const mavlink_message_t& msg);
void _requestNextMissionItem(int sequenceNumber);
......@@ -97,8 +97,16 @@ private:
int _writeSequenceCount; ///< Numbers of items about to be written
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;
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