Merge remote-tracking branch 'origin/master' into apmseveritybugfix

Conflicts:
	src/FirmwarePlugin/APM/APMFirmwarePlugin.h

.appveyor.yml

@@ -14,8 +14,8 @@ install:
   - cinst nsis -y -installArgs /D="%programfiles(x86)%\NSIS"
 
 build_script:
-  - C:\Qt\5.4\msvc2013_opengl\bin\qmake -r CONFIG-=debug_and_release CONFIG+=%CONFIG% CONFIG+=WarningsAsErrorsOn qgroundcontrol.pro
-  - jom -j 4
+  - C:\Qt\5.4\msvc2013_opengl\bin\qmake -r CONFIG-=debug_and_release CONFIG+=%CONFIG% CONFIG+=WarningsAsErrorsOn CONFIG+=jombuild qgroundcontrol.pro
+  - jom
 
 test_script:
   - if "%CONFIG%" EQU "debug" ( debug\qgroundcontrol --unittest )

QGCCommon.pri

@@ -168,8 +168,11 @@ WindowsBuild {
 	DEFINES += __STDC_LIMIT_MACROS
 	# Specify multi-process compilation within Visual Studio.
 	# (drastically improves compilation times for multi-core computers)
-	QMAKE_CXXFLAGS_DEBUG += -MP
-	QMAKE_CXXFLAGS_RELEASE += -MP
+	!jombuild {
+		message("When using jom/QtCreator please add CONFIG+=jombuild to your project build settings")
+		QMAKE_CXXFLAGS_DEBUG += -MP
+		QMAKE_CXXFLAGS_RELEASE += -MP
+	}
 }
 
 #

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -321,3 +321,15 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
 
     mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
 }
+
+void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
+{
+    // Streams are not started automatically on APM stack
+    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,        2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS,    2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,        2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,           3);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,             10);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,             10);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,             3);
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -62,15 +62,15 @@ class APMFirmwarePlugin : public FirmwarePlugin
     
 public:
     // Overrides from FirmwarePlugin
-    
     virtual bool isCapable(FirmwareCapabilities capabilities);
     virtual QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle);
     virtual QStringList flightModes(void);
     virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
     virtual int manualControlReservedButtonCount(void);
-    virtual void adjustMavlinkMessage(mavlink_message_t *message);
-
+    virtual void adjustMavlinkMessage(mavlink_message_t* message);
+    virtual void initializeVehicle(Vehicle* vehicle);
+    
 private:
     /// All access to singleton is through AutoPilotPluginManager::instance
     APMFirmwarePlugin(QObject* parent = NULL);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -35,6 +35,8 @@
 #include <QList>
 #include <QString>
 
+class Vehicle;
+
 /// This is the base class for Firmware specific plugins
 ///
 /// The FirmwarePlugin class is the abstract base class which represents the methods and objects
@@ -92,6 +94,9 @@ public:
     ///     @param message[in,out] Mavlink message to adjust if needed.
     virtual void adjustMavlinkMessage(mavlink_message_t* message) = 0;
     
+    /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware.
+    virtual void initializeVehicle(Vehicle* vehicle) = 0;
+    
 protected:
     FirmwarePlugin(QObject* parent = NULL) : QGCSingleton(parent) { }
 };

src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc

@@ -103,3 +103,10 @@ void GenericFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
     
     // Generic plugin does no message adjustment
 }
+
+void GenericFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
+{
+    Q_UNUSED(vehicle);
+    
+    // Generic Flight Stack is by definition "generic", so no extra work
+}

src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h

@@ -45,6 +45,7 @@ public:
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
     virtual int manualControlReservedButtonCount(void);
     virtual void adjustMavlinkMessage(mavlink_message_t* message);
+    virtual void initializeVehicle(Vehicle* vehicle);
 
 private:
     /// All access to singleton is through AutoPilotPluginManager::instance

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -193,3 +193,10 @@ bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
 {
     return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities;
 }
+
+void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
+{
+    Q_UNUSED(vehicle);
+    
+    // PX4 Flight Stack doesn't need to do any extra work
+}

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -45,6 +45,7 @@ public:
     virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
     virtual int manualControlReservedButtonCount(void);
     virtual void adjustMavlinkMessage(mavlink_message_t* message);
+    virtual void initializeVehicle(Vehicle* vehicle);
 
 private:
     /// All access to singleton is through AutoPilotPluginManager::instance

src/MissionManager/MissionManager.cc

@@ -55,6 +55,7 @@ MissionManager::~MissionManager()
 
 void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
 {
+    _retryCount = 0;
     _missionItems.clear();
     for (int i=0; i<missionItems.count(); i++) {
         _missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i])));
@@ -64,13 +65,14 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
     
     if (inProgress()) {
         qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress";
-        // FIXME: Better error handling
         return;
     }
     
     mavlink_message_t       message;
     mavlink_mission_count_t missionCount;
     
+    _expectedSequenceNumber = 0;
+    
     missionCount.target_system = _vehicle->id();
     missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
     missionCount.count = _missionItems.count();
@@ -78,10 +80,29 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
     mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount);
     
     _vehicle->sendMessage(message);
-    _startAckTimeout(AckMissionRequest, message);
+    _startAckTimeout(AckMissionRequest);
     emit inProgressChanged(true);
 }
 
+void MissionManager::_retryWrite(void)
+{
+    qCDebug(MissionManagerLog) << "_retryWrite";
+    
+    mavlink_message_t       message;
+    mavlink_mission_count_t missionCount;
+    
+    _expectedSequenceNumber = 0;
+    
+    missionCount.target_system = _vehicle->id();
+    missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
+    missionCount.count = _missionItems.count();
+    
+    mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount);
+    
+    _vehicle->sendMessage(message);
+    _startAckTimeout(AckMissionRequest);
+}
+
 void MissionManager::requestMissionItems(void)
 {
     qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
@@ -89,6 +110,7 @@ void MissionManager::requestMissionItems(void)
     mavlink_message_t               message;
     mavlink_mission_request_list_t  request;
     
+    _retryCount = 0;
     _clearMissionItems();
     
     request.target_system = _vehicle->id();
@@ -97,49 +119,72 @@ void MissionManager::requestMissionItems(void)
     mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request);
     
     _vehicle->sendMessage(message);
-    _startAckTimeout(AckMissionCount, message);
+    _startAckTimeout(AckMissionCount);
     emit inProgressChanged(true);
 }
 
+void MissionManager::_retryRead(void)
+{
+    qCDebug(MissionManagerLog) << "_retryRead";
+    
+    mavlink_message_t               message;
+    mavlink_mission_request_list_t  request;
+    
+    _clearMissionItems();
+    
+    request.target_system = _vehicle->id();
+    request.target_component = MAV_COMP_ID_MISSIONPLANNER;
+    
+    mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request);
+    
+    _vehicle->sendMessage(message);
+    _startAckTimeout(AckMissionCount);
+}
+
 void MissionManager::_ackTimeout(void)
 {
-    if (_retryAck == AckNone) {
+    AckType_t timedOutAck = _retryAck;
+    
+    _retryAck = AckNone;
+    
+    if (timedOutAck == AckNone) {
         qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
+        _sendError(InternalError, "Internal error occured during Mission Item communication: _ackTimeOut:_retryAck == AckNone");
         return;
     }
     
-    if (++_retryCount <= _maxRetryCount) {
-        qCDebug(MissionManagerLog) << "_ackTimeout retry _retryAck:_retryCount" << _retryAck << _retryCount;
-        _vehicle->sendMessage(_retryMessage);
-    } else {
-        qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << _retryAck << _retryCount;
+    if (!_retrySequence(timedOutAck)) {
+        qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << timedOutAck << _retryCount;
+        _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(timedOutAck));
     }
 }
 
-void MissionManager::_startAckTimeout(AckType_t ack, const mavlink_message_t& message)
+void MissionManager::_startAckTimeout(AckType_t ack)
 {
     _retryAck = ack;
-    _retryCount = 0;
-    _retryMessage = message;
-    
     _ackTimeoutTimer->start();
 }
 
 bool MissionManager::_stopAckTimeout(AckType_t expectedAck)
 {
-    bool success;
+    bool        success = false;
+    AckType_t   savedRetryAck = _retryAck;
+    
+    _retryAck = AckNone;
     
     _ackTimeoutTimer->stop();
     
-    if (_retryAck != expectedAck) {
-        qCWarning(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << _retryAck << expectedAck;
+    if (savedRetryAck != expectedAck) {
+        qCDebug(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << savedRetryAck << expectedAck;
+        
+        if (_retrySequence(expectedAck)) {
+            _sendError(ProtocolOrderError, QString("Vehicle responded incorrectly to mission item protocol sequence: %1:%2").arg(savedRetryAck).arg(expectedAck));
+        }
         success = false;
     } else {
         success = true;
     }
     
-    _retryAck = AckNone;
-    
     return success;
 }
 
@@ -189,6 +234,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber)
     
     if (sequenceNumber >= _cMissionItems) {
         qCWarning(MissionManagerLog) << "_requestNextMissionItem requested seqeuence number > item count sequenceNumber::_cMissionItems" << sequenceNumber << _cMissionItems;
+        _sendError(InternalError, QString("QGroundControl requested mission item outside of range (internal error): %1:%2").arg(sequenceNumber).arg(_cMissionItems));
         return;
     }
     
@@ -203,7 +249,7 @@ void MissionManager::_requestNextMissionItem(int sequenceNumber)
     mavlink_msg_mission_request_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionRequest);
     
     _vehicle->sendMessage(message);
-    _startAckTimeout(AckMissionItem, message);
+    _startAckTimeout(AckMissionItem);
 }
 
 void MissionManager::_handleMissionItem(const mavlink_message_t& message)
@@ -220,6 +266,9 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
     
     if (missionItem.seq != _expectedSequenceNumber) {
         qCDebug(MissionManagerLog) << "_handleMissionItem mission item received out of sequence expected:actual" << _expectedSequenceNumber << missionItem.seq;
+        if (!_retrySequence(AckMissionItem)) {
+            _sendError(ItemMismatchError, QString("Vehicle returned incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionItem.seq));
+        }
         return;
     }
         
@@ -267,11 +316,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
     
     qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq;
     
-    if (missionRequest.seq >= _missionItems.count()) {
-        qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested:count" << missionRequest.seq << _missionItems.count();
+    if (missionRequest.seq != _expectedSequenceNumber) {
+        qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested: _expectedSequenceNumber:missionRequest.seq" << _expectedSequenceNumber << missionRequest.seq;
+        
+        if (!_retrySequence(AckMissionRequest)) {
+            _sendError(ItemMismatchError, QString("Vehicle requested incorrect mission item: %1:%2").arg(_expectedSequenceNumber).arg(missionRequest.seq));
+        }
         return;
     }
     
+    _expectedSequenceNumber++;
+    
     mavlink_message_t       messageOut;
     mavlink_mission_item_t  missionItem;
     
@@ -295,28 +350,68 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
     mavlink_msg_mission_item_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &messageOut, &missionItem);
     
     _vehicle->sendMessage(messageOut);
-    // FIXME: This ack sequence isn't quite write
-    _startAckTimeout(AckMissionRequest, messageOut);
+    _startAckTimeout(AckMissionRequest);
 }
 
 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);
     
-    if (missionAck.type == MAV_MISSION_ACCEPTED) {
-        qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
-    } else {
-        _missionItems.clear();
-        emit newMissionItemsAvailable();
-        qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type;
+    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";
+                    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;
     }
-    emit inProgressChanged(false);
 }
 
 /// Called when a new mavlink message for out vehicle is received
@@ -359,3 +454,44 @@ QmlObjectListModel* MissionManager::copyMissionItems(void)
     
     return list;
 }
+
+void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
+{
+    emit inProgressChanged(false);
+    emit error(errorCode, errorMsg);
+}
+
+/// Retry the protocol sequence given the specified ack
+/// @return true: sequence retried, false: out of retries
+bool MissionManager::_retrySequence(AckType_t ackType)
+{
+    qCDebug(MissionManagerLog) << "_retrySequence ackType:" << ackType << "_retryCount" << _retryCount;
+    
+    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;
+    }
+}

src/MissionManager/MissionManager.h

@@ -67,12 +67,28 @@ public:
     /// Returns a copy of the current set of mission items. Caller is responsible for
     /// freeing returned object.
     QmlObjectListModel* copyMissionItems(void);
+    
+    /// Error codes returned in error signal
+    typedef enum {
+        InternalError,
+        AckTimeoutError,        ///< Timed out waiting for response from vehicle
+        ProtocolOrderError,     ///< Incorrect protocol sequence from vehicle
+        RequestRangeError,      ///< Vehicle requested item out of range
+        ItemMismatchError,      ///< Vehicle returned item with seq # different than requested
+        VehicleError,           ///< Vehicle returned error
+        MissingRequestsError,   ///< Vehicle did not request all items during write sequence
+    } ErrorCode_t;
 
+    // These values are public so the unit test can set appropriate signal wait times
+    static const int _ackTimeoutMilliseconds= 2000;
+    static const int _maxRetryCount = 5;
+    
 signals:
     // Public signals
     void canEditChanged(bool canEdit);
     void newMissionItemsAvailable(void);
     void inProgressChanged(bool inProgress);
+    void error(int errorCode, const QString& errorMsg);
     
 private slots:
     void _mavlinkMessageReceived(const mavlink_message_t& message);
@@ -86,7 +102,7 @@ private:
         AckMissionRequest,  ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
     } AckType_t;
     
-    void _startAckTimeout(AckType_t ack, const mavlink_message_t& message);
+    void _startAckTimeout(AckType_t ack);
     bool _stopAckTimeout(AckType_t expectedAck);
     void _sendTransactionComplete(void);
     void _handleMissionCount(const mavlink_message_t& message);
@@ -95,6 +111,10 @@ private:
     void _handleMissionAck(const mavlink_message_t& message);
     void _requestNextMissionItem(int sequenceNumber);
     void _clearMissionItems(void);
+    void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
+    void _retryWrite(void);
+    void _retryRead(void);
+    bool _retrySequence(AckType_t ackType);
 
 private:
     Vehicle*            _vehicle;
@@ -104,7 +124,6 @@ private:
 
     QTimer*             _ackTimeoutTimer;
     AckType_t           _retryAck;
-    mavlink_message_t   _retryMessage;
     int                 _retryCount;
     
     int                 _expectedSequenceNumber;
@@ -112,9 +131,6 @@ private:
     QMutex _dataMutex;
     
     QmlObjectListModel  _missionItems;
-    
-    static const int _ackTimeoutMilliseconds= 1000;
-    static const int _maxRetryCount = 5;
 };
 
 #endif

src/MissionManager/MissionManagerTest.cc

@@ -70,14 +70,15 @@ void MissionManagerTest::init(void)
     _rgSignals[canEditChangedSignalIndex] =             SIGNAL(canEditChanged(bool));
     _rgSignals[newMissionItemsAvailableSignalIndex] =   SIGNAL(newMissionItemsAvailable(void));
     _rgSignals[inProgressChangedSignalIndex] =          SIGNAL(inProgressChanged(bool));
+    _rgSignals[errorSignalIndex] =                      SIGNAL(error(int, const QString&));
 
     _multiSpy = new MultiSignalSpy();
     Q_CHECK_PTR(_multiSpy);
     QCOMPARE(_multiSpy->init(_missionManager, _rgSignals, _cSignals), true);
     
     if (_missionManager->inProgress()) {
-        _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000);
-        _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
+        _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, _signalWaitTime);
+        _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, _signalWaitTime);
         QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
         QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalIndex), true);
     }
@@ -122,8 +123,8 @@ void MissionManagerTest::_readEmptyVehicle(void)
     
     // 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);
+    _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, _signalWaitTime);
+    _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, _signalWaitTime);
     QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
     QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true);
     _checkInProgressValues(false);
@@ -133,8 +134,14 @@ void MissionManagerTest::_readEmptyVehicle(void)
     QCOMPARE(_missionManager->canEdit(), true);
 }
 
-void MissionManagerTest::_roundTripItems(void)
+void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly)
 {
+    _mockLink->setMissionItemFailureMode(failureMode, failFirstTimeOnly);
+    if (failFirstTimeOnly) {
+        // Should fail first time, then retry should succed
+        failureMode = MockLinkMissionItemHandler::FailNone;
+    }
+    
     // Setup our test case data
     const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
     QmlObjectListModel* list = new QmlObjectListModel();
@@ -152,7 +159,7 @@ void MissionManagerTest::_roundTripItems(void)
     
     // Send the items to the vehicle
     _missionManager->writeMissionItems(*list);
-
+    
     // writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it
     QVERIFY(_missionManager->inProgress());
     QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
@@ -160,18 +167,81 @@ void MissionManagerTest::_roundTripItems(void)
     
     _multiSpy->clearAllSignals();
     
-    // Now wait for write sequence to complete. We should only get an inProgressChanged signal to signal completion.
-    _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
-    QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
-    _checkInProgressValues(false);
+    if (failureMode == MockLinkMissionItemHandler::FailNone) {
+        // This should be clean run
+        
+        // Wait for write sequence to complete. We should get:
+        //      inProgressChanged(false) signal
+        _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, _signalWaitTime);
+        QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
+        
+        // Validate inProgressChanged signal value
+        _checkInProgressValues(false);
+
+        // We should have gotten back all mission items
+        QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
+    } else {
+        // This should be a failed run
+        
+        // Wait for write sequence to complete. We should get:
+        //      inProgressChanged(false) signal
+        //      error(errorCode, QString) signal
+        _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, _signalWaitTime);
+        QCOMPARE(_multiSpy->checkSignalByMask(inProgressChangedSignalMask | errorSignalMask), true);
+        
+        // Validate inProgressChanged signal value
+        _checkInProgressValues(false);
+        
+        // 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);
+
+        /*
+         // FIXME: This should be on the read side
+        // Validate correct number of mission items
+        int expectedMissionCount = 0;
+        
+        switch (ErrorCode) {
+            case FailWriteRequest0NoResponse:
+                // Don't respond to MISSION_COUNT with MISSION_REQUEST 0
+                expectedMissionCount = 0;
+                break;
+                
+            case FailWriteRequest1NoResponse:        // Don't respond to MISSION_ITEM 0 with MISSION_REQUEST 1
+            case FailWriteRequest0IncorrectSequence: // Respond to MISSION_COUNT 0 with MISSION_REQUEST with wrong sequence number
+            case FailWriteRequest1IncorrectSequence: // Respond to MISSION_ITEM 0 with MISSION_REQUEST with wrong sequence number
+            case FailWriteRequest0ErrorAck:          // Respond to MISSION_COUNT 0 with MISSION_ACK error
+            case FailWriteRequest1ErrorAck:          // Respond to MISSION_ITEM 0 with MISSION_ACK error
+            case FailWriteFinalAckNoResponse:        // Don't send the final MISSION_ACK
+            case FailWriteFinalAckErrorAck:          // Send an error as the final MISSION_ACK
+            case FailWriteFinalAckMissingRequests:   // Send the MISSION_ACK before all items have been requested
+                break;
+        }
+        // FIXME: Count depends on errorCode
+        //QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
+        */
+    }
     
-    QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
     QCOMPARE(_missionManager->canEdit(), true);
     
     delete list;
     list = NULL;
     _multiSpy->clearAllSignals();
+}
+
+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();
     
@@ -182,20 +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, _signalWaitTime);
+        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, _signalWaitTime);
+        QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask | errorSignalMask), true);
+        
+        // Validate inProgressChanged signal value
+        _checkInProgressValues(false);
+        
+        // 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);
+    }
     
-    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; i<cTestCases; i++) {
+    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));
         
@@ -211,4 +330,106 @@ 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
+     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; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+        qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:false";
+        _roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, false);
+        _mockLink->resetMissionItemHandler();
+        qDebug() << "TEST CASE " << rgTestCases[i].failureText << "errorCode:" << rgTestCases[i].errorCode << "failFirstTimeOnly:true";
+        _roundTripItems(rgTestCases[i].failureMode, rgTestCases[i].errorCode, true);
+        _mockLink->resetMissionItemHandler();
+    }
 }

src/MissionManager/MissionManagerTest.h

@@ -40,12 +40,16 @@ private slots:
     void init(void);
     void cleanup(void);
     
-    void _readEmptyVehicle(void);
-    void _roundTripItems(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;
     
@@ -53,6 +57,7 @@ private:
         canEditChangedSignalIndex = 0,
         newMissionItemsAvailableSignalIndex,
         inProgressChangedSignalIndex,
+        errorSignalIndex,
         maxSignalIndex
     };
     
@@ -60,6 +65,7 @@ private:
         canEditChangedSignalMask =              1 << canEditChangedSignalIndex,
         newMissionItemsAvailableSignalMask =    1 << newMissionItemsAvailableSignalIndex,
         inProgressChangedSignalMask =           1 << inProgressChangedSignalIndex,
+        errorSignalMask =                       1 << errorSignalIndex,
     };
 
     MultiSignalSpy*     _multiSpy;
@@ -85,6 +91,7 @@ private:
     } TestCase_t;
 
     static const TestCase_t _rgTestCases[];
+    static const int        _signalWaitTime = MissionManager::_ackTimeoutMilliseconds * MissionManager::_maxRetryCount * 2;
 };
 
 #endif

src/Vehicle/Vehicle.cc

@@ -85,9 +85,11 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
     , _satelliteLock(0)
     , _wpm(NULL)
     , _updateCount(0)
+    , _missionManager(NULL)
     , _armed(false)
     , _base_mode(0)
     , _custom_mode(0)
+    , _nextSendMessageMultipleIndex(0)
 {
     _addLink(link);
     
@@ -160,10 +162,18 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
     if (qgcApp()->useNewMissionEditor()) {
         _missionManager = new MissionManager(this);
     }
+    
+    _firmwarePlugin->initializeVehicle(this);
+    
+    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
+    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
 }
 
 Vehicle::~Vehicle()
 {
+    delete _missionManager;
+    _missionManager = NULL;
+    
     // Stop listening for system messages
     disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
     // Disconnect any previously connected active MAV
@@ -1065,3 +1075,49 @@ bool Vehicle::missingParameters(void)
 {
     return _autopilotPlugin->missingParameters();
 }
+
+void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate)
+{
+    mavlink_message_t               msg;
+    mavlink_request_data_stream_t   dataStream;
+    
+    dataStream.req_stream_id = stream;
+    dataStream.req_message_rate = rate;
+    dataStream.start_stop = 1;  // start
+    dataStream.target_system = id();
+    dataStream.target_component = 0;
+    
+    mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream);
+
+    // We use sendMessageMultiple since we really want these to make it to the vehicle
+    sendMessageMultiple(msg);
+}
+
+void Vehicle::_sendMessageMultipleNext(void)
+{
+    if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) {
+        qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid;
+        
+        sendMessage(_sendMessageMultipleList[_nextSendMessageMultipleIndex].message);
+        
+        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
+            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
+        } else {
+            _nextSendMessageMultipleIndex++;
+        }
+    }
+    
+    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
+        _nextSendMessageMultipleIndex = 0;
+    }
+}
+
+void Vehicle::sendMessageMultiple(mavlink_message_t message)
+{
+    SendMessageMultipleInfo_t   info;
+    
+    info.message =      message;
+    info.retryCount =   _sendMessageMultipleRetries;
+    
+    _sendMessageMultipleList.append(info);
+}

src/Vehicle/Vehicle.h

@@ -151,6 +151,10 @@ public:
     /// Sends this specified message to all links accociated with this vehicle
     void sendMessage(mavlink_message_t message);
     
+    /// Sends the specified messages multiple times to the vehicle in order to attempt to
+    /// guarantee that it makes it to the vehicle.
+    void sendMessageMultiple(mavlink_message_t message);
+    
     /// Provides access to uas from vehicle. Temporary workaround until UAS is fully phased out.
     UAS* uas(void) { return _uas; }
     
@@ -180,6 +184,11 @@ public:
     bool hilMode(void);
     void setHilMode(bool hilMode);
     
+    /// Requests the specified data stream from the vehicle
+    ///     @param stream Stream which is being requested
+    ///     @param rate Rate at which to send stream in Hz
+    void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate);
+    
     bool missingParameters(void);
     
     typedef enum {
@@ -289,7 +298,8 @@ private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _linkDisconnected(LinkInterface* link);
     void _sendMessage(mavlink_message_t message);
-    
+    void _sendMessageMultipleNext(void);
+
     void _handleTextMessage                 (int newCount);
     /** @brief Attitude from main autopilot / system state */
     void _updateAttitude                    (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
@@ -397,7 +407,22 @@ private:
     bool    _armed;         ///< true: vehicle is armed
     uint8_t _base_mode;     ///< base_mode from HEARTBEAT
     uint32_t _custom_mode;  ///< custom_mode from HEARTBEAT
+
+    /// Used to store a message being sent by sendMessageMultiple
+    typedef struct {
+        mavlink_message_t   message;    ///< Message to send multiple times
+        int                 retryCount; ///< Number of retries left
+    } SendMessageMultipleInfo_t;
+    
+    QList<SendMessageMultipleInfo_t> _sendMessageMultipleList;    ///< List of messages being sent multiple times
+    
+    static const int _sendMessageMultipleRetries = 5;
+    static const int _sendMessageMultipleIntraMessageDelay = 500;
+    
+    QTimer  _sendMultipleTimer;
+    int     _nextSendMessageMultipleIndex;
     
+    // Settings keys
     static const char* _settingsGroup;
     static const char* _joystickModeSettingsKey;
     static const char* _joystickEnabledSettingsKey;

src/VehicleSetup/JoystickConfigController.cc

@@ -253,14 +253,14 @@ void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t fu
     _nextButton->setEnabled(true);
 }
 
-bool JoystickConfigController::_stickSettleComplete(int value)
+bool JoystickConfigController::_stickSettleComplete(int axis, int value)
 {
     // We are waiting for the stick to settle out to a max position
     
     if (abs(_stickDetectValue - value) > _calSettleDelta) {
         // Stick is moving too much to consider stopped
         
-        qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, _stickDetectValue:value" << _stickDetectValue << value;
+        qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
 
         _stickDetectValue = value;
         _stickDetectSettleStarted = false;
@@ -272,12 +272,13 @@ bool JoystickConfigController::_stickSettleComplete(int value)
             
             if (_stickDetectSettleElapsed.elapsed() > _stickDetectSettleMSecs) {
                 // Stick has stayed positioned in one place long enough, detection is complete.
+                qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete detection complete, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
                 return true;
             }
         } else {
             // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs
             
-            qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, _stickDetectValue:value" << _stickDetectValue << value;
+            qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, axis:_stickDetectValue:value" << axis << _stickDetectValue << value;
             
             _stickDetectSettleStarted = true;
             _stickDetectSettleElapsed.start();
@@ -310,11 +311,9 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
             _stickDetectValue = value;
         }
     } else if (axis == _stickDetectAxis) {
-        if (_stickSettleComplete(value)) {
+        if (_stickSettleComplete(axis, value)) {
             AxisInfo* info = &_rgAxisInfo[axis];
             
-            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect settle complete, function:axis:value" << function << axis << value;
-            
             // Stick detection is complete. Stick should be at max position.
             // Map the axis to the function
             _rgFunctionAxisMapping[function] = axis;
@@ -322,7 +321,6 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
             
             // Axis should be at max value, if it is below initial set point the the axis is reversed.
             info->reversed = value < _axisValueSave[axis];
-            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect reversed:value:_axisValueSave" << info->reversed << value << _axisValueSave[axis];
             
             if (info->reversed) {
                 _rgAxisInfo[axis].axisMin = value;
@@ -330,6 +328,8 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
                 _rgAxisInfo[axis].axisMax = value;
             }
             
+            qCDebug(JoystickConfigControllerLog) << "_inputStickDetect saving values, function:axis:value:reversed:_axisValueSave" << function << axis << value << info->reversed << _axisValueSave[axis];
+            
             _signalAllAttiudeValueChanges();
             
             _advanceState();
@@ -339,6 +339,8 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
 
 void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value)
 {
+    qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value;
+    
     // We only care about the axis mapped to the function we are working on
     if (_rgFunctionAxisMapping[function] != axis) {
         return;
@@ -351,18 +353,20 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function,
                 _stickDetectAxis = axis;
                 _stickDetectInitialValue = value;
                 _stickDetectValue = value;
+                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
             }
         } else {
             if (value < _calCenterPoint - _calMoveDelta) {
                 _stickDetectAxis = axis;
                 _stickDetectInitialValue = value;
                 _stickDetectValue = value;
+                qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected movement _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
             }
         }
     } else {
         // We are waiting for the selected axis to settle out
         
-        if (_stickSettleComplete(value)) {
+        if (_stickSettleComplete(axis, value)) {
             AxisInfo* info = &_rgAxisInfo[axis];
             
             // Stick detection is complete. Stick should be at min position.
@@ -376,8 +380,8 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function,
             if (function == Joystick::throttleFunction) {
                 _rgAxisInfo[axis].axisTrim = value;
             }
-            // XXX to support configs which can reverse they need to check a reverse
-            // flag here and not do this.
+            
+            qCDebug(JoystickConfigControllerLog) << "_inputStickMin saving values, function:axis:value:reversed" << function << axis << value << info->reversed;
             
             _advanceState();
         }
@@ -386,6 +390,8 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function,
 
 void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value)
 {
+    qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value;
+    
     // We only care about the axis mapped to the function we are working on
     if (_rgFunctionAxisMapping[function] != axis) {
         return;
@@ -399,9 +405,10 @@ void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t functio
             _stickDetectAxis = axis;
             _stickDetectInitialValue = value;
             _stickDetectValue = value;
+            qCDebug(JoystickConfigControllerLog) << "_inputStickMin detected possible center _stickDetectAxis:_stickDetectInitialValue" << _stickDetectAxis << _stickDetectInitialValue;
         }
     } else {
-        if (_stickSettleComplete(value)) {
+        if (_stickSettleComplete(axis, value)) {
             _advanceState();
         }
     }

src/VehicleSetup/JoystickConfigController.h

@@ -182,7 +182,7 @@ private:
     void _skipFlaps(void);
     void _saveAllTrims(void);
     
-    bool _stickSettleComplete(int value);
+    bool _stickSettleComplete(int axis, int value);
     
     void _validateCalibration(void);
     void _writeCalibration(void);

src/comm/MockLink.cc

@@ -67,17 +67,18 @@ union px4_custom_mode {
     float data_float;
 };
 
-MockLink::MockLink(MockConfiguration* config) :
-    _name("MockLink"),
-    _connected(false),
-    _vehicleSystemId(128),     // FIXME: Pull from eventual parameter manager
-    _vehicleComponentId(200),  // FIXME: magic number?
-    _inNSH(false),
-    _mavlinkStarted(false),
-    _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
-    _mavState(MAV_STATE_STANDBY),
-    _autopilotType(MAV_AUTOPILOT_PX4),
-    _fileServer(NULL)
+MockLink::MockLink(MockConfiguration* config)
+    : _missionItemHandler(this)
+    , _name("MockLink")
+    , _connected(false)
+    , _vehicleSystemId(128)     // FIXME: Pull from eventual parameter manager
+    , _vehicleComponentId(200)  // FIXME: magic number?
+    , _inNSH(false)
+    , _mavlinkStarted(false)
+    , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
+    , _mavState(MAV_STATE_STANDBY)
+    , _autopilotType(MAV_AUTOPILOT_PX4)
+    , _fileServer(NULL)
 {
     _config = config;
     union px4_custom_mode   px4_cm;
@@ -89,10 +90,8 @@ MockLink::MockLink(MockConfiguration* config) :
     _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this);
     Q_CHECK_PTR(_fileServer);
     
-    _missionItemHandler = new MockLinkMissionItemHandler(this);
-    Q_CHECK_PTR(_missionItemHandler);
-
     moveToThread(this);
+    
     _loadParams();
     QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes);
 }
@@ -150,6 +149,8 @@ void MockLink::run(void)
     QObject::disconnect(&_timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks);
     QObject::disconnect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
     QObject::disconnect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);
+    
+    _missionItemHandler.shutdown();
 }
 
 void MockLink::_run1HzTasks(void)
@@ -304,8 +305,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
             continue;
         }
         
-        Q_ASSERT(_missionItemHandler);
-        if (_missionItemHandler->handleMessage(msg)) {
+        if (_missionItemHandler.handleMessage(msg)) {
             continue;
         }
 
@@ -669,3 +669,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
     }
 }
 
+void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, bool firstTimeOnly)
+{
+    _missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly);
+}

src/comm/MockLink.h

@@ -86,6 +86,23 @@ public:
     bool disconnect(void);
 
     LinkConfiguration* getLinkConfiguration() { return _config; }
+    
+    /// 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);
+    
+    /// 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(); }
+    
+    /// Reset the state of the MissionItemHandler to no items, no transactions in progress.
+    void resetMissionItemHandler(void) { _missionItemHandler.reset(); }
 
 signals:
     /// @brief Used internally to move data to the thread.
@@ -127,7 +144,7 @@ private:
     float _floatUnionForParam(int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
 
-    MockLinkMissionItemHandler* _missionItemHandler;
+    MockLinkMissionItemHandler  _missionItemHandler;
 
     QString _name;
     bool    _connected;

src/comm/MockLinkMissionItemHandler.cc

@@ -29,12 +29,27 @@
 QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
 
 MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink)
-    : QObject(mockLink)
-    , _mockLink(mockLink)
+    : _mockLink(mockLink)
+    , _missionItemResponseTimer(NULL)
+    , _failureMode(FailNone)
 {
     Q_ASSERT(mockLink);
 }
 
+MockLinkMissionItemHandler::~MockLinkMissionItemHandler()
+{
+
+}
+
+void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
+{
+    if (!_missionItemResponseTimer) {
+        _missionItemResponseTimer = new QTimer();
+        connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout);
+    }
+    _missionItemResponseTimer->start(500);
+}
+
 bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
 {
     switch (msg.msgid) {
@@ -78,21 +93,29 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
 {
     qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
     
-    mavlink_mission_request_list_t request;
-    
-    mavlink_msg_mission_request_list_decode(&msg, &request);
-    
-    Q_ASSERT(request.target_system == _mockLink->vehicleId());
-    
-    mavlink_message_t   responseMsg;
+    if (_failureMode != FailReadRequestListNoResponse) {
+        mavlink_mission_request_list_t request;
+        
+        mavlink_msg_mission_request_list_decode(&msg, &request);
+        
+        Q_ASSERT(request.target_system == _mockLink->vehicleId());
+        
+        mavlink_message_t   responseMsg;
+        
+        mavlink_msg_mission_count_pack(_mockLink->vehicleId(),
+                                       MAV_COMP_ID_MISSIONPLANNER,
+                                       &responseMsg,            // Outgoing message
+                                       msg.sysid,               // Target is original sender
+                                       msg.compid,              // Target is original sender
+                                       _missionItems.count());  // Number of mission items
+        _mockLink->respondWithMavlinkMessage(responseMsg);
+    } else {
+        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
+    }
     
-    mavlink_msg_mission_count_pack(_mockLink->vehicleId(),
-                                   MAV_COMP_ID_MISSIONPLANNER,
-                                   &responseMsg,            // Outgoing message
-                                   msg.sysid,               // Target is original sender
-                                   msg.compid,              // Target is original sender
-                                   _missionItems.count());  // Number of mission items
-    _mockLink->respondWithMavlinkMessage(responseMsg);
+    if (_failureFirstTimeOnly) {
+        _failureMode = FailNone;
+    }
 }
 
 void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
@@ -106,29 +129,50 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
     Q_ASSERT(request.target_system == _mockLink->vehicleId());
     Q_ASSERT(request.seq < _missionItems.count());
     
-    mavlink_message_t   responseMsg;
-    
-    mavlink_mission_item_t item = _missionItems[request.seq];
+    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
+        
+        if ((_failureMode == FailReadRequest0IncorrectSequence && request.seq == 0) ||
+                (_failureMode == FailReadRequest1IncorrectSequence && request.seq == 1)) {
+            // Send back the incorrect sequence number
+            qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number";
+            request.seq++;
+        }
+        
+        if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
+            (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
+            _sendAck(MAV_MISSION_ERROR);
+        } else {
+            mavlink_message_t   responseMsg;
+            
+            mavlink_mission_item_t item = _missionItems[request.seq];
+            
+            mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
+                                          MAV_COMP_ID_MISSIONPLANNER,
+                                          &responseMsg,            // Outgoing message
+                                          msg.sysid,               // Target is original sender
+                                          msg.compid,              // Target is original sender
+                                          request.seq,             // Index of mission item being sent
+                                          item.frame,
+                                          item.command,
+                                          item.current,
+                                          item.autocontinue,
+                                          item.param1, item.param2, item.param3, item.param4,
+                                          item.x, item.y, item.z);
+            _mockLink->respondWithMavlinkMessage(responseMsg);
+        }
+    }
     
-    mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
-                                  MAV_COMP_ID_MISSIONPLANNER,
-                                  &responseMsg,            // Outgoing message
-                                  msg.sysid,               // Target is original sender
-                                  msg.compid,              // Target is original sender
-                                  request.seq,             // Index of mission item being sent
-                                  item.frame,
-                                  item.command,
-                                  item.current,
-                                  item.autocontinue,
-                                  item.param1, item.param2, item.param3, item.param4,
-                                  item.x, item.y, item.z);
-    _mockLink->respondWithMavlinkMessage(responseMsg);
+    if (_failureFirstTimeOnly) {
+        _failureMode = FailNone;
+    }
 }
 
 void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
 {
-    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence";
-    
     mavlink_mission_count_t missionCount;
     
     mavlink_msg_mission_count_decode(&msg, &missionCount);
@@ -137,12 +181,12 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
     _writeSequenceCount = missionCount.count;
     Q_ASSERT(_writeSequenceCount >= 0);
     
-    // FIXME: Set up a timer for a failed write sequence
+    qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
     
     _missionItems.clear();
     
     if (_writeSequenceCount == 0) {
-        // FIXME: NYI
+        _sendAck(MAV_MISSION_ACCEPTED);
     } else {
         _writeSequenceIndex = 0;
         _requestNextMissionItem(_writeSequenceIndex);
@@ -151,31 +195,68 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
 
 void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
 {
-    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber;
+    qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode;
     
-    if (sequenceNumber >= _writeSequenceCount) {
-        qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
-        return;
+    if ((_failureMode == FailWriteRequest0NoResponse && sequenceNumber == 0) ||
+        (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1)) {
+        qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode";
+    } else {
+        if (sequenceNumber >= _writeSequenceCount) {
+            qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
+            return;
+        }
+        
+        if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
+            (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
+            sequenceNumber ++;
+        }
+        
+        if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
+            (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
+            qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
+            _sendAck(MAV_MISSION_ERROR);
+        } else {
+            mavlink_message_t           message;
+            mavlink_mission_request_t   missionRequest;
+            
+            missionRequest.target_system =      MAVLinkProtocol::instance()->getSystemId();
+            missionRequest.target_component =   MAVLinkProtocol::instance()->getComponentId();
+            missionRequest.seq =                sequenceNumber;
+            
+            mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest);
+            _mockLink->respondWithMavlinkMessage(message);
+
+            // If response with Mission Item doesn't come before timer fires it's an error
+            _startMissionItemResponseTimer();
+        }
     }
     
-    mavlink_message_t           message;
-    mavlink_mission_request_t   missionRequest;
+    if (_failureFirstTimeOnly) {
+        _failureMode = FailNone;
+    }
+}
+
+void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
+{
+    qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
+    
+    mavlink_message_t       message;
+    mavlink_mission_ack_t   missionAck;
     
-    missionRequest.target_system =      MAVLinkProtocol::instance()->getSystemId();
-    missionRequest.target_component =   MAVLinkProtocol::instance()->getComponentId();
-    missionRequest.seq =                sequenceNumber;
+    missionAck.target_system =      MAVLinkProtocol::instance()->getSystemId();
+    missionAck.target_component =   MAVLinkProtocol::instance()->getComponentId();
+    missionAck.type =               ackType;
     
-    mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest);
+    mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck);
     _mockLink->respondWithMavlinkMessage(message);
-
-    // FIXME: Timeouts
 }
 
-
 void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg)
 {
     qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
     
+    _missionItemResponseTimer->stop();
+    
     mavlink_mission_item_t missionItem;
     
     mavlink_msg_mission_item_decode(&msg, &missionItem);
@@ -187,21 +268,61 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
     
     _missionItems[missionItem.seq] = missionItem;
     
-    // FIXME: Timeouts
-    
     _writeSequenceIndex++;
     if (_writeSequenceIndex < _writeSequenceCount) {
-        _requestNextMissionItem(_writeSequenceIndex);
+        if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
+            // Send MAV_MISSION_ACCPETED ack too early
+            _sendAck(MAV_MISSION_ACCEPTED);
+        } else {
+            _requestNextMissionItem(_writeSequenceIndex);
+        }
     } else {
-        qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem sending final ack, write sequence complete";
-        mavlink_message_t       message;
-        mavlink_mission_ack_t   missionAck;
-        
-        missionAck.target_system =      MAVLinkProtocol::instance()->getSystemId();
-        missionAck.target_component =   MAVLinkProtocol::instance()->getComponentId();
-        missionAck.type =               MAV_MISSION_ACCEPTED;
-        
-        mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck);
-        _mockLink->respondWithMavlinkMessage(message);
+        if (_failureMode != FailWriteFinalAckNoResponse) {
+            MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED;
+            
+            if (_failureMode ==  FailWriteFinalAckErrorAck) {
+                ack = MAV_MISSION_ERROR;
+            }
+            _sendAck(ack);
+        }
+    }
+    if (_failureFirstTimeOnly) {
+        _failureMode = FailNone;
+    }
+}
+
+void MockLinkMissionItemHandler::_missionItemResponseTimeout(void)
+{
+    qWarning() << "Timeout waiting for next MISSION_ITEM";
+    Q_ASSERT(false);
+}
+
+void MockLinkMissionItemHandler::sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType)
+{
+    _sendAck(ackType);
+}
+
+void MockLinkMissionItemHandler::sendUnexpectedMissionItem(void)
+{
+    // FIXME: NYI
+    Q_ASSERT(false);
+}
+
+void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void)
+{
+    // FIXME: NYI
+    Q_ASSERT(false);
+}
+
+void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode, bool firstTimeOnly)
+{
+    _failureFirstTimeOnly = firstTimeOnly;
+    _failureMode = failureMode;
+}
+
+void MockLinkMissionItemHandler::shutdown(void)
+{
+    if (_missionItemResponseTimer) {
+        delete _missionItemResponseTimer;
     }
 }

src/comm/MockLinkMissionItemHandler.h

@@ -26,6 +26,7 @@
 
 #include <QObject>
 #include <QMap>
+#include <QTimer>
 
 #include "QGCMAVLink.h"
 #include "QGCLoggingCategory.h"
@@ -40,11 +41,55 @@ class MockLinkMissionItemHandler : public QObject
 
 public:
     MockLinkMissionItemHandler(MockLink* mockLink);
+    ~MockLinkMissionItemHandler();
+    
+    // Prepares for destruction on correct thread
+    void shutdown(void);
 
     /// @brief Called to handle mission item related messages. All messages should be passed to this method.
     ///         It will handle the appropriate set.
     /// @return true: message handled
     bool handleMessage(const mavlink_message_t& msg);
+    
+    typedef enum {
+        FailNone,                           // No failures
+        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
+    } FailureMode_t;
+
+    /// 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);
+    
+    /// Called to send a MISSION_ACK message while the MissionManager is in idle state
+    void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType);
+    
+    /// Called to send a MISSION_ITEM message while the MissionManager is in idle state
+    void sendUnexpectedMissionItem(void);
+    
+    /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
+    void sendUnexpectedMissionRequest(void);
+    
+    /// Reset the state of the MissionItemHandler to no items, no transactions in progress.
+    void reset(void) { _missionItems.clear(); }
+
+private slots:
+    void _missionItemResponseTimeout(void);
 
 private:
     void _handleMissionRequestList(const mavlink_message_t& msg);
@@ -52,6 +97,8 @@ private:
     void _handleMissionItem(const mavlink_message_t& msg);
     void _handleMissionCount(const mavlink_message_t& msg);
     void _requestNextMissionItem(int sequenceNumber);
+    void _sendAck(MAV_MISSION_RESULT ackType);
+    void _startMissionItemResponseTimer(void);
 
 private:
     MockLink* _mockLink;
@@ -61,6 +108,10 @@ private:
     
     typedef QMap<uint16_t, mavlink_mission_item_t>   MissionList_t;
     MissionList_t   _missionItems;
+    
+    QTimer* _missionItemResponseTimer;
+    FailureMode_t   _failureMode;
+    bool            _failureFirstTimeOnly;
 };
 
 #endif