diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index fa0ce387b47ab6f5c78e78a13338ee36efc9c0dc..472dd453fc89ba7b771716bcc0a74ab96daa6af4 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -23,7 +23,7 @@ MissionManager::MissionManager(Vehicle* vehicle) : _vehicle(vehicle) , _dedicatedLink(NULL) , _ackTimeoutTimer(NULL) - , _retryAck(AckNone) + , _expectedAck(AckNone) , _readTransactionInProgress(false) , _writeTransactionInProgress(false) , _currentMissionItem(-1) @@ -81,7 +81,17 @@ void MissionManager::writeMissionItems(const QList& missionItems) for (int i=0; i<_missionItems.count(); i++) { _itemIndicesToWrite << i; } + _writeTransactionInProgress = true; + _retryCount = 0; + emit inProgressChanged(true); + _writeMissionCount(); +} + +/// This begins the write sequence with the vehicle. This may be called during a retry. +void MissionManager::_writeMissionCount(void) +{ + qCDebug(MissionManagerLog) << "_writeMissionCount retry count" << _retryCount; mavlink_message_t message; mavlink_mission_count_t missionCount; @@ -99,7 +109,6 @@ void MissionManager::writeMissionItems(const QList& missionItems) _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionRequest); - emit inProgressChanged(true); } void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly) @@ -153,75 +162,128 @@ void MissionManager::requestMissionItems(void) qCDebug(MissionManagerLog) << "requestMissionItems called while transaction in progress"; return; } - + + _retryCount = 0; + emit inProgressChanged(true); + _requestList(); +} + +/// Internal call to request list of mission items. May be called during a retry sequence. +void MissionManager::_requestList(void) +{ + qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount; + mavlink_message_t message; mavlink_mission_request_list_t request; - - _requestItemRetryCount = 0; + _itemIndicesToRead.clear(); _readTransactionInProgress = true; _clearMissionItems(); - + request.target_system = _vehicle->id(); request.target_component = MAV_COMP_ID_MISSIONPLANNER; - + _dedicatedLink = _vehicle->priorityLink(); mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), _dedicatedLink->mavlinkChannel(), &message, &request); - + _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionCount); - emit inProgressChanged(true); } void MissionManager::_ackTimeout(void) { - AckType_t timedOutAck = _retryAck; - - _retryAck = AckNone; - - if (timedOutAck == AckNone) { - qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone"; - _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_retryAck == AckNone"); + if (_expectedAck == AckNone) { return; } - - _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(timedOutAck))); - _finishTransaction(false); + + switch (_expectedAck) { + case AckNone: + qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone"; + _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone"); + break; + case AckMissionCount: + // MISSION_COUNT message expected + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount; + _requestList(); + } + break; + case AckMissionItem: + // MISSION_ITEM expected + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount; + _requestNextMissionItem(); + } + break; + case AckMissionRequest: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + if (_itemIndicesToWrite.count() == 0) { + // Vehicle did not send final MISSION_ACK at end of sequence + _sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack.")); + _finishTransaction(false); + } else if (_itemIndicesToWrite[0] == 0) { + // Vehicle did not respond to MISSION_COUNT, try again + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount; + _writeMissionCount(); + } + } else { + // Vehicle did not request all items from ground station + _sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); + _expectedAck = AckNone; + _finishTransaction(false); + } + break; + case AckGuidedItem: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + default: + _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck))); + _expectedAck = AckNone; + _finishTransaction(false); + } } void MissionManager::_startAckTimeout(AckType_t ack) { - _retryAck = ack; + _expectedAck = ack; _ackTimeoutTimer->start(); } -bool MissionManager::_stopAckTimeout(AckType_t expectedAck) +/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped. +/// @return true: received ack matches expected ack +bool MissionManager::_checkForExpectedAck(AckType_t receivedAck) { - bool success = false; - AckType_t savedRetryAck = _retryAck; - - _retryAck = AckNone; - - _ackTimeoutTimer->stop(); - - if (savedRetryAck != expectedAck) { - if (savedRetryAck == AckNone) { - // Don't annoy the user with warnings about unexpected mission commands, just ignore them; ArduPilot updates home position using + if (receivedAck == _expectedAck) { + _expectedAck = AckNone; + _ackTimeoutTimer->stop(); + return true; + } else { + if (_expectedAck == AckNone) { + // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using // spurious MISSION_ITEMs. } else { - _sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(_ackTypeToString(savedRetryAck)).arg(_ackTypeToString(expectedAck))); - _finishTransaction(false); + // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers. + // Whatever it is we let the ack timeout handle any error output to the user. + qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck)); } - success = false; - } else { - success = true; + return false; } - - return success; } void MissionManager::_readTransactionComplete(void) @@ -251,9 +313,11 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) { mavlink_mission_count_t missionCount; - if (!_stopAckTimeout(AckMissionCount)) { + if (!_checkForExpectedAck(AckMissionCount)) { return; } + + _retryCount = 0; mavlink_msg_mission_count_decode(&message, &missionCount); qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count; @@ -271,13 +335,13 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) void MissionManager::_requestNextMissionItem(void) { - qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << _itemIndicesToRead[0]; - if (_itemIndicesToRead.count() == 0) { _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read"); return; } - + + qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount; + mavlink_message_t message; mavlink_mission_request_t missionRequest; @@ -299,7 +363,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) { mavlink_mission_item_t missionItem; - if (!_stopAckTimeout(AckMissionItem)) { + if (!_checkForExpectedAck(AckMissionItem)) { return; } @@ -308,7 +372,6 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq; if (_itemIndicesToRead.contains(missionItem.seq)) { - _requestItemRetryCount = 0; _itemIndicesToRead.removeOne(missionItem.seq); MissionItem* item = new MissionItem(missionItem.seq, @@ -333,13 +396,12 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) _missionItems.append(item); } else { qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq; - if (++_requestItemRetryCount > _maxRetryCount) { - _sendError(RequestRangeError, QString("Vehicle would not send item %1 after max retries. Read from Vehicle failed.").arg(_itemIndicesToRead[0])); - _finishTransaction(false); - return; - } + // We have to put the ack timeout back since it was removed above + _startAckTimeout(AckMissionItem); + return; } + _retryCount = 0; if (_itemIndicesToRead.count() == 0) { _readTransactionComplete(); } else { @@ -357,7 +419,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) { mavlink_mission_request_t missionRequest; - if (!_stopAckTimeout(AckMissionRequest)) { + if (!_checkForExpectedAck(AckMissionRequest)) { return; } @@ -411,14 +473,14 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) { mavlink_mission_ack_t missionAck; - // Save the retry ack before calling _stopAckTimeout since we'll need it to determine what + // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what // type of a protocol sequence we are in. - AckType_t savedRetryAck = _retryAck; + AckType_t savedExpectedAck = _expectedAck; // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not - // a protocol sequence error. Call _stopAckTimeout with _retryAck so it will succeed no + // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no // matter what. - if (!_stopAckTimeout(_retryAck)) { + if (!_checkForExpectedAck(_expectedAck)) { return; } @@ -426,7 +488,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); - switch (savedRetryAck) { + switch (savedExpectedAck) { case AckNone: // State machine is idle. Vehicle is confused. _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); @@ -438,7 +500,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) break; case AckMissionItem: // MISSION_ITEM expected - _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _finishTransaction(false); break; case AckMissionRequest: @@ -459,7 +521,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) case AckGuidedItem: // MISSION_REQUEST is expected, or MISSION_ACK to end sequence if (missionAck.type == MAV_MISSION_ACCEPTED) { - qCDebug(MissionManagerLog) << "_handleMissionAck guide mode item accepted"; + qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted"; _finishTransaction(true); } else { _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); @@ -587,12 +649,14 @@ void MissionManager::_finishTransaction(bool success) emit newMissionItemsAvailable(); } - _readTransactionInProgress = false; - _writeTransactionInProgress = false; _itemIndicesToRead.clear(); _itemIndicesToWrite.clear(); - emit inProgressChanged(false); + if (_readTransactionInProgress || _writeTransactionInProgress) { + _readTransactionInProgress = false; + _writeTransactionInProgress = false; + emit inProgressChanged(false); + } } bool MissionManager::inProgress(void) diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 6a4602abeb37bf897667959ebee47083399ce3f7..7e3ce2171f9dff31409adce137d0ae87db689093 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -62,7 +62,7 @@ public: } ErrorCode_t; // These values are public so the unit test can set appropriate signal wait times - static const int _ackTimeoutMilliseconds= 2000; + static const int _ackTimeoutMilliseconds = 1000; static const int _maxRetryCount = 5; signals: @@ -85,7 +85,7 @@ private: } AckType_t; void _startAckTimeout(AckType_t ack); - bool _stopAckTimeout(AckType_t expectedAck); + bool _checkForExpectedAck(AckType_t receivedAck); void _readTransactionComplete(void); void _handleMissionCount(const mavlink_message_t& message); void _handleMissionItem(const mavlink_message_t& message); @@ -98,14 +98,16 @@ private: QString _ackTypeToString(AckType_t ackType); QString _missionResultToString(MAV_MISSION_RESULT result); void _finishTransaction(bool success); + void _requestList(void); + void _writeMissionCount(void); private: Vehicle* _vehicle; LinkInterface* _dedicatedLink; QTimer* _ackTimeoutTimer; - AckType_t _retryAck; - int _requestItemRetryCount; + AckType_t _expectedAck; + int _retryCount; bool _readTransactionInProgress; bool _writeTransactionInProgress; diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index 35a91d71e17cb1016a89d9cf7c2b7395520e16b5..f6e94581c45a3454a8136dcc78af91cce2f8b3e4 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -27,7 +27,7 @@ MissionManagerTest::MissionManagerTest(void) } -void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode) +void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail) { _mockLink->setMissionItemFailureMode(failureMode); @@ -67,29 +67,9 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f _multiSpyMissionManager->clearAllSignals(); - if (failureMode == MockLinkMissionItemHandler::FailNone) { - // This should be clean run - - // Wait for write sequence to complete. We should get: - // inProgressChanged(false) signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true); - - // Validate inProgressChanged signal value - _checkInProgressValues(false); - - // Validate item count in mission manager - - int expectedCount = (int)_cTestCases; - if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - // Home position at position 0 comes from vehicle - expectedCount++; - } - - QCOMPARE(_missionManager->missionItems().count(), expectedCount); - } else { + if (shouldFail) { // This should be a failed run - + setExpectedMessageBox(QMessageBox::Ok); // Wait for write sequence to complete. We should get: @@ -97,10 +77,10 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f // error(errorCode, QString) signal _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); QCOMPARE(_multiSpyMissionManager->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true); - + // Validate inProgressChanged signal value _checkInProgressValues(false); - + // Validate error signal values QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex); QList signalArgs = spy->takeFirst(); @@ -108,14 +88,34 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f qDebug() << signalArgs[1].toString(); checkExpectedMessageBox(); + } else { + // This should be clean run + + // Wait for write sequence to complete. We should get: + // inProgressChanged(false) signal + _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); + QCOMPARE(_multiSpyMissionManager->checkOnlySignalByMask(inProgressChangedSignalMask), true); + + // Validate inProgressChanged signal value + _checkInProgressValues(false); + + // Validate item count in mission manager + + int expectedCount = (int)_cTestCases; + if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // Home position at position 0 comes from vehicle + expectedCount++; + } + + QCOMPARE(_missionManager->missionItems().count(), expectedCount); } _multiSpyMissionManager->clearAllSignals(); } -void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode) +void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail) { - _writeItems(MockLinkMissionItemHandler::FailNone); + _writeItems(MockLinkMissionItemHandler::FailNone, false); _mockLink->setMissionItemFailureMode(failureMode); @@ -129,19 +129,9 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode _multiSpyMissionManager->clearAllSignals(); - if (failureMode == MockLinkMissionItemHandler::FailNone) { - // This should be clean run - - // Now wait for read sequence to complete. We should get: - // inProgressChanged(false) signal to signal completion - // newMissionItemsAvailable signal - _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); - QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); - _checkInProgressValues(false); - - } else { + if (shouldFail) { // This should be a failed run - + setExpectedMessageBox(QMessageBox::Ok); // Wait for read sequence to complete. We should get: @@ -150,17 +140,26 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode // newMissionItemsAvailable signal _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true); - + // Validate inProgressChanged signal value _checkInProgressValues(false); - + // Validate error signal values QSignalSpy* spy = _multiSpyMissionManager->getSpyByIndex(errorSignalIndex); QList signalArgs = spy->takeFirst(); QCOMPARE(signalArgs.count(), 2); qDebug() << signalArgs[1].toString(); - + checkExpectedMessageBox(); + } else { + // This should be clean run + + // Now wait for read sequence to complete. We should get: + // inProgressChanged(false) signal to signal completion + // newMissionItemsAvailable signal + _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); + QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); + _checkInProgressValues(false); } _multiSpyMissionManager->clearAllSignals(); @@ -169,14 +168,14 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode size_t cMissionItemsExpected; - if (failureMode == MockLinkMissionItemHandler::FailNone) { + if (shouldFail) { + cMissionItemsExpected = 0; + } else { cMissionItemsExpected = (int)_cTestCases; if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { // Home position at position 0 comes from vehicle cMissionItemsExpected++; } - } else { - cMissionItemsExpected = 0; } QCOMPARE(_missionManager->missionItems().count(), (int)cMissionItemsExpected); @@ -233,24 +232,27 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void) typedef struct { const char* failureText; MockLinkMissionItemHandler::FailureMode_t failureMode; - } TestCase_t; + bool shouldFail; + } WriteTestCase_t; - static const TestCase_t rgTestCases[] = { - { "No Failure", MockLinkMissionItemHandler::FailNone }, - { "FailWriteRequest0NoResponse", MockLinkMissionItemHandler::FailWriteRequest0NoResponse }, - { "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse }, - { "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence }, - { "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence }, - { "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck }, - { "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck }, - { "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse }, - { "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck }, - { "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests }, + static const WriteTestCase_t rgTestCases[] = { + { "No Failure", MockLinkMissionItemHandler::FailNone, false }, + { "FailWriteMissionCountNoResponse", MockLinkMissionItemHandler::FailWriteMissionCountNoResponse, true }, + { "FailWriteMissionCountFirstResponse", MockLinkMissionItemHandler::FailWriteMissionCountFirstResponse, false }, + { "FailWriteRequest1NoResponse", MockLinkMissionItemHandler::FailWriteRequest1NoResponse, true }, + { "FailWriteRequest0IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest0IncorrectSequence, true }, + { "FailWriteRequest1IncorrectSequence", MockLinkMissionItemHandler::FailWriteRequest1IncorrectSequence, true }, + { "FailWriteRequest0ErrorAck", MockLinkMissionItemHandler::FailWriteRequest0ErrorAck, true }, + { "FailWriteRequest1ErrorAck", MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, true }, + { "FailWriteFinalAckNoResponse", MockLinkMissionItemHandler::FailWriteFinalAckNoResponse, true }, + { "FailWriteFinalAckErrorAck", MockLinkMissionItemHandler::FailWriteFinalAckErrorAck, true }, + { "FailWriteFinalAckMissingRequests", MockLinkMissionItemHandler::FailWriteFinalAckMissingRequests, true }, }; for (size_t i=0; ifailureText; + _writeItems(pCase->failureMode, pCase->shouldFail); _mockLink->resetMissionItemHandler(); } } @@ -271,22 +273,31 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void) typedef struct { const char* failureText; MockLinkMissionItemHandler::FailureMode_t failureMode; - } TestCase_t; + bool shouldFail; + } ReadTestCase_t; - static const TestCase_t rgTestCases[] = { - { "No Failure", MockLinkMissionItemHandler::FailNone }, - { "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse }, - { "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse }, - { "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse }, - { "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence }, - { "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence }, - { "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck }, - { "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck }, + /* + static const ReadTestCase_t rgTestCases[] = { + { "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false }, + };*/ + + static const ReadTestCase_t rgTestCases[] = { + { "No Failure", MockLinkMissionItemHandler::FailNone, false }, + { "FailReadRequestListNoResponse", MockLinkMissionItemHandler::FailReadRequestListNoResponse, true }, + { "FailReadRequestListFirstResponse", MockLinkMissionItemHandler::FailReadRequestListFirstResponse, false }, + { "FailReadRequest0NoResponse", MockLinkMissionItemHandler::FailReadRequest0NoResponse, true }, + { "FailReadRequest1NoResponse", MockLinkMissionItemHandler::FailReadRequest1NoResponse, true }, + { "FailReadRequest1FirstResponse", MockLinkMissionItemHandler::FailReadRequest1FirstResponse, false }, + { "FailReadRequest0IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence, true }, + { "FailReadRequest1IncorrectSequence", MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence, true }, + { "FailReadRequest0ErrorAck", MockLinkMissionItemHandler::FailReadRequest0ErrorAck, true }, + { "FailReadRequest1ErrorAck", MockLinkMissionItemHandler::FailReadRequest1ErrorAck, true }, }; - + for (size_t i=0; ifailureText; + _roundTripItems(pCase->failureMode, pCase->shouldFail); _mockLink->resetMissionItemHandler(); _multiSpyMissionManager->clearAllSignals(); } diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 03cfd07ab61db1f3a297ae3a54c93b9d18203f47..e8bd7a5970f4536a933df45b91bb5926667656e4 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -33,13 +33,13 @@ private slots: void _testReadFailureHandlingAPM(void); private: - void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode); - void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode); + void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail); + void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail); void _testWriteFailureHandlingWorker(void); void _testReadFailureHandlingWorker(void); - static const MissionControllerManagerTest::TestCase_t _rgTestCases[]; - static const size_t _cTestCases; + static const TestCase_t _rgTestCases[]; + static const size_t _cTestCases; }; #endif diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 146e5d7f31bb321401f383498f3c1117ecb103e1..e9a0bff65f4aa15c7d8e11a9242d3d95711c756d 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -21,6 +21,9 @@ MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLi , _failureMode(FailNone) , _sendHomePositionOnEmptyList(false) , _mavlinkProtocol(mavlinkProtocol) + , _failReadRequestListFirstResponse(true) + , _failReadRequest1FirstResponse(true) + , _failWriteMissionCountFirstResponse(true) { Q_ASSERT(mockLink); } @@ -82,9 +85,17 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence"; - if (_failureMode != FailReadRequestListNoResponse) { + _failReadRequest1FirstResponse = true; + + if (_failureMode == FailReadRequestListNoResponse) { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse"; + } else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) { + _failReadRequestListFirstResponse = false; + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse"; + } else { mavlink_mission_request_list_t request; + _failReadRequestListFirstResponse = true; mavlink_msg_mission_request_list_decode(&msg, &request); Q_ASSERT(request.target_system == _mockLink->vehicleId()); @@ -104,8 +115,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message msg.compid, // Target is original sender itemCount); // Number of mission items _mockLink->respondWithMavlinkMessage(responseMsg); - } else { - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode"; } } @@ -120,9 +129,13 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& Q_ASSERT(request.target_system == _mockLink->vehicleId()); Q_ASSERT(request.seq < _missionItems.count()); - if ((_failureMode == FailReadRequest0NoResponse && request.seq == 0) || - (_failureMode == FailReadRequest1NoResponse && request.seq == 1)) { - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode"; + if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse"; + } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse"; + } else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) { + _failReadRequest1FirstResponse = false; + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse"; } else { // FIXME: Track whether all items are requested, or requested in sequence @@ -185,6 +198,16 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms if (_writeSequenceCount == 0) { _sendAck(MAV_MISSION_ACCEPTED); } else { + if (_failureMode == FailWriteMissionCountNoResponse) { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse"; + return; + } + if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) { + _failWriteMissionCountFirstResponse = false; + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse"; + return; + } + _failWriteMissionCountFirstResponse = true; _writeSequenceIndex = 0; _requestNextMissionItem(_writeSequenceIndex); } @@ -194,9 +217,8 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) { qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode; - if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) || - (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) { - qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode"; + if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) { + qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse"; } else { if (sequenceNumber >= _writeSequenceCount) { qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 77c9ea238f63ac6bedf8c6eda674d5c586356cbe..765058fd62aa3fda8fedd38b06833af0ef5ac35d 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -42,18 +42,21 @@ public: typedef enum { FailNone, // No failures FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST + FailReadRequestListFirstResponse, // Don't send MISSION_COUNT in response to first MISSION_REQUEST_LIST, allow subsequent to go through FailReadRequest0NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 0 FailReadRequest1NoResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1 + FailReadRequest1FirstResponse, // Don't send MISSION_ITEM in response to MISSION_REQUEST item 1 on first try, allow subsequent request to go through FailReadRequest0IncorrectSequence, // Respond to MISSION_REQUEST 0 with incorrect sequence number in MISSION_ITEM FailReadRequest1IncorrectSequence, // Respond to MISSION_REQUEST 1 with incorrect sequence number in MISSION_ITEM FailReadRequest0ErrorAck, // Respond to MISSION_REQUEST 0 with MISSION_ACK error FailReadRequest1ErrorAck, // Respond to MISSION_REQUEST 1 bogus MISSION_ACK error - FailWriteRequest0NoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0 + FailWriteMissionCountNoResponse, // Don't respond to MISSION_COUNT with MISSION_REQUEST 0 + FailWriteMissionCountFirstResponse, // Don't respond to first MISSION_COUNT with MISSION_REQUEST 0, respond to subsequent MISSION_COUNT requests FailWriteRequest1NoResponse, // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1 - FailWriteRequest0IncorrectSequence, // Respond to MISSION_COUNT 0 with MISSION_REQUEST with wrong sequence number - FailWriteRequest1IncorrectSequence, // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number - FailWriteRequest0ErrorAck, // Respond to MISSION_COUNT 0 with MISSION_ACK error - FailWriteRequest1ErrorAck, // Respond to MISSION_ITEM 0 with MISSION_ACK error + FailWriteRequest0IncorrectSequence, // Item 0 MISSION_REQUEST sent has wrong sequence number + FailWriteRequest1IncorrectSequence, // Item 1 MISSION_REQUEST sent has wrong sequence number + FailWriteRequest0ErrorAck, // Instead of sending MISSION_REQUEST 0, send MISSION_ACK error + FailWriteRequest1ErrorAck, // Instead of sending MISSION_REQUEST 1, send MISSION_ACK error FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested @@ -102,6 +105,9 @@ private: FailureMode_t _failureMode; bool _sendHomePositionOnEmptyList; MAVLinkProtocol* _mavlinkProtocol; + bool _failReadRequestListFirstResponse; + bool _failReadRequest1FirstResponse; + bool _failWriteMissionCountFirstResponse; }; #endif