Skip to content

Q
qgroundcontrol
Unverified Commit cb8b124f authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Change unit test to use MISSION_ITEM_INT to catch bugs (#9157) 

* MockLink is always MISSION_ITEM_INT to catch bugs in unit tests

* Fix typo
parent d4be4f34
Hide whitespace changes
Inline Side-by-side
Showing with 65 additions and 104 deletions
src/MissionManager/PlanManager.cc
View file @ cb8b124f
......@@ -395,7 +395,7 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = missionItem.frame == MAV_FRAME_MISSION ? (double)missionItem.x : (double)missionItem.x * 1e-7;
param6 = missionItem.frame == MAV_FRAME_MISSION ? (double)missionItem.y : (double)missionItem.x * 1e-7;
param6 = missionItem.frame == MAV_FRAME_MISSION ? (double)missionItem.y : (double)missionItem.y * 1e-7;
param7 = (double)missionItem.z;
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
......
src/comm/MockLink.cc
View file @ cb8b124f
......@@ -1129,11 +1129,14 @@ void MockLink::_respondWithAutopilotVersion(void)
#if !defined(NO_ARDUPILOT_DIALECT)
}
#endif
uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT |
(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0);
mavlink_msg_autopilot_version_pack_chan(_vehicleSystemId,
_vehicleComponentId,
_mavlinkChannel,
&msg,
MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0),
capabilities,
flightVersion, // flight_sw_version,
0, // middleware_sw_version,
0, // os_sw_version,
......
src/comm/MockLinkMissionItemHandler.cc
View file @ cb8b124f
......@@ -16,14 +16,14 @@
QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol)
: _mockLink(mockLink)
, _missionItemResponseTimer(nullptr)
, _failureMode(FailNone)
, _sendHomePositionOnEmptyList(false)
, _mavlinkProtocol(mavlinkProtocol)
, _failReadRequestListFirstResponse(true)
, _failReadRequest1FirstResponse(true)
, _failWriteMissionCountFirstResponse(true)
: _mockLink (mockLink)
, _missionItemResponseTimer (nullptr)
, _failureMode (FailNone)
, _sendHomePositionOnEmptyList (false)
, _mavlinkProtocol (mavlinkProtocol)
, _failReadRequestListFirstResponse (true)
, _failReadRequest1FirstResponse (true)
, _failWriteMissionCountFirstResponse (true)
{
Q_ASSERT(mockLink);
}
......@@ -49,16 +49,12 @@ bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
_handleMissionRequestList(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
_handleMissionRequest(msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg, false);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(msg, true);
_handleMissionItem(msg);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
......@@ -175,9 +171,9 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
{
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";
mavlink_mission_request_t request;
mavlink_mission_request_int_t request;
mavlink_msg_mission_request_decode(&msg, &request);
mavlink_msg_mission_request_int_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId());
......@@ -202,63 +198,44 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
(_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(_failureAckResult);
} else {
MissionItemBoth_t missionItemBoth;
mavlink_mission_item_int_t missionItemInt;
switch (request.mission_type) {
case MAV_MISSION_TYPE_MISSION:
if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
memset(&missionItemBoth, 0, sizeof(missionItemBoth));
missionItemBoth.missionItem.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
missionItemBoth.missionItem.command = MAV_CMD_NAV_WAYPOINT;
missionItemBoth.missionItem.autocontinue = true;
memset(&missionItemInt, 0, sizeof(missionItemInt));
missionItemInt.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
missionItemInt.command = MAV_CMD_NAV_WAYPOINT;
missionItemInt.autocontinue = true;
} else {
missionItemBoth = _missionItems[request.seq];
missionItemInt = _missionItems[request.seq];
}
break;
case MAV_MISSION_TYPE_FENCE:
missionItemBoth = _fenceItems[request.seq];
missionItemInt = _fenceItems[request.seq];
break;
case MAV_MISSION_TYPE_RALLY:
missionItemBoth = _rallyItems[request.seq];
missionItemInt = _rallyItems[request.seq];
break;
default:
Q_ASSERT(false);
}
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(),
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,
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
request.seq, // Index of mission item being sent
missionItemInt.frame,
missionItemInt.command,
missionItemInt.current,
missionItemInt.autocontinue,
missionItemInt.param1, missionItemInt.param2, missionItemInt.param3, missionItemInt.param4,
missionItemInt.x, missionItemInt.y, missionItemInt.z,
_requestType);
}
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
......@@ -331,14 +308,14 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
} else {
mavlink_message_t message;
mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&message,
_mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
sequenceNumber,
_requestType);
mavlink_msg_mission_request_int_pack_chan(_mockLink->vehicleId(),
MAV_COMP_ID_AUTOPILOT1,
_mockLink->mavlinkChannel(),
&message,
_mavlinkProtocol->getSystemId(),
_mavlinkProtocol->getComponentId(),
sequenceNumber,
_requestType);
_mockLink->respondWithMavlinkMessage(message);
// If response with Mission Item doesn't come before timer fires it's an error
......@@ -364,40 +341,29 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
_mockLink->respondWithMavlinkMessage(message);
}
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg, bool missionItemInt)
void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
_missionItemResponseTimer->stop();
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;
}
MAV_MISSION_TYPE missionType;
uint16_t seq;
mavlink_mission_item_int_t missionItemInt;
mavlink_msg_mission_item_int_decode(&msg, &missionItemInt);
missionType = static_cast<MAV_MISSION_TYPE>(missionItemInt.mission_type);
seq = missionItemInt.seq;
switch (missionType) {
case MAV_MISSION_TYPE_MISSION:
_missionItems[seq] = missionItemBoth;
_missionItems[seq] = missionItemInt;
break;
case MAV_MISSION_TYPE_FENCE:
_fenceItems[seq] = missionItemBoth;
_fenceItems[seq] = missionItemInt;
break;
case MAV_MISSION_TYPE_RALLY:
_rallyItems[seq] = missionItemBoth;
_rallyItems[seq] = missionItemInt;
break;
case MAV_MISSION_TYPE_ENUM_END:
case MAV_MISSION_TYPE_ALL:
......@@ -427,7 +393,7 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
{
qWarning() << "Timeout waiting for next MISSION_ITEM";
qWarning() << "Timeout waiting for next MISSION_ITEM_INT";
Q_ASSERT(false);
}
......
src/comm/MockLinkMissionItemHandler.h
View file @ cb8b124f
......@@ -84,14 +84,14 @@ private slots:
void _missionItemResponseTimeout(void);
private:
void _handleMissionRequestList(const mavlink_message_t& msg);
void _handleMissionRequest(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);
void _sendAck(MAV_MISSION_RESULT ackType);
void _startMissionItemResponseTimer(void);
void _handleMissionRequestList (const mavlink_message_t& msg);
void _handleMissionRequest (const mavlink_message_t& msg);
void _handleMissionItem (const mavlink_message_t& msg);
void _handleMissionCount (const mavlink_message_t& msg);
void _handleMissionClearAll (const mavlink_message_t& msg);
void _requestNextMissionItem (int sequenceNumber);
void _sendAck (MAV_MISSION_RESULT ackType);
void _startMissionItemResponseTimer (void);
private:
MockLink* _mockLink;
......@@ -99,15 +99,7 @@ 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, MissionItemBoth_t> MissionItemList_t;
typedef QMap<uint16_t, mavlink_mission_item_int_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