From 4131c134d2e21d390a87ab41419b63da0528cc5a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 5 Jan 2016 13:36:55 -0800 Subject: [PATCH] Mission protocol robustness rework --- src/MissionManager/MissionManager.cc | 265 +++++++++-------------- src/MissionManager/MissionManager.h | 18 +- src/MissionManager/MissionManagerTest.cc | 93 +++----- src/MissionManager/MissionManagerTest.h | 8 +- src/comm/MockLink.cc | 4 +- src/comm/MockLink.h | 3 +- src/comm/MockLinkMissionItemHandler.cc | 21 +- src/comm/MockLinkMissionItemHandler.h | 4 +- 8 files changed, 153 insertions(+), 263 deletions(-) diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 66447ea7d..3de74c53c 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -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= _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; +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 43a23909e..1c09f7dbf 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -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 _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle + QList _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle QMutex _dataMutex; diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index cbfbaddac..a449a3b5f 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -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 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 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; iresetMissionItemHandler(); - 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; iresetMissionItemHandler(); - _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(); } diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 1bc75ae5f..7343ad3bd 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -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); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 67994f69a..073383094 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -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) diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index e276610e5..c3f6fd300 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -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); } diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 42527cce3..30c3bbb16 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -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; } diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 4f7e7da44..a5703be96 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -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; }; -- 2.22.0