Skip to content

Q
qgroundcontrol
Commit 4c604d0d authored by Don Gagne's avatar Don Gagne
Browse files
Options

Harden mission protocol to lost packets 

Retry in all the right/possible places. Don’t freak out about things
coming in unknown sequences.
parent 8eab3f6e
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 259 additions and 154 deletions
src/MissionManager/MissionManager.cc
View file @ 4c604d0d
This diff is collapsed.
src/MissionManager/MissionManager.h
View file @ 4c604d0d
......@@ -62,7 +62,7 @@ public:
} ErrorCode_t;
// These values are public so the unit test can set appropriate signal wait times
static const int _ackTimeoutMilliseconds= 2000;
static const int _ackTimeoutMilliseconds = 1000;
static const int _maxRetryCount = 5;
signals:
......@@ -85,7 +85,7 @@ private:
} AckType_t;
void _startAckTimeout(AckType_t ack);
bool _stopAckTimeout(AckType_t expectedAck);
bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message);
......@@ -98,14 +98,16 @@ private:
QString _ackTypeToString(AckType_t ackType);
QString _missionResultToString(MAV_MISSION_RESULT result);
void _finishTransaction(bool success);
void _requestList(void);
void _writeMissionCount(void);
private:
Vehicle* _vehicle;
LinkInterface* _dedicatedLink;
QTimer* _ackTimeoutTimer;
AckType_t _retryAck;
int _requestItemRetryCount;
AckType_t _expectedAck;
int _retryCount;
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
......
src/MissionManager/MissionManagerTest.cc
View file @ 4c604d0d
This diff is collapsed.
src/MissionManager/MissionManagerTest.h
View file @ 4c604d0d
......@@ -33,13 +33,13 @@ private slots:
void _testReadFailureHandlingAPM(void);
private:
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail);
void _testWriteFailureHandlingWorker(void);
void _testReadFailureHandlingWorker(void);
static const MissionControllerManagerTest::TestCase_t _rgTestCases[];
static const size_t _cTestCases;
static const TestCase_t _rgTestCases[];
static const size_t _cTestCases;
};
#endif
src/comm/MockLinkMissionItemHandler.cc
View file @ 4c604d0d
......@@ -21,6 +21,9 @@ MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLi
, _failureMode(FailNone)
, _sendHomePositionOnEmptyList(false)
, _mavlinkProtocol(mavlinkProtocol)
, _failReadRequestListFirstResponse(true)
, _failReadRequest1FirstResponse(true)
, _failWriteMissionCountFirstResponse(true)
{
Q_ASSERT(mockLink);
}
......@@ -82,9 +85,17 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
{
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
if (_failureMode != FailReadRequestListNoResponse) {
_failReadRequest1FirstResponse = true;
if (_failureMode == FailReadRequestListNoResponse) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse";
} else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) {
_failReadRequestListFirstResponse = false;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse";
} else {
mavlink_mission_request_list_t request;
_failReadRequestListFirstResponse = true;
mavlink_msg_mission_request_list_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId());
......@@ -104,8 +115,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
msg.compid, // Target is original sender
itemCount); // Number of mission items
_mockLink->respondWithMavlinkMessage(responseMsg);
} else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
}
}
......@@ -120,9 +129,13 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
Q_ASSERT(request.target_system == _mockLink->vehicleId());
Q_ASSERT(request.seq < _missionItems.count());
if ((_failureMode == FailReadRequest0NoResponse && request.seq == 0) ||
(_failureMode == FailReadRequest1NoResponse && request.seq == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode";
if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
} else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse";
} else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) {
_failReadRequest1FirstResponse = false;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse";
} else {
// FIXME: Track whether all items are requested, or requested in sequence
......@@ -185,6 +198,16 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
if (_writeSequenceCount == 0) {
_sendAck(MAV_MISSION_ACCEPTED);
} else {
if (_failureMode == FailWriteMissionCountNoResponse) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
return;
}
if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) {
_failWriteMissionCountFirstResponse = false;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse";
return;
}
_failWriteMissionCountFirstResponse = true;
_writeSequenceIndex = 0;
_requestNextMissionItem(_writeSequenceIndex);
}
......@@ -194,9 +217,8 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) ||
(_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode";
if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse";
} else {
if (sequenceNumber >= _writeSequenceCount) {
qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
......
src/comm/MockLinkMissionItemHandler.h
View file @ 4c604d0d
......@@ -42,18 +42,21 @@ public:
typedef enum {
FailNone, // No failures
FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST
FailReadRequestListFirstResponse, // Don't send MISSION_COUNT in response to first MISSION_REQUEST_LIST, allow subsequent to go through
FailReadRequest0NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 0
FailReadRequest1NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1
FailReadRequest1FirstResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1 on first try, allow subsequent request to go through
FailReadRequest0IncorrectSequence, // Respond to MISSION_REQUEST 0 with incorrect sequence number in MISSION_ITEM
FailReadRequest1IncorrectSequence, // Respond to MISSION_REQUEST 1 with incorrect sequence number in MISSION_ITEM
FailReadRequest0ErrorAck, // Respond to MISSION_REQUEST 0 with MISSION_ACK error
FailReadRequest1ErrorAck, // Respond to MISSION_REQUEST 1 bogus MISSION_ACK error
FailWriteRequest0NoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0
FailWriteMissionCountNoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0
FailWriteMissionCountFirstResponse, // Don't respond to first MISSION_COUNT with MISSION_REQUEST 0, respond to subsequent MISSION_COUNT requests
FailWriteRequest1NoResponse, // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1
FailWriteRequest0IncorrectSequence, // Respond to MISSION_COUNT 0 with MISSION_REQUEST with wrong sequence number
FailWriteRequest1IncorrectSequence, // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number
FailWriteRequest0ErrorAck, // Respond to MISSION_COUNT 0 with MISSION_ACK error
FailWriteRequest1ErrorAck, // Respond to MISSION_ITEM 0 with MISSION_ACK error
FailWriteRequest0IncorrectSequence, // Item 0 MISSION_REQUEST sent has wrong sequence number
FailWriteRequest1IncorrectSequence, // Item 1 MISSION_REQUEST sent has wrong sequence number
FailWriteRequest0ErrorAck, // Instead of sending MISSION_REQUEST 0, send MISSION_ACK error
FailWriteRequest1ErrorAck, // Instead of sending MISSION_REQUEST 1, send MISSION_ACK error
FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK
FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK
FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested
......@@ -102,6 +105,9 @@ private:
FailureMode_t _failureMode;
bool _sendHomePositionOnEmptyList;
MAVLinkProtocol* _mavlinkProtocol;
bool _failReadRequestListFirstResponse;
bool _failReadRequest1FirstResponse;
bool _failWriteMissionCountFirstResponse;
};
#endif
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