Move UAS and MAVLinkProtocol back to main thread

Plus a large number of other changes to allow for orderly shutdown of
objects without crashes or asserts. This is need for unit tests to
create/delete global state around every test.

src/comm/LinkManager.h

@@ -27,7 +27,6 @@ This file is part of the PIXHAWK project
 #ifndef _LINKMANAGER_H_
 #define _LINKMANAGER_H_
 
-#include <QThread>
 #include <QList>
 #include <QMultiMap>
 #include <QMutex>
@@ -89,9 +88,6 @@ public:
     /// Disconnect the specified link
     bool disconnectLink(LinkInterface* link);
     
-    /// Returns the one mavlink protocol object in the system
-    MAVLinkProtocol* mavlink(void) { return _mavlink; }
-    
 signals:
     void newLink(LinkInterface* link);
     void linkDeleted(LinkInterface* link);
@@ -99,9 +95,10 @@ signals:
 private:
     /// All access to LinkManager is through LinkManager::instance
     LinkManager(QObject* parent = NULL);
-    
     ~LinkManager();
     
+    virtual void _shutdown(void);
+
     bool _connectionsSuspendedMsg(void);
     
     QList<LinkInterface*>   _links;         ///< List of available links
@@ -109,8 +106,6 @@ private:
     
     bool    _connectionsSuspended;          ///< true: all new connections should not be allowed
     QString _connectionsSuspendedReason;    ///< User visible reason for suspension
-    
-    MAVLinkProtocol* _mavlink;  ///< The one and only mavlink protocol
 };
 
 #endif

src/comm/MAVLinkProtocol.cc

@@ -31,6 +31,8 @@
 #include "QGCApplication.h"
 
 Q_DECLARE_METATYPE(mavlink_message_t)
+IMPLEMENT_QGC_SINGLETON(MAVLinkProtocol, MAVLinkProtocol)
+Q_LOGGING_CATEGORY(MAVLinkProtocolLog, "MAVLinkProtocolLog")
 
 const char* MAVLinkProtocol::_tempLogFileTemplate = "FlightDataXXXXXX"; ///< Template for temporary log file
 const char* MAVLinkProtocol::_logFileExtension = "mavlink";             ///< Extension for log files
@@ -39,10 +41,8 @@ const char* MAVLinkProtocol::_logFileExtension = "mavlink";             ///< Ext
  * The default constructor will create a new MAVLink object sending heartbeats at
  * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
  */
-MAVLinkProtocol::MAVLinkProtocol(LinkManager* linkMgr) :
-    heartbeatTimer(NULL),
-    heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
-    m_heartbeatsEnabled(true),
+MAVLinkProtocol::MAVLinkProtocol(QObject* parent) :
+    QGCSingleton(parent),
     m_multiplexingEnabled(false),
     m_authEnabled(false),
     m_enable_version_check(true),
@@ -53,19 +53,19 @@ MAVLinkProtocol::MAVLinkProtocol(LinkManager* linkMgr) :
     m_actionRetransmissionTimeout(100),
     versionMismatchIgnore(false),
     systemId(QGC::defaultSystemId),
-    _should_exit(false),
     _logSuspendError(false),
     _logSuspendReplay(false),
     _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension)),
     _protocolStatusMessageConnected(false),
     _saveTempFlightDataLogConnected(false),
-    _linkMgr(linkMgr)
+    _linkMgr(LinkManager::instance()),
+    _heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
+    _heartbeatsEnabled(true)
 {
     qRegisterMetaType<mavlink_message_t>("mavlink_message_t");
     
     m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
     loadSettings();
-    moveToThread(this);
 
     // All the *Counter variables are not initialized here, as they should be initialized
     // on a per-link basis before those links are used. @see resetMetadataForLink().
@@ -79,17 +79,26 @@ MAVLinkProtocol::MAVLinkProtocol(LinkManager* linkMgr) :
         }
     }
     
-    start(QThread::HighPriority);
+    // Start heartbeat timer, emitting a heartbeat at the configured rate
+    connect(&_heartbeatTimer, &QTimer::timeout, this, &MAVLinkProtocol::sendHeartbeat);
+    _heartbeatTimer.start(1000/_heartbeatRate);
 
     emit versionCheckChanged(m_enable_version_check);
 }
 
+MAVLinkProtocol::~MAVLinkProtocol()
+{
+    storeSettings();
+    
+    _closeLogFile();
+}
+
 void MAVLinkProtocol::loadSettings()
 {
     // Load defaults from settings
     QSettings settings;
     settings.beginGroup("QGC_MAVLINK_PROTOCOL");
-    enableHeartbeats(settings.value("HEARTBEATS_ENABLED", m_heartbeatsEnabled).toBool());
+    enableHeartbeats(settings.value("HEARTBEATS_ENABLED", _heartbeatsEnabled).toBool());
     enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool());
     enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool());
 
@@ -119,7 +128,7 @@ void MAVLinkProtocol::storeSettings()
     // Store settings
     QSettings settings;
     settings.beginGroup("QGC_MAVLINK_PROTOCOL");
-    settings.setValue("HEARTBEATS_ENABLED", m_heartbeatsEnabled);
+    settings.setValue("HEARTBEATS_ENABLED", _heartbeatsEnabled);
     settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
     settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled);
     settings.setValue("GCS_SYSTEM_ID", systemId);
@@ -132,43 +141,6 @@ void MAVLinkProtocol::storeSettings()
     settings.endGroup();
 }
 
-MAVLinkProtocol::~MAVLinkProtocol()
-{
-    storeSettings();
-    
-    _closeLogFile();
-
-    // Tell the thread to exit
-    _should_exit = true;
-    // Wait for it to exit
-    wait();
-}
-
-/**
- * @brief Runs the thread
- *
- **/
-void MAVLinkProtocol::run()
-{
-    heartbeatTimer = new QTimer();
-    heartbeatTimer->moveToThread(this);
-    // Start heartbeat timer, emitting a heartbeat at the configured rate
-    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
-    heartbeatTimer->start(1000/heartbeatRate);
-
-    while(!_should_exit) {
-
-        if (isFinished()) {
-            delete heartbeatTimer;
-            qDebug() << "MAVLINK WORKER DONE!";
-            return;
-        }
-
-        QCoreApplication::processEvents();
-        QGC::SLEEP::msleep(2);
-    }
-}
-
 void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
 {
     int linkId = link->getId();
@@ -197,6 +169,7 @@ void MAVLinkProtocol::linkDisconnected(void)
 
 void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected)
 {
+    qCDebug(MAVLinkProtocolLog) << "_linkStatusChanged" << QString("%1").arg((long)link, 0, 16) << connected;
     Q_ASSERT(link);
     
     if (connected) {
@@ -559,7 +532,7 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message
  */
 void MAVLinkProtocol::sendHeartbeat()
 {
-    if (m_heartbeatsEnabled)
+    if (_heartbeatsEnabled)
     {
         mavlink_message_t beat;
         mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_MANUAL_ARMED, 0, MAV_STATE_ACTIVE);
@@ -576,10 +549,10 @@ void MAVLinkProtocol::sendHeartbeat()
     }
 }
 
-/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
+/** @param enabled true to enable heartbeats emission at _heartbeatRate, false to disable */
 void MAVLinkProtocol::enableHeartbeats(bool enabled)
 {
-    m_heartbeatsEnabled = enabled;
+    _heartbeatsEnabled = enabled;
     emit heartbeatChanged(enabled);
 }
 
@@ -655,14 +628,14 @@ void MAVLinkProtocol::enableVersionCheck(bool enabled)
  */
 void MAVLinkProtocol::setHeartbeatRate(int rate)
 {
-    heartbeatRate = rate;
-    heartbeatTimer->setInterval(1000/heartbeatRate);
+    _heartbeatRate = rate;
+    _heartbeatTimer.setInterval(1000/_heartbeatRate);
 }
 
 /** @return heartbeat rate in Hertz */
 int MAVLinkProtocol::getHeartbeatRate()
 {
-    return heartbeatRate;
+    return _heartbeatRate;
 }
 
 /// @brief Closes the log file if it is open
@@ -705,7 +678,8 @@ void MAVLinkProtocol::_startLogging(void)
 void MAVLinkProtocol::_stopLogging(void)
 {
     if (_closeLogFile()) {
-        if (qgcApp()->promptFlightDataSave()) {
+        // If the signals are not connected it means we are running a unit test. In that case just delete log files
+        if (_protocolStatusMessageConnected && _saveTempFlightDataLogConnected && qgcApp()->promptFlightDataSave()) {
             emit saveTempFlightDataLog(_tempLogFile.fileName());
         } else {
             QFile::remove(_tempLogFile.fileName());
@@ -761,11 +735,6 @@ void MAVLinkProtocol::_checkLostLogFiles(void)
 
 void MAVLinkProtocol::suspendLogForReplay(bool suspend)
 {
-    // This must come through a slot so that we handle this on the right thread. This will
-    // also cause the signal to be queued behind any bytesReady signals which must be flushed
-    // out of the queue before we change suspend state.
-    Q_ASSERT(QThread::currentThread() == this);
-    
     _logSuspendReplay = suspend;
 }
 

src/comm/MAVLinkProtocol.h

@@ -37,14 +37,18 @@ This file is part of the QGROUNDCONTROL project
 #include <QFile>
 #include <QMap>
 #include <QByteArray>
+#include <QLoggingCategory>
 
 #include "LinkInterface.h"
 #include "QGCMAVLink.h"
 #include "QGC.h"
 #include "QGCTemporaryFile.h"
+#include "QGCSingleton.h"
 
 class LinkManager;
 
+Q_DECLARE_LOGGING_CATEGORY(MAVLinkProtocolLog)
+
 /**
  * @brief MAVLink micro air vehicle protocol reference implementation.
  *
@@ -52,14 +56,13 @@ class LinkManager;
  * for more information, please see the official website.
  * @ref http://pixhawk.ethz.ch/software/mavlink/
  **/
-class MAVLinkProtocol : public QThread
+class MAVLinkProtocol : public QGCSingleton
 {
     Q_OBJECT
     
-public:
-    MAVLinkProtocol(LinkManager *linkMgr);
-    ~MAVLinkProtocol();
+    DECLARE_QGC_SINGLETON(MAVLinkProtocol, MAVLinkProtocol)
 
+public:
     /** @brief Get the human-friendly name of this protocol */
     QString getName();
     /** @brief Get the system id of this application */
@@ -70,7 +73,7 @@ public:
     int getHeartbeatRate();
     /** @brief Get heartbeat state */
     bool heartbeatsEnabled() const {
-        return m_heartbeatsEnabled;
+        return _heartbeatsEnabled;
     }
     
     /** @brief Get protocol version check state */
@@ -139,7 +142,8 @@ public:
      */
     virtual void resetMetadataForLink(const LinkInterface *link);
     
-    void run();
+    /// Suspend/Restart logging during replay.
+    void suspendLogForReplay(bool suspend);
 
 public slots:
     /** @brief Receive bytes from a communication interface */
@@ -199,11 +203,6 @@ public slots:
     /** @brief Store protocol settings */
     void storeSettings();
     
-    /// @brief Suspend/Restart logging during replay. This must be emitted as a signal
-    ///         and not called directly in order to synchronize with the bytesReady signal
-    ///         which may be ahead of it in the signal queue.
-    void suspendLogForReplay(bool suspend);
-    
     /// @brief Deletes any log files which are in the temp directory
     static void deleteTempLogFiles(void);
 
@@ -211,9 +210,6 @@ protected:
     // Override from QObject
     virtual void connectNotify(const QMetaMethod& signal);
 
-    QTimer *heartbeatTimer;    ///< Timer to emit heartbeats
-    int heartbeatRate;         ///< Heartbeat rate, controls the timer interval
-    bool m_heartbeatsEnabled;  ///< Enabled/disable heartbeat emission
     bool m_multiplexingEnabled; ///< Enable/disable packet multiplexing
     bool m_authEnabled;        ///< Enable authentication token broadcast
     QString m_authKey;         ///< Authentication key
@@ -232,7 +228,6 @@ protected:
     int currLossCounter[MAVLINK_COMM_NUM_BUFFERS];        ///< Lost messages during this sample time window. Used for calculating loss %.
     bool versionMismatchIgnore;
     int systemId;
-    bool _should_exit;
 
 signals:
     /** @brief Message received and directly copied via signal */
@@ -282,6 +277,9 @@ signals:
     void saveTempFlightDataLog(QString tempLogfile);
     
 private:
+    MAVLinkProtocol(QObject* parent = NULL);
+    ~MAVLinkProtocol();
+
     void _linkStatusChanged(LinkInterface* link, bool connected);
     bool _closeLogFile(void);
     void _startLogging(void);
@@ -301,6 +299,10 @@ private:
     bool _saveTempFlightDataLogConnected;   ///< true: saveTempFlightDataLog signal has been connected
     
     LinkManager* _linkMgr;
+    
+    QTimer  _heartbeatTimer;    ///< Timer to emit heartbeats
+    int     _heartbeatRate;     ///< Heartbeat rate, controls the timer interval
+    bool    _heartbeatsEnabled; ///< Enabled/disable heartbeat emission
 };
 
 #endif // MAVLINKPROTOCOL_H_

src/comm/SerialLink.cc

@@ -238,8 +238,6 @@ void SerialLink::run()
                 m_port->close();
                 delete m_port;
                 m_port = NULL;
-
-                emit disconnected();
             }
 
             QGC::SLEEP::msleep(500);
@@ -337,8 +335,6 @@ void SerialLink::run()
         m_port->close();
         delete m_port;
         m_port = NULL;
-
-        emit disconnected();
     }
 }
 
@@ -474,7 +470,9 @@ bool SerialLink::hardwareConnect(QString &type)
         return false; // couldn't create serial port.
     }
 
-    QObject::connect(m_port,SIGNAL(aboutToClose()),this,SIGNAL(disconnected()));
+    // We need to catch this signal and then emit disconnected. You can't connect
+    // signal to signal otherwise disonnected will have the wrong QObject::Sender
+    QObject::connect(m_port, SIGNAL(aboutToClose()), this, SLOT(_rerouteDisconnected()));
     QObject::connect(m_port, SIGNAL(error(QSerialPort::SerialPortError)), this, SLOT(linkError(QSerialPort::SerialPortError)));
 
     checkIfCDC();
@@ -899,3 +897,8 @@ bool SerialLink::setStopBitsType(int stopBits)
     }
     return accepted;
 }
+
+void SerialLink::_rerouteDisconnected(void)
+{
+    emit disconnected();
+}

src/comm/SerialLink.h

@@ -163,6 +163,9 @@ protected:
     QString type;
     bool m_is_cdc;
     
+private slots:
+    void _rerouteDisconnected(void);
+
 private:
     // From LinkInterface
     virtual bool _connect(void);

src/main.cc

@@ -118,7 +118,7 @@ int main(int argc, char *argv[])
         // Add additional command line option flags here
     };
     
-    ParseCmdLineOptions(argc, argv, rgCmdLineOptions, sizeof(rgCmdLineOptions)/sizeof(rgCmdLineOptions[0]), true);
+    ParseCmdLineOptions(argc, argv, rgCmdLineOptions, sizeof(rgCmdLineOptions)/sizeof(rgCmdLineOptions[0]), false);
     
     if (quietWindowsAsserts) {
 #ifdef Q_OS_WIN
@@ -157,7 +157,7 @@ int main(int argc, char *argv[])
         }
         
         // Run the test
-        int failures = UnitTest::run(argc-1, argv, rgCmdLineOptions[0].optionArg);
+        int failures = UnitTest::run(rgCmdLineOptions[0].optionArg);
         if (failures == 0) {
             qDebug() << "ALL TESTS PASSED";
         } else {

src/qgcunittest/LinkManagerTest.cc

@@ -60,6 +60,8 @@ void LinkManagerTest::cleanup(void)
     Q_ASSERT(_linkMgr);
     Q_ASSERT(_multiSpy);
     
+    _linkMgr->_shutdown();
+    
     delete _linkMgr;
     delete _multiSpy;
     

src/qgcunittest/MockLink.cc

@@ -29,6 +29,8 @@
 
 #include <string.h>
 
+Q_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog")
+
 /// @file
 ///     @brief Mock implementation of a Link.
 ///
@@ -199,9 +201,9 @@ void MockLink::_loadParams(void)
                 break;
         }
         
-        _parameters[paramName] = paramValue;
+        _mapParamName2Value[paramName] = paramValue;
+        _mapParamName2MavParamType[paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
     }
-    _cParameters = _parameters.count();
 }
 
 void MockLink::_sendHeartBeat(void)
@@ -352,49 +354,118 @@ void MockLink::_handleSetMode(const mavlink_message_t& msg)
     mavlink_set_mode_t request;
     mavlink_msg_set_mode_decode(&msg, &request);
     
-    if (request.target_system == _vehicleSystemId) {
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+    
     _mavBaseMode = request.base_mode;
     _mavCustomMode = request.custom_mode;
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
+}
+
+void MockLink::_setParamFloatUnionIntoMap(const QString& paramName, float paramFloat)
+{
+    mavlink_param_union_t   valueUnion;
+    
+    Q_ASSERT(_mapParamName2Value.contains(paramName));
+    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
+    
+    valueUnion.param_float = paramFloat;
+    
+    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
+    
+    QVariant paramVariant;
+    
+    switch (paramType) {
+        case MAV_PARAM_TYPE_INT8:
+            paramVariant = QVariant::fromValue(valueUnion.param_int8);
+            break;
+            
+        case MAV_PARAM_TYPE_INT32:
+            paramVariant = QVariant::fromValue(valueUnion.param_int32);
+            break;
+            
+        case MAV_PARAM_TYPE_UINT32:
+            paramVariant = QVariant::fromValue(valueUnion.param_uint32);
+            break;
+            
+        case MAV_PARAM_TYPE_REAL32:
+            paramVariant = QVariant::fromValue(valueUnion.param_float);
+            break;
+            
+        default:
+            qCritical() << "Invalid parameter type" << paramType;
     }
+    
+    qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
+    _mapParamName2Value[paramName] = paramVariant;
 }
 
-void MockLink::_errorInvalidTargetSystem(int targetId)
+/// Convert from a parameter variant to the float value from mavlink_param_union_t
+float MockLink::_floatUnionForParam(const QString& paramName)
 {
-    QString errMsg("MSG_ID_SET_MODE received incorrect target system: actual(%1) expected(%2)");
-    emit error(errMsg.arg(targetId).arg(_vehicleSystemId));
+    mavlink_param_union_t   valueUnion;
+    
+    Q_ASSERT(_mapParamName2Value.contains(paramName));
+    Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
+    
+    MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
+    QVariant paramVar = _mapParamName2Value[paramName];
+    
+    switch (paramType) {
+        case MAV_PARAM_TYPE_INT8:
+            valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
+            break;
+            
+        case MAV_PARAM_TYPE_INT32:
+            valueUnion.param_int32 = paramVar.toInt();
+            break;
+            
+        case MAV_PARAM_TYPE_UINT32:
+            valueUnion.param_uint32 = paramVar.toUInt();
+            break;
+            
+        case MAV_PARAM_TYPE_REAL32:
+            valueUnion.param_float = paramVar.toFloat();
+            break;
+            
+        default:
+            qCritical() << "Invalid parameter type" << paramType;
+    }
+    
+    return valueUnion.param_float;
 }
 
 void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
 {
+    uint16_t paramIndex = 0;
     mavlink_param_request_list_t request;
     
     mavlink_msg_param_request_list_decode(&msg, &request);
     
-    uint16_t paramIndex = 0;
-    if (request.target_system == _vehicleSystemId) {
-        ParamMap_t::iterator param;
+    int cParameters = _mapParamName2Value.count();
     
-        for (param = _parameters.begin(); param != _parameters.end(); param++) {
-            mavlink_message_t   responseMsg;
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+    
+    foreach(QString paramName, _mapParamName2Value.keys()) {
         char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
+        mavlink_message_t       responseMsg;
+        
+        Q_ASSERT(_mapParamName2Value.contains(paramName));
+        Q_ASSERT(_mapParamName2MavParamType.contains(paramName));
+        
+        MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName];
+        
+        Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
+        strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
         
-            Q_ASSERT(param.key().length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
-            strncpy(paramId, param.key().toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
         mavlink_msg_param_value_pack(_vehicleSystemId,
                                      _vehicleComponentId,
                                      &responseMsg,                      // Outgoing message
                                      paramId,                           // Parameter name
-                                         param.value().toFloat(),   // Parameter vluae
-                                         MAV_PARAM_TYPE_REAL32,     // FIXME: Pull from QVariant type
-                                         _cParameters,              // Total number of parameters
+                                     _floatUnionForParam(paramName),    // Parameter value
+                                     paramType,                         // MAV_PARAM_TYPE
+                                     cParameters,                       // Total number of parameters
                                      paramIndex++);                     // Index of this parameter
         _emitMavlinkMessage(responseMsg);
     }
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
-    }
 }
 
 void MockLink::_handleParamSet(const mavlink_message_t& msg)
@@ -402,33 +473,29 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg)
     mavlink_param_set_t request;
     mavlink_msg_param_set_decode(&msg, &request);
     
-    if (request.target_system == _vehicleSystemId) {
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+
     // Param may not be null terminated if exactly fits
     char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
     strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
     
-        if (_parameters.contains(paramId))
-        {
-            _parameters[paramId] = request.param_value;
+    Q_ASSERT(_mapParamName2Value.contains(paramId));
+    Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]);
     
-            mavlink_message_t   responseMsg;
+    // Save the new value
+    _setParamFloatUnionIntoMap(paramId, request.param_value);
     
+    // Respond with a param_value to ack
+    mavlink_message_t responseMsg;
     mavlink_msg_param_value_pack(_vehicleSystemId,
                                  _vehicleComponentId,
                                  &responseMsg,                  // Outgoing message
                                  paramId,                       // Parameter name
-                                         request.param_value,                   // Parameter vluae
-                                         MAV_PARAM_TYPE_REAL32,                 // FIXME: Pull from QVariant type
-                                         _cParameters,                          // Total number of parameters
-                                         _parameters.keys().indexOf(paramId));  // Index of this parameter
+                                 request.param_value,           // Send same value back
+                                 request.param_type,            // Send same type back
+                                 _mapParamName2Value.count(),   // Total number of parameters
+                                 _mapParamName2Value.keys().indexOf(paramId));  // Index of this parameter
     _emitMavlinkMessage(responseMsg);
-        } else {
-            QString errMsg("MSG_ID_PARAM_SET requested unknown param id (%1)");
-            emit error(errMsg.arg(paramId));
-        }
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
-    }
 }
 
 void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
@@ -439,24 +506,23 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
     char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
     paramId[0] = 0;
     
-    if (request.target_system == _vehicleSystemId) {
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+    
     if (request.param_index == -1) {
         // Request is by param name. Param may not be null terminated if exactly fits
         strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
     } else {
-            if (request.param_index >= 0 && request.param_index < _cParameters) {
         // Request is by index
-                QString key = _parameters.keys().at(request.param_index);
+        
+        Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value.count());
+        
+        QString key = _mapParamName2Value.keys().at(request.param_index);
         Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
         strcpy(paramId, key.toLocal8Bit().constData());
-            } else {
-                QString errMsg("MSG_ID_PARAM_REQUEST_READ requested unknown index: requested(%1) count(%2)");
-                emit error(errMsg.arg(request.param_index).arg(_cParameters));
-            }
     }
 
-        if (paramId[0] && _parameters.contains(paramId)) {
-            float paramValue = _parameters[paramId].toFloat();
+    Q_ASSERT(_mapParamName2Value.contains(paramId));
+    Q_ASSERT(_mapParamName2MavParamType.contains(paramId));
     
     mavlink_message_t   responseMsg;
     
@@ -464,15 +530,11 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
                                  _vehicleComponentId,
                                  &responseMsg,                          // Outgoing message
                                  paramId,                               // Parameter name
-                                         paramValue,                            // Parameter vluae
-                                         MAV_PARAM_TYPE_REAL32,                 // FIXME: Pull from QVariant type
-                                         _cParameters,                          // Total number of parameters
-                                         _parameters.keys().indexOf(paramId));  // Index of this parameter
+                                 _floatUnionForParam(paramId),          // Parameter value
+                                 _mapParamName2MavParamType[paramId],   // Parameter type
+                                 _mapParamName2Value.count(),           // Total number of parameters
+                                 _mapParamName2Value.keys().indexOf(paramId));  // Index of this parameter
     _emitMavlinkMessage(responseMsg);
-        }
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
-    }
 }
 
 void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
@@ -481,7 +543,8 @@ void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
     
     mavlink_msg_mission_request_list_decode(&msg, &request);
     
-    if (request.target_system == _vehicleSystemId) {
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+    
     mavlink_message_t   responseMsg;
 
     mavlink_msg_mission_count_pack(_vehicleSystemId,
@@ -491,9 +554,6 @@ void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
                                    msg.compid,              // Target is original sender
                                    _missionItems.count());  // Number of mission items
     _emitMavlinkMessage(responseMsg);
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
-    }
 }
 
 void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
@@ -502,8 +562,9 @@ void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
     
     mavlink_msg_mission_request_decode(&msg, &request);
     
-    if (request.target_system == _vehicleSystemId) {
-        if (request.seq < _missionItems.count()) {
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+    Q_ASSERT(request.seq < _missionItems.count());
+    
     mavlink_message_t   responseMsg;
 
     mavlink_mission_item_t item = _missionItems[request.seq];
@@ -521,13 +582,6 @@ void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
                                   item.param1, item.param2, item.param3, item.param4,
                                   item.x, item.y, item.z);
     _emitMavlinkMessage(responseMsg);
-        } else {
-            QString errMsg("MSG_ID_MISSION_REQUEST requested unknown sequence number: requested(%1) count(%2)");
-            emit error(errMsg.arg(request.seq).arg(_missionItems.count()));
-        }
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
-    }
 }
 
 void MockLink::_handleMissionItem(const mavlink_message_t& msg)
@@ -536,12 +590,10 @@ void MockLink::_handleMissionItem(const mavlink_message_t& msg)
     
     mavlink_msg_mission_item_decode(&msg, &request);
     
-    if (request.target_system == _vehicleSystemId) {
+    Q_ASSERT(request.target_system == _vehicleSystemId);
+    
     // FIXME: What do you do with duplication sequence numbers?
     Q_ASSERT(!_missionItems.contains(request.seq));
     
     _missionItems[request.seq] = request;
-    } else {
-        _errorInvalidTargetSystem(request.target_system);
-    }
 }

src/qgcunittest/MockLink.h

@@ -25,10 +25,13 @@
 #define MOCKLINK_H
 
 #include <QMap>
+#include <QLoggingCategory>
 
 #include "MockLinkMissionItemHandler.h"
 #include "LinkInterface.h"
-#include "mavlink.h"
+#include "QGCMAVLink.h"
+
+Q_DECLARE_LOGGING_CATEGORY(MockLinkLog)
 
 /// @file
 ///     @brief Mock implementation of a Link.
@@ -57,8 +60,6 @@ public:
     bool disconnect(void);
     
 signals:
-    void error(const QString& errorMsg);
-    
     /// @brief Used internally to move data to the thread.
     void _incomingBytes(const QByteArray bytes);
     
@@ -88,7 +89,6 @@ private:
     void _handleIncomingNSHBytes(const char* bytes, int cBytes);
     void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes);
     void _loadParams(void);
-    void _errorInvalidTargetSystem(int targetId);
     void _emitMavlinkMessage(const mavlink_message_t& msg);
     void _handleHeartBeat(const mavlink_message_t& msg);
     void _handleSetMode(const mavlink_message_t& msg);
@@ -98,6 +98,8 @@ private:
     void _handleMissionRequestList(const mavlink_message_t& msg);
     void _handleMissionRequest(const mavlink_message_t& msg);
     void _handleMissionItem(const mavlink_message_t& msg);
+    float _floatUnionForParam(const QString& paramName);
+    void _setParamFloatUnionIntoMap(const QString& paramName, float paramFloat);
 
     MockLinkMissionItemHandler* _missionItemHandler;
     
@@ -111,9 +113,8 @@ private:
     bool    _inNSH;
     bool    _mavlinkStarted;
     
-    typedef QMap<QString, QVariant> ParamMap_t;
-    ParamMap_t  _parameters;
-    uint16_t    _cParameters;
+    QMap<QString, QVariant>         _mapParamName2Value;
+    QMap<QString, MAV_PARAM_TYPE>   _mapParamName2MavParamType;
     
     typedef QMap<uint16_t, mavlink_mission_item_t>   MissionList_t;
     MissionList_t   _missionItems;

src/qgcunittest/MockUAS.h

@@ -81,7 +81,7 @@ public:
     virtual QString getUASName() const { Q_ASSERT(false); return _bogusString; };
     virtual const QString& getShortState() const { Q_ASSERT(false); return _bogusString; };
     virtual const QString& getShortMode() const { Q_ASSERT(false); return _bogusString; };
-    static QString getShortModeTextFor(int id) { Q_UNUSED(id); Q_ASSERT(false); return _bogusStaticString; };
+    virtual QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const { Q_UNUSED(base_mode); Q_UNUSED(custom_mode); Q_ASSERT(false); return _bogusStaticString; };
     virtual quint64 getUptime() const { Q_ASSERT(false); return 0; };
     virtual int getCommunicationStatus() const { Q_ASSERT(false); return 0; };
     virtual double getLocalX() const { Q_ASSERT(false); return std::numeric_limits<double>::quiet_NaN(); };
@@ -100,7 +100,7 @@ public:
     virtual bool isArmed() const { Q_ASSERT(false); return false; };
     virtual int getAirframe() const { Q_ASSERT(false); return 0; };
     virtual UASWaypointManager* getWaypointManager(void) { Q_ASSERT(false); return NULL; };
-    virtual QList<LinkInterface*>* getLinks() { Q_ASSERT(false); return NULL; };
+    virtual QList<LinkInterface*> getLinks() { Q_ASSERT(false); return QList<LinkInterface*>(); };
     virtual bool systemCanReverse() const { Q_ASSERT(false); return false; };
     virtual QString getSystemTypeName() { Q_ASSERT(false); return _bogusString; };
     virtual int getAutopilotType() { return MAV_AUTOPILOT_PX4; };

src/qgcunittest/MockUASManager.h

@@ -106,6 +106,7 @@ public slots:
         { Q_ASSERT(false); Q_UNUSED(uav); Q_UNUSED(lat); Q_UNUSED(lon); Q_UNUSED(alt); }
     virtual void loadSettings() { Q_ASSERT(false); }
     virtual void storeSettings() { Q_ASSERT(false); }
+    virtual void _shutdown(void) { Q_ASSERT(false); }
     
 private:
     MockUAS*                _mockUAS;

src/qgcunittest/UASUnitTest.cc

 #include "UASUnitTest.h"
 #include <stdio.h>
 #include <QObject>
-#include <QThread>
 
 UT_REGISTER_TEST(UASUnitTest)
 
@@ -14,7 +13,7 @@ void UASUnitTest::init()
     UnitTest::init();
     
     _mavlink = new MAVLinkProtocol();
-    _uas = new UAS(_mavlink, QThread::currentThread(), UASID);
+    _uas = new UAS(_mavlink, UASID);
     _uas->deleteSettings();
 }
 //this function is called after every test
@@ -32,7 +31,7 @@ void UASUnitTest::cleanup()
 void UASUnitTest::getUASID_test()
 {
     // Test a default ID of zero is assigned
-    UAS* uas2 = new UAS(_mavlink, QThread::currentThread());
+    UAS* uas2 = new UAS(_mavlink);
     QCOMPARE(uas2->getUASID(), 0);
     delete uas2;
 
@@ -57,7 +56,7 @@ void UASUnitTest::getUASName_test()
 
 void UASUnitTest::getUpTime_test()
 {
-    UAS* uas2 = new UAS(_mavlink, QThread::currentThread());
+    UAS* uas2 = new UAS(_mavlink);
     // Test that the uptime starts at zero to a
     // precision of seconds
     QCOMPARE(floor(uas2->getUptime()/1000.0), 0.0);
@@ -289,7 +288,7 @@ void UASUnitTest::signalWayPoint_test()
     delete _uas;// delete(destroyed) _uas for validating
     _uas = NULL;
     QCOMPARE(spyDestroyed.count(), 1);// count destroyed _uas should are 1
-    _uas = new UAS(_mavlink, QThread::currentThread(), UASID);
+    _uas = new UAS(_mavlink, UASID);
     QSignalSpy spy2(_uas->getWaypointManager(), SIGNAL(waypointEditableListChanged()));
     QCOMPARE(spy2.count(), 0);
     Waypoint* wp2 = new Waypoint(0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,false, false, MAV_FRAME_GLOBAL, MAV_CMD_MISSION_START, "blah");

src/qgcunittest/UnitTest.cc

@@ -81,13 +81,15 @@ QList<QObject*>& UnitTest::_testList(void)
 	return tests;
 }
 
-int UnitTest::run(int argc, char *argv[], QString& singleTest)
+int UnitTest::run(QString& singleTest)
 {
     int ret = 0;
     
     foreach (QObject* test, _testList()) {
         if (singleTest.isEmpty() || singleTest == test->objectName()) {
-            ret += QTest::qExec(test, argc, argv);
+            QStringList args;
+            args << "*" << "-maxwarnings" << "0";
+            ret += QTest::qExec(test, args);
         }
     }
     

src/qgcunittest/UnitTest.h

@@ -49,10 +49,8 @@ public:
     virtual ~UnitTest(void);
     
     /// @brief Called to run all the registered unit tests
-    ///     @param argc argc from main
-    ///     @param argv argv from main
     ///     @param singleTest Name of test to just run a single test
-    static int run(int argc, char *argv[], QString& singleTest);
+    static int run(QString& singleTest);
     
     /// @brief Sets up for an expected QGCMessageBox
     ///     @param response Response to take on message box
@@ -103,18 +101,6 @@ protected slots:
     virtual void cleanup(void);
     
 protected:
-    /// @brief Must be called first by derived class implementation
-    void _initTestCase(void);
-    
-    /// @brief Must be called first by derived class implementation
-    void _cleanupTestCase(void);
-    
-    /// @brief Must be called first by derived class implementation
-    void _init(void);
-    
-    /// @brief Must be called first by derived class implementation
-    void _cleanup(void);
-    
     bool _expectMissedFileDialog;   // true: expect a missed file dialog, used for internal testing
     bool _expectMissedMessageBox;   // true: expect a missed message box, used for internal testing
     

src/uas/QGCMAVLinkUASFactory.cc

 #include "QGCMAVLinkUASFactory.h"
 #include "UASManager.h"
-#include "QGCUASWorker.h"
 #include "QGXPX4UAS.h"
 
 QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) :
@@ -23,13 +22,11 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
 
     UASInterface* uas;
 
-    QGCUASWorker* worker = new QGCUASWorker();
-
     switch (heartbeat->autopilot)
     {
     case MAV_AUTOPILOT_GENERIC:
     {
-        UAS* mav = new UAS(mavlink, worker, sysid);
+        UAS* mav = new UAS(mavlink, sysid);
         // Set the system type
         mav->setSystemType((int)heartbeat->type);
 
@@ -41,7 +38,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
     break;
     case MAV_AUTOPILOT_PX4:
     {
-        QGXPX4UAS* px4 = new QGXPX4UAS(mavlink, worker, sysid);
+        QGXPX4UAS* px4 = new QGXPX4UAS(mavlink, sysid);
         // Set the system type
         px4->setSystemType((int)heartbeat->type);
 
@@ -55,7 +52,7 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
     break;
     default:
     {
-        UAS* mav = new UAS(mavlink, worker, sysid);
+        UAS* mav = new UAS(mavlink, sysid);
         mav->setSystemType((int)heartbeat->type);
 
         // Connect this robot to the UAS object
@@ -68,10 +65,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
     break;
     }
 
-    // Get the UAS ready
-    worker->start(QThread::HighPriority);
-    connect(uas, SIGNAL(destroyed()), worker, SLOT(quit()));
-
     // Set the autopilot type
     uas->setAutopilotType((int)heartbeat->autopilot);
 

src/uas/QGCUASFileManager.cc

@@ -481,7 +481,7 @@ void QGCUASFileManager::_sendRequest(Request* request)
     request->hdr.seqNumber = _lastOutgoingSeqNumber;
     
     if (_systemIdQGC == 0) {
-        _systemIdQGC = LinkManager::instance()->mavlink()->getSystemId();
+        _systemIdQGC = MAVLinkProtocol::instance()->getSystemId();
     }
     
     Q_ASSERT(_mav);

src/uas/QGCUASParamManager.cc

@@ -217,7 +217,7 @@ void QGCUASParamManager::requestRcCalibrationParamsUpdate() {
 
 void QGCUASParamManager::_parameterListUpToDate(void)
 {
-    qDebug() << "Emitting parameters ready, count:" << paramDataModel.countOnboardParams();
+    //qDebug() << "Emitting parameters ready, count:" << paramDataModel.countOnboardParams();
 
     _parametersReady = true;
     emit parameterListUpToDate();

src/uas/QGCUASWorker.cc

-#include "QGCUASWorker.h"
-
-#include <QGC.h>
-#include <QCoreApplication>
-#include <QDebug>
-
-QGCUASWorker::QGCUASWorker() : QThread(),
-    _should_exit(false)
-{
-}
-
-void QGCUASWorker::quit()
-{
-    _should_exit = true;
-}
-
-void QGCUASWorker::run()
-{
-    while(!_should_exit) {
-
-        QCoreApplication::processEvents();
-        QGC::SLEEP::msleep(2);
-    }
-}

src/uas/QGCUASWorker.h

-#ifndef QGCUASWORKER_H
-#define QGCUASWORKER_H
-
-#include <QThread>
-
-class QGCUASWorker : public QThread
-{
-public:
-    QGCUASWorker();
-
-public slots:
-    void quit();
-
-protected:
-    void run();
-
-    bool _should_exit;
-};
-
-#endif // QGCUASWORKER_H

src/uas/QGXPX4UAS.cc

 #include "QGXPX4UAS.h"
 
-QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id) :
-    UAS(mavlink, thread, id)
+QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, int id) :
+    UAS(mavlink, id)
 {
 }