Commit 4131c134 authored by Don Gagne's avatar Don Gagne

Mission protocol robustness rework

parent 7f43851e
This diff is collapsed.
......@@ -52,7 +52,7 @@ public:
// Property accessors
bool inProgress(void) { return _retryAck != AckNone; }
bool inProgress(void);
QmlObjectListModel* missionItems(void) { return &_missionItems; }
// C++ methods
......@@ -76,6 +76,7 @@ public:
ItemMismatchError, ///< Vehicle returned item with seq # different than requested
VehicleError, ///< Vehicle returned error
MissingRequestsError, ///< Vehicle did not request all items during write sequence
MaxRetryExceeded, ///< Retry failed
} ErrorCode_t;
// These values are public so the unit test can set appropriate signal wait times
......@@ -106,25 +107,24 @@ private:
void _handleMissionItem(const mavlink_message_t& message);
void _handleMissionRequest(const mavlink_message_t& message);
void _handleMissionAck(const mavlink_message_t& message);
void _requestNextMissionItem(int sequenceNumber);
void _requestNextMissionItem(void);
void _clearMissionItems(void);
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
void _retryWrite(void);
void _retryRead(void);
bool _retrySequence(AckType_t ackType);
QString _ackTypeToString(AckType_t ackType);
QString _missionResultToString(MAV_MISSION_RESULT result);
void _finishTransaction(bool success);
private:
Vehicle* _vehicle;
int _cMissionItems; ///< Mission items on vehicle
QTimer* _ackTimeoutTimer;
AckType_t _retryAck;
int _retryCount;
int _requestItemRetryCount;
int _expectedSequenceNumber;
bool _readTransactionInProgress;
bool _writeTransactionInProgress;
QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
QMutex _dataMutex;
......
This diff is collapsed.
......@@ -40,14 +40,14 @@ public:
MissionManagerTest(void);
private slots:
void _testWriteFailureHandlingAPM(void);
void _testReadFailureHandlingAPM(void);
void _testWriteFailureHandlingPX4(void);
void _testWriteFailureHandlingAPM(void);
void _testReadFailureHandlingPX4(void);
void _testReadFailureHandlingAPM(void);
private:
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
void _testWriteFailureHandlingWorker(void);
void _testReadFailureHandlingWorker(void);
......
......@@ -783,9 +783,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
}
}
void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly)
void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
{
_missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly);
_missionItemHandler.setMissionItemFailureMode(failureMode);
}
void MockLink::_sendHomePosition(void)
......
......@@ -131,8 +131,7 @@ public:
/// Sets a failure mode for unit testing
/// @param failureMode Type of failure to simulate
/// @param firstTimeOnly true: fail first call, success subsequent calls, false: fail all calls
void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly);
void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
......
......@@ -119,10 +119,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
} else {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
......@@ -181,10 +177,6 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
......@@ -246,10 +238,6 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
_startMissionItemResponseTimer();
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
......@@ -279,9 +267,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
Q_ASSERT(!_missionItems.contains(missionItem.seq));
Q_ASSERT(missionItem.seq == _writeSequenceIndex);
_missionItems[missionItem.seq] = missionItem;
_writeSequenceIndex++;
......@@ -302,9 +287,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
_sendAck(ack);
}
}
if (_failureFirstTimeOnly) {
_failureMode = FailNone;
}
}
void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
......@@ -330,9 +312,8 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
Q_ASSERT(false);
}
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly)
void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
{
_failureFirstTimeOnly = firstTimeOnly;
_failureMode = failureMode;
}
......
......@@ -74,8 +74,7 @@ public:
/// Sets a failure mode for unit testing
/// @param failureMode Type of failure to simulate
/// @param firstTimeOnly true: fail first call, success subsequent calls, false: fail all calls
void setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly);
void setMissionItemFailureMode(FailureMode_t failureMode);
/// Called to send a MISSION_ACK message while the MissionManager is in idle state
void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType);
......@@ -114,7 +113,6 @@ private:
QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode;
bool _failureFirstTimeOnly;
bool _sendHomePositionOnEmptyList;
MAVLinkProtocol* _mavlinkProtocol;
};
......
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