Skip to content

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

Merge pull request #4221 from DonLakeFlyer/MissionProtocolHardening 

Harden mission protocol to lost packets
parents cbfa7cc3 4c604d0d
Hide whitespace changes
Inline Side-by-side
Showing with 259 additions and 154 deletions
src/MissionManager/MissionManager.cc
View file @ cfe59f34
......@@ -23,7 +23,7 @@ MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle)
, _dedicatedLink(NULL)
, _ackTimeoutTimer(NULL)
, _retryAck(AckNone)
, _expectedAck(AckNone)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
, _currentMissionItem(-1)
......@@ -81,7 +81,17 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_writeTransactionInProgress = true;
_retryCount = 0;
emit inProgressChanged(true);
_writeMissionCount();
}
/// This begins the write sequence with the vehicle. This may be called during a retry.
void MissionManager::_writeMissionCount(void)
{
qCDebug(MissionManagerLog) << "_writeMissionCount retry count" << _retryCount;
mavlink_message_t message;
mavlink_mission_count_t missionCount;
......@@ -99,7 +109,6 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
}
void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
......@@ -153,75 +162,128 @@ void MissionManager::requestMissionItems(void)
qCDebug(MissionManagerLog) << "requestMissionItems called while transaction in progress";
return;
}
_retryCount = 0;
emit inProgressChanged(true);
_requestList();
}
/// Internal call to request list of mission items. May be called during a retry sequence.
void MissionManager::_requestList(void)
{
qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount;
mavlink_message_t message;
mavlink_mission_request_list_t request;
_requestItemRetryCount = 0;
_itemIndicesToRead.clear();
_readTransactionInProgress = true;
_clearMissionItems();
request.target_system = _vehicle->id();
request.target_component = MAV_COMP_ID_MISSIONPLANNER;
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&request);
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionCount);
emit inProgressChanged(true);
}
void MissionManager::_ackTimeout(void)
{
AckType_t timedOutAck = _retryAck;
_retryAck = AckNone;
if (timedOutAck == AckNone) {
qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
_sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_retryAck == AckNone");
if (_expectedAck == AckNone) {
return;
}
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck)));
_finishTransaction(false);
switch (_expectedAck) {
case AckNone:
qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
_sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
break;
case AckMissionCount:
// MISSION_COUNT message expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount;
_requestList();
}
break;
case AckMissionItem:
// MISSION_ITEM expected
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount;
_requestNextMissionItem();
}
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (_itemIndicesToWrite.count() == 0) {
// Vehicle did not send final MISSION_ACK at end of sequence
_sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
_finishTransaction(false);
} else if (_itemIndicesToWrite[0] == 0) {
// Vehicle did not respond to MISSION_COUNT, try again
if (_retryCount > _maxRetryCount) {
_sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
_finishTransaction(false);
} else {
_retryCount++;
qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount;
_writeMissionCount();
}
} else {
// Vehicle did not request all items from ground station
_sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
default:
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
_expectedAck = AckNone;
_finishTransaction(false);
}
}
void MissionManager::_startAckTimeout(AckType_t ack)
{
_retryAck = ack;
_expectedAck = ack;
_ackTimeoutTimer->start();
}
bool MissionManager::_stopAckTimeout(AckType_t expectedAck)
/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
/// @return true: received ack matches expected ack
bool MissionManager::_checkForExpectedAck(AckType_t receivedAck)
{
bool success = false;
AckType_t savedRetryAck = _retryAck;
_retryAck = AckNone;
_ackTimeoutTimer->stop();
if (savedRetryAck != expectedAck) {
if (savedRetryAck == AckNone) {
// Don't annoy the user with warnings about unexpected mission commands, just ignore them; ArduPilot updates home position using
if (receivedAck == _expectedAck) {
_expectedAck = AckNone;
_ackTimeoutTimer->stop();
return true;
} else {
if (_expectedAck == AckNone) {
// Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
// spurious MISSION_ITEMs.
} else {
_sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck)));
_finishTransaction(false);
// We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
// Whatever it is we let the ack timeout handle any error output to the user.
qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck));
}
success = false;
} else {
success = true;
return false;
}
return success;
}
void MissionManager::_readTransactionComplete(void)
......@@ -251,9 +313,11 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
{
mavlink_mission_count_t missionCount;
if (!_stopAckTimeout(AckMissionCount)) {
if (!_checkForExpectedAck(AckMissionCount)) {
return;
}
_retryCount = 0;
mavlink_msg_mission_count_decode(&message, &missionCount);
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
......@@ -271,13 +335,13 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
void MissionManager::_requestNextMissionItem(void)
{
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << _itemIndicesToRead[0];
if (_itemIndicesToRead.count() == 0) {
_sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
return;
}
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;
mavlink_message_t message;
mavlink_mission_request_t missionRequest;
......@@ -299,7 +363,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
{
mavlink_mission_item_t missionItem;
if (!_stopAckTimeout(AckMissionItem)) {
if (!_checkForExpectedAck(AckMissionItem)) {
return;
}
......@@ -308,7 +372,6 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq;
if (_itemIndicesToRead.contains(missionItem.seq)) {
_requestItemRetryCount = 0;
_itemIndicesToRead.removeOne(missionItem.seq);
MissionItem* item = new MissionItem(missionItem.seq,
......@@ -333,13 +396,12 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
_missionItems.append(item);
} else {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq;
if (++_requestItemRetryCount > _maxRetryCount) {
_sendError(RequestRangeError, QString("Vehicle would not send item %1 after max retries. Read from Vehicle failed.").arg(_itemIndicesToRead[0]));
_finishTransaction(false);
return;
}
// We have to put the ack timeout back since it was removed above
_startAckTimeout(AckMissionItem);
return;
}
_retryCount = 0;
if (_itemIndicesToRead.count() == 0) {
_readTransactionComplete();
} else {
......@@ -357,7 +419,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
{
mavlink_mission_request_t missionRequest;
if (!_stopAckTimeout(AckMissionRequest)) {
if (!_checkForExpectedAck(AckMissionRequest)) {
return;
}
......@@ -411,14 +473,14 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
{
mavlink_mission_ack_t missionAck;
// Save the retry ack before calling _stopAckTimeout since we'll need it to determine what
// Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
// type of a protocol sequence we are in.
AckType_t savedRetryAck = _retryAck;
AckType_t savedExpectedAck = _expectedAck;
// We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
// a protocol sequence error. Call _stopAckTimeout with _retryAck so it will succeed no
// a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
// matter what.
if (!_stopAckTimeout(_retryAck)) {
if (!_checkForExpectedAck(_expectedAck)) {
return;
}
......@@ -426,7 +488,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedRetryAck) {
switch (savedExpectedAck) {
case AckNone:
// State machine is idle. Vehicle is confused.
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
......@@ -438,7 +500,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
break;
case AckMissionItem:
// MISSION_ITEM expected
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionRequest:
......@@ -459,7 +521,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck guide mode item accepted";
qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
_finishTransaction(true);
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
......@@ -587,12 +649,14 @@ void MissionManager::_finishTransaction(bool success)
emit newMissionItemsAvailable();
}
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
emit inProgressChanged(false);
if (_readTransactionInProgress || _writeTransactionInProgress) {
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
emit inProgressChanged(false);
}
}
bool MissionManager::inProgress(void)
......
src/MissionManager/MissionManager.h
View file @ cfe59f34
......@@ -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 @ cfe59f34
......@@ -27,7 +27,7 @@ MissionManagerTest::MissionManagerTest(void)
}
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
{
_mockLink->setMissionItemFailureMode(failureMode);
......@@ -67,29 +67,9 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
_multiSpyMissionManager->clearAllSignals();
if (failureMode == MockLinkMissionItemHandler::FailNone) {
// This should be clean run
// Wait for write sequence to complete. We should get:
// inProgressChanged(false) signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate item count in mission manager
int expectedCount = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
expectedCount++;
}
QCOMPARE(_missionManager->missionItems().count(), expectedCount);
} else {
if (shouldFail) {
// This should be a failed run
setExpectedMessageBox(QMessageBox::Ok);
// Wait for write sequence to complete. We should get:
......@@ -97,10 +77,10 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
// error(errorCode, QString) signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate error signal values
QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
QList<QVariant> signalArgs = spy->takeFirst();
......@@ -108,14 +88,34 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
qDebug() << signalArgs[1].toString();
checkExpectedMessageBox();
} else {
// This should be clean run
// Wait for write sequence to complete. We should get:
// inProgressChanged(false) signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate item count in mission manager
int expectedCount = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
expectedCount++;
}
QCOMPARE(_missionManager->missionItems().count(), expectedCount);
}
_multiSpyMissionManager->clearAllSignals();
}
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail)
{
_writeItems(MockLinkMissionItemHandler::FailNone);
_writeItems(MockLinkMissionItemHandler::FailNone, false);
_mockLink->setMissionItemFailureMode(failureMode);
......@@ -129,19 +129,9 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
_multiSpyMissionManager->clearAllSignals();
if (failureMode == MockLinkMissionItemHandler::FailNone) {
// This should be clean run
// Now wait for read sequence to complete. We should get:
// inProgressChanged(false) signal to signal completion
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
_checkInProgressValues(false);
} else {
if (shouldFail) {
// This should be a failed run
setExpectedMessageBox(QMessageBox::Ok);
// Wait for read sequence to complete. We should get:
......@@ -150,17 +140,26 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true);
// Validate inProgressChanged signal value
_checkInProgressValues(false);
// Validate error signal values
QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex);
QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
checkExpectedMessageBox();
} else {
// This should be clean run
// Now wait for read sequence to complete. We should get:
// inProgressChanged(false) signal to signal completion
// newMissionItemsAvailable signal
_multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime);
QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
_checkInProgressValues(false);
}
_multiSpyMissionManager->clearAllSignals();
......@@ -169,14 +168,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
size_t cMissionItemsExpected;
if (failureMode == MockLinkMissionItemHandler::FailNone) {
if (shouldFail) {
cMissionItemsExpected = 0;
} else {
cMissionItemsExpected = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
cMissionItemsExpected++;
}
} else {
cMissionItemsExpected = 0;
}
QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected);
......@@ -233,24 +232,27 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void)
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
} TestCase_t;
bool shouldFail;
} WriteTestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests },
static const WriteTestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, false },
{ "FailWriteMissionCountNoResponse", MockLinkMissionItemHandler::FailWriteMissionCountNoResponse, true },
{ "FailWriteMissionCountFirstResponse", MockLinkMissionItemHandler::FailWriteMissionCountFirstResponse, false },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, true },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, true },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, true },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, true },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, true },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, true },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, true },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, true },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_writeItems(rgTestCases[i].failureMode);
const WriteTestCase_t* pCase = &rgTestCases[i];
qDebug() << "TEST CASE " << pCase->failureText;
_writeItems(pCase->failureMode, pCase->shouldFail);
_mockLink->resetMissionItemHandler();
}
}
......@@ -271,22 +273,31 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void)
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
} TestCase_t;
bool shouldFail;
} ReadTestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck },
/*
static const ReadTestCase_t rgTestCases[] = {
{ "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false },
};*/
static const ReadTestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, false },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse, true },
{ "FailReadRequestListFirstResponse", MockLinkMissionItemHandler::FailReadRequestListFirstResponse, false },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse, true },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse, true },
{ "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence, true },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence, true },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck, true },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck, true },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_roundTripItems(rgTestCases[i].failureMode);
const ReadTestCase_t* pCase = &rgTestCases[i];
qDebug() << "TEST CASE " << pCase->failureText;
_roundTripItems(pCase->failureMode, pCase->shouldFail);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
}
......
src/MissionManager/MissionManagerTest.h
View file @ cfe59f34
......@@ -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 @ cfe59f34
......@@ -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 @ cfe59f34
......@@ -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