Merge pull request #2537 from DonLakeFlyer/MissionProtocol

Mission protocol robustness rework

src/MissionManager/MissionManager.h

@@ -52,7 +52,7 @@ public:
     
     // Property accessors
     
-    bool inProgress(void) { return _retryAck != AckNone; }
+    bool inProgress(void);
     QmlObjectListModel* missionItems(void) { return &_missionItems; }
     
     // C++ methods
@@ -76,6 +76,7 @@ public:
         ItemMismatchError,      ///< Vehicle returned item with seq # different than requested
         VehicleError,           ///< Vehicle returned error
         MissingRequestsError,   ///< Vehicle did not request all items during write sequence
+        MaxRetryExceeded,       ///< Retry failed
     } ErrorCode_t;
 
     // These values are public so the unit test can set appropriate signal wait times
@@ -106,25 +107,24 @@ private:
     void _handleMissionItem(const mavlink_message_t& message);
     void _handleMissionRequest(const mavlink_message_t& message);
     void _handleMissionAck(const mavlink_message_t& message);
-    void _requestNextMissionItem(int sequenceNumber);
+    void _requestNextMissionItem(void);
     void _clearMissionItems(void);
     void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
-    void _retryWrite(void);
-    void _retryRead(void);
-    bool _retrySequence(AckType_t ackType);
     QString _ackTypeToString(AckType_t ackType);
     QString _missionResultToString(MAV_MISSION_RESULT result);
+    void _finishTransaction(bool success);
 
 private:
     Vehicle*            _vehicle;
     
-    int                 _cMissionItems;     ///< Mission items on vehicle
-
     QTimer*             _ackTimeoutTimer;
     AckType_t           _retryAck;
-    int                 _retryCount;
+    int                 _requestItemRetryCount;
     
-    int                 _expectedSequenceNumber;
+    bool        _readTransactionInProgress;
+    bool        _writeTransactionInProgress;
+    QList<int>  _itemIndicesToWrite;    ///< List of mission items which still need to be written to vehicle
+    QList<int>  _itemIndicesToRead;     ///< List of mission items which still need to be requested from vehicle
     
     QMutex _dataMutex;
     

src/MissionManager/MissionManagerTest.h

@@ -40,14 +40,14 @@ public:
     MissionManagerTest(void);
     
 private slots:
-    void _testWriteFailureHandlingAPM(void);
-    void _testReadFailureHandlingAPM(void);
     void _testWriteFailureHandlingPX4(void);
+    void _testWriteFailureHandlingAPM(void);
     void _testReadFailureHandlingPX4(void);
+    void _testReadFailureHandlingAPM(void);
 
 private:
-    void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
-    void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly);
+    void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
+    void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode);
     void _testWriteFailureHandlingWorker(void);
     void _testReadFailureHandlingWorker(void);
     

src/comm/MockLink.cc

@@ -783,9 +783,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     }
 }
 
-void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly)
+void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode)
 {
-    _missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly);
+    _missionItemHandler.setMissionItemFailureMode(failureMode);
 }
 
 void MockLink::_sendHomePosition(void)

src/comm/MockLink.h

@@ -131,8 +131,7 @@ public:
 
     /// Sets a failure mode for unit testing
     ///     @param failureMode Type of failure to simulate
-    ///     @param firstTimeOnly true: fail first call, success subsequent calls, false: fail all calls
-    void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly);
+    void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode);
 
     /// Called to send a MISSION_ACK message while the MissionManager is in idle state
     void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }

src/comm/MockLinkMissionItemHandler.cc

@@ -119,10 +119,6 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
     } else {
         qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
     }
-    
-    if (_failureFirstTimeOnly) {
-        _failureMode = FailNone;
-    }
 }
 
 void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
@@ -181,10 +177,6 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
             _mockLink->respondWithMavlinkMessage(responseMsg);
         }
     }
-    
-    if (_failureFirstTimeOnly) {
-        _failureMode = FailNone;
-    }
 }
 
 void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
@@ -246,10 +238,6 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
             _startMissionItemResponseTimer();
         }
     }
-    
-    if (_failureFirstTimeOnly) {
-        _failureMode = FailNone;
-    }
 }
 
 void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
@@ -279,9 +267,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
     
     Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
     
-    Q_ASSERT(!_missionItems.contains(missionItem.seq));
-    Q_ASSERT(missionItem.seq == _writeSequenceIndex);
-    
     _missionItems[missionItem.seq] = missionItem;
     
     _writeSequenceIndex++;
@@ -302,9 +287,6 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
             _sendAck(ack);
         }
     }
-    if (_failureFirstTimeOnly) {
-        _failureMode = FailNone;
-    }
 }
 
 void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
@@ -330,9 +312,8 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
     Q_ASSERT(false);
 }
 
-void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly)
+void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode)
 {
-    _failureFirstTimeOnly = firstTimeOnly;
     _failureMode = failureMode;
 }
 

src/comm/MockLinkMissionItemHandler.h

@@ -74,8 +74,7 @@ public:
 
     /// Sets a failure mode for unit testing
     ///     @param failureMode Type of failure to simulate
-    ///     @param firstTimeOnly true: fail first call, success subsequent calls, false: fail all calls
-    void setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly);
+    void setMissionItemFailureMode(FailureMode_t failureMode);
     
     /// Called to send a MISSION_ACK message while the MissionManager is in idle state
     void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType);
@@ -114,7 +113,6 @@ private:
     
     QTimer*             _missionItemResponseTimer;
     FailureMode_t       _failureMode;
-    bool                _failureFirstTimeOnly;
     bool                _sendHomePositionOnEmptyList;
     MAVLinkProtocol*    _mavlinkProtocol;
 };