diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 414b3b151e98d98a3dee8ce1a22df91f17054345..fb3e0dc72f5e513371e0978bc3d71189f933abc0 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -130,6 +130,8 @@ void MissionManager::_retryRead(void) mavlink_message_t message; mavlink_mission_request_list_t request; + _clearMissionItems(); + request.target_system = _vehicle->id(); request.target_component = MAV_COMP_ID_MISSIONPLANNER; @@ -137,7 +139,6 @@ void MissionManager::_retryRead(void) _vehicle->sendMessage(message); _startAckTimeout(AckMissionCount); - emit inProgressChanged(true); } void MissionManager::_ackTimeout(void) @@ -356,29 +357,60 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) { mavlink_mission_ack_t missionAck; - if (!_stopAckTimeout(AckMissionRequest)) { + // Save th retry ack before calling _stopAckTimeout since we'll need it to determine what + // type of a protocol sequence we are in. + AckType_t savedRetryAck = _retryAck; + + // 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 + // matter what. + if (!_stopAckTimeout(_retryAck)) { return; } mavlink_msg_mission_ack_decode(&message, &missionAck); qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type; - - if (missionAck.type == MAV_MISSION_ACCEPTED) { - 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())); + + switch (savedRetryAck) { + case AckNone: + // State machine is idle. Vehicle is confused. + qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: type:" << missionAck.type; + _sendError(VehicleError, "Vehicle sent unexpected MISSION_ACK message."); + break; + case AckMissionCount: + // MISSION_COUNT message expected + qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: type:" << missionAck.type; + if (!_retrySequence(AckMissionCount)) { + _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(missionAck.type)); } - } - } else { - qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; - if (!_retrySequence(AckMissionRequest)) { - _sendError(VehicleError, QString("Vehicle returned error: %1").arg(missionAck.type)); - } + break; + case AckMissionItem: + // MISSION_ITEM expected + qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: type:" << missionAck.type; + if (!_retrySequence(AckMissionItem)) { + _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(missionAck.type)); + } + break; + case AckMissionRequest: + // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + if (missionAck.type == MAV_MISSION_ACCEPTED) { + 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. Vehicle only has partial list of mission items.").arg(_expectedSequenceNumber).arg(_missionItems.count())); + } + } + } else { + qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; + if (!_retrySequence(AckMissionRequest)) { + _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(missionAck.type)); + } + } + break; } } @@ -435,26 +467,31 @@ bool MissionManager::_retrySequence(AckType_t ackType) { qCDebug(MissionManagerLog) << "_retrySequence ackType:" << ackType << "_retryCount" << _retryCount; - if (++_retryCount <= _maxRetryCount) { - switch (ackType) { - case AckMissionCount: - case AckMissionItem: + switch (ackType) { + case AckMissionCount: + case AckMissionItem: + if (++_retryCount <= _maxRetryCount) { // We are in the middle of a read sequence, start over _retryRead(); return true; - break; - case AckMissionRequest: + } 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; - 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)); + } else { return false; - } - } else { - qCDebug(MissionManagerLog) << "Retries exhausted"; - return false; + } + 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; } } diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index efe97fde222a738a6d4c6fba224a04d0a6cc71e1..0e66d56571f82e6533e6432e41b5060b86d3e38a 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -232,10 +232,16 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f _multiSpy->clearAllSignals(); } -void MissionManagerTest::_roundTripItems(void) +void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly) { _writeItems(MockLinkMissionItemHandler::FailNone, MissionManager::InternalError, false); + _mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly); + if (failFirstTimeOnly) { + // Should fail first time, then retry should succed + failureMode = MockLinkMissionItemHandler::FailNone; + } + // Read the items back from the vehicle _missionManager->requestMissionItems(); @@ -246,21 +252,69 @@ void MissionManagerTest::_roundTripItems(void) _multiSpy->clearAllSignals(); - // Now wait for read sequence to complete. We should get both a newMissionItemsAvailable and a - // inProgressChanged signal to signal completion. - _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000); - _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000); - QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); - QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); - _checkInProgressValues(false); + 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 + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 10000); + QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); + QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); + _checkInProgressValues(false); + } else { + // This should be a failed run + + // Wait for read sequence to complete. We should get: + // inProgressChanged(false) signal to signal completion + // error(errorCode, QString) signal + // newMissionItemsAvailable signal + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 10000); + QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | 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); + } - const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); - QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); - QCOMPARE(_missionManager->canEdit(), true); + _multiSpy->clearAllSignals(); + + // Validate returned items - // Validate the returned items against our test data + size_t cMissionItemsExpected; - for (size_t i=0; imissionItems()->count(), (int)cMissionItemsExpected); + QCOMPARE(_missionManager->canEdit(), true); + + for (size_t i=0; i(_missionManager->missionItems()->get(i)); @@ -276,29 +330,11 @@ void MissionManagerTest::_roundTripItems(void) QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue); 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); } @@ -338,3 +374,62 @@ void MissionManagerTest::_testWriteFailureHandling(void) _mockLink->resetMissionItemHandler(); } } + +void MissionManagerTest::_testReadFailureHandling(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 }, + { "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 }, + }; + + for (size_t i=0; iresetMissionItemHandler(); + qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true"; + _roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true); + _mockLink->resetMissionItemHandler(); + } +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 178c9ba614dd68cb4de876a4fc799ddf845fac86..5c6849b250cc3cb0c6852de49206e6165e04b233 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -40,14 +40,16 @@ private slots: void init(void); void cleanup(void); - void _readEmptyVehicle(void); - void _roundTripItems(void); - void _testWriteFailureHandling(void); + void _testReadFailureHandling(void); private: void _checkInProgressValues(bool inProgress); + 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 _readEmptyVehicle(void); + void _testWriteFailureHandling(void); + MockLink* _mockLink; MissionManager* _missionManager; diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index eb40e377282b8b3220cb9da2dc541a9c692dd29c..e33cb6a8768fb80fd2465c8de628ab1c0d3d98b6 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -129,8 +129,8 @@ 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)) { + 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