Merge pull request #1930 from DonLakeFlyer/MissionManagerUTRead

MissionManager read hardening unit test

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,7 +357,14 @@ 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;
     }
     
@@ -364,6 +372,28 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
     
     qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type;
 
+    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));
+            }
+            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";
@@ -371,15 +401,17 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
                 } 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()));
+                        _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").arg(missionAck.type));
+                    _sendError(VehicleError, QString("Vehicle returned error: %1.  Vehicle only has partial list of mission items.").arg(missionAck.type));
                 }
             }
+            break;
+    }
 }
 
 /// Called when a new mavlink message for out vehicle is received
@@ -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:
+            if (++_retryCount <= _maxRetryCount) {
                 // We are in the middle of a read sequence, start over
                 _retryRead();
                 return true;
+            } else {
+                // Read sequence failed, signal for what we have up to this point
+                emit newMissionItemsAvailable();
+                return false;
+            }
             break;
         case AckMissionRequest:
+            if (++_retryCount <= _maxRetryCount) {
                 // We are in the middle of a write sequence, start over
                 _retryWrite();
                 return true;
+            } else {
+                return false;
+            }
             break;
         default:
             qCWarning(MissionManagerLog) << "_retrySequence fell through switch: ackType:" << 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;
-    }
 }

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);
+    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
         
-    const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
-    QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
-    QCOMPARE(_missionManager->canEdit(), true);
+        // 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 the returned items against our test data
+        // Validate inProgressChanged signal value
+        _checkInProgressValues(false);
         
-    for (size_t i=0; i<cTestCases; i++) {
+        // Validate error signal values
+        QSignalSpy* spy = _multiSpy->getSpyByIndex(errorSignalIndex);
+        QList<QVariant> signalArgs = spy->takeFirst();
+        QCOMPARE(signalArgs.count(), 2);
+        qDebug() << signalArgs[1].toString();
+        QCOMPARE(signalArgs[0].toInt(), (int)errorCode);
+    }
+    
+    _multiSpy->clearAllSignals();
+
+    // Validate returned items
+    
+    size_t cMissionItemsExpected;
+    
+    if (failureMode == MockLinkMissionItemHandler::FailNone || failFirstTimeOnly == true) {
+        cMissionItemsExpected = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
+    } else {
+        switch (failureMode) {
+            case MockLinkMissionItemHandler::FailReadRequestListNoResponse:
+            case MockLinkMissionItemHandler::FailReadRequest0NoResponse:
+            case MockLinkMissionItemHandler::FailReadRequest0IncorrectSequence:
+            case MockLinkMissionItemHandler::FailReadRequest0ErrorAck:
+                cMissionItemsExpected = 0;
+                break;
+            case MockLinkMissionItemHandler::FailReadRequest1NoResponse:
+            case MockLinkMissionItemHandler::FailReadRequest1IncorrectSequence:
+            case MockLinkMissionItemHandler::FailReadRequest1ErrorAck:
+                cMissionItemsExpected = 1;
+                break;
+            default:
+                // Internal error
+                Q_ASSERT(false);
+                break;
+        }
+    }
+    
+    QCOMPARE(_missionManager->missionItems()->count(), (int)cMissionItemsExpected);
+    QCOMPARE(_missionManager->canEdit(), true);
+    
+    for (size_t i=0; i<cMissionItemsExpected; i++) {
         const TestCase_t* testCase = &_rgTestCases[i];
         MissionItem* actual = qobject_cast<MissionItem*>(_missionManager->missionItems()->get(i));
         
@@ -276,9 +330,52 @@ void MissionManagerTest::_roundTripItems(void)
         QCOMPARE(actual->autoContinue(),            testCase->expectedItem.autocontinue);
         QCOMPARE(actual->frame(),                   testCase->expectedItem.frame);
     }
+    
 }
 
 void MissionManagerTest::_testWriteFailureHandling(void)
+{
+    /*
+    /// 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; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+        qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
+        _writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
+        _mockLink->resetMissionItemHandler();
+        qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
+        _writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
+        _mockLink->resetMissionItemHandler();
+    }
+}
+
+void MissionManagerTest::_testReadFailureHandling(void)
 {
     /*
      FailReadRequestListNoResponse,      // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST
@@ -318,23 +415,21 @@ void MissionManagerTest::_testWriteFailureHandling(void)
     
     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 },
+        { "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; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
         qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
-        _writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
+        _roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
         _mockLink->resetMissionItemHandler();
         qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
-        _writeItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
+        _roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
         _mockLink->resetMissionItemHandler();
     }
 }

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;
     

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