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.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