From b05575faf2e4a621c4b35a0c409513e1b222eb09 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 7 Oct 2015 11:45:29 -0700 Subject: [PATCH] MissionManagerTest protocol hardening tests - Fixed bugs found to unit test --- src/MissionManager/MissionManager.cc | 153 +++++++++++--- src/MissionManager/MissionManager.h | 21 +- src/MissionManager/MissionManagerTest.cc | 140 ++++++++++++- src/MissionManager/MissionManagerTest.h | 4 + src/Vehicle/Vehicle.cc | 4 + src/comm/MockLink.cc | 36 ++-- src/comm/MockLink.h | 19 +- src/comm/MockLinkMissionItemHandler.cc | 245 +++++++++++++++++------ src/comm/MockLinkMissionItemHandler.h | 51 +++++ 9 files changed, 557 insertions(+), 116 deletions(-) diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 190ea67a2..414b3b151 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -55,6 +55,7 @@ MissionManager::~MissionManager() void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) { + _retryCount = 0; _missionItems.clear(); for (int i=0; i(missionItems[i]))); @@ -64,13 +65,14 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) if (inProgress()) { qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress"; - // FIXME: Better error handling 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(); @@ -78,10 +80,29 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount); _vehicle->sendMessage(message); - _startAckTimeout(AckMissionRequest, message); + _startAckTimeout(AckMissionRequest); emit inProgressChanged(true); } +void MissionManager::_retryWrite(void) +{ + qCDebug(MissionManagerLog) << "_retryWrite"; + + 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(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount); + + _vehicle->sendMessage(message); + _startAckTimeout(AckMissionRequest); +} + void MissionManager::requestMissionItems(void) { qCDebug(MissionManagerLog) << "requestMissionItems read sequence"; @@ -89,6 +110,7 @@ void MissionManager::requestMissionItems(void) mavlink_message_t message; mavlink_mission_request_list_t request; + _retryCount = 0; _clearMissionItems(); request.target_system = _vehicle->id(); @@ -97,49 +119,71 @@ void MissionManager::requestMissionItems(void) mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request); _vehicle->sendMessage(message); - _startAckTimeout(AckMissionCount, message); + _startAckTimeout(AckMissionCount); + emit inProgressChanged(true); +} + +void MissionManager::_retryRead(void) +{ + qCDebug(MissionManagerLog) << "_retryRead"; + + mavlink_message_t message; + mavlink_mission_request_list_t request; + + request.target_system = _vehicle->id(); + request.target_component = MAV_COMP_ID_MISSIONPLANNER; + + mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request); + + _vehicle->sendMessage(message); + _startAckTimeout(AckMissionCount); emit inProgressChanged(true); } void MissionManager::_ackTimeout(void) { - if (_retryAck == AckNone) { + AckType_t timedOutAck = _retryAck; + + _retryAck = AckNone; + + if (timedOutAck == AckNone) { qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone"; + _sendError(InternalError, "Internal error occured during Mission Item communication: _ackTimeOut:_retryAck == AckNone"); return; } - if (++_retryCount <= _maxRetryCount) { - qCDebug(MissionManagerLog) << "_ackTimeout retry _retryAck:_retryCount" << _retryAck << _retryCount; - _vehicle->sendMessage(_retryMessage); - } else { - qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << _retryAck << _retryCount; + if (!_retrySequence(timedOutAck)) { + qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << timedOutAck << _retryCount; + _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(timedOutAck)); } } -void MissionManager::_startAckTimeout(AckType_t ack, const mavlink_message_t& message) +void MissionManager::_startAckTimeout(AckType_t ack) { _retryAck = ack; - _retryCount = 0; - _retryMessage = message; - _ackTimeoutTimer->start(); } bool MissionManager::_stopAckTimeout(AckType_t expectedAck) { - bool success; + bool success = false; + AckType_t savedRetryAck = _retryAck; + + _retryAck = AckNone; _ackTimeoutTimer->stop(); - if (_retryAck != expectedAck) { - qCWarning(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << _retryAck << expectedAck; + if (savedRetryAck != expectedAck) { + qCDebug(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << savedRetryAck << expectedAck; + + if (_retrySequence(expectedAck)) { + _sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(savedRetryAck).arg(expectedAck)); + } success = false; } else { success = true; } - _retryAck = AckNone; - return success; } @@ -189,6 +233,7 @@ void MissionManager::_requestNextMissionItem(int 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)); return; } @@ -203,7 +248,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber) mavlink_msg_mission_request_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionRequest); _vehicle->sendMessage(message); - _startAckTimeout(AckMissionItem, message); + _startAckTimeout(AckMissionItem); } void MissionManager::_handleMissionItem(const mavlink_message_t& message) @@ -220,6 +265,9 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) 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)); + } return; } @@ -267,11 +315,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq; - if (missionRequest.seq >= _missionItems.count()) { - qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested:count" << missionRequest.seq << _missionItems.count(); + 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)); + } return; } + _expectedSequenceNumber++; + mavlink_message_t messageOut; mavlink_mission_item_t missionItem; @@ -295,8 +349,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) mavlink_msg_mission_item_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &messageOut, &missionItem); _vehicle->sendMessage(messageOut); - // FIXME: This ack sequence isn't quite write - _startAckTimeout(AckMissionRequest, messageOut); + _startAckTimeout(AckMissionRequest); } void MissionManager::_handleMissionAck(const mavlink_message_t& message) @@ -309,14 +362,24 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) mavlink_msg_mission_ack_decode(&message, &missionAck); + qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type; + if (missionAck.type == MAV_MISSION_ACCEPTED) { - qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; + if (_expectedSequenceNumber == _missionItems.count()) { + qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; + emit inProgressChanged(false); + } 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").arg(_expectedSequenceNumber).arg(_missionItems.count())); + } + } } else { - _missionItems.clear(); - emit newMissionItemsAvailable(); qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; + if (!_retrySequence(AckMissionRequest)) { + _sendError(VehicleError, QString("Vehicle returned error: %1").arg(missionAck.type)); + } } - emit inProgressChanged(false); } /// Called when a new mavlink message for out vehicle is received @@ -359,3 +422,39 @@ QmlObjectListModel* MissionManager::copyMissionItems(void) return list; } + +void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) +{ + emit inProgressChanged(false); + emit 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:" << ackType << "_retryCount" << _retryCount; + + if (++_retryCount <= _maxRetryCount) { + switch (ackType) { + case AckMissionCount: + case AckMissionItem: + // We are in the middle of a read sequence, start over + _retryRead(); + return true; + break; + case AckMissionRequest: + // We are in the middle of a write sequence, start over + _retryWrite(); + return true; + break; + default: + qCWarning(MissionManagerLog) << "_retrySequence fell through switch: ackType:" << ackType; + _sendError(InternalError, QString("Internal error occured during Mission Item communication: _retrySequence fell through switch: ackType:").arg(ackType)); + return false; + } + } else { + qCDebug(MissionManagerLog) << "Retries exhausted"; + return false; + } +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index d039f4630..830344d5a 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -67,12 +67,24 @@ public: /// Returns a copy of the current set of mission items. Caller is responsible for /// freeing returned object. QmlObjectListModel* copyMissionItems(void); + + /// Error codes returned in error signal + typedef enum { + InternalError, + AckTimeoutError, ///< Timed out waiting for response from vehicle + ProtocolOrderError, ///< Incorrect protocol sequence from vehicle + RequestRangeError, ///< Vehicle requested item out of range + ItemMismatchError, ///< Vehicle returned item with seq # different than requested + VehicleError, ///< Vehicle returned error + MissingRequestsError, ///< Vehicle did not request all items during write sequence + } ErrorCode_t; signals: // Public signals void canEditChanged(bool canEdit); void newMissionItemsAvailable(void); void inProgressChanged(bool inProgress); + void error(int errorCode, const QString& errorMsg); private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); @@ -86,7 +98,7 @@ private: AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence } AckType_t; - void _startAckTimeout(AckType_t ack, const mavlink_message_t& message); + void _startAckTimeout(AckType_t ack); bool _stopAckTimeout(AckType_t expectedAck); void _sendTransactionComplete(void); void _handleMissionCount(const mavlink_message_t& message); @@ -95,6 +107,10 @@ private: void _handleMissionAck(const mavlink_message_t& message); void _requestNextMissionItem(int sequenceNumber); void _clearMissionItems(void); + void _sendError(ErrorCode_t errorCode, const QString& errorMsg); + void _retryWrite(void); + void _retryRead(void); + bool _retrySequence(AckType_t ackType); private: Vehicle* _vehicle; @@ -104,7 +120,6 @@ private: QTimer* _ackTimeoutTimer; AckType_t _retryAck; - mavlink_message_t _retryMessage; int _retryCount; int _expectedSequenceNumber; @@ -113,7 +128,7 @@ private: QmlObjectListModel _missionItems; - static const int _ackTimeoutMilliseconds= 1000; + static const int _ackTimeoutMilliseconds= 500; static const int _maxRetryCount = 5; }; diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index 991d674ae..efe97fde2 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -70,6 +70,7 @@ void MissionManagerTest::init(void) _rgSignals[canEditChangedSignalIndex] = SIGNAL(canEditChanged(bool)); _rgSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(void)); _rgSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool)); + _rgSignals[errorSignalIndex] = SIGNAL(error(int, const QString&)); _multiSpy = new MultiSignalSpy(); Q_CHECK_PTR(_multiSpy); @@ -133,8 +134,14 @@ void MissionManagerTest::_readEmptyVehicle(void) QCOMPARE(_missionManager->canEdit(), true); } -void MissionManagerTest::_roundTripItems(void) +void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly) { + _mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly); + if (failFirstTimeOnly) { + // Should fail first time, then retry should succed + failureMode = MockLinkMissionItemHandler::FailNone; + } + // Setup our test case data const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); QmlObjectListModel* list = new QmlObjectListModel(); @@ -152,7 +159,7 @@ void MissionManagerTest::_roundTripItems(void) // Send the items to the vehicle _missionManager->writeMissionItems(*list); - + // writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it QVERIFY(_missionManager->inProgress()); QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); @@ -160,17 +167,74 @@ void MissionManagerTest::_roundTripItems(void) _multiSpy->clearAllSignals(); - // Now wait for write sequence to complete. We should only get an inProgressChanged signal to signal completion. - _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000); - QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); - _checkInProgressValues(false); + if (failureMode == MockLinkMissionItemHandler::FailNone) { + // This should be clean run + + // Wait for write sequence to complete. We should get: + // inProgressChanged(false) signal + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 10000); + QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); + + // Validate inProgressChanged signal value + _checkInProgressValues(false); + + // We should have gotten back all mission items + QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); + } else { + // This should be a failed run + + // Wait for write sequence to complete. We should get: + // inProgressChanged(false) signal + // error(errorCode, QString) signal + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 10000); + QCOMPARE(_multiSpy->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true); + + // Validate inProgressChanged signal value + _checkInProgressValues(false); + + // Validate error signal values + QSignalSpy* spy = _multiSpy->getSpyByIndex(errorSignalIndex); + QList signalArgs = spy->takeFirst(); + QCOMPARE(signalArgs.count(), 2); + qDebug() << signalArgs[1].toString(); + QCOMPARE(signalArgs[0].toInt(), (int)errorCode); + + /* + // FIXME: This should be on the read side + // Validate correct number of mission items + int expectedMissionCount = 0; + + switch (ErrorCode) { + case FailWriteRequest0NoResponse: + // Don't respond to MISSION_COUNT with MISSION_REQUEST 0 + expectedMissionCount = 0; + break; + + case FailWriteRequest1NoResponse: // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1 + case FailWriteRequest0IncorrectSequence: // Respond to MISSION_COUNT 0 with MISSION_REQUEST with wrong sequence number + case FailWriteRequest1IncorrectSequence: // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number + case FailWriteRequest0ErrorAck: // Respond to MISSION_COUNT 0 with MISSION_ACK error + case FailWriteRequest1ErrorAck: // Respond to MISSION_ITEM 0 with MISSION_ACK error + case FailWriteFinalAckNoResponse: // Don't send the final MISSION_ACK + case FailWriteFinalAckErrorAck: // Send an error as the final MISSION_ACK + case FailWriteFinalAckMissingRequests: // Send the MISSION_ACK before all items have been requested + break; + } + // FIXME: Count depends on errorCode + //QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); + */ + } - QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); QCOMPARE(_missionManager->canEdit(), true); delete list; list = NULL; _multiSpy->clearAllSignals(); +} + +void MissionManagerTest::_roundTripItems(void) +{ + _writeItems(MockLinkMissionItemHandler::FailNone, MissionManager::InternalError, false); // Read the items back from the vehicle _missionManager->requestMissionItems(); @@ -190,6 +254,7 @@ void MissionManagerTest::_roundTripItems(void) QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); _checkInProgressValues(false); + const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); QCOMPARE(_missionManager->canEdit(), true); @@ -212,3 +277,64 @@ void MissionManagerTest::_roundTripItems(void) QCOMPARE(actual->frame(), testCase->expectedItem.frame); } } + +void MissionManagerTest::_testWriteFailureHandling(void) +{ + /* + FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST + 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 + 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 + 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 + 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 + */ + + /* + /// Called to send a MISSION_ACK message while the MissionManager is in idle state + void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } + + /// Called to send a MISSION_ITEM message while the MissionManager is in idle state + void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); } + + /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state + void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } + */ + + 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 }, + }; + + 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); + _mockLink->resetMissionItemHandler(); + } +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 22ee3903f..178c9ba61 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -42,9 +42,11 @@ private slots: void _readEmptyVehicle(void); void _roundTripItems(void); + void _testWriteFailureHandling(void); private: void _checkInProgressValues(bool inProgress); + void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly); MockLink* _mockLink; MissionManager* _missionManager; @@ -53,6 +55,7 @@ private: canEditChangedSignalIndex = 0, newMissionItemsAvailableSignalIndex, inProgressChangedSignalIndex, + errorSignalIndex, maxSignalIndex }; @@ -60,6 +63,7 @@ private: canEditChangedSignalMask = 1 << canEditChangedSignalIndex, newMissionItemsAvailableSignalMask = 1 << newMissionItemsAvailableSignalIndex, inProgressChangedSignalMask = 1 << inProgressChangedSignalIndex, + errorSignalMask = 1 << errorSignalIndex, }; MultiSignalSpy* _multiSpy; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ee95fad33..e31895b98 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -85,6 +85,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) , _satelliteLock(0) , _wpm(NULL) , _updateCount(0) + , _missionManager(NULL) , _armed(false) , _base_mode(0) , _custom_mode(0) @@ -164,6 +165,9 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) Vehicle::~Vehicle() { + delete _missionManager; + _missionManager = NULL; + // Stop listening for system messages disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage); // Disconnect any previously connected active MAV diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index e4cd1e30c..4f6293969 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -66,17 +66,18 @@ union px4_custom_mode { float data_float; }; -MockLink::MockLink(MockConfiguration* config) : - _name("MockLink"), - _connected(false), - _vehicleSystemId(128), // FIXME: Pull from eventual parameter manager - _vehicleComponentId(200), // FIXME: magic number? - _inNSH(false), - _mavlinkStarted(false), - _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), - _mavState(MAV_STATE_STANDBY), - _autopilotType(MAV_AUTOPILOT_PX4), - _fileServer(NULL) +MockLink::MockLink(MockConfiguration* config) + : _missionItemHandler(this) + , _name("MockLink") + , _connected(false) + , _vehicleSystemId(128) // FIXME: Pull from eventual parameter manager + , _vehicleComponentId(200) // FIXME: magic number? + , _inNSH(false) + , _mavlinkStarted(false) + , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) + , _mavState(MAV_STATE_STANDBY) + , _autopilotType(MAV_AUTOPILOT_PX4) + , _fileServer(NULL) { _config = config; union px4_custom_mode px4_cm; @@ -88,10 +89,8 @@ MockLink::MockLink(MockConfiguration* config) : _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this); Q_CHECK_PTR(_fileServer); - _missionItemHandler = new MockLinkMissionItemHandler(this); - Q_CHECK_PTR(_missionItemHandler); - moveToThread(this); + _loadParams(); QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes); } @@ -149,6 +148,8 @@ void MockLink::run(void) QObject::disconnect(&_timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); QObject::disconnect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); QObject::disconnect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks); + + _missionItemHandler.shutdown(); } void MockLink::_run1HzTasks(void) @@ -303,8 +304,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) continue; } - Q_ASSERT(_missionItemHandler); - if (_missionItemHandler->handleMessage(msg)) { + if (_missionItemHandler.handleMessage(msg)) { continue; } @@ -668,3 +668,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) } } +void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly) +{ + _missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly); +} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 6c7950cbc..1a44b4073 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -85,6 +85,23 @@ public: bool disconnect(void); LinkConfiguration* getLinkConfiguration() { return _config; } + + /// 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); + + /// Called to send a MISSION_ACK message while the MissionManager is in idle state + void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } + + /// Called to send a MISSION_ITEM message while the MissionManager is in idle state + void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); } + + /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state + void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } + + /// Reset the state of the MissionItemHandler to no items, no transactions in progress. + void resetMissionItemHandler(void) { _missionItemHandler.reset(); } signals: /// @brief Used internally to move data to the thread. @@ -126,7 +143,7 @@ private: float _floatUnionForParam(int componentId, const QString& paramName); void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); - MockLinkMissionItemHandler* _missionItemHandler; + MockLinkMissionItemHandler _missionItemHandler; QString _name; bool _connected; diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 59bf01331..eb40e3772 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -29,12 +29,27 @@ QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog") MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink) - : QObject(mockLink) - , _mockLink(mockLink) + : _mockLink(mockLink) + , _missionItemResponseTimer(NULL) + , _failureMode(FailNone) { Q_ASSERT(mockLink); } +MockLinkMissionItemHandler::~MockLinkMissionItemHandler() +{ + +} + +void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void) +{ + if (!_missionItemResponseTimer) { + _missionItemResponseTimer = new QTimer(); + connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout); + } + _missionItemResponseTimer->start(500); +} + bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) { switch (msg.msgid) { @@ -78,21 +93,29 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence"; - mavlink_mission_request_list_t request; - - mavlink_msg_mission_request_list_decode(&msg, &request); - - Q_ASSERT(request.target_system == _mockLink->vehicleId()); - - mavlink_message_t responseMsg; + if (_failureMode != FailReadRequestListNoResponse) { + mavlink_mission_request_list_t request; + + mavlink_msg_mission_request_list_decode(&msg, &request); + + Q_ASSERT(request.target_system == _mockLink->vehicleId()); + + mavlink_message_t responseMsg; + + mavlink_msg_mission_count_pack(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + _missionItems.count()); // Number of mission items + _mockLink->respondWithMavlinkMessage(responseMsg); + } else { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode"; + } - mavlink_msg_mission_count_pack(_mockLink->vehicleId(), - MAV_COMP_ID_MISSIONPLANNER, - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - _missionItems.count()); // Number of mission items - _mockLink->respondWithMavlinkMessage(responseMsg); + if (_failureFirstTimeOnly) { + _failureMode = FailNone; + } } void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg) @@ -106,29 +129,50 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& Q_ASSERT(request.target_system == _mockLink->vehicleId()); Q_ASSERT(request.seq < _missionItems.count()); - mavlink_message_t responseMsg; - - mavlink_mission_item_t item = _missionItems[request.seq]; + if ((_failureMode == FailReadRequest0NoResponse && request.seq != 0) || + (_failureMode == FailReadRequest1NoResponse && request.seq != 1)) { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode"; + } else { + // FIXME: Track whether all items are requested, or requested in sequence + + if ((_failureMode == FailReadRequest0IncorrectSequence && request.seq == 0) || + (_failureMode == FailReadRequest1IncorrectSequence && request.seq == 1)) { + // Send back the incorrect sequence number + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number"; + request.seq++; + } + + if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) || + (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { + _sendAck(MAV_MISSION_ERROR); + } else { + mavlink_message_t responseMsg; + + mavlink_mission_item_t item = _missionItems[request.seq]; + + mavlink_msg_mission_item_pack(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + request.seq, // Index of mission item being sent + item.frame, + item.command, + item.current, + item.autocontinue, + item.param1, item.param2, item.param3, item.param4, + item.x, item.y, item.z); + _mockLink->respondWithMavlinkMessage(responseMsg); + } + } - mavlink_msg_mission_item_pack(_mockLink->vehicleId(), - MAV_COMP_ID_MISSIONPLANNER, - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - request.seq, // Index of mission item being sent - item.frame, - item.command, - item.current, - item.autocontinue, - item.param1, item.param2, item.param3, item.param4, - item.x, item.y, item.z); - _mockLink->respondWithMavlinkMessage(responseMsg); + if (_failureFirstTimeOnly) { + _failureMode = FailNone; + } } void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg) { - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence"; - mavlink_mission_count_t missionCount; mavlink_msg_mission_count_decode(&msg, &missionCount); @@ -137,12 +181,12 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms _writeSequenceCount = missionCount.count; Q_ASSERT(_writeSequenceCount >= 0); - // FIXME: Set up a timer for a failed write sequence + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount; _missionItems.clear(); if (_writeSequenceCount == 0) { - // FIXME: NYI + _sendAck(MAV_MISSION_ACCEPTED); } else { _writeSequenceIndex = 0; _requestNextMissionItem(_writeSequenceIndex); @@ -151,31 +195,68 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) { - qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber; + qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode; - if (sequenceNumber >= _writeSequenceCount) { - qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; - return; + if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) || + (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) { + qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode"; + } else { + if (sequenceNumber >= _writeSequenceCount) { + qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; + return; + } + + if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) || + (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) { + sequenceNumber ++; + } + + if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) || + (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { + qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; + _sendAck(MAV_MISSION_ERROR); + } else { + mavlink_message_t message; + mavlink_mission_request_t missionRequest; + + missionRequest.target_system = MAVLinkProtocol::instance()->getSystemId(); + missionRequest.target_component = MAVLinkProtocol::instance()->getComponentId(); + missionRequest.seq = sequenceNumber; + + mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest); + _mockLink->respondWithMavlinkMessage(message); + + // If response with Mission Item doesn't come before timer fires it's an error + _startMissionItemResponseTimer(); + } } - mavlink_message_t message; - mavlink_mission_request_t missionRequest; + if (_failureFirstTimeOnly) { + _failureMode = FailNone; + } +} + +void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) +{ + qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType; + + mavlink_message_t message; + mavlink_mission_ack_t missionAck; - missionRequest.target_system = MAVLinkProtocol::instance()->getSystemId(); - missionRequest.target_component = MAVLinkProtocol::instance()->getComponentId(); - missionRequest.seq = sequenceNumber; + missionAck.target_system = MAVLinkProtocol::instance()->getSystemId(); + missionAck.target_component = MAVLinkProtocol::instance()->getComponentId(); + missionAck.type = ackType; - mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest); + mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck); _mockLink->respondWithMavlinkMessage(message); - - // FIXME: Timeouts } - void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence"; + _missionItemResponseTimer->stop(); + mavlink_mission_item_t missionItem; mavlink_msg_mission_item_decode(&msg, &missionItem); @@ -187,21 +268,61 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg _missionItems[missionItem.seq] = missionItem; - // FIXME: Timeouts - _writeSequenceIndex++; if (_writeSequenceIndex < _writeSequenceCount) { - _requestNextMissionItem(_writeSequenceIndex); + if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) { + // Send MAV_MISSION_ACCPETED ack too early + _sendAck(MAV_MISSION_ACCEPTED); + } else { + _requestNextMissionItem(_writeSequenceIndex); + } } else { - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem sending final ack, write sequence complete"; - mavlink_message_t message; - mavlink_mission_ack_t missionAck; - - missionAck.target_system = MAVLinkProtocol::instance()->getSystemId(); - missionAck.target_component = MAVLinkProtocol::instance()->getComponentId(); - missionAck.type = MAV_MISSION_ACCEPTED; - - mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck); - _mockLink->respondWithMavlinkMessage(message); + if (_failureMode != FailWriteFinalAckNoResponse) { + MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED; + + if (_failureMode == FailWriteFinalAckErrorAck) { + ack = MAV_MISSION_ERROR; + } + _sendAck(ack); + } + } + if (_failureFirstTimeOnly) { + _failureMode = FailNone; + } +} + +void MockLinkMissionItemHandler::_missionItemResponseTimeout(void) +{ + qWarning() << "Timeout waiting for next MISSION_ITEM"; + Q_ASSERT(false); +} + +void MockLinkMissionItemHandler::sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) +{ + _sendAck(ackType); +} + +void MockLinkMissionItemHandler::sendUnexpectedMissionItem(void) +{ + // FIXME: NYI + Q_ASSERT(false); +} + +void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void) +{ + // FIXME: NYI + Q_ASSERT(false); +} + +void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly) +{ + _failureFirstTimeOnly = firstTimeOnly; + _failureMode = failureMode; +} + +void MockLinkMissionItemHandler::shutdown(void) +{ + if (_missionItemResponseTimer) { + delete _missionItemResponseTimer; } } diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 2978b55cc..57e3d2948 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -26,6 +26,7 @@ #include #include +#include #include "QGCMAVLink.h" #include "QGCLoggingCategory.h" @@ -40,11 +41,55 @@ class MockLinkMissionItemHandler : public QObject public: MockLinkMissionItemHandler(MockLink* mockLink); + ~MockLinkMissionItemHandler(); + + // Prepares for destruction on correct thread + void shutdown(void); /// @brief Called to handle mission item related messages. All messages should be passed to this method. /// It will handle the appropriate set. /// @return true: message handled bool handleMessage(const mavlink_message_t& msg); + + typedef enum { + FailNone, // No failures + FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST + 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 + 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 + 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 + 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 + } FailureMode_t; + + /// 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); + + /// Called to send a MISSION_ACK message while the MissionManager is in idle state + void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType); + + /// Called to send a MISSION_ITEM message while the MissionManager is in idle state + void sendUnexpectedMissionItem(void); + + /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state + void sendUnexpectedMissionRequest(void); + + /// Reset the state of the MissionItemHandler to no items, no transactions in progress. + void reset(void) { _missionItems.clear(); } + +private slots: + void _missionItemResponseTimeout(void); private: void _handleMissionRequestList(const mavlink_message_t& msg); @@ -52,6 +97,8 @@ private: void _handleMissionItem(const mavlink_message_t& msg); void _handleMissionCount(const mavlink_message_t& msg); void _requestNextMissionItem(int sequenceNumber); + void _sendAck(MAV_MISSION_RESULT ackType); + void _startMissionItemResponseTimer(void); private: MockLink* _mockLink; @@ -61,6 +108,10 @@ private: typedef QMap MissionList_t; MissionList_t _missionItems; + + QTimer* _missionItemResponseTimer; + FailureMode_t _failureMode; + bool _failureFirstTimeOnly; }; #endif -- 2.22.0