zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #8169 from DonLakeFlyer/ArduPilotMissionItemInt 

Always send MISSION_ITEM_INT to ArduPilot
QGC4.4
Don Gagne 5 years ago committed by GitHub
parent
fc33708b51 f39e88c81c
commit
1adc4aa733
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 115 additions and 57 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 51
      src/MissionManager/PlanManager.cc
  2. 109
      src/comm/MockLinkMissionItemHandler.cc
  3. 12
      src/comm/MockLinkMissionItemHandler.h

51
src/MissionManager/PlanManager.cc
Unescape View File

@ -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)
{
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 @@ -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 @@ -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(),

109
src/comm/MockLinkMissionItemHandler.cc
Unescape View File

@ -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;
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& @@ -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) @@ -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;
}

12
src/comm/MockLinkMissionItemHandler.h
Unescape View File

@ -85,7 +85,7 @@ private slots: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save