diff --git a/files/qml/FactTextInput.qml b/files/qml/FactTextInput.qml
index 560ff0d6a5aa8a00222240bd9e961f7b87159034..41212c0824aedb46d745a93782988778405744df 100644
--- a/files/qml/FactTextInput.qml
+++ b/files/qml/FactTextInput.qml
@@ -1,4 +1,4 @@
-import QtQuick 2.0
+import QtQuick 2.2
import QtQuick.Controls 1.2
import QGroundControl.FactSystem 1.0
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 20ab90558b941c270e50a7879e58148d2bb477d4..846c8945c9e73aa80b2ec921b64eeefca742b619 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -484,7 +484,6 @@ HEADERS += \
src/uas/QGCUASParamManagerInterface.h \
src/uas/QGCUASFileManager.h \
src/ui/QGCUASFileView.h \
- src/uas/QGCUASWorker.h \
src/CmdLineOptParser.h \
src/uas/QGXPX4UAS.h \
src/QGCFileDialog.h \
@@ -625,7 +624,6 @@ SOURCES += \
src/ui/menuactionhelper.cpp \
src/uas/QGCUASFileManager.cc \
src/ui/QGCUASFileView.cc \
- src/uas/QGCUASWorker.cc \
src/CmdLineOptParser.cc \
src/uas/QGXPX4UAS.cc \
src/QGCFileDialog.cc \
@@ -670,7 +668,8 @@ HEADERS += \
src/qgcunittest/LinkManagerTest.h \
src/qgcunittest/MainWindowTest.h \
src/AutoPilotPlugins/PX4/Tests/FlightModeConfigTest.h \
- src/qgcunittest/MavlinkLogTest.h
+ src/qgcunittest/MavlinkLogTest.h \
+ src/FactSystem/FactSystemTest.h
SOURCES += \
src/qgcunittest/UnitTest.cc \
@@ -691,7 +690,8 @@ SOURCES += \
src/qgcunittest/LinkManagerTest.cc \
src/qgcunittest/MainWindowTest.cc \
src/AutoPilotPlugins/PX4/Tests/FlightModeConfigTest.cc \
- src/qgcunittest/MavlinkLogTest.cc
+ src/qgcunittest/MavlinkLogTest.cc \
+ src/FactSystem/FactSystemTest.cc
}
#
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 30f19bf6272d4f8d639b8d7a6b643d33a6f9a174..e86f2173bbcd5990b3c5e417a1f0af4651eed85e 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -230,6 +230,7 @@
src/qgcunittest/MockLink.param
+ src/FactSystem/FactSystemTest.qml
files/QLoggingCategory/qtlogging.ini
diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h
index 9fe0bb9bf6d9e55ba4ad744fe66bb308c311d553..377bcc7c7566b014673e40c87e99fca0da63e275 100644
--- a/src/AutoPilotPlugins/AutoPilotPlugin.h
+++ b/src/AutoPilotPlugins/AutoPilotPlugin.h
@@ -21,6 +21,9 @@
======================================================================*/
+/// @file
+/// @author Don Gagne
+
#ifndef AUTOPILOTPLUGIN_H
#define AUTOPILOTPLUGIN_H
@@ -33,12 +36,12 @@
#include "VehicleComponent.h"
#include "FactSystem.h"
-/// @file
-/// @brief The AutoPilotPlugin class is an abstract base class which represent the methods and objects
-/// which are specific to a certain AutoPilot. This is the only place where AutoPilot specific
-/// code should reside in QGroundControl. The remainder of the QGroundControl source is
-/// generic to a common mavlink implementation.
-/// @author Don Gagne
+/// This is the base class for AutoPilot plugins
+///
+/// The AutoPilotPlugin class is an abstract base class which represent the methods and objects
+/// which are specific to a certain AutoPilot. This is the only place where AutoPilot specific
+/// code should reside in QGroundControl. The remainder of the QGroundControl source is
+/// generic to a common mavlink implementation.
class AutoPilotPlugin : public QObject
{
@@ -46,21 +49,28 @@ class AutoPilotPlugin : public QObject
public:
/// @brief Returns the list of VehicleComponent objects associated with the AutoPilot.
- virtual QList getVehicleComponents(UASInterface* uas) const = 0;
+ virtual QList getVehicleComponents(void) const = 0;
+
+ /// Returns the parameter facts for the specified UAS.
+ ///
+ /// Access to parameter properties is done through QObject::property or the full
+ /// QMetaObject methods. The property name is the parameter name. You should not
+ /// request parameter facts until the plugin reports that it is ready.
+ virtual QObject* parameterFacts(void) const = 0;
- typedef struct {
- uint8_t baseMode;
- uint32_t customMode;
- } FullMode_t;
+ /// Adds the FactSystem properties to the Qml context. You should not call
+ /// this method until the plugin reports that it is ready.
+ virtual void addFactsToQmlContext(QQmlContext* context) const = 0;
- /// @brief Returns the list of modes which are available for this AutoPilot.
- virtual QList getModes(void) const = 0;
+ /// Returns true if the plugin is ready for use
+ virtual bool pluginIsReady(void) const = 0;
- /// @brief Returns a human readable short description for the specified mode.
- virtual QString getShortModeText(uint8_t baseMode, uint32_t customMode) const = 0;
+ /// FIXME: Kind of hacky
+ static void clearStaticData(void);
- /// @brief Adds the FactSystem properties associated with this AutoPilot to the Qml context.
- virtual void addFactsToQmlContext(QQmlContext* context, UASInterface* uas) const = 0;
+signals:
+ /// Signalled when plugin is ready for use
+ void pluginReady(void);
protected:
// All access to AutoPilotPugin objects is through getInstanceForAutoPilotPlugin
diff --git a/src/AutoPilotPlugins/AutoPilotPluginManager.cc b/src/AutoPilotPlugins/AutoPilotPluginManager.cc
index ac4de4f928630058679039cc1126cd2d2c4208b6..c2a2206cdb22588f6bdfe51e6056cdd302be8adc 100644
--- a/src/AutoPilotPlugins/AutoPilotPluginManager.cc
+++ b/src/AutoPilotPlugins/AutoPilotPluginManager.cc
@@ -28,29 +28,109 @@
#include "PX4/PX4AutoPilotPlugin.h"
#include "Generic/GenericAutoPilotPlugin.h"
#include "QGCApplication.h"
+#include "UASManager.h"
IMPLEMENT_QGC_SINGLETON(AutoPilotPluginManager, AutoPilotPluginManager)
AutoPilotPluginManager::AutoPilotPluginManager(QObject* parent) :
QGCSingleton(parent)
{
- // All plugins are constructed here so that they end up on the correct thread
- _pluginMap[MAV_AUTOPILOT_PX4] = new PX4AutoPilotPlugin(this);
- Q_ASSERT(_pluginMap.contains(MAV_AUTOPILOT_PX4));
+ UASManagerInterface* uasMgr = UASManager::instance();
+ Q_ASSERT(uasMgr);
+
+ // We need to track uas coming and going so that we can instantiate plugins for each uas
+ connect(uasMgr, &UASManagerInterface::UASCreated, this, &AutoPilotPluginManager::_uasCreated);
+ connect(uasMgr, &UASManagerInterface::UASDeleted, this, &AutoPilotPluginManager::_uasDeleted);
+}
- _pluginMap[MAV_AUTOPILOT_GENERIC] = new GenericAutoPilotPlugin(this);
- Q_ASSERT(_pluginMap.contains(MAV_AUTOPILOT_GENERIC));
+AutoPilotPluginManager::~AutoPilotPluginManager()
+{
+#ifdef QT_DEBUG
+ foreach(MAV_AUTOPILOT mavType, _pluginMap.keys()) {
+ Q_ASSERT_X(_pluginMap[mavType].count() == 0, "AutoPilotPluginManager", "LinkManager::_shutdown should have already closed all uas");
+ }
+#endif
+ _pluginMap.clear();
+
+ PX4AutoPilotPlugin::clearStaticData();
+ GenericAutoPilotPlugin::clearStaticData();
+}
+
+/// Create the plugin for this uas
+void AutoPilotPluginManager::_uasCreated(UASInterface* uas)
+{
+ Q_ASSERT(uas);
+
+ MAV_AUTOPILOT autopilotType = static_cast(uas->getAutopilotType());
+ int uasId = uas->getUASID();
+ Q_ASSERT(uasId != 0);
+
+ if (_pluginMap.contains(autopilotType)) {
+ Q_ASSERT_X(!_pluginMap[autopilotType].contains(uasId), "AutoPilotPluginManager", "Either we have duplicate UAS ids, or a UAS was not removed correctly.");
+ }
+
+ AutoPilotPlugin* plugin;
+ switch (autopilotType) {
+ case MAV_AUTOPILOT_PX4:
+ plugin = new PX4AutoPilotPlugin(uas, this);
+ Q_CHECK_PTR(plugin);
+ _pluginMap[MAV_AUTOPILOT_PX4][uasId] = plugin;
+ break;
+ case MAV_AUTOPILOT_GENERIC:
+ default:
+ plugin = new GenericAutoPilotPlugin(uas, this);
+ Q_CHECK_PTR(plugin);
+ _pluginMap[MAV_AUTOPILOT_GENERIC][uasId] = plugin;
+ }
+}
+
+/// Destroy the plugin associated with this uas
+void AutoPilotPluginManager::_uasDeleted(UASInterface* uas)
+{
+ Q_ASSERT(uas);
+
+ MAV_AUTOPILOT autopilotType = static_cast(uas->getAutopilotType());
+ int uasId = uas->getUASID();
+ Q_ASSERT(uasId != 0);
+
+ Q_ASSERT(_pluginMap.contains(autopilotType));
+ Q_ASSERT(_pluginMap[autopilotType].contains(uasId));
+ delete _pluginMap[autopilotType][uasId];
+ _pluginMap[autopilotType].remove(uasId);
+}
+
+AutoPilotPlugin* AutoPilotPluginManager::getInstanceForAutoPilotPlugin(UASInterface* uas)
+{
+ Q_ASSERT(uas);
+
+ MAV_AUTOPILOT autopilotType = static_cast(uas->getAutopilotType());
+ int uasId = uas->getUASID();
+ Q_ASSERT(uasId != 0);
+
+ Q_ASSERT(_pluginMap.contains(autopilotType));
+ Q_ASSERT(_pluginMap[autopilotType].contains(uasId));
+
+ return _pluginMap[autopilotType][uasId];
+}
+
+QList AutoPilotPluginManager::getModes(int autopilotType) const
+{
+ switch (autopilotType) {
+ case MAV_AUTOPILOT_PX4:
+ return PX4AutoPilotPlugin::getModes();
+ case MAV_AUTOPILOT_GENERIC:
+ default:
+ return GenericAutoPilotPlugin::getModes();
+ }
}
-AutoPilotPlugin* AutoPilotPluginManager::getInstanceForAutoPilotPlugin(int autopilotType)
+QString AutoPilotPluginManager::getShortModeText(uint8_t baseMode, uint32_t customMode, int autopilotType) const
{
switch (autopilotType) {
case MAV_AUTOPILOT_PX4:
- Q_ASSERT(_pluginMap.contains(MAV_AUTOPILOT_PX4));
- return _pluginMap[MAV_AUTOPILOT_PX4];
-
+ return PX4AutoPilotPlugin::getShortModeText(baseMode, customMode);
+ case MAV_AUTOPILOT_GENERIC:
default:
- Q_ASSERT(_pluginMap.contains(MAV_AUTOPILOT_GENERIC));
- return _pluginMap[MAV_AUTOPILOT_GENERIC];
+ return GenericAutoPilotPlugin::getShortModeText(baseMode, customMode);
}
}
diff --git a/src/AutoPilotPlugins/AutoPilotPluginManager.h b/src/AutoPilotPlugins/AutoPilotPluginManager.h
index b3b242e1f2335000cb4f482cf62810fb111d0a24..c1417c24b7feb48ea7e05bc4597969603e77e3cd 100644
--- a/src/AutoPilotPlugins/AutoPilotPluginManager.h
+++ b/src/AutoPilotPlugins/AutoPilotPluginManager.h
@@ -21,6 +21,9 @@
======================================================================*/
+/// @file
+/// @author Don Gagne
+
#ifndef AUTOPILOTPLUGINMANAGER_H
#define AUTOPILOTPLUGINMANAGER_H
@@ -32,11 +35,10 @@
#include "VehicleComponent.h"
#include "AutoPilotPlugin.h"
#include "QGCSingleton.h"
+#include "QGCMAVLink.h"
-/// @file
-/// @brief The AutoPilotPlugin manager is a singleton which maintains the list of AutoPilotPlugin objects.
-///
-/// @author Don Gagne
+
+/// AutoPilotPlugin manager is a singleton which maintains the list of AutoPilotPlugin objects.
class AutoPilotPluginManager : public QGCSingleton
{
@@ -45,15 +47,31 @@ class AutoPilotPluginManager : public QGCSingleton
DECLARE_QGC_SINGLETON(AutoPilotPluginManager, AutoPilotPluginManager)
public:
- /// @brief Returns the singleton AutoPilot instance for the specified auto pilot type.
- /// @param autopilotType Specified using the MAV_AUTOPILOT_* values.
- AutoPilotPlugin* getInstanceForAutoPilotPlugin(int autopilotType);
+ /// Returns the singleton AutoPilotPlugin instance for the specified uas.
+ /// @param uas Uas to get plugin for
+ AutoPilotPlugin* getInstanceForAutoPilotPlugin(UASInterface* uas);
+
+ typedef struct {
+ uint8_t baseMode;
+ uint32_t customMode;
+ } FullMode_t;
+
+ /// Returns the list of modes which are available for the specified autopilot type.
+ QList getModes(int autopilotType) const;
+ /// @brief Returns a human readable short description for the specified mode.
+ QString getShortModeText(uint8_t baseMode, uint32_t customMode, int autopilotType) const;
+
+private slots:
+ void _uasCreated(UASInterface* uas);
+ void _uasDeleted(UASInterface* uas);
+
private:
/// All access to singleton is through AutoPilotPluginManager::instance
AutoPilotPluginManager(QObject* parent = NULL);
+ ~AutoPilotPluginManager();
- QMap _pluginMap;
+ QMap > _pluginMap; ///< Map of AutoPilot plugins _pluginMap[MAV_TYPE][UASid]
};
#endif
diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc
index 5170edc0c3cfa40c83a23a8c3ed9dc17b4aa2166..d61e4c8529030928e0b99c7774c336a1e78d5588 100644
--- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc
+++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc
@@ -26,24 +26,22 @@
#include "GenericAutoPilotPlugin.h"
-GenericAutoPilotPlugin::GenericAutoPilotPlugin(QObject* parent) :
+GenericAutoPilotPlugin::GenericAutoPilotPlugin(UASInterface* uas, QObject* parent) :
AutoPilotPlugin(parent)
{
-
+ Q_UNUSED(uas);
}
-QList GenericAutoPilotPlugin::getVehicleComponents(UASInterface* uas) const
+QList GenericAutoPilotPlugin::getVehicleComponents(void) const
{
- Q_UNUSED(uas);
-
// Generic autopilot has no configurable components
return QList();
}
-QList GenericAutoPilotPlugin::getModes(void) const
+QList GenericAutoPilotPlugin::getModes(void)
{
- QList modeList;
- FullMode_t fullMode;
+ AutoPilotPluginManager::FullMode_t fullMode;
+ QList modeList;
fullMode.customMode = 0;
@@ -62,7 +60,7 @@ QList GenericAutoPilotPlugin::getModes(void) const
return modeList;
}
-QString GenericAutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMode) const
+QString GenericAutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMode)
{
Q_UNUSED(customMode);
@@ -85,11 +83,20 @@ QString GenericAutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t cust
return mode;
}
-void GenericAutoPilotPlugin::addFactsToQmlContext(QQmlContext* context, UASInterface* uas) const
+void GenericAutoPilotPlugin::addFactsToQmlContext(QQmlContext* context) const
{
Q_UNUSED(context);
- Q_UNUSED(uas);
// Qml not yet supported for Generic
Q_ASSERT(false);
}
+
+QObject* GenericAutoPilotPlugin::parameterFacts(void) const
+{
+ return NULL;
+}
+
+void GenericAutoPilotPlugin::clearStaticData(void)
+{
+ // No Static data yet
+}
diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h
index e1513739508c42118d1df9b3178713952b5ca24e..4d94bbb0c1a4f77a4b76e998f710124003f69b45 100644
--- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h
+++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h
@@ -25,6 +25,7 @@
#define GENERICAUTOPILOT_H
#include "AutoPilotPlugin.h"
+#include "AutoPilotPluginManager.h"
/// @file
/// @brief This is the generic implementation of the AutoPilotPlugin class for mavs
@@ -36,13 +37,17 @@ class GenericAutoPilotPlugin : public AutoPilotPlugin
Q_OBJECT
public:
- GenericAutoPilotPlugin(QObject* parent = NULL);
+ GenericAutoPilotPlugin(UASInterface* uas, QObject* parent = NULL);
// Overrides from AutoPilotPlugin
- virtual QList getVehicleComponents(UASInterface* uas) const ;
- virtual QList getModes(void) const;
- virtual QString getShortModeText(uint8_t baseMode, uint32_t customMode) const;
- virtual void addFactsToQmlContext(QQmlContext* context, UASInterface* uas) const;
+ virtual QList getVehicleComponents(void) const ;
+ virtual void addFactsToQmlContext(QQmlContext* context) const;
+ virtual QObject* parameterFacts(void) const;
+ virtual bool pluginIsReady(void) const { return true; }
+
+ static QList getModes(void);
+ static QString getShortModeText(uint8_t baseMode, uint32_t customMode);
+ static void clearStaticData(void);
};
#endif
diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc
index b8dd70cefc8e4bce6964b784f3461bf86821acc8..d1834e68237c227541c78c79046cbdecdf3a9633 100644
--- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc
+++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc
@@ -65,59 +65,59 @@ union px4_custom_mode {
float data_float;
};
-PX4AutoPilotPlugin::PX4AutoPilotPlugin(QObject* parent) :
- AutoPilotPlugin(parent)
+PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) :
+ AutoPilotPlugin(parent),
+ _uas(uas),
+ _parameterFacts(NULL)
{
- UASManagerInterface* uasMgr = UASManager::instance();
- Q_ASSERT(uasMgr);
+ Q_ASSERT(uas);
+
+ _parameterFacts = new PX4ParameterFacts(uas, this);
+ Q_CHECK_PTR(_parameterFacts);
- // We need to track uas coming and going so that we can create PX4ParameterFacts instances for each uas
- connect(uasMgr, &UASManagerInterface::UASCreated, this, &PX4AutoPilotPlugin::_uasCreated);
- connect(uasMgr, &UASManagerInterface::UASDeleted, this, &PX4AutoPilotPlugin::_uasDeleted);
+ connect(_parameterFacts, &PX4ParameterFacts::factsReady, this, &PX4AutoPilotPlugin::pluginReady);
PX4ParameterFacts::loadParameterFactMetaData();
}
PX4AutoPilotPlugin::~PX4AutoPilotPlugin()
{
+ delete _parameterFacts;
PX4ParameterFacts::deleteParameterFactMetaData();
-
- foreach(UASInterface* uas, _mapUas2ParameterFacts.keys()) {
- delete _mapUas2ParameterFacts[uas];
- }
- _mapUas2ParameterFacts.clear();
}
-QList PX4AutoPilotPlugin::getVehicleComponents(UASInterface* uas) const
+QList PX4AutoPilotPlugin::getVehicleComponents(void) const
{
+ Q_ASSERT(_uas);
+
QList components;
VehicleComponent* component;
- component = new AirframeComponent(uas);
+ component = new AirframeComponent(_uas);
Q_CHECK_PTR(component);
components.append(component);
- component = new RadioComponent(uas);
+ component = new RadioComponent(_uas);
Q_CHECK_PTR(component);
components.append(component);
- component = new FlightModesComponent(uas);
+ component = new FlightModesComponent(_uas);
Q_CHECK_PTR(component);
components.append(component);
- component = new SensorsComponent(uas);
+ component = new SensorsComponent(_uas);
Q_CHECK_PTR(component);
components.append(component);
return components;
}
-QList PX4AutoPilotPlugin::getModes(void) const
+QList PX4AutoPilotPlugin::getModes(void)
{
- QList modeList;
- FullMode_t fullMode;
- union px4_custom_mode px4_cm;
+ union px4_custom_mode px4_cm;
+ AutoPilotPluginManager::FullMode_t fullMode;
+ QList modeList;
px4_cm.data = 0;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
@@ -152,7 +152,7 @@ QList PX4AutoPilotPlugin::getModes(void) const
return modeList;
}
-QString PX4AutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMode) const
+QString PX4AutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMode)
{
QString mode;
@@ -187,51 +187,40 @@ QString PX4AutoPilotPlugin::getShortModeText(uint8_t baseMode, uint32_t customMo
mode = "|OFFBOARD";
}
} else {
- mode = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(MAV_AUTOPILOT_GENERIC)->getShortModeText(baseMode, customMode);
+ // use base_mode - not autopilot-specific
+ if (baseMode == 0) {
+ mode = "|PREFLIGHT";
+ } else if (baseMode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) {
+ mode = "|AUTO";
+ } else if (baseMode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) {
+ mode = "|MANUAL";
+ if (baseMode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) {
+ mode += "|GUIDED";
+ } else if (baseMode & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) {
+ mode += "|STABILIZED";
+ }
+ }
+
}
return mode;
}
-void PX4AutoPilotPlugin::addFactsToQmlContext(QQmlContext* context, UASInterface* uas) const
+void PX4AutoPilotPlugin::addFactsToQmlContext(QQmlContext* context) const
{
Q_ASSERT(context);
- Q_ASSERT(uas);
-
- QGCUASParamManagerInterface* paramMgr = uas->getParamManager();
- Q_UNUSED(paramMgr);
- Q_ASSERT(paramMgr);
- Q_ASSERT(paramMgr->parametersReady());
- PX4ParameterFacts* facts = _parameterFactsForUas(uas);
- Q_ASSERT(facts);
+ Q_ASSERT(_parameterFacts->factsAreReady());
- context->setContextProperty("parameterFacts", facts);
+ context->setContextProperty("parameterFacts", _parameterFacts);
}
-/// @brief When a new uas is create we add a new set of parameter facts for it
-void PX4AutoPilotPlugin::_uasCreated(UASInterface* uas)
+void PX4AutoPilotPlugin::clearStaticData(void)
{
- Q_ASSERT(uas);
- Q_ASSERT(!_mapUas2ParameterFacts.contains(uas));
-
- // Each uas has it's own set of parameter facts
- PX4ParameterFacts* facts = new PX4ParameterFacts(uas, this);
- Q_CHECK_PTR(facts);
- _mapUas2ParameterFacts[uas] = facts;
+ PX4ParameterFacts::clearStaticData();
}
-/// @brief When the uas is deleted we remove the parameter facts for it from the system
-void PX4AutoPilotPlugin::_uasDeleted(UASInterface* uas)
+bool PX4AutoPilotPlugin::pluginIsReady(void) const
{
- delete _parameterFactsForUas(uas);
- _mapUas2ParameterFacts.remove(uas);
-}
-
-PX4ParameterFacts* PX4AutoPilotPlugin::_parameterFactsForUas(UASInterface* uas) const
-{
- Q_ASSERT(uas);
- Q_ASSERT(_mapUas2ParameterFacts.contains(uas));
-
- return _mapUas2ParameterFacts[uas];
+ return _parameterFacts->factsAreReady();
}
diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h
index 1b9a0674d87bc8947a6e6ff1684ee0e059e253a6..a0aa6611cdcda3e37eaa06fe1b94c8d04b83523a 100644
--- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h
+++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h
@@ -25,6 +25,7 @@
#define PX4AUTOPILOT_H
#include "AutoPilotPlugin.h"
+#include "AutoPilotPluginManager.h"
#include "UASInterface.h"
#include "PX4ParameterFacts.h"
@@ -37,23 +38,23 @@ class PX4AutoPilotPlugin : public AutoPilotPlugin
Q_OBJECT
public:
- PX4AutoPilotPlugin(QObject* parent);
+ PX4AutoPilotPlugin(UASInterface* uas, QObject* parent);
~PX4AutoPilotPlugin();
// Overrides from AutoPilotPlugin
- virtual QList getVehicleComponents(UASInterface* uas) const ;
- virtual QList getModes(void) const;
- virtual QString getShortModeText(uint8_t baseMode, uint32_t customMode) const;
- virtual void addFactsToQmlContext(QQmlContext* context, UASInterface* uas) const;
-
-private slots:
- void _uasCreated(UASInterface* uas);
- void _uasDeleted(UASInterface* uas);
+ virtual QList getVehicleComponents(void) const ;
+ virtual void addFactsToQmlContext(QQmlContext* context) const;
+ virtual QObject* parameterFacts(void) const { return _parameterFacts; }
+ virtual bool pluginIsReady(void) const;
+
+ static QList getModes(void);
+ static QString getShortModeText(uint8_t baseMode, uint32_t customMode);
+ static void clearStaticData(void);
private:
- PX4ParameterFacts* _parameterFactsForUas(UASInterface* uas) const;
-
- QMap _mapUas2ParameterFacts;
+ UASInterface* _uas;
+ PX4ParameterFacts* _parameterFacts;
+ bool _pluginReady;
};
#endif
diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc
index fe9b0233d3dbf470b124207fcfb3c15bb1fda615..d2c1971f5590b3e575321bb5c6a7f60fc761b01b 100644
--- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc
+++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc
@@ -25,6 +25,7 @@
/// @author Don Gagne
#include "PX4ParameterFacts.h"
+#include "QGCApplication.h"
#include
#include
@@ -32,12 +33,14 @@
Q_LOGGING_CATEGORY(PX4ParameterFactsLog, "PX4ParameterFactsLog")
Q_LOGGING_CATEGORY(PX4ParameterFactsMetaDataLog, "PX4ParameterFactsMetaDataLog")
+bool PX4ParameterFacts::_parameterMetaDataLoaded = false;
QMap PX4ParameterFacts::_mapParameterName2FactMetaData;
PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) :
QObject(parent),
_lastSeenComponent(-1),
- _paramMgr(NULL)
+ _paramMgr(NULL),
+ _factsReady(false)
{
Q_ASSERT(uas);
@@ -46,11 +49,14 @@ PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) :
_paramMgr = uas->getParamManager();
Q_ASSERT(_paramMgr);
+ // We need to be initialized before param mgr starts sending parameters so we catch each one
+ Q_ASSERT(!_paramMgr->parametersReady());
+
+ // We need to know when the param mgr is done sending the initial set of paramters
+ connect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate()));
+
// UASInterface::parameterChanged has multiple overrides so we need to use SIGNAL/SLOT style connect
connect(uas, SIGNAL(parameterChanged(int, int, QString, QVariant)), this, SLOT(_parameterChanged(int, int, QString, QVariant)));
-
- // Fact meta data should already be loaded
- Q_ASSERT(_mapParameterName2FactMetaData.count() != 0);
}
PX4ParameterFacts::~PX4ParameterFacts()
@@ -90,7 +96,7 @@ void PX4ParameterFacts::_parameterChanged(int uas, int component, QString parame
// If we don't have meta data for the parameter it can't be part of the FactSystem
if (!_mapParameterName2FactMetaData.contains(parameterName)) {
// FIXME: Debug or Warning. Warning will fail TC
- qCDebug(PX4ParameterFactsLog) << "FactSystem meta data out of date. Missing parameter:" << parameterName;
+ qDebug() << "FactSystem meta data out of date. Missing parameter:" << parameterName;
return;
}
@@ -99,19 +105,22 @@ void PX4ParameterFacts::_parameterChanged(int uas, int component, QString parame
fact->setMetaData(_mapParameterName2FactMetaData[parameterName]);
- // We need to know when the fact changes so that we can send the new value to the parameter managers
- connect(fact, &Fact::valueUpdated, this, &PX4ParameterFacts::_valueUpdated);
-
_mapParameterName2Fact[parameterName] = fact;
_mapFact2ParameterName[fact] = parameterName;
+
+ // We need to know when the fact changes so that we can send the new value to the parameter manager
+ connect(fact, &Fact::_containerValueChanged, this, &PX4ParameterFacts::_valueUpdated);
+
+ //qDebug() << "Adding new fact" << parameterName;
}
- _mapParameterName2Fact[parameterName]->updateValue(value);
+ //qDebug() << "Updating fact value" << parameterName << value;
+ _mapParameterName2Fact[parameterName]->_containerSetValue(value);
}
/// Connected to Fact::valueUpdated
///
/// Sets the new value into the Parameter Manager. Paramter is persisted after send.
-void PX4ParameterFacts::_valueUpdated(QVariant& value)
+void PX4ParameterFacts::_valueUpdated(QVariant value)
{
Fact* fact = qobject_cast(sender());
Q_ASSERT(fact);
@@ -285,6 +294,11 @@ QVariant PX4ParameterFacts::_stringToTypedVariant(const QString& string, FactMet
/// The meta data comes from firmware parameters.xml file.
void PX4ParameterFacts::loadParameterFactMetaData(void)
{
+ if (_parameterMetaDataLoaded) {
+ return;
+ }
+ _parameterMetaDataLoaded = true;
+
qCDebug(PX4ParameterFactsMetaDataLog) << "Loading PX4 parameter fact meta data";
Q_ASSERT(_mapParameterName2FactMetaData.count() == 0);
@@ -347,3 +361,29 @@ void PX4ParameterFacts::loadParameterFactMetaData(void)
xml.readNext();
}
}
+
+// Called when param mgr list is up to date
+void PX4ParameterFacts::_paramMgrParameterListUpToDate(void)
+{
+ if (!_factsReady) {
+ _factsReady = true;
+
+ // We don't need this any more
+ disconnect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate()));
+
+ // There may be parameterUpdated signals still in our queue. Flush them out.
+ qgcApp()->processEvents();
+
+ // We should have all paramters now so we can signal ready
+ emit factsReady();
+ }
+}
+
+void PX4ParameterFacts::clearStaticData(void)
+{
+ foreach(QString parameterName, _mapParameterName2FactMetaData.keys()) {
+ delete _mapParameterName2FactMetaData[parameterName];
+ }
+ _mapParameterName2FactMetaData.clear();
+ _parameterMetaDataLoaded = false;
+}
\ No newline at end of file
diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h
index 67deb983a69d7c58415d65bbc0f76e93404d3f9d..508b08d298854379eef7503173b88bfe639e02c1 100644
--- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h
+++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h
@@ -487,6 +487,7 @@ class PX4ParameterFacts : public QObject
Q_PROPERTY(Fact* UAVCAN_BITRATE READ getUAVCAN_BITRATE CONSTANT) Fact* getUAVCAN_BITRATE(void) { return _mapParameterName2Fact["UAVCAN_BITRATE"]; }
Q_PROPERTY(Fact* UAVCAN_ENABLE READ getUAVCAN_ENABLE CONSTANT) Fact* getUAVCAN_ENABLE(void) { return _mapParameterName2Fact["UAVCAN_ENABLE"]; }
Q_PROPERTY(Fact* UAVCAN_NODE_ID READ getUAVCAN_NODE_ID CONSTANT) Fact* getUAVCAN_NODE_ID(void) { return _mapParameterName2Fact["UAVCAN_NODE_ID"]; }
+ Q_PROPERTY(QString testString READ getTestString CONSTANT)
public:
/// @param uas Uas which this set of facts is associated with
@@ -496,13 +497,20 @@ public:
static void loadParameterFactMetaData(void);
static void deleteParameterFactMetaData(void);
+ static void clearStaticData(void);
+
+ /// Returns true if the full set of facts are ready
+ bool factsAreReady(void) { return _factsReady; }
+
+signals:
+ /// Signalled when the full set of facts are ready
+ void factsReady(void);
private slots:
- /// Connected to UASInterface::parameterChanged
void _parameterChanged(int uas, int component, QString parameterName, QVariant value);
-
- /// Signalled from Fact to indicate value was changed through the property write accessor
- void _valueUpdated(QVariant& value);
+ void _valueUpdated(QVariant value);
+ void _paramMgrParameterListUpToDate(void);
+ QString getTestString(void) { return QString("foo"); }
private:
static FactMetaData* _parseParameter(QXmlStreamReader& xml, const QString& group);
@@ -512,12 +520,15 @@ private:
QMap _mapParameterName2Fact; ///< Maps from a parameter name to a Fact
QMap _mapFact2ParameterName; ///< Maps from a Fact to a parameter name
+ static bool _parameterMetaDataLoaded; ///< true: parameter meta data already loaded
static QMap _mapParameterName2FactMetaData; ///< Maps from a parameter name to FactMetaData
int _uasId; ///< Id for uas which this set of Facts are associated with
int _lastSeenComponent;
QGCUASParamManagerInterface* _paramMgr;
+
+ bool _factsReady; ///< All facts received from param mgr
};
#endif
\ No newline at end of file
diff --git a/src/AutoPilotPlugins/PX4/Tests/FlightModeConfigTest.cc b/src/AutoPilotPlugins/PX4/Tests/FlightModeConfigTest.cc
index 17e84034b2d68aed69f93536789039033f501eff..a563cc1236b6979656aa6167a34d6bdcf6d914c6 100644
--- a/src/AutoPilotPlugins/PX4/Tests/FlightModeConfigTest.cc
+++ b/src/AutoPilotPlugins/PX4/Tests/FlightModeConfigTest.cc
@@ -93,7 +93,6 @@ void FlightModeConfigTest::init(void)
_configWidget = new FlightModeConfig;
Q_CHECK_PTR(_configWidget);
- _configWidget->setVisible(true);
// Setup combo maps
diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc
index 806b0e7531e7c46d11ad9a6ea81c33976968944d..e6bb1b4754a58b0d0aa8f3ccab9a35703fd1acea 100644
--- a/src/FactSystem/Fact.cc
+++ b/src/FactSystem/Fact.cc
@@ -34,14 +34,15 @@ Fact::Fact(QObject* parent) :
}
-void Fact::setValue(QVariant& value)
+void Fact::setValue(const QVariant& value)
{
_value = value;
- emit valueUpdated(value);
+ emit valueChanged(_value);
+ emit _containerValueChanged(_value);
}
-void Fact::updateValue(QVariant& value)
+void Fact::_containerSetValue(const QVariant& value)
{
_value = value;
- emit valueChanged(value);
+ emit valueChanged(_value);
}
diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h
index 1047162b5882c863c062d6ad43f7922b1d5be4c1..23ee2d6829e9a9a68680780f086f1e02bb09e260 100644
--- a/src/FactSystem/Fact.h
+++ b/src/FactSystem/Fact.h
@@ -62,7 +62,7 @@ public:
QVariant value(void) const { return _value; }
/// Write accessor for value property
- void setValue(QVariant& value);
+ void setValue(const QVariant& value);
/// Read accesor for defaultValue property
QVariant defaultValue(void) { return _metaData->defaultValue; }
@@ -85,24 +85,21 @@ public:
/// Read accesor for max property
QVariant max(void) { return _metaData->max; }
- /// Used to update the value property from C++ code.
- ///
- /// The setValue method is only for use by the QObject Property system. It should not be called directly by C++ app code.
- void updateValue(QVariant& value);
-
/// Sets the meta data associated with the Fact.
void setMetaData(FactMetaData* metaData) { _metaData = metaData; }
+ void _containerSetValue(const QVariant& value);
+
signals:
/// QObject Property System signal for value property changes
///
/// This signal is only meant for use by the QT property system. It should not be connected to by client code.
- void valueChanged(QVariant& value);
+ void valueChanged(QVariant value);
/// Signalled when property has been changed by a call to the property write accessor
///
- /// This signal is meant for use by client code.
- void valueUpdated(QVariant& value);
+ /// This signal is meant for use by Fact container implementations.
+ void _containerValueChanged(QVariant& value);
private:
QVariant _value; ///< Fact value
diff --git a/src/FactSystem/FactSystem.h b/src/FactSystem/FactSystem.h
index 93716c8936f4c98ae4de224dd2b779675f3ae168..1883ed61a5c430923c42d1fd3beadbbf1b4b233c 100644
--- a/src/FactSystem/FactSystem.h
+++ b/src/FactSystem/FactSystem.h
@@ -50,7 +50,6 @@ class FactSystem : public QGCSingleton
private:
/// All access to FactSystem is through FactSystem::instance, so constructor is private
FactSystem(QObject* parent = NULL);
-
~FactSystem();
static const char* _factSystemQmlUri; ///< URI for FactSystem QML imports
diff --git a/src/FactSystem/FactSystemTest.cc b/src/FactSystem/FactSystemTest.cc
new file mode 100644
index 0000000000000000000000000000000000000000..c64b92727ba8b2e534232fd7450ff00b37aa2008
--- /dev/null
+++ b/src/FactSystem/FactSystemTest.cc
@@ -0,0 +1,194 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see .
+
+ ======================================================================*/
+
+/// @file
+/// @author Don Gagne
+
+#include "FactSystemTest.h"
+#include "LinkManager.h"
+#include "MockLink.h"
+#include "AutoPilotPluginManager.h"
+#include "UASManager.h"
+#include "QGCApplication.h"
+#include "QGCQuickWidget.h"
+
+#include
+
+UT_REGISTER_TEST(FactSystemTest)
+
+/// FactSystem Unit Test
+FactSystemTest::FactSystemTest(void)
+{
+
+}
+
+void FactSystemTest::init(void)
+{
+ UnitTest::init();
+
+ LinkManager* _linkMgr = LinkManager::instance();
+
+ MockLink* link = new MockLink();
+ _linkMgr->addLink(link);
+ _linkMgr->connectLink(link);
+
+ // Wait for the uas to work it's way through the various threads
+
+ QSignalSpy spyUas(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)));
+ QCOMPARE(spyUas.wait(5000), true);
+
+ _uas = UASManager::instance()->getActiveUAS();
+ Q_ASSERT(_uas);
+
+ _paramMgr = _uas->getParamManager();
+ Q_ASSERT(_paramMgr);
+
+ // Get the plugin for the uas
+
+ AutoPilotPluginManager* pluginMgr = AutoPilotPluginManager::instance();
+ Q_ASSERT(pluginMgr);
+
+ _plugin = pluginMgr->getInstanceForAutoPilotPlugin(_uas);
+ Q_ASSERT(_plugin);
+
+ // Wait for the plugin to be ready
+
+ QSignalSpy spyPlugin(_plugin, SIGNAL(pluginReady()));
+ if (!_plugin->pluginIsReady()) {
+ QCOMPARE(spyPlugin.wait(5000), true);
+ }
+ Q_ASSERT(_plugin->pluginIsReady());
+}
+
+void FactSystemTest::cleanup(void)
+{
+ UnitTest::cleanup();
+}
+
+/// Basic test of parameter values in Fact System
+void FactSystemTest::_parameter_test(void)
+{
+ // Get the parameter facts from the AutoPilot
+
+ AutoPilotPluginManager* pluginMgr = AutoPilotPluginManager::instance();
+ Q_ASSERT(pluginMgr);
+
+ AutoPilotPlugin* plugin = pluginMgr->getInstanceForAutoPilotPlugin(_uas);
+ Q_ASSERT(plugin);
+
+ QObject* parameterFacts = plugin->parameterFacts();
+ QVERIFY(parameterFacts != NULL);
+
+ // Compare the value in the Parameter Manager with the value from the FactSystem
+
+ QVariant factVariant = parameterFacts->property("RC_MAP_THROTTLE");
+ Fact* fact = factVariant.value();
+ QVERIFY(fact != NULL);
+ QVariant factValue = fact->value();
+ QCOMPARE(factValue.isValid(), true);
+
+ QVariant paramValue;
+ Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue));
+
+ QCOMPARE(factValue.toInt(), paramValue.toInt());
+}
+
+/// Test that QML can reference a Fact
+void FactSystemTest::_qml_test(void)
+{
+ QGCQuickWidget* widget = new QGCQuickWidget;
+
+ widget->setSource(QUrl::fromUserInput("qrc:unittest/FactSystemTest.qml"));
+
+ QQuickItem* rootObject = widget->rootObject();
+ QObject* control = rootObject->findChild("testControl");
+ QVERIFY(control != NULL);
+ QVariant qmlValue = control->property("text").toInt();
+
+ QVariant paramMgrValue;
+ Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramMgrValue));
+
+ QCOMPARE(qmlValue.toInt(), paramMgrValue.toInt());
+}
+
+// Test correct behavior when the Param Manager gets a parameter update
+void FactSystemTest::_paramMgrSignal_test(void)
+{
+ // Get the parameter Fact from the AutoPilot
+
+ AutoPilotPluginManager* pluginMgr = AutoPilotPluginManager::instance();
+ Q_ASSERT(pluginMgr);
+
+ AutoPilotPlugin* plugin = pluginMgr->getInstanceForAutoPilotPlugin(_uas);
+ Q_ASSERT(plugin);
+
+ QObject* parameterFacts = plugin->parameterFacts();
+ QVERIFY(parameterFacts != NULL);
+
+ QVariant factVariant = parameterFacts->property("RC_MAP_THROTTLE");
+ Fact* fact = factVariant.value();
+ QVERIFY(fact != NULL);
+
+ // Setting a new value into the parameter should trigger a valueChanged signal on the Fact
+
+ QSignalSpy spyFact(fact, SIGNAL(valueChanged(QVariant)));
+
+ QVariant paramValue = 12;
+ _paramMgr->setParameter(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue);
+ _paramMgr->sendPendingParameters(true, false);
+
+ // Wait for the Fact::valueChanged signal to come through
+ QCOMPARE(spyFact.wait(5000), true);
+
+ // Make sure the signal has the right value
+ QList arguments = spyFact.takeFirst();
+ qDebug() << arguments.at(0).type();
+ QCOMPARE(arguments.at(0).toInt(), 12);
+
+ // Make sure the Fact has the new value
+ QCOMPARE(fact->value().toInt(), 12);
+}
+
+/// Test QML getting an updated Fact value
+void FactSystemTest::_qmlUpdate_test(void)
+{
+ QGCQuickWidget* widget = new QGCQuickWidget;
+
+ widget->setSource(QUrl::fromUserInput("qrc:unittest/FactSystemTest.qml"));
+
+ // Change the value using param manager
+
+ QVariant paramValue = 12;
+ _paramMgr->setParameter(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue);
+ _paramMgr->sendPendingParameters(true, false);
+
+ QTest::qWait(500); // Let the signals flow through
+
+ // Make sure the qml has the right value
+
+ QQuickItem* rootObject = widget->rootObject();
+ QObject* control = rootObject->findChild("testControl");
+ QVERIFY(control != NULL);
+ QCOMPARE(control->property("text").toInt(), 12);
+}
+
diff --git a/src/FactSystem/FactSystemTest.h b/src/FactSystem/FactSystemTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..07d43f2d0611991245e40437cd31a283bc6fcdcf
--- /dev/null
+++ b/src/FactSystem/FactSystemTest.h
@@ -0,0 +1,58 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see .
+
+ ======================================================================*/
+
+/// @file
+/// @author Don Gagne
+
+#ifndef FactSystemTest_H
+#define FactSystemTest_H
+
+#include "UnitTest.h"
+#include "UASInterface.h"
+#include "AutoPilotPlugin.h"
+
+// Unit Test for Fact System
+class FactSystemTest : public UnitTest
+{
+ Q_OBJECT
+
+public:
+ FactSystemTest(void);
+
+private slots:
+ void init(void);
+ void cleanup(void);
+
+ void _parameter_test(void);
+ void _qml_test(void);
+ void _paramMgrSignal_test(void);
+ void _qmlUpdate_test(void);
+
+private:
+ UASInterface* _uas;
+ QGCUASParamManagerInterface* _paramMgr;
+ AutoPilotPlugin* _plugin;
+ LinkManager* _linkMgr;
+};
+
+#endif
diff --git a/src/FactSystem/FactSystemTest.qml b/src/FactSystem/FactSystemTest.qml
new file mode 100644
index 0000000000000000000000000000000000000000..6559a3ec01cf73105e44abcfce2228ffedaf1053
--- /dev/null
+++ b/src/FactSystem/FactSystemTest.qml
@@ -0,0 +1,16 @@
+import QtQuick 2.2
+import QtQuick.Controls 1.2
+import QGroundControl.FactSystem 1.0
+import QGroundControlFactControls 1.0
+
+Item {
+ TextInput {
+ objectName: "testControl"
+ text: parameterFacts.RC_MAP_THROTTLE.value
+ font.family: "Helvetica"
+ font.pointSize: 24
+ color: "red"
+ focus: true
+ onAccepted: { parameterFacts.RC_MAP_THROTTLE.value = text; }
+ }
+}
\ No newline at end of file
diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc
index e0134a5c800ac85a5879dd9f42fbce85aa1e9be6..7b3861617c7245f6b7280898eeeb68f44bcc73b2 100644
--- a/src/GAudioOutput.cc
+++ b/src/GAudioOutput.cc
@@ -32,37 +32,22 @@ This file is part of the QGROUNDCONTROL project
#include
#include
-#include "GAudioOutput.h"
#include
-#include
-/**
- * This class follows the singleton design pattern
- * @see http://en.wikipedia.org/wiki/Singleton_pattern
- * A call to this function thus returns the only instance of this object
- * the call can occur at any place in the code, no reference to the
- * GAudioOutput object has to be passed.
- */
-GAudioOutput *GAudioOutput::instance()
-{
- static GAudioOutput *_instance = 0;
-
- if (_instance == 0)
- {
- _instance = new GAudioOutput();
- // Set the application as parent to ensure that this object
- // will be destroyed when the main application exits
- _instance->setParent(qApp);
- }
+#include "GAudioOutput.h"
+#include "QGCApplication.h"
+#include "QGC.h"
- return _instance;
-}
+IMPLEMENT_QGC_SINGLETON(GAudioOutput, GAudioOutput)
-GAudioOutput::GAudioOutput(QObject *parent) : QObject(parent),
+GAudioOutput::GAudioOutput(QObject *parent) :
+ QGCSingleton(parent),
muted(false),
thread(new QThread()),
worker(new QGCAudioWorker())
{
+ muted = qgcApp()->runningUnitTests();
+
worker->moveToThread(thread);
connect(this, SIGNAL(textToSpeak(QString,int)), worker, SLOT(say(QString,int)));
connect(this, SIGNAL(beepOnce()), worker, SLOT(beep()));
@@ -72,9 +57,8 @@ GAudioOutput::GAudioOutput(QObject *parent) : QObject(parent),
GAudioOutput::~GAudioOutput()
{
thread->quit();
- while (thread->isRunning()) {
- QGC::SLEEP::usleep(100);
- }
+ thread->wait();
+
delete worker;
delete thread;
}
@@ -82,19 +66,19 @@ GAudioOutput::~GAudioOutput()
void GAudioOutput::mute(bool mute)
{
- // XXX handle muting
- Q_UNUSED(mute);
+ muted = mute;
}
bool GAudioOutput::isMuted()
{
- // XXX return right stuff
- return false;
+ return muted;
}
bool GAudioOutput::say(QString text, int severity)
{
- emit textToSpeak(text, severity);
+ if (!muted) {
+ emit textToSpeak(text, severity);
+ }
return true;
}
@@ -103,7 +87,9 @@ bool GAudioOutput::say(QString text, int severity)
*/
bool GAudioOutput::alert(QString text)
{
- emit textToSpeak(text, 1);
+ if (!muted) {
+ emit textToSpeak(text, 1);
+ }
return true;
}
@@ -172,5 +158,7 @@ bool GAudioOutput::stopEmergency()
void GAudioOutput::beep()
{
- emit beepOnce();
+ if (!muted) {
+ emit beepOnce();
+ }
}
diff --git a/src/GAudioOutput.h b/src/GAudioOutput.h
index b9440e5856e2af1a5f6c0e2fafb39396ed8e9806..a765bd73c17042114bdf816d07b66104ae7bbd3a 100644
--- a/src/GAudioOutput.h
+++ b/src/GAudioOutput.h
@@ -37,19 +37,21 @@ This file is part of the PIXHAWK project
#include
#include
-#include
+#include "QGCAudioWorker.h"
+#include "QGCSingleton.h"
/**
* @brief Audio Output (speech synthesizer and "beep" output)
* This class follows the singleton design pattern
* @see http://en.wikipedia.org/wiki/Singleton_pattern
*/
-class GAudioOutput : public QObject
+class GAudioOutput : public QGCSingleton
{
Q_OBJECT
+
+ DECLARE_QGC_SINGLETON(GAudioOutput, GAudioOutput)
+
public:
- /** @brief Get the singleton instance */
- static GAudioOutput *instance();
/** @brief List available voices */
QStringList listVoices(void);
enum
@@ -104,6 +106,7 @@ protected:
bool muted;
QThread* thread;
QGCAudioWorker* worker;
+
private:
GAudioOutput(QObject *parent = NULL);
~GAudioOutput();
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index d7f1cda30f3bf5e6020a4c38167d25a67fdbaccd..aff39d56aa3871bb6b01c85f54fd3fcd990698bd 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -31,7 +31,6 @@
#include
#include
-#include
#include
#include
#include
@@ -182,8 +181,6 @@ void QGCApplication::_initCommon(void)
{
QSettings settings;
- _createSingletons();
-
// Show user an upgrade message if the settings version has been bumped up
bool settingsUpgraded = false;
if (settings.contains(_settingsVersionKey)) {
@@ -244,6 +241,8 @@ bool QGCApplication::_initForNormalAppBoot(void)
{
QSettings settings;
+ _createSingletons();
+
enum MainWindow::CUSTOM_MODE mode = (enum MainWindow::CUSTOM_MODE) settings.value("QGC_CUSTOM_MODE", (int)MainWindow::CUSTOM_MODE_PX4).toInt();
// Show splash screen
@@ -425,6 +424,11 @@ QGCApplication* qgcApp(void)
void QGCApplication::_createSingletons(void)
{
// The order here is important since the singletons reference each other
+
+ GAudioOutput* audio = GAudioOutput::_createSingleton();
+ Q_UNUSED(audio);
+ Q_ASSERT(audio);
+
LinkManager* linkManager = LinkManager::_createSingleton();
Q_UNUSED(linkManager);
Q_ASSERT(linkManager);
@@ -443,14 +447,34 @@ void QGCApplication::_createSingletons(void)
FactSystem* factSystem = FactSystem::_createSingleton();
Q_UNUSED(factSystem);
Q_ASSERT(factSystem);
+
+ // Needs everything!
+ MAVLinkProtocol* mavlink = MAVLinkProtocol::_createSingleton();
+ Q_UNUSED(mavlink);
+ Q_ASSERT(mavlink);
}
void QGCApplication::_destroySingletons(void)
{
+ if (LinkManager::instance(true /* nullOk */)) {
+ // This will close/delete all connections
+ LinkManager::instance()->_shutdown();
+ }
+
+ if (UASManager::instance(true /* nullOk */)) {
+ // This will delete all uas from the system
+ UASManager::instance()->_shutdown();
+ }
+
+ // Let the signals flow through the main thread
+ processEvents(QEventLoop::ExcludeUserInputEvents);
+
// Take down singletons in reverse order of creation
+ MAVLinkProtocol::_deleteSingleton();
FactSystem::_deleteSingleton();
AutoPilotPluginManager::_deleteSingleton();
UASManager::_deleteSingleton();
LinkManager::_deleteSingleton();
+ GAudioOutput::_deleteSingleton();
}
diff --git a/src/QGCQuickWidget.cc b/src/QGCQuickWidget.cc
index 7815b8e38dc6d9bb02fdb8b86c026947a0bb2d83..40057075a183dad7a89b413f4f8ef42810769a8f 100644
--- a/src/QGCQuickWidget.cc
+++ b/src/QGCQuickWidget.cc
@@ -43,9 +43,7 @@ QGCQuickWidget::QGCQuickWidget(QWidget* parent) :
UASInterface* uas = uasMgr->getActiveUAS();
Q_ASSERT(uas);
- AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(uas->getAutopilotType())->addFactsToQmlContext(rootContext(), uas);
-
rootContext()->engine()->addImportPath("qrc:/qml");
- setSource(QUrl::fromLocalFile("/Users/Don/repos/qgroundcontrol/test.qml"));
+ AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(uas)->addFactsToQmlContext(rootContext());
}
diff --git a/src/QGCSingleton.h b/src/QGCSingleton.h
index 5cbcef7db58fe3999145578ade35f014cdfb5a15..fdcc140cc0fe179a59710d5055842890ffdc8d27 100644
--- a/src/QGCSingleton.h
+++ b/src/QGCSingleton.h
@@ -37,7 +37,7 @@
/// For example DECLARE_QGC_SINGLETON(UASManager, UASManagerInterface)
#define DECLARE_QGC_SINGLETON(className, interfaceName) \
public: \
- static interfaceName* instance(void); \
+ static interfaceName* instance(bool nullOk = false); \
static void setMockInstance(interfaceName* mock); \
private: \
static interfaceName* _createSingleton(void); \
@@ -75,9 +75,11 @@
} \
} \
\
- interfaceName* className::instance(void) \
+ interfaceName* className::instance(bool nullOk) \
{ \
- Q_ASSERT_X(_instance, "QGCSingleton", "If you hit this, then you have run into a startup or shutdown sequence bug."); \
+ if (!nullOk) { \
+ Q_ASSERT_X(_instance, "QGCSingleton", "If you hit this, then you have run into a startup or shutdown sequence bug."); \
+ } \
return _instance; \
} \
\
diff --git a/src/VehicleSetup/SetupView.cc b/src/VehicleSetup/SetupView.cc
index 48b10746f51e6f36d59ab3d8b322e3d3076b0f16..6843fe327e0f0e471d437adb71d18cd74264fabd 100644
--- a/src/VehicleSetup/SetupView.cc
+++ b/src/VehicleSetup/SetupView.cc
@@ -208,7 +208,7 @@ void SetupView::_parametersReady(void)
_ui->summaryButton->setVisible(true);
- _components = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_uasCurrent->getAutopilotType())->getVehicleComponents(_uasCurrent);
+ _components = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_uasCurrent)->getVehicleComponents();
foreach(VehicleComponent* component, _components) {
VehicleComponentButton* button = new VehicleComponentButton(component, _ui->navBarWidget);
diff --git a/src/audio/QGCAudioWorker.cpp b/src/audio/QGCAudioWorker.cpp
index 0d2ee3e6b0304023232eaf3039a36efb9b6dfe95..5b7f4eb6d15218e8d35f38139703eecd9a9044b1 100644
--- a/src/audio/QGCAudioWorker.cpp
+++ b/src/audio/QGCAudioWorker.cpp
@@ -76,21 +76,17 @@ void QGCAudioWorker::init()
}
#endif
-
- // Prepare regular emergency signal, will be fired off on calling startEmergency()
- emergencyTimer = new QTimer();
- connect(emergencyTimer, SIGNAL(timeout()), this, SLOT(beep()));
-
}
QGCAudioWorker::~QGCAudioWorker()
{
#if defined _MSC_VER && defined QGC_SPEECH_ENABLED
- pVoice->Release();
- pVoice = NULL;
+ if (pVoice) {
+ pVoice->Release();
+ pVoice = NULL;
+ }
::CoUninitialize();
#endif
- delete emergencyTimer;
}
void QGCAudioWorker::say(QString text, int severity)
diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc
index ab954ef6481be1fa1d8919cf17b45ad04cca9cca..4c680035e2349689022f3c76d082cbcb5f587e3e 100644
--- a/src/comm/LinkManager.cc
+++ b/src/comm/LinkManager.cc
@@ -47,27 +47,14 @@ IMPLEMENT_QGC_SINGLETON(LinkManager, LinkManager)
**/
LinkManager::LinkManager(QObject* parent) :
QGCSingleton(parent),
- _connectionsSuspended(false),
- _mavlink(NULL)
+ _connectionsSuspended(false)
{
- _mavlink = new MAVLinkProtocol(this);
- Q_CHECK_PTR(_mavlink);
+
}
LinkManager::~LinkManager()
{
- disconnectAll();
-
- foreach (LinkInterface* link, _links) {
- Q_ASSERT(link);
- deleteLink(link);
- }
- _links.clear();
-
- // Clear out the queue so disconnects make it all the way through threads
- qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
-
- delete _mavlink;
+ Q_ASSERT_X(_links.count() == 0, "LinkManager", "LinkManager::_shutdown should have been called previously");
}
void LinkManager::addLink(LinkInterface* link)
@@ -92,10 +79,11 @@ void LinkManager::addLink(LinkInterface* link)
connect(link, SIGNAL(communicationError(QString,QString)), MainWindow::instance(), SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection);
}
- connect(link, &LinkInterface::bytesReceived, _mavlink, &MAVLinkProtocol::receiveBytes);
- connect(link, &LinkInterface::connected, _mavlink, &MAVLinkProtocol::linkConnected);
- connect(link, &LinkInterface::disconnected, _mavlink, &MAVLinkProtocol::linkDisconnected);
- _mavlink->resetMetadataForLink(link);
+ MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
+ connect(link, &LinkInterface::bytesReceived, mavlink, &MAVLinkProtocol::receiveBytes);
+ connect(link, &LinkInterface::connected, mavlink, &MAVLinkProtocol::linkConnected);
+ connect(link, &LinkInterface::disconnected, mavlink, &MAVLinkProtocol::linkDisconnected);
+ mavlink->resetMetadataForLink(link);
}
bool LinkManager::connectAll()
@@ -221,3 +209,12 @@ void LinkManager::setConnectionsSuspended(QString reason)
_connectionsSuspendedReason = reason;
Q_ASSERT(!reason.isEmpty());
}
+
+void LinkManager::_shutdown(void)
+{
+ QList links = _links;
+ foreach(LinkInterface* link, links) {
+ disconnectLink(link);
+ deleteLink(link);
+ }
+}
diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h
index 88a9eaf4e25ad932c39e1a27916417eba6cf6bb5..d749edd511159ce2959797f5986d088b940bef8b 100644
--- a/src/comm/LinkManager.h
+++ b/src/comm/LinkManager.h
@@ -27,7 +27,6 @@ This file is part of the PIXHAWK project
#ifndef _LINKMANAGER_H_
#define _LINKMANAGER_H_
-#include
#include
#include
#include
@@ -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,8 +95,9 @@ signals:
private:
/// All access to LinkManager is through LinkManager::instance
LinkManager(QObject* parent = NULL);
-
~LinkManager();
+
+ virtual void _shutdown(void);
bool _connectionsSuspendedMsg(void);
@@ -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
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 8fb72a3d418836d1b55c901444ccfbf01a73552a..bf86c3bdc02b7b559e8e6de9c511f2daf3e1eeb5 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/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");
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().
@@ -78,18 +78,27 @@ MAVLinkProtocol::MAVLinkProtocol(LinkManager* linkMgr) :
lastIndex[i][j] = -1;
}
}
-
- 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;
}
diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h
index 1c87ed0a1dd4168578b4c684392bfea0569261bb..1ccb3f08b6a2aca97c5795417a1893f71af7afe2 100644
--- a/src/comm/MAVLinkProtocol.h
+++ b/src/comm/MAVLinkProtocol.h
@@ -37,14 +37,18 @@ This file is part of the QGROUNDCONTROL project
#include
#include
#include
+#include
#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_
diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
index dd8bdaeb7a91daac7179fd826a2679008dcc0f19..d24a3adfb39a86032ef4d30bf1b568925f94b8a5 100644
--- a/src/comm/SerialLink.cc
+++ b/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();
+}
diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h
index 3368991cabbc8002c45c4e08078273f04b183544..df32f1d83e46bbf2440216bca03330c2bc6660fa 100644
--- a/src/comm/SerialLink.h
+++ b/src/comm/SerialLink.h
@@ -162,6 +162,9 @@ protected:
QMutex m_writeMutex; // Mutex for accessing the m_transmitBuffer.
QString type;
bool m_is_cdc;
+
+private slots:
+ void _rerouteDisconnected(void);
private:
// From LinkInterface
diff --git a/src/main.cc b/src/main.cc
index e85b8cd5f9cd56dcfdb568fceb1f98753e11a6eb..3c9534af4a0253a53aa55803af31fed10c555b2a 100644
--- a/src/main.cc
+++ b/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 {
diff --git a/src/qgcunittest/LinkManagerTest.cc b/src/qgcunittest/LinkManagerTest.cc
index 45c33811d140a1ded395798b130307e842fbdb2b..f613683dd719d74b1934a5a073519d12af4eb347 100644
--- a/src/qgcunittest/LinkManagerTest.cc
+++ b/src/qgcunittest/LinkManagerTest.cc
@@ -60,6 +60,8 @@ void LinkManagerTest::cleanup(void)
Q_ASSERT(_linkMgr);
Q_ASSERT(_multiSpy);
+ _linkMgr->_shutdown();
+
delete _linkMgr;
delete _multiSpy;
diff --git a/src/qgcunittest/MockLink.cc b/src/qgcunittest/MockLink.cc
index 5d0ae01919d15a5ccb75e0c778d3b73d5ac4ee06..2e70295322dc39f366f26279f69b4aacded9abfd 100644
--- a/src/qgcunittest/MockLink.cc
+++ b/src/qgcunittest/MockLink.cc
@@ -29,6 +29,8 @@
#include
+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(paramType);
}
- _cParameters = _parameters.count();
}
void MockLink::_sendHeartBeat(void)
@@ -352,48 +354,117 @@ 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) {
- _mavBaseMode = request.base_mode;
- _mavCustomMode = request.custom_mode;
- } else {
- _errorInvalidTargetSystem(request.target_system);
+ Q_ASSERT(request.target_system == _vehicleSystemId);
+
+ _mavBaseMode = request.base_mode;
+ _mavCustomMode = request.custom_mode;
+}
+
+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();
+
+ Q_ASSERT(request.target_system == _vehicleSystemId);
+
+ foreach(QString paramName, _mapParamName2Value.keys()) {
+ char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
+ mavlink_message_t responseMsg;
- for (param = _parameters.begin(); param != _parameters.end(); param++) {
- mavlink_message_t responseMsg;
- char paramId[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
- paramIndex++); // Index of this parameter
- _emitMavlinkMessage(responseMsg);
- }
- } else {
- _errorInvalidTargetSystem(request.target_system);
+ 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);
+
+ mavlink_msg_param_value_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ paramId, // Parameter name
+ _floatUnionForParam(paramName), // Parameter value
+ paramType, // MAV_PARAM_TYPE
+ cParameters, // Total number of parameters
+ paramIndex++); // Index of this parameter
+ _emitMavlinkMessage(responseMsg);
}
}
@@ -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) {
- // 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;
-
- 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
- _emitMavlinkMessage(responseMsg);
- } else {
- QString errMsg("MSG_ID_PARAM_SET requested unknown param id (%1)");
- emit error(errMsg.arg(paramId));
- }
- } else {
- _errorInvalidTargetSystem(request.target_system);
- }
+ 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);
+
+ Q_ASSERT(_mapParamName2Value.contains(paramId));
+ Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]);
+
+ // 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, // 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);
}
void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
@@ -439,40 +506,35 @@ 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) {
- 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(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();
-
- mavlink_message_t responseMsg;
-
- mavlink_msg_param_value_pack(_vehicleSystemId,
- _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
- _emitMavlinkMessage(responseMsg);
- }
+ 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 {
- _errorInvalidTargetSystem(request.target_system);
+ // Request is by 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());
}
+
+ Q_ASSERT(_mapParamName2Value.contains(paramId));
+ Q_ASSERT(_mapParamName2MavParamType.contains(paramId));
+
+ mavlink_message_t responseMsg;
+
+ mavlink_msg_param_value_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ paramId, // Parameter name
+ _floatUnionForParam(paramId), // Parameter value
+ _mapParamName2MavParamType[paramId], // Parameter type
+ _mapParamName2Value.count(), // Total number of parameters
+ _mapParamName2Value.keys().indexOf(paramId)); // Index of this parameter
+ _emitMavlinkMessage(responseMsg);
}
void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
@@ -481,19 +543,17 @@ void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
mavlink_msg_mission_request_list_decode(&msg, &request);
- if (request.target_system == _vehicleSystemId) {
- mavlink_message_t responseMsg;
-
- mavlink_msg_mission_count_pack(_vehicleSystemId,
- _vehicleComponentId,
- &responseMsg, // Outgoing message
- msg.sysid, // Target is original sender
- msg.compid, // Target is original sender
- _missionItems.count()); // Number of mission items
- _emitMavlinkMessage(responseMsg);
- } else {
- _errorInvalidTargetSystem(request.target_system);
- }
+ Q_ASSERT(request.target_system == _vehicleSystemId);
+
+ mavlink_message_t responseMsg;
+
+ mavlink_msg_mission_count_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ msg.sysid, // Target is original sender
+ msg.compid, // Target is original sender
+ _missionItems.count()); // Number of mission items
+ _emitMavlinkMessage(responseMsg);
}
void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
@@ -502,32 +562,26 @@ 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()) {
- mavlink_message_t responseMsg;
-
- mavlink_mission_item_t item = _missionItems[request.seq];
-
- mavlink_msg_mission_item_pack(_vehicleSystemId,
- _vehicleComponentId,
- &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);
- _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);
- }
+ Q_ASSERT(request.target_system == _vehicleSystemId);
+ Q_ASSERT(request.seq < _missionItems.count());
+
+ mavlink_message_t responseMsg;
+
+ mavlink_mission_item_t item = _missionItems[request.seq];
+
+ mavlink_msg_mission_item_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &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);
+ _emitMavlinkMessage(responseMsg);
}
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) {
- // FIXME: What do you do with duplication sequence numbers?
- Q_ASSERT(!_missionItems.contains(request.seq));
-
- _missionItems[request.seq] = request;
- } else {
- _errorInvalidTargetSystem(request.target_system);
- }
+ 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;
}
diff --git a/src/qgcunittest/MockLink.h b/src/qgcunittest/MockLink.h
index 049d267f96b616ae8e9cb6a32a0aa9fd64a1c7be..d6814b42ec050ac5b0cabf6c55c3ee834c1dd0de 100644
--- a/src/qgcunittest/MockLink.h
+++ b/src/qgcunittest/MockLink.h
@@ -25,10 +25,13 @@
#define MOCKLINK_H
#include
+#include
#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,7 +98,9 @@ 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;
int _linkId;
@@ -111,9 +113,8 @@ private:
bool _inNSH;
bool _mavlinkStarted;
- typedef QMap ParamMap_t;
- ParamMap_t _parameters;
- uint16_t _cParameters;
+ QMap _mapParamName2Value;
+ QMap _mapParamName2MavParamType;
typedef QMap MissionList_t;
MissionList_t _missionItems;
diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h
index 993307cf4675965e8fb54846505a8e4d8dd4f496..6e7ee75509e468698fab7c7bcbbe8696835eae7b 100644
--- a/src/qgcunittest/MockUAS.h
+++ b/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::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* getLinks() { Q_ASSERT(false); return NULL; };
+ virtual QList getLinks() { Q_ASSERT(false); return QList(); };
virtual bool systemCanReverse() const { Q_ASSERT(false); return false; };
virtual QString getSystemTypeName() { Q_ASSERT(false); return _bogusString; };
virtual int getAutopilotType() { return MAV_AUTOPILOT_PX4; };
diff --git a/src/qgcunittest/MockUASManager.h b/src/qgcunittest/MockUASManager.h
index 286f60920b9726b98e1ca482ea575e649858bff7..a1e83eca2322ffdf3393b951b7baa53a98cd165e 100644
--- a/src/qgcunittest/MockUASManager.h
+++ b/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;
diff --git a/src/qgcunittest/PX4RCCalibrationTest.cc b/src/qgcunittest/PX4RCCalibrationTest.cc
index a565aafbe96b63568026ff69db8da820074ebcad..05a9269906112b62775cb1f39d8e142b2473f2a2 100644
--- a/src/qgcunittest/PX4RCCalibrationTest.cc
+++ b/src/qgcunittest/PX4RCCalibrationTest.cc
@@ -166,9 +166,7 @@ void PX4RCCalibrationTest::init(void)
// This will instatiate the widget with an active uas with ready parameters
_calWidget = new PX4RCCalibration();
Q_CHECK_PTR(_calWidget);
- _calWidget->_setUnitTestMode();
- _calWidget->setVisible(true);
-
+ _calWidget->_setUnitTestMode();
// Get pointers to the push buttons
_cancelButton = _calWidget->findChild("rcCalCancel");
diff --git a/src/qgcunittest/UASUnitTest.cc b/src/qgcunittest/UASUnitTest.cc
index 8b2991f5119ab532cf0b4adfbf71180218cf8a6a..8502343d708c93d75e49705f05a8a20d9bbac42d 100644
--- a/src/qgcunittest/UASUnitTest.cc
+++ b/src/qgcunittest/UASUnitTest.cc
@@ -1,7 +1,6 @@
#include "UASUnitTest.h"
#include
#include
-#include
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");
diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc
index c3ac1fa9115184bc86f46fbc1053b856685d6ce7..33ae73ef4f446f9c8a25f3cc306099c2eefc87c2 100644
--- a/src/qgcunittest/UnitTest.cc
+++ b/src/qgcunittest/UnitTest.cc
@@ -81,13 +81,15 @@ QList& 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);
}
}
diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h
index cb27d3a20a337b9bcfeef4b94211495b1f28162d..93ff1679d5ed626e14acff0b87e7eb3a8037d61d 100644
--- a/src/qgcunittest/UnitTest.h
+++ b/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
diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc
index 07bfcf2053d97b6a0fe7aa1fe1f4abc72ded8738..1957ea3c650a98646234a2f4e94832bbc9c4e5fd 100644
--- a/src/uas/QGCMAVLinkUASFactory.cc
+++ b/src/uas/QGCMAVLinkUASFactory.cc
@@ -1,6 +1,5 @@
#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);
diff --git a/src/uas/QGCUASFileManager.cc b/src/uas/QGCUASFileManager.cc
index 0889981b7e1b6c1f71f6831c9930f8c9b67133c7..28091f95ad755de4ab38c879178f43fab26b1d13 100644
--- a/src/uas/QGCUASFileManager.cc
+++ b/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);
diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc
index f1de5ab4f104d8470379943afb3bb1d9a79a54b5..36e9fb481e56b24acfe571b00225d6b0cda6fa04 100644
--- a/src/uas/QGCUASParamManager.cc
+++ b/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();
diff --git a/src/uas/QGCUASWorker.cc b/src/uas/QGCUASWorker.cc
deleted file mode 100644
index b726e120b40b14b8988bafb84f272d41b3ec7a31..0000000000000000000000000000000000000000
--- a/src/uas/QGCUASWorker.cc
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "QGCUASWorker.h"
-
-#include
-#include
-#include
-
-QGCUASWorker::QGCUASWorker() : QThread(),
- _should_exit(false)
-{
-}
-
-void QGCUASWorker::quit()
-{
- _should_exit = true;
-}
-
-void QGCUASWorker::run()
-{
- while(!_should_exit) {
-
- QCoreApplication::processEvents();
- QGC::SLEEP::msleep(2);
- }
-}
diff --git a/src/uas/QGCUASWorker.h b/src/uas/QGCUASWorker.h
deleted file mode 100644
index 39ea5f011dc923ae9e29b7da057e94ec7525ceae..0000000000000000000000000000000000000000
--- a/src/uas/QGCUASWorker.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef QGCUASWORKER_H
-#define QGCUASWORKER_H
-
-#include
-
-class QGCUASWorker : public QThread
-{
-public:
- QGCUASWorker();
-
-public slots:
- void quit();
-
-protected:
- void run();
-
- bool _should_exit;
-};
-
-#endif // QGCUASWORKER_H
diff --git a/src/uas/QGXPX4UAS.cc b/src/uas/QGXPX4UAS.cc
index a64c53ee822ffd9cfd59c5eb184a0c18e5591227..b96ca4b2e5196ba4b061e3de21dd524e55ada20a 100644
--- a/src/uas/QGXPX4UAS.cc
+++ b/src/uas/QGXPX4UAS.cc
@@ -1,7 +1,7 @@
#include "QGXPX4UAS.h"
-QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id) :
- UAS(mavlink, thread, id)
+QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, int id) :
+ UAS(mavlink, id)
{
}
diff --git a/src/uas/QGXPX4UAS.h b/src/uas/QGXPX4UAS.h
index 9876ee49bdfbbaa8ec14945123693ae8635ef45a..5aaf8ab1adba7c98e39310faffbaf5c044d4f3ea 100644
--- a/src/uas/QGXPX4UAS.h
+++ b/src/uas/QGXPX4UAS.h
@@ -8,7 +8,7 @@ class QGXPX4UAS : public UAS
Q_OBJECT
Q_INTERFACES(UASInterface)
public:
- QGXPX4UAS(MAVLinkProtocol* mavlink, QThread* thread, int id);
+ QGXPX4UAS(MAVLinkProtocol* mavlink, int id);
public slots:
/** @brief Receive a MAVLink message from this MAV */
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index e14448536683825c79c4eddb95850934a3084b8c..26fbda4bf1812ce2b5c340397c7a0bb7a45df704 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -32,6 +32,8 @@
#include "AutoPilotPluginManager.h"
#include "QGCMessageBox.h"
+Q_LOGGING_CATEGORY(UASLog, "UASLog")
+
/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* by calling readSettings. This means the new UAS will have the same settings
@@ -39,17 +41,15 @@
* creating the UAS.
*/
-UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
+UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
lipoFull(4.2f),
lipoEmpty(3.5f),
uasId(id),
- links(new QList()),
unknownPackets(),
mavlink(protocol),
commStatus(COMM_DISCONNECTED),
receiveDropRate(0),
sendDropRate(0),
- statusTimeout(thread),
name(""),
type(MAV_TYPE_GENERIC),
@@ -150,7 +150,6 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
paramsOnceRequested(false),
paramMgr(this),
simulation(0),
- _thread(thread),
// The protected members.
connectionLost(false),
@@ -163,7 +162,6 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
lastSendTimeSensors(0),
lastSendTimeOpticalFlow(0)
{
- moveToThread(thread);
for (unsigned int i = 0; i<255;++i)
{
@@ -176,57 +174,57 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
// Store a list of available actions for this UAS.
// Basically everything exposed as a SLOT with no return value or arguments.
- QAction* newAction = new QAction(tr("Arm"), thread);
+ QAction* newAction = new QAction(tr("Arm"), this);
newAction->setToolTip(tr("Enable the UAS so that all actuators are online"));
connect(newAction, SIGNAL(triggered()), this, SLOT(armSystem()));
actions.append(newAction);
- newAction = new QAction(tr("Disarm"), thread);
+ newAction = new QAction(tr("Disarm"), this);
newAction->setToolTip(tr("Disable the UAS so that all actuators are offline"));
connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem()));
actions.append(newAction);
- newAction = new QAction(tr("Toggle armed"), thread);
+ newAction = new QAction(tr("Toggle armed"), this);
newAction->setToolTip(tr("Toggle between armed and disarmed"));
connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
actions.append(newAction);
- newAction = new QAction(tr("Go home"), thread);
+ newAction = new QAction(tr("Go home"), this);
newAction->setToolTip(tr("Command the UAS to return to its home position"));
connect(newAction, SIGNAL(triggered()), this, SLOT(home()));
actions.append(newAction);
- newAction = new QAction(tr("Land"), thread);
+ newAction = new QAction(tr("Land"), this);
newAction->setToolTip(tr("Command the UAS to land"));
connect(newAction, SIGNAL(triggered()), this, SLOT(land()));
actions.append(newAction);
- newAction = new QAction(tr("Launch"), thread);
+ newAction = new QAction(tr("Launch"), this);
newAction->setToolTip(tr("Command the UAS to launch itself and begin its mission"));
connect(newAction, SIGNAL(triggered()), this, SLOT(launch()));
actions.append(newAction);
- newAction = new QAction(tr("Resume"), thread);
+ newAction = new QAction(tr("Resume"), this);
newAction->setToolTip(tr("Command the UAS to continue its mission"));
connect(newAction, SIGNAL(triggered()), this, SLOT(go()));
actions.append(newAction);
- newAction = new QAction(tr("Stop"), thread);
+ newAction = new QAction(tr("Stop"), this);
newAction->setToolTip(tr("Command the UAS to halt and hold position"));
connect(newAction, SIGNAL(triggered()), this, SLOT(halt()));
actions.append(newAction);
- newAction = new QAction(tr("Go autonomous"), thread);
+ newAction = new QAction(tr("Go autonomous"), this);
newAction->setToolTip(tr("Set the UAS into an autonomous control mode"));
connect(newAction, SIGNAL(triggered()), this, SLOT(goAutonomous()));
actions.append(newAction);
- newAction = new QAction(tr("Go manual"), thread);
+ newAction = new QAction(tr("Go manual"), this);
newAction->setToolTip(tr("Set the UAS into a manual control mode"));
connect(newAction, SIGNAL(triggered()), this, SLOT(goManual()));
actions.append(newAction);
- newAction = new QAction(tr("Toggle autonomy"), thread);
+ newAction = new QAction(tr("Toggle autonomy"), this);
newAction->setToolTip(tr("Toggle between manual and full-autonomy"));
connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
actions.append(newAction);
@@ -251,11 +249,6 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
UAS::~UAS()
{
writeSettings();
-
- _thread->quit();
-
- delete links;
- delete simulation;
}
/**
@@ -391,8 +384,7 @@ bool UAS::getSelected() const
void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
- if (!link) return;
- if (!links->contains(link))
+ if (!links.contains(link))
{
addLink(link);
// qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName();
@@ -550,7 +542,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
modechanged = true;
this->base_mode = state.base_mode;
this->custom_mode = state.custom_mode;
- shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode, this->autopilot);
+ shortModeText = getShortModeTextFor(this->base_mode, this->custom_mode);
emit modeChanged(this->getUASID(), shortModeText, "");
@@ -1738,22 +1730,14 @@ void UAS::sendMessage(mavlink_message_t message)
return;
}
- if (links->count() < 1) {
+ if (links.count() < 1) {
qDebug() << "NO LINK AVAILABLE TO SEND!";
}
// Emit message on all links that are currently connected
- foreach (LinkInterface* link, *links)
- {
- if (LinkManager::instance()->getLinks().contains(link))
- {
- if (link->isConnected())
- sendMessage(link, message);
- }
- else
- {
- // Remove from list
- links->removeAt(links->indexOf(link));
+ foreach (LinkInterface* link, links) {
+ if (link->isConnected()) {
+ sendMessage(link, message);
}
}
}
@@ -1774,9 +1758,9 @@ void UAS::forwardMessage(mavlink_message_t message)
SerialLink* serial = dynamic_cast(link);
if(serial != 0)
{
- for(int i=0; isize(); i++)
+ for(int i=0; iat(i))
+ if(serial != links.at(i))
{
if (link->isConnected()) {
qDebug()<<"Antenna tracking: Forwarding Over link: "<getName()<<" "<remove(paramName);
}
- QVariant param;
+ QVariant paramValue;
// Insert with correct type
// TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling.
- switch (rawValue.param_type)
- {
- case MAV_PARAM_TYPE_REAL32:
- {
- if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
- param = QVariant(paramValue.param_float);
- }
- else {
- param = QVariant(paramValue.param_float);
- }
- parameters.value(compId)->insert(paramName, param);
- // Emit change
- emit parameterChanged(uasId, compId, paramName, param);
- emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param);
-// qDebug() << "RECEIVED PARAM:" << param;
- }
- break;
- case MAV_PARAM_TYPE_UINT8:
- {
- if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
- param = QVariant(QChar((unsigned char)paramValue.param_float));
- }
- else {
- param = QVariant(QChar((unsigned char)paramValue.param_uint8));
- }
- parameters.value(compId)->insert(paramName, param);
- // Emit change
- emit parameterChanged(uasId, compId, paramName, param);
- emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param);
- //qDebug() << "RECEIVED PARAM:" << param;
- }
- break;
- case MAV_PARAM_TYPE_INT8:
- {
- if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
- param = QVariant(QChar((char)paramValue.param_float));
- }
- else {
- param = QVariant(QChar((char)paramValue.param_int8));
- }
- parameters.value(compId)->insert(paramName, param);
- // Emit change
- emit parameterChanged(uasId, compId, paramName, param);
- emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param);
- //qDebug() << "RECEIVED PARAM:" << param;
- }
- break;
- case MAV_PARAM_TYPE_INT16:
- {
- if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
- param = QVariant((short)paramValue.param_float);
- }
- else {
- param = QVariant(paramValue.param_int16);
- }
- parameters.value(compId)->insert(paramName, param);
- // Emit change
- emit parameterChanged(uasId, compId, paramName, param);
- emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param);
- //qDebug() << "RECEIVED PARAM:" << param;
- }
- break;
- case MAV_PARAM_TYPE_UINT32:
- {
- if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
- param = QVariant((unsigned int)paramValue.param_float);
- }
- else {
- param = QVariant(paramValue.param_uint32);
- }
- parameters.value(compId)->insert(paramName, param);
- // Emit change
- emit parameterChanged(uasId, compId, paramName, param);
- emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param);
- }
- break;
- case MAV_PARAM_TYPE_INT32:
- {
- if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
- param = QVariant((int)paramValue.param_float);
- }
- else {
- param = QVariant(paramValue.param_int32);
- }
- parameters.value(compId)->insert(paramName, param);
- // Emit change
- emit parameterChanged(uasId, compId, paramName, param);
- emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param);
-// qDebug() << "RECEIVED PARAM:" << param;
+
+ switch (rawValue.param_type) {
+ case MAV_PARAM_TYPE_REAL32:
+ if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ paramValue = QVariant(paramUnion.param_float);
+ } else {
+ paramValue = QVariant(paramUnion.param_float);
+ }
+ break;
+
+ case MAV_PARAM_TYPE_UINT8:
+ if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ paramValue = QVariant(QChar((unsigned char)paramUnion.param_float));
+ } else {
+ paramValue = QVariant(QChar((unsigned char)paramUnion.param_uint8));
+ }
+ break;
+
+ case MAV_PARAM_TYPE_INT8:
+ if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ paramValue = QVariant(QChar((char)paramUnion.param_float));
+ } else {
+ paramValue = QVariant(QChar((char)paramUnion.param_int8));
+ }
+ break;
+
+ case MAV_PARAM_TYPE_INT16:
+ if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ paramValue = QVariant((short)paramUnion.param_float);
+ } else {
+ paramValue = QVariant(paramUnion.param_int16);
+ }
+ break;
+
+ case MAV_PARAM_TYPE_UINT32:
+ if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ paramValue = QVariant((unsigned int)paramUnion.param_float);
+ } else {
+ paramValue = QVariant(paramUnion.param_uint32);
+ }
+ break;
+ case MAV_PARAM_TYPE_INT32:
+ if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ paramValue = QVariant((int)paramUnion.param_float);
+ } else {
+ paramValue = QVariant(paramUnion.param_int32);
+ }
+ break;
+
+ default:
+ qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
}
- break;
- default:
- qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type;
- } //switch (value.param_type)
+ qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type;
+
+ parameters.value(compId)->insert(paramName, paramValue);
+ emit parameterChanged(uasId, compId, paramName, paramValue);
+ emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, paramValue);
}
/**
@@ -3290,9 +3237,9 @@ QString UAS::getAudioModeTextFor(int id)
* The mode returned can be auto, stabilized, test, manual, preflight or unknown.
* @return the short text of the mode for the id given.
*/
-QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot)
+QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const
{
- QString mode = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(autopilot)->getShortModeText(base_mode, custom_mode);
+ QString mode = AutoPilotPluginManager::instance()->getShortModeText(base_mode, custom_mode, autopilot);
if (mode.length() == 0)
{
@@ -3331,29 +3278,33 @@ const QString& UAS::getShortMode() const
*/
void UAS::addLink(LinkInterface* link)
{
- if (!links->contains(link))
+ if (!links.contains(link))
{
- links->append(link);
+ links.append(link);
+ qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16);
connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
}
}
void UAS::removeLink(QObject* object)
{
+ qCDebug(UASLog) << "removeLink:" << QString("%1").arg((ulong)object, 0, 16);
+ qCDebug(UASLog) << "link count:" << links.count();
+
// Do not dynamic cast or de-reference QObject, since object is either in destructor or may have already
// been destroyed.
LinkInterface* link = (LinkInterface*)object;
- int index = links->indexOf(link);
+ int index = links.indexOf(link);
Q_ASSERT(index != -1);
- links->removeAt(index);
+ links.removeAt(index);
}
/**
* @return the list of links
*/
-QList* UAS::getLinks()
+QList UAS::getLinks()
{
return links;
}
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index ed203a81af6813a7187f595f1235f1715c11f789..86b037de5bc8c6cf55f3eafe6ef55bb50b86bf56 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project
#ifndef _UAS_H_
#define _UAS_H_
-#include
#include "UASInterface.h"
#include
#include
@@ -44,6 +43,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCUASParamManager.h"
#include "QGCUASFileManager.h"
+Q_DECLARE_LOGGING_CATEGORY(UASLog)
/**
* @brief A generic MAVLINK-connected MAV/UAV
@@ -57,7 +57,7 @@ class UAS : public UASInterface
{
Q_OBJECT
public:
- UAS(MAVLinkProtocol* protocol, QThread* thread, int id = 0);
+ UAS(MAVLinkProtocol* protocol, int id = 0);
~UAS();
float lipoFull; ///< 100% charged voltage
@@ -72,7 +72,7 @@ public:
/** @brief Get short mode */
const QString& getShortMode() const;
/** @brief Translate from mode id to text */
- static QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot);
+ QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const;
/** @brief Translate from mode id to audio text */
static QString getAudioModeTextFor(int id);
/** @brief Get the unique system id */
@@ -92,7 +92,7 @@ public:
/** @brief Add one measurement and get low-passed voltage */
float filterVoltage(float value) const;
/** @brief Get the links associated with this robot */
- QList* getLinks();
+ QList getLinks();
Q_PROPERTY(double localX READ getLocalX WRITE setLocalX NOTIFY localXChanged)
Q_PROPERTY(double localY READ getLocalY WRITE setLocalY NOTIFY localYChanged)
@@ -341,7 +341,7 @@ protected: //COMMENTS FOR TEST UNIT
/// LINK ID AND STATUS
int uasId; ///< Unique system ID
QMap components;///< IDs and names of all detected onboard components
- QList* links; ///< List of links this UAS can be reached by
+ QList links; ///< List of links this UAS can be reached by
QList unknownPackets; ///< Packet IDs which are unknown and have been received
MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance
CommStatus commStatus; ///< Communication status
@@ -473,7 +473,6 @@ protected: //COMMENTS FOR TEST UNIT
/// SIMULATION
QGCHilLink* simulation; ///< Hardware in the loop simulation link
- QThread* _thread;
public:
/** @brief Set the current battery type */
@@ -952,13 +951,6 @@ protected slots:
void writeSettings();
/** @brief Read settings from disk */
void readSettings();
-
-// // MESSAGE RECEPTION
-// /** @brief Receive a named value message */
-// void receiveMessageNamedValue(const mavlink_message_t& message);
-
-private:
-// unsigned int mode; ///< The current mode of the MAV
};
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 6a6b56660a6ff379c246f7b136dc95ceefcdfc67..17c7fc4f7007b84221d24dad38abbf6971ad654f 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -103,7 +103,7 @@ public:
/** @brief Get short mode */
virtual const QString& getShortMode() const = 0;
/** @brief Translate mode id into text */
- static QString getShortModeTextFor(int id);
+ virtual QString getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode) const = 0;
//virtual QColor getColor() = 0;
virtual int getUASID() const = 0; ///< Get the ID of the connected UAS
/** @brief The time interval the robot is switched on **/
@@ -187,7 +187,7 @@ public:
* based on the fact that a message for this robot has been received through that
* interface. The LinkInterface can support multiple protocols.
**/
- virtual QList* getLinks() = 0;
+ virtual QList getLinks() = 0;
/**
* @brief Get the color for this UAS
diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc
index ffb41c36a853080936c1c97270894d9e12942a27..5e015709f997cd749f5c26c2df00f8a4c6a36acf 100644
--- a/src/uas/UASManager.cc
+++ b/src/uas/UASManager.cc
@@ -42,10 +42,19 @@ UASManager::UASManager(QObject* parent) :
UASManager::~UASManager()
{
storeSettings();
- // Delete all systems
- foreach (UASInterface* mav, systems) {
- // deleteLater so it ends up on correct thread
- mav->deleteLater();
+ Q_ASSERT_X(systems.count() == 0, "~UASManager", "_shutdown should have already removed all uas");
+}
+
+void UASManager::_shutdown(void)
+{
+ QList uasList;
+
+ foreach(UASInterface* uas, systems) {
+ uasList.append(uas);
+ }
+
+ foreach(UASInterface* uas, uasList) {
+ removeUAS(uas);
}
}
@@ -280,7 +289,8 @@ void UASManager::addUAS(UASInterface* uas)
if (firstUAS)
{
setActiveUAS(uas);
- if (offlineUASWaypointManager->getWaypointEditableList().size() > 0)
+ // Call getActiveUASWaypointManager instead of referencing variable to make sure of creation
+ if (getActiveUASWaypointManager()->getWaypointEditableList().size() > 0)
{
if (QGCMessageBox::question(tr("Question"), tr("Do you want to append the offline waypoints to the ones currently on the UAV?"), QMessageBox::Yes, QMessageBox::No) == QMessageBox::Yes)
{
diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h
index 65ea09924f5036338ac354502b8bae4dbcc7f9fd..41792f44c0b165281f1b0467e496f6ab32db25ae 100644
--- a/src/uas/UASManager.h
+++ b/src/uas/UASManager.h
@@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project
#define _UASMANAGER_H_
#include "UASManagerInterface.h"
-#include
#include
#include
#include
@@ -241,6 +240,8 @@ public slots:
void loadSettings();
/** @brief Store settings */
void storeSettings();
+
+ void _shutdown(void);
protected:
@@ -262,7 +263,6 @@ protected:
private:
/// @brief All access to UASManager singleton is through UASManager::instance
UASManager(QObject* parent = NULL);
-
~UASManager();
public:
diff --git a/src/uas/UASManagerInterface.h b/src/uas/UASManagerInterface.h
index 83619396b95d9ac1368c88b2ed3594e7846a11f1..f7c38f1cc124253eff0999c0f2e885afb00f0ea1 100644
--- a/src/uas/UASManagerInterface.h
+++ b/src/uas/UASManagerInterface.h
@@ -31,7 +31,6 @@
#ifndef _UASMANAGERINTERFACE_H_
#define _UASMANAGERINTERFACE_H_
-#include
#include
#include
@@ -91,6 +90,7 @@ public slots:
virtual void uavChangedHomePosition(int uav, double lat, double lon, double alt) = 0;
virtual void loadSettings() = 0;
virtual void storeSettings() = 0;
+ virtual void _shutdown(void) = 0;
signals:
/** A new system was created */
diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc
index e41a7b57e1d626415d47940305bcae65d4d6b5e1..e355a690c2cd9c47756e835aec2c49aa5e6b910c 100644
--- a/src/uas/UASParameterCommsMgr.cc
+++ b/src/uas/UASParameterCommsMgr.cc
@@ -71,7 +71,7 @@ void UASParameterCommsMgr::loadParamCommsSettings()
void UASParameterCommsMgr::_sendParamRequestListMsg(void)
{
- MAVLinkProtocol* mavlink = LinkManager::instance()->mavlink();
+ MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
Q_ASSERT(mavlink);
mavlink_message_t msg;
@@ -411,7 +411,7 @@ void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsS
void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value)
{
- qCDebug(UASParameterCommsMgrLog) << QString("Received parameter update for: name(%1) count(%2) index(%3)").arg(paramName).arg(paramCount).arg(paramId);
+ qCDebug(UASParameterCommsMgrLog) << "Received parameter update for:" << paramName << "count" << paramCount << "index" << paramId << "value" << value;
Q_UNUSED(uas); //this object is assigned to one UAS only
lastReceiveTime = QGC::groundTimeMilliseconds();
diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc
index 22a2edb28c3ebc5511998b70c88318a9970b1ad7..a813dc15939df12d88c930e87b46c35c8f790c70 100644
--- a/src/ui/CommConfigurationWindow.cc
+++ b/src/ui/CommConfigurationWindow.cc
@@ -224,7 +224,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, QWidget *p
connect(ui.linkType,SIGNAL(currentIndexChanged(int)),this,SLOT(linkCurrentIndexChanged(int)));
// Open details pane for MAVLink if necessary
- MAVLinkProtocol* mavlink = LinkManager::instance()->mavlink();
+ MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
QWidget* conf = new MAVLinkSettingsWidget(mavlink, this);
ui.protocolScrollArea->setWidget(conf);
ui.protocolGroupBox->setTitle(mavlink->getName()+" (Global Settings)");
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 6ea72cffbadaffc0d7db11485f2e97dfe00fb96b..8a3e1ced2e1588e0de86d3ac126c6d742f8a086c 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -130,8 +130,8 @@ MainWindow::MainWindow(QSplashScreen* splashScreen, enum MainWindow::CUSTOM_MODE
this->setAttribute(Qt::WA_DeleteOnClose);
connect(menuActionHelper, SIGNAL(needToShowDockWidget(QString,bool)),SLOT(showDockWidget(QString,bool)));
- connect(LinkManager::instance()->mavlink(), SIGNAL(protocolStatusMessage(const QString&, const QString&)), this, SLOT(showCriticalMessage(const QString&, const QString&)));
- connect(LinkManager::instance()->mavlink(), SIGNAL(saveTempFlightDataLog(QString)), this, SLOT(_saveTempFlightDataLog(QString)));
+ connect(MAVLinkProtocol::instance(), SIGNAL(protocolStatusMessage(const QString&, const QString&)), this, SLOT(showCriticalMessage(const QString&, const QString&)));
+ connect(MAVLinkProtocol::instance(), SIGNAL(saveTempFlightDataLog(QString)), this, SLOT(_saveTempFlightDataLog(QString)));
loadSettings();
@@ -294,7 +294,6 @@ MainWindow::MainWindow(QSplashScreen* splashScreen, enum MainWindow::CUSTOM_MODE
{
// Restore the window geometry
restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
- show();
}
else
{
@@ -305,12 +304,10 @@ MainWindow::MainWindow(QSplashScreen* splashScreen, enum MainWindow::CUSTOM_MODE
if (screenWidth < 1500)
{
resize(screenWidth, screenHeight - 80);
- show();
}
else
{
resize(screenWidth*0.67f, qMin(screenHeight, (int)(screenWidth*0.67f*0.67f)));
- show();
}
}
@@ -358,7 +355,10 @@ MainWindow::MainWindow(QSplashScreen* splashScreen, enum MainWindow::CUSTOM_MODE
connect(&windowNameUpdateTimer, SIGNAL(timeout()), this, SLOT(configureWindowName()));
windowNameUpdateTimer.start(15000);
emit initStatusChanged(tr("Done"), Qt::AlignLeft | Qt::AlignBottom, QColor(62, 93, 141));
- show();
+
+ if (!qgcApp()->runningUnitTests()) {
+ show();
+ }
}
MainWindow::~MainWindow()
@@ -489,12 +489,12 @@ void MainWindow::buildCustomWidget()
void MainWindow::buildCommonWidgets()
{
// Add generic MAVLink decoder
- mavlinkDecoder = new MAVLinkDecoder(LinkManager::instance()->mavlink(), this);
+ mavlinkDecoder = new MAVLinkDecoder(MAVLinkProtocol::instance(), this);
connect(mavlinkDecoder, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)),
this, SIGNAL(valueChanged(int,QString,QString,QVariant,quint64)));
// Log player
- logPlayer = new QGCMAVLinkLogPlayer(LinkManager::instance()->mavlink(), statusBar());
+ logPlayer = new QGCMAVLinkLogPlayer(MAVLinkProtocol::instance(), statusBar());
statusBar()->addPermanentWidget(logPlayer);
// Initialize all of the views, if they haven't been already, and add their central widgets
@@ -581,7 +581,7 @@ void MainWindow::buildCommonWidgets()
createDockWidget(simView, new PrimaryFlightDisplay(this), tr("Primary Flight Display"), "PRIMARY_FLIGHT_DISPLAY_DOCKWIDGET", VIEW_SIMULATION, Qt::RightDockWidgetArea);
// Add dock widgets for the engineering view
- createDockWidget(engineeringView, new QGCMAVLinkInspector(LinkManager::instance()->mavlink(), this), tr("MAVLink Inspector"), "MAVLINK_INSPECTOR_DOCKWIDGET", VIEW_ENGINEER, Qt::RightDockWidgetArea);
+ createDockWidget(engineeringView, new QGCMAVLinkInspector(MAVLinkProtocol::instance(), this), tr("MAVLink Inspector"), "MAVLINK_INSPECTOR_DOCKWIDGET", VIEW_ENGINEER, Qt::RightDockWidgetArea);
createDockWidget(engineeringView, new ParameterInterface(this), tr("Onboard Parameters"), "PARAMETER_INTERFACE_DOCKWIDGET", VIEW_ENGINEER, Qt::RightDockWidgetArea);
createDockWidget(engineeringView, new QGCUASFileViewMulti(this), tr("Onboard Files"), "FILE_VIEW_DOCKWIDGET", VIEW_ENGINEER, Qt::RightDockWidgetArea);
createDockWidget(engineeringView, new HUD(320, 240, this), tr("Video Downlink"), "HEAD_UP_DISPLAY_DOCKWIDGET", VIEW_ENGINEER, Qt::RightDockWidgetArea);
@@ -662,7 +662,7 @@ void MainWindow::loadDockWidget(const QString& name)
}
else if (name == "MAVLINK_INSPECTOR_DOCKWIDGET")
{
- createDockWidget(centerStack->currentWidget(),new QGCMAVLinkInspector(LinkManager::instance()->mavlink(),this),tr("MAVLink Inspector"),"MAVLINK_INSPECTOR_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
+ createDockWidget(centerStack->currentWidget(),new QGCMAVLinkInspector(MAVLinkProtocol::instance(),this),tr("MAVLink Inspector"),"MAVLINK_INSPECTOR_DOCKWIDGET",currentView,Qt::RightDockWidgetArea);
}
else if (name == "PARAMETER_INTERFACE_DOCKWIDGET")
{
@@ -816,7 +816,7 @@ void MainWindow::connectCommonWidgets()
{
if (infoDockWidget && infoDockWidget->widget())
{
- connect(LinkManager::instance()->mavlink(), SIGNAL(receiveLossChanged(int, float)),
+ connect(MAVLinkProtocol::instance(), SIGNAL(receiveLossChanged(int, float)),
infoDockWidget->widget(), SLOT(updateSendLoss(int, float)));
}
}
diff --git a/src/ui/OpalLinkConfigurationWindow.cc b/src/ui/OpalLinkConfigurationWindow.cc
index 5788fbede2a638cd5b416b5b301a23ddc9d569b6..922d15851505ab2800759203cae5195f58ade96e 100644
--- a/src/ui/OpalLinkConfigurationWindow.cc
+++ b/src/ui/OpalLinkConfigurationWindow.cc
@@ -15,7 +15,7 @@ OpalLinkConfigurationWindow::OpalLinkConfigurationWindow(OpalLink* link,
connect(ui.opalInstIDSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setOpalInstID(int)));
connect(link, &LinkInterface::connected, this, OpalLinkConfigurationWindow::_linkConnected);
- connect(link, &LinkInterface::disconnected, this, OpalLinkConfigurationWindow::_linkDisConnected);
+ connect(link, &LinkInterface::disconnected, this, OpalLinkConfigurationWindow::_linkDisconnected);
this->show();
}
diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc
index 3dbe2a9dc502fd468581bc48afeed569e125c422..9e4c145ca1406a03411453a55fab198cb185b4e6 100644
--- a/src/ui/QGCMAVLinkLogPlayer.cc
+++ b/src/ui/QGCMAVLinkLogPlayer.cc
@@ -43,10 +43,6 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
connect(ui->positionSlider, &QSlider::valueChanged, this, &QGCMAVLinkLogPlayer::jumpToSliderVal);
connect(ui->positionSlider, &QSlider::sliderPressed, this, &QGCMAVLinkLogPlayer::pause);
- // We use this to queue the signal over to mavlink. This way it will be behind any remaining
- // bytesReady signals in the queue.
- connect(this, &QGCMAVLinkLogPlayer::suspendLogForReplay, mavlink, &MAVLinkProtocol::suspendLogForReplay);
-
setAccelerationFactorInt(49);
ui->speedSlider->setValue(49);
updatePositionSliderUi(0.0);
@@ -84,7 +80,7 @@ void QGCMAVLinkLogPlayer::play()
Q_ASSERT(logFile.isOpen());
LinkManager::instance()->setConnectionsSuspended(tr("Connect not allowed during Flight Data replay."));
- emit suspendLogForReplay(true);
+ mavlink->suspendLogForReplay(true);
// Disable the log file selector button
ui->selectFileButton->setEnabled(false);
@@ -122,7 +118,7 @@ void QGCMAVLinkLogPlayer::play()
void QGCMAVLinkLogPlayer::pause()
{
LinkManager::instance()->setConnectionsAllowed();
- emit suspendLogForReplay(false);
+ mavlink->suspendLogForReplay(false);
loopTimer.stop();
isPlaying = false;
@@ -642,7 +638,7 @@ void QGCMAVLinkLogPlayer::_finishPlayback(void)
emit logFileEndReached();
- emit suspendLogForReplay(false);
+ mavlink->suspendLogForReplay(false);
LinkManager::instance()->setConnectionsAllowed();
}
diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h
index 0b3807b25b57b07f96f9eb1b458523943d614f76..eb4ab1ed63ae8b9830cf8b6577bf93adc29a29c6 100644
--- a/src/ui/QGCMAVLinkLogPlayer.h
+++ b/src/ui/QGCMAVLinkLogPlayer.h
@@ -59,9 +59,6 @@ signals:
/** @brief Send ready bytes */
void bytesReady(LinkInterface* link, const QByteArray& bytes);
void logFileEndReached();
-
- /// @brief Connected to the MAVLinkProtocol::suspendLogForReplay
- void suspendLogForReplay(bool suspend);
protected:
quint64 playbackStartTime; ///< The time when the logfile was first played back. This is used to pace out replaying the messages to fix long-term drift/skew. 0 indicates that the player hasn't initiated playback of this log file. In units of milliseconds since epoch UTC.
diff --git a/src/ui/SettingsDialog.cc b/src/ui/SettingsDialog.cc
index 328a6d89d92ba2c7578e3e42daeb8bd13704869c..9146a944cb7641a4d9fec8206fe4c40b0babb7b0 100644
--- a/src/ui/SettingsDialog.cc
+++ b/src/ui/SettingsDialog.cc
@@ -52,7 +52,7 @@ _ui(new Ui::SettingsDialog)
// Add the joystick settings pane
_ui->tabWidget->addTab(new JoystickWidget(joystick, this), "Controllers");
- MAVLinkSettingsWidget* msettings = new MAVLinkSettingsWidget(LinkManager::instance()->mavlink(), this);
+ MAVLinkSettingsWidget* msettings = new MAVLinkSettingsWidget(MAVLinkProtocol::instance(), this);
_ui->tabWidget->addTab(msettings, "MAVLink");
this->window()->setWindowTitle(tr("QGroundControl Settings"));
diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc
index 7841be3d6f5feaec649a619495e2935cb7f0bc30..ff23e461843cf88f2ad835ace7e111e303d5a7fd 100644
--- a/src/ui/uas/UASControlWidget.cc
+++ b/src/ui/uas/UASControlWidget.cc
@@ -59,23 +59,19 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
void UASControlWidget::updateModesList()
{
- // Detect autopilot type
- int autopilot = MAV_AUTOPILOT_GENERIC;
- if (this->uasID >= 0) {
- UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
- if (uas) {
- autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
- }
+ if (this->uasID == 0) {
+ return;
}
- AutoPilotPlugin* autopilotPlugin = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(autopilot);
+ UASInterface*uas = UASManager::instance()->getUASForId(this->uasID);
+ Q_ASSERT(uas);
- _modeList = autopilotPlugin->getModes();
+ _modeList = AutoPilotPluginManager::instance()->getModes(uas->getAutopilotType());
// Set combobox items
ui.modeComboBox->clear();
- foreach (AutoPilotPlugin::FullMode_t fullMode, _modeList) {
- ui.modeComboBox->addItem(UAS::getShortModeTextFor(fullMode.baseMode, fullMode.customMode, autopilot).remove(0, 2));
+ foreach (AutoPilotPluginManager::FullMode_t fullMode, _modeList) {
+ ui.modeComboBox->addItem(uas->getShortModeTextFor(fullMode.baseMode, fullMode.customMode).remove(0, 2));
}
// Select first mode in list
@@ -197,7 +193,7 @@ void UASControlWidget::transmitMode()
UASInterface* uas_iface = UASManager::instance()->getUASForId(this->uasID);
if (uas_iface) {
if (modeIdx >= 0 && modeIdx < _modeList.count()) {
- AutoPilotPlugin::FullMode_t fullMode = _modeList[modeIdx];
+ AutoPilotPluginManager::FullMode_t fullMode = _modeList[modeIdx];
// include armed state
if (armed) {
fullMode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
diff --git a/src/ui/uas/UASControlWidget.h b/src/ui/uas/UASControlWidget.h
index 997849df98016593987c97a1c1527f0abe01d0f4..2bf9f93efba45f09aae305de12ca79b32b5fad04 100644
--- a/src/ui/uas/UASControlWidget.h
+++ b/src/ui/uas/UASControlWidget.h
@@ -38,7 +38,7 @@ This file is part of the QGROUNDCONTROL project
#include
#include
#include
-#include "AutoPilotPlugin.h"
+#include "AutoPilotPluginManager.h"
/**
* @brief Widget controlling one MAV
@@ -79,7 +79,7 @@ protected slots:
protected:
int uasID; ///< Reference to the current uas
- QList _modeList; ///< Mode list for the current UAS
+ QList _modeList; ///< Mode list for the current UAS
int modeIdx; ///< Current uas mode index
bool armed; ///< Engine state
diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc
index 87a3b14a3e331552b80f3d8550a36a9bc946d4d8..fa5194bb181ebc5d54e4743ba929e4f45be61914 100644
--- a/src/ui/uas/UASListWidget.cc
+++ b/src/ui/uas/UASListWidget.cc
@@ -124,7 +124,7 @@ void UASListWidget::updateStatus()
if (!link)
continue;
- MAVLinkProtocol* mavlink = LinkManager::instance()->mavlink();
+ MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
// Build the tooltip out of the protocol parsing data: received, dropped, and parsing errors.
QString displayString("");
@@ -167,10 +167,10 @@ void UASListWidget::addUAS(UASInterface* uas)
if (!uasViews.contains(uas))
{
// Only display the UAS in a single link.
- QList* x = uas->getLinks();
- if (x->size())
+ QList x = uas->getLinks();
+ if (x.size())
{
- LinkInterface* li = x->first();
+ LinkInterface* li = x.first();
// Find an existing QGroupBox for this LinkInterface or create a
// new one.