Skip to content

Q
qgroundcontrol
Commit 4131c134 authored by Don Gagne's avatar Don Gagne
Browse files
Options

Mission protocol robustness rework

parent 7f43851e
Hide whitespace changes
Inline Side-by-side
Showing with 153 additions and 263 deletions
src/MissionManager/MissionManager.cc
View file @ 4131c134
......@@ -34,9 +34,10 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle)
: _vehicle(vehicle)
, _cMissionItems(0)
, _ackTimeoutTimer(NULL)
, _retryAck(AckNone)
, _readTransactionInProgress(false)
, _writeTransactionInProgress(false)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
......@@ -56,7 +57,6 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
{
bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
_retryCount = 0;
_missionItems.clear();
int firstIndex = skipFirstItem ? 1 : 0;
......@@ -82,40 +82,25 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress";
return;
}
mavlink_message_t message;
mavlink_mission_count_t missionCount;
_expectedSequenceNumber = 0;
missionCount.target_system = _vehicle->id();
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionCount.count = _missionItems.count();
mavlink_msg_mission_count_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionCount);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
}
void MissionManager::_retryWrite(void)
{
qCDebug(MissionManagerLog) << "_retryWrite";
// Prime write list
for (int i=0; i<_missionItems.count(); i++) {
_itemIndicesToWrite << i;
}
_writeTransactionInProgress = true;
mavlink_message_t message;
mavlink_mission_count_t missionCount;
_expectedSequenceNumber = 0;
missionCount.target_system = _vehicle->id();
missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionCount.count = _missionItems.count();
mavlink_msg_mission_count_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionCount);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest);
emit inProgressChanged(true);
}
void MissionManager::requestMissionItems(void)
......@@ -125,7 +110,9 @@ void MissionManager::requestMissionItems(void)
mavlink_message_t message;
mavlink_mission_request_list_t request;
_retryCount = 0;
_requestItemRetryCount = 0;
_itemIndicesToRead.clear();
_readTransactionInProgress = true;
_clearMissionItems();
request.target_system = _vehicle->id();
......@@ -138,24 +125,6 @@ void MissionManager::requestMissionItems(void)
emit inProgressChanged(true);
}
void MissionManager::_retryRead(void)
{
qCDebug(MissionManagerLog) << "_retryRead";
mavlink_message_t message;
mavlink_mission_request_list_t request;
_clearMissionItems();
request.target_system = _vehicle->id();
request.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_msg_mission_request_list_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &request);
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionCount);
}
void MissionManager::_ackTimeout(void)
{
AckType_t timedOutAck = _retryAck;
......@@ -168,10 +137,8 @@ void MissionManager::_ackTimeout(void)
return;
}
if (!_retrySequence(timedOutAck)) {
qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << _ackTypeToString(timedOutAck) << _retryCount;
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck)));
}
_sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck)));
_finishTransaction(false);
}
void MissionManager::_startAckTimeout(AckType_t ack)
......@@ -190,11 +157,8 @@ bool MissionManager::_stopAckTimeout(AckType_t expectedAck)
_ackTimeoutTimer->stop();
if (savedRetryAck != expectedAck) {
qCDebug(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << _ackTypeToString(savedRetryAck) << _ackTypeToString(expectedAck);
if (_retrySequence(expectedAck)) {
_sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck)));
}
_sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck)));
_finishTransaction(false);
success = false;
} else {
success = true;
......@@ -219,7 +183,7 @@ void MissionManager::_readTransactionComplete(void)
_vehicle->sendMessage(message);
emit newMissionItemsAvailable();
emit inProgressChanged(false);
_finishTransaction(true);
}
void MissionManager::_handleMissionCount(const mavlink_message_t& message)
......@@ -231,24 +195,27 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
}
mavlink_msg_mission_count_decode(&message, &missionCount);
_cMissionItems = missionCount.count;
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << _cMissionItems;
if (_cMissionItems == 0) {
qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
if (missionCount.count == 0) {
_readTransactionComplete();
} else {
_requestNextMissionItem(0);
// Prime read list
for (int i=0; i<missionCount.count; i++) {
_itemIndicesToRead << i;
}
_requestNextMissionItem();
}
}
void MissionManager::_requestNextMissionItem(int sequenceNumber)
void MissionManager::_requestNextMissionItem(void)
{
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << sequenceNumber;
if (sequenceNumber >= _cMissionItems) {
qCWarning(MissionManagerLog) << "_requestNextMissionItem requested seqeuence number > item count sequenceNumber::_cMissionItems" << sequenceNumber << _cMissionItems;
_sendError(InternalError, QString("QGroundControl requested mission item outside of range (internal error): %1:%2").arg(sequenceNumber).arg(_cMissionItems));
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;
}
......@@ -257,8 +224,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber)
missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = sequenceNumber;
_expectedSequenceNumber = sequenceNumber;
missionRequest.seq = _itemIndicesToRead[0];
mavlink_msg_mission_request_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionRequest);
......@@ -278,40 +244,43 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq;
if (missionItem.seq != _expectedSequenceNumber) {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received out of sequence expected:actual" << _expectedSequenceNumber << missionItem.seq;
if (!_retrySequence(AckMissionItem)) {
_sendError(ItemMismatchError, QString("Vehicle returned incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionItem.seq));
if (_itemIndicesToRead.contains(missionItem.seq)) {
_requestItemRetryCount = 0;
_itemIndicesToRead.removeOne(missionItem.seq);
MissionItem* item = new MissionItem(missionItem.seq,
(MAV_CMD)missionItem.command,
(MAV_FRAME)missionItem.frame,
missionItem.param1,
missionItem.param2,
missionItem.param3,
missionItem.param4,
missionItem.x,
missionItem.y,
missionItem.z,
missionItem.autocontinue,
missionItem.current,
this);
_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;
}
return;
}
MissionItem* item = new MissionItem(missionItem.seq,
(MAV_CMD)missionItem.command,
(MAV_FRAME)missionItem.frame,
missionItem.param1,
missionItem.param2,
missionItem.param3,
missionItem.param4,
missionItem.x,
missionItem.y,
missionItem.z,
missionItem.autocontinue,
missionItem.current,
this);
_missionItems.append(item);
int nextSequenceNumber = missionItem.seq + 1;
if (nextSequenceNumber == _cMissionItems) {
if (_itemIndicesToRead.count() == 0) {
_readTransactionComplete();
} else {
_requestNextMissionItem(nextSequenceNumber);
_requestNextMissionItem();
}
}
void MissionManager::_clearMissionItems(void)
{
_cMissionItems = 0;
_itemIndicesToRead.clear();
_missionItems.clear();
}
......@@ -327,17 +296,18 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq;
if (missionRequest.seq != _expectedSequenceNumber) {
qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested: _expectedSequenceNumber:missionRequest.seq" << _expectedSequenceNumber << missionRequest.seq;
if (!_retrySequence(AckMissionRequest)) {
_sendError(ItemMismatchError, QString("Vehicle requested incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionRequest.seq));
if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
if (missionRequest.seq > _missionItems.count()) {
_sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq));
_finishTransaction(false);
return;
} else {
qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
}
return;
} else {
_itemIndicesToWrite.removeOne(missionRequest.seq);
}
_expectedSequenceNumber++;
mavlink_message_t messageOut;
mavlink_mission_item_t missionItem;
......@@ -386,41 +356,31 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
switch (savedRetryAck) {
case AckNone:
// State machine is idle. Vehicle is confused.
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionCount)) {
_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 AckMissionItem:
// MISSION_ITEM expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionItem)) {
_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. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
break;
case AckMissionRequest:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
if (_expectedSequenceNumber == _missionItems.count()) {
if (_itemIndicesToWrite.count() == 0) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
emit inProgressChanged(false);
_finishTransaction(true);
} else {
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle did not reqeust all items: _expectedSequenceNumber:_missionItems.count" << _expectedSequenceNumber << _missionItems.count();
if (!_retrySequence(AckMissionRequest)) {
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence %1:%2. Vehicle only has partial list of mission items.").arg(_expectedSequenceNumber).arg(_missionItems.count()));
}
_sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count()));
_finishTransaction(false);
}
} else {
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionRequest)) {
_sendError(VehicleError,
QString("Vehicle returned error: %1 on item %2. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)).arg(_expectedSequenceNumber));
}
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
}
......@@ -469,43 +429,9 @@ QmlObjectListModel* MissionManager::copyMissionItems(void)
void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{
emit inProgressChanged(false);
emit error(errorCode, errorMsg);
}
qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg;
/// Retry the protocol sequence given the specified ack
/// @return true: sequence retried, false: out of retries
bool MissionManager::_retrySequence(AckType_t ackType)
{
qCDebug(MissionManagerLog) << "_retrySequence ackType:" << _ackTypeToString(ackType) << "_retryCount" << _retryCount;
switch (ackType) {
case AckMissionCount:
case AckMissionItem:
if (++_retryCount <= _maxRetryCount) {
// We are in the middle of a read sequence, start over
_retryRead();
return true;
} else {
// Read sequence failed, signal for what we have up to this point
emit newMissionItemsAvailable();
return false;
}
break;
case AckMissionRequest:
if (++_retryCount <= _maxRetryCount) {
// We are in the middle of a write sequence, start over
_retryWrite();
return true;
} else {
return false;
}
break;
default:
qCWarning(MissionManagerLog) << "_retrySequence fell through switch: ackType:" << _ackTypeToString(ackType);
_sendError(InternalError, QString("Internal error occured during Mission Item communication: _retrySequence fell through switch: ackType:").arg(_ackTypeToString(ackType)));
return false;
}
emit error(errorCode, errorMsg);
}
QString MissionManager::_ackTypeToString(AckType_t ackType)
......@@ -578,3 +504,24 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
return QString("QGC Internal Error");
}
}
void MissionManager::_finishTransaction(bool success)
{
if (!success && _readTransactionInProgress) {
// Read from vehicle failed, clear partial list
_missionItems.clear();
emit newMissionItemsAvailable();
}
_readTransactionInProgress = false;
_writeTransactionInProgress = false;
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
emit inProgressChanged(false);
}
bool MissionManager::inProgress(void)
{
return _readTransactionInProgress || _writeTransactionInProgress;
}
src/MissionManager/MissionManager.h
View file @ 4131c134
......@@ -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;
......
src/MissionManager/MissionManagerTest.cc
View file @ 4131c134
......@@ -42,13 +42,9 @@ MissionManagerTest::MissionManagerTest(void)
}
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly)
void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
{
_mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
_mockLink->setMissionItemFailureMode(failureMode);
// Setup our test case data
QmlObjectListModel* list = new QmlObjectListModel();
......@@ -131,7 +127,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
checkExpectedMessageBox();
}
......@@ -141,15 +136,11 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
_multiSpyMissionManager->clearAllSignals();
}
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly)
void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode)
{
_writeItems(MockLinkMissionItemHandler::FailNone, MissionManager::InternalError, false);
_writeItems(MockLinkMissionItemHandler::FailNone);
_mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly);
if (failFirstTimeOnly) {
// Should fail first time, then retry should succed
failureMode = MockLinkMissionItemHandler::FailNone;
}
_mockLink->setMissionItemFailureMode(failureMode);
// Read the items back from the vehicle
_missionManager->requestMissionItems();
......@@ -191,7 +182,6 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 2);
qDebug() << signalArgs[1].toString();
QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
checkExpectedMessageBox();
}
......@@ -202,30 +192,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
size_t cMissionItemsExpected;
if (failureMode == MockLinkMissionItemHandler::FailNone || failFirstTimeOnly == true) {
if (failureMode == MockLinkMissionItemHandler::FailNone) {
cMissionItemsExpected = (int)_cTestCases;
if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
// Home position at position 0 comes from vehicle
cMissionItemsExpected++;
}
} else {
switch (failureMode) {
case MockLinkMissionItemHandler::FailReadRequestListNoResponse:
case MockLinkMissionItemHandler::FailReadRequest0NoResponse:
case MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence:
case MockLinkMissionItemHandler::FailReadRequest0ErrorAck:
cMissionItemsExpected = 0;
break;
case MockLinkMissionItemHandler::FailReadRequest1NoResponse:
case MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence:
case MockLinkMissionItemHandler::FailReadRequest1ErrorAck:
cMissionItemsExpected = 1;
break;
default:
// Internal error
Q_ASSERT(false);
break;
}
cMissionItemsExpected = 0;
}
QCOMPARE(_missionManager->missionItems()->count(), (int)cMissionItemsExpected);
......@@ -287,28 +261,24 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void)
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
MissionManager::ErrorCode_t errorCode;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, MissionManager::AckTimeoutError },
{ "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse, MissionManager::AckTimeoutError },
{ "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, MissionManager::AckTimeoutError },
{ "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, MissionManager::VehicleError },
{ "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, MissionManager::VehicleError },
{ "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, MissionManager::AckTimeoutError },
{ "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, MissionManager::VehicleError },
{ "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, MissionManager::MissingRequestsError },
{ "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 },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
_writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
_mockLink->resetMissionItemHandler();
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
_writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_writeItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
}
}
......@@ -329,27 +299,22 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void)
typedef struct {
const char* failureText;
MockLinkMissionItemHandler::FailureMode_t failureMode;
MissionManager::ErrorCode_t errorCode;
} TestCase_t;
static const TestCase_t rgTestCases[] = {
{ "No Failure", MockLinkMissionItemHandler::FailNone, MissionManager::AckTimeoutError },
{ "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse, MissionManager::AckTimeoutError },
{ "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse, MissionManager::AckTimeoutError },
{ "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse, MissionManager::AckTimeoutError },
{ "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence, MissionManager::ItemMismatchError },
{ "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck, MissionManager::VehicleError },
{ "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck, MissionManager::VehicleError },
{ "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 },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
_roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
_roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
qDebug() << "TEST CASE " << rgTestCases[i].failureText;
_roundTripItems(rgTestCases[i].failureMode);
_mockLink->resetMissionItemHandler();
_multiSpyMissionManager->clearAllSignals();
}
......
src/MissionManager/MissionManagerTest.h
View file @ 4131c134
......@@ -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);
......
src/comm/MockLink.cc
View file @ 4131c134
......@@ -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)
......
src/comm/MockLink.h
View file @ 4131c134
......@@ -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); }
......
src/comm/MockLinkMissionItemHandler.cc
View file @ 4131c134
......@@ -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;
}
......
src/comm/MockLinkMissionItemHandler.h
View file @ 4131c134
......@@ -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