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.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
\ No newline at end of file

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