diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 5908a0fbf751afdf03334398f709ad161bcfc3c5..5b8ddd670e8585e320754e5aeb09624335f87a66 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -25,6 +25,7 @@ /// @author Don Gagne #include "AutoPilotPlugin.h" +#include "QGCUASParamManagerInterface.h" AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) : QObject(parent), @@ -34,11 +35,36 @@ AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) : Q_ASSERT(_uas); } +void AutoPilotPlugin::refreshAllParameters(void) +{ + _getParameterLoader()->refreshAllParameters(); +} + +void AutoPilotPlugin::refreshParameter(int componentId, const QString& name) +{ + _getParameterLoader()->refreshParameter(componentId, name); +} + +void AutoPilotPlugin::refreshParametersPrefix(int componentId, const QString& namePrefix) +{ + _getParameterLoader()->refreshParametersPrefix(componentId, namePrefix); +} + +bool AutoPilotPlugin::parameterExists(const QString& name) +{ + return _getParameterLoader()->parameterExists(FactSystem::defaultComponentId, name); +} + +Fact* AutoPilotPlugin::getParameterFact(const QString& name) +{ + return _getParameterLoader()->getFact(FactSystem::defaultComponentId, name); +} + bool AutoPilotPlugin::factExists(FactSystem::Provider_t provider, int componentId, const QString& name) { switch (provider) { case FactSystem::ParameterProvider: - return getParameterLoader()->factExists(componentId, name); + return _getParameterLoader()->parameterExists(componentId, name); // Other providers will go here once they come online } @@ -51,7 +77,7 @@ Fact* AutoPilotPlugin::getFact(FactSystem::Provider_t provider, int componentId, { switch (provider) { case FactSystem::ParameterProvider: - return getParameterLoader()->getFact(componentId, name); + return _getParameterLoader()->getFact(componentId, name); // Other providers will go here once they come online } @@ -59,3 +85,40 @@ Fact* AutoPilotPlugin::getFact(FactSystem::Provider_t provider, int componentId, Q_ASSERT(false); return NULL; } + +QStringList AutoPilotPlugin::parameterNames(void) +{ + return _getParameterLoader()->parameterNames(); +} + +const QMap >& AutoPilotPlugin::getGroupMap(void) +{ + return _getParameterLoader()->getGroupMap(); +} + +void AutoPilotPlugin::writeParametersToStream(QTextStream &stream) +{ + Q_ASSERT(_uas); + + _uas->getParamManager()->writeOnboardParamsToStream(stream, _uas->getUASName()); +} + +void AutoPilotPlugin::readParametersFromStream(QTextStream &stream) +{ + Q_ASSERT(_uas); + + Fact* autoSaveFact = NULL; + int previousAutoSave = 0; + + if (parameterExists("COM_AUTOS_PAR")) { + autoSaveFact = getParameterFact("COM_AUTOS_PAR"); + previousAutoSave = autoSaveFact->value().toInt(); + autoSaveFact->setValue(1); + } + + _uas->getParamManager()->readPendingParamsFromStream(stream); + + if (autoSaveFact) { + autoSaveFact->setValue(previousAutoSave); + } +} diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index b25e418ab23726f8a55db36a974f2656dd7f0b7d..dc70d0e2f63c4ddf918aaeeef22540d7e2bb95b7 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -57,14 +57,32 @@ public: Q_PROPERTY(QVariantList vehicleComponents READ vehicleComponents CONSTANT) /// Re-request the full set of parameters from the autopilot - Q_INVOKABLE void refreshAllParameters(void) { getParameterLoader()->refreshAllParameters(); } + Q_INVOKABLE void refreshAllParameters(void); /// Request a refresh on the specific parameter - Q_INVOKABLE void refreshParameter(int componentId, const QString& name) { getParameterLoader()->refreshParameter(componentId, name); } + Q_INVOKABLE void refreshParameter(int componentId, const QString& name); /// Request a refresh on all parameters that begin with the specified prefix - Q_INVOKABLE void refreshParametersPrefix(int componentId, const QString& namePrefix) { getParameterLoader()->refreshParametersPrefix(componentId, namePrefix); } - + Q_INVOKABLE void refreshParametersPrefix(int componentId, const QString& namePrefix); + + /// Returns true if the specifed parameter exists from the default component + Q_INVOKABLE bool parameterExists(const QString& name); + + /// Returns all parameter names + /// FIXME: component id missing, generic to fact + QStringList parameterNames(void); + + /// Returns the specified parameter Fact from the default component + /// WARNING: Will assert if fact does not exists. If that possibility exists, check for existince first with + /// factExists. + Fact* getParameterFact(const QString& name); + + /// Writes the parameter facts to the specified stream + void writeParametersToStream(QTextStream &stream); + + /// Reads the parameters from the stream and updates values + void readParametersFromStream(QTextStream &stream); + /// Returns true if the specifed fact exists Q_INVOKABLE bool factExists(FactSystem::Provider_t provider, ///< fact provider int componentId, ///< fact component, -1=default component @@ -76,21 +94,12 @@ public: Fact* getFact(FactSystem::Provider_t provider, ///< fact provider int componentId, ///< fact component, -1=default component const QString& name); ///< fact name - - /// Returns true if the specifed parameter exists from the default component - Q_INVOKABLE bool parameterExists(const QString& name) { return getParameterLoader()->factExists(FactSystem::defaultComponentId, name); } - - /// Returns the specified parameter Fact from the default component - /// WARNING: Will assert if fact does not exists. If that possibility exists, check for existince first with - /// factExists. - Fact* getParameterFact(const QString& name) { return getParameterLoader()->getFact(FactSystem::defaultComponentId, name); } + const QMap >& getGroupMap(void); + // Must be implemented by derived class virtual const QVariantList& vehicleComponents(void) = 0; - /// Returns the ParameterLoader - virtual ParameterLoader* getParameterLoader(void) = 0; - /// FIXME: Kind of hacky static void clearStaticData(void); @@ -106,6 +115,9 @@ protected: /// All access to AutoPilotPugin objects is through getInstanceForAutoPilotPlugin AutoPilotPlugin(QObject* parent = NULL) : QObject(parent) { } + /// Returns the ParameterLoader + virtual ParameterLoader* _getParameterLoader(void) = 0; + UASInterface* _uas; bool _pluginReady; }; diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h index 3840b04387cffd5455e8f0cbb69682abdd92ff27..87f37de0880c17be7fdee1c3c3edb8edd481655d 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h @@ -42,7 +42,6 @@ public: // Overrides from AutoPilotPlugin virtual const QVariantList& vehicleComponents(void); - virtual ParameterLoader* getParameterLoader(void) { return _parameterFacts; } static QList getModes(void); static QString getShortModeText(uint8_t baseMode, uint32_t customMode); @@ -52,6 +51,9 @@ private slots: void _parametersReady(void); private: + // Overrides from AutoPilotPlugin + virtual ParameterLoader* _getParameterLoader(void) { return _parameterFacts; } + GenericParameterFacts* _parameterFacts; }; diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 995853ef12fb8024224a81ecb38585268d5f9aa8..8bcecdebcdcebef74b3bccfb67c21e10e9108c49 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -90,7 +90,6 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) : PX4AutoPilotPlugin::~PX4AutoPilotPlugin() { delete _parameterFacts; - PX4ParameterFacts::deleteParameterFactMetaData(); } QList PX4AutoPilotPlugin::getModes(void) diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index 9db1554ca432af11f03d22f2803aa638aabfa771..05ee9d1e2fbfa15befa92d41d4d5e07c4cb5c352 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -51,7 +51,6 @@ public: // Overrides from AutoPilotPlugin virtual const QVariantList& vehicleComponents(void); - virtual ParameterLoader* getParameterLoader(void) { return _parameterFacts; } static QList getModes(void); static QString getShortModeText(uint8_t baseMode, uint32_t customMode); @@ -69,6 +68,9 @@ private slots: void _pluginReadyPreChecks(void); private: + // Overrides from AutoPilotPlugin + virtual ParameterLoader* _getParameterLoader(void) { return _parameterFacts; } + PX4ParameterFacts* _parameterFacts; QVariantList _components; AirframeComponent* _airframeComponent; diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc index 1d1272930e3f29baef05f00ba88b51b5723daad6..e74d0b282696f46bbc255124d66c11127a55cb75 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc +++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc @@ -42,14 +42,6 @@ PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) : Q_ASSERT(uas); } -void PX4ParameterFacts::deleteParameterFactMetaData(void) -{ - foreach (QString param, _mapParameterName2FactMetaData.keys()) { - delete _mapParameterName2FactMetaData[param]; - } - _mapParameterName2FactMetaData.clear(); -} - /// Parse the Parameter element of parameter xml meta data /// @param[in] xml stream reader /// @param[in] group fact group associated with this Param element @@ -69,6 +61,8 @@ FactMetaData* PX4ParameterFacts::_parseParameter(QXmlStreamReader& xml, const QS FactMetaData* metaData = new FactMetaData(); Q_CHECK_PTR(metaData); + metaData->group = group; + // Convert type from string to FactMetaData::ValueType_t struct String2Type { diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h index 3b733777620aa717ab23433aea788c3407db20c9..018f4b5aa84f64e562869dc17a89517f14b3ed25 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h +++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h @@ -52,7 +52,6 @@ public: virtual QString getDefaultComponentIdParam(void) const { return QString("SYS_AUTOSTART"); } static void loadParameterFactMetaData(void); - static void deleteParameterFactMetaData(void); static void clearStaticData(void); private: diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index d50d429ee02610881f57b94eae4e521ed3df9886..a8390b5e9f5956e4252b010cb1b91920247967aa 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -107,7 +107,7 @@ QString Fact::shortDescription(void) if (_metaData) { return _metaData->shortDescription; } else { - return QString(); + return QString(""); } } @@ -116,7 +116,7 @@ QString Fact::longDescription(void) if (_metaData) { return _metaData->longDescription; } else { - return QString(); + return QString(""); } } @@ -125,7 +125,7 @@ QString Fact::units(void) if (_metaData) { return _metaData->units; } else { - return QString(); + return QString(""); } } @@ -141,6 +141,15 @@ QVariant Fact::max(void) return _metaData->max; } +QString Fact::group(void) +{ + if (_metaData) { + return _metaData->group; + } else { + return "Default Group"; + } +} + void Fact::setMetaData(FactMetaData* metaData) { _metaData = metaData; diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index 798b23e61982dc5e631865cc869a64a0b0f36dd8..7e92102032ba18d726451246856e137236e1747d 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -54,6 +54,7 @@ class Fact : public QObject Q_PROPERTY(QString units READ units CONSTANT) Q_PROPERTY(QVariant min READ min CONSTANT) Q_PROPERTY(QVariant max READ max CONSTANT) + Q_PROPERTY(QString group READ group CONSTANT) Q_ENUMS(FactMetaData::ValueType_t) @@ -63,41 +64,19 @@ public: // Property system methods - /// Read accessor for name property QString name(void) const; - - /// Read accessor for componentId property int componentId(void) const; - - /// Read accessor for value property QVariant value(void) const; - - /// Read accessor for valueString property QString valueString(void) const; - - /// Write accessor for value property void setValue(const QVariant& value); - - /// Read accesor for defaultValue property QVariant defaultValue(void); - - /// Read accesor for type property FactMetaData::ValueType_t type(void); - - /// Read accesor for shortDescription property QString shortDescription(void); - - /// Read accesor for longDescription property QString longDescription(void); - - /// Read accesor for units property QString units(void); - - /// Read accesor for min property QVariant min(void); - - /// Read accesor for max property - QVariant max(void); + QVariant max(void); + QString group(void); /// Sets the meta data associated with the Fact. void setMetaData(FactMetaData* metaData); diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 47924be5ee81e5a1fcde420f35c4f129874e9ac9..b7e2bfa4671179af383b80bf2cca41322c9045f8 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -57,7 +57,7 @@ public: /// Initialize the meta data given only the type. void initFromTypeOnly(ValueType_t initType); - // FIXME: This needs to switch over to Q_PROPERTY mechanism + QString group; ValueType_t type; QVariant defaultValue; QString shortDescription; diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 5d66a9ccbec3101f10881505a6217968602d427c..c1c28e36bc5d7bdef4bc9ce30d10da1cff479a1e 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -80,9 +80,6 @@ void ParameterLoader::_parameterUpdate(int uas, int componentId, QString paramet } if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(parameterName)) { - // These should not get our of sync - Q_ASSERT(_mapParameterName2Variant.contains(componentId) == _mapFact2ParameterName.contains(componentId)); - qCDebug(ParameterLoaderLog) << "Adding new fact (component:" << componentId << "name:" << parameterName << ")"; FactMetaData::ValueType_t factType; @@ -121,7 +118,6 @@ void ParameterLoader::_parameterUpdate(int uas, int componentId, QString paramet setMetaData = true; _mapParameterName2Variant[componentId][parameterName] = QVariant::fromValue(fact); - _mapFact2ParameterName[componentId][fact] = parameterName; // We need to know when the fact changes from QML so that we can send the new value to the parameter manager connect(fact, &Fact::_containerValueChanged, this, &ParameterLoader::_valueUpdated); @@ -151,8 +147,6 @@ void ParameterLoader::_valueUpdated(const QVariant& value) int componentId = fact->componentId(); Q_ASSERT(_paramMgr); - Q_ASSERT(_mapFact2ParameterName.contains(componentId)); - Q_ASSERT(_mapFact2ParameterName[componentId].contains(fact)); QVariant typedValue; switch (fact->type()) { @@ -196,6 +190,7 @@ void ParameterLoader::_paramMgrParameterListUpToDate(void) qgcApp()->processEvents(); _determineDefaultComponentId(); + _setupGroupMap(); // We should have all parameters now so we can signal ready emit parametersReady(); @@ -254,7 +249,6 @@ void ParameterLoader::refreshParametersPrefix(int componentId, const QString& na Q_ASSERT(_paramMgr); componentId = _actualComponentId(componentId); - Q_ASSERT(_mapFact2ParameterName.contains(componentId)); foreach(QString name, _mapParameterName2Variant[componentId].keys()) { if (name.startsWith(namePrefix)) { refreshParameter(componentId, name); @@ -262,7 +256,7 @@ void ParameterLoader::refreshParametersPrefix(int componentId, const QString& na } } -bool ParameterLoader::factExists(int componentId, const QString& name) +bool ParameterLoader::parameterExists(int componentId, const QString& name) { componentId = _actualComponentId(componentId); if (_mapParameterName2Variant.contains(componentId)) { @@ -280,3 +274,29 @@ Fact* ParameterLoader::getFact(int componentId, const QString& name) Q_ASSERT(fact); return fact; } + +QStringList ParameterLoader::parameterNames(void) +{ + QStringList names; + + foreach(QString paramName, _mapParameterName2Variant[_defaultComponentId].keys()) { + names << paramName; + } + + return names; +} + +void ParameterLoader::_setupGroupMap(void) +{ + foreach (int componentId, _mapParameterName2Variant.keys()) { + foreach (QString name, _mapParameterName2Variant[componentId].keys()) { + Fact* fact = _mapParameterName2Variant[componentId][name].value(); + _mapGroup2ParameterName[componentId][fact->group()] += name; + } + } +} + +const QMap >& ParameterLoader::getGroupMap(void) +{ + return _mapGroup2ParameterName; +} diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 6c1951059cd89a2db12670b9ed07c601d646e397..0237e757ea9e9b79da42fc525bbe3196d816c40b 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -60,16 +60,22 @@ public: /// Request a refresh on all parameters that begin with the specified prefix void refreshParametersPrefix(int componentId, const QString& namePrefix); - /// Returns true if the specifed fact exists - bool factExists(int componentId, ///< fact component, -1=default component - const QString& name); ///< fact name + /// Returns true if the specifed parameter exists + bool parameterExists(int componentId, ///< fact component, -1=default component + const QString& name); ///< fact name + + /// Returns all parameter names + /// FIXME: component id missing + QStringList parameterNames(void); /// Returns the specified Fact. - /// WARNING: Will assert if fact does not exists. If that possibily exists, check for existince first with - /// factExists. + /// WARNING: Will assert if parameter does not exists. If that possibily exists, check for existince first with + /// parameterExists. Fact* getFact(int componentId, ///< fact component, -1=default component const QString& name); ///< fact name + const QMap >& getGroupMap(void); + /// Return the parameter for which the default component id is derived from. Return an empty /// string is this is not available. virtual QString getDefaultComponentIdParam(void) const = 0; @@ -92,11 +98,8 @@ private: static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); int _actualComponentId(int componentId); void _determineDefaultComponentId(void); + void _setupGroupMap(void); - /// First mapping is by component id - /// Second mapping is parameter name, to Fact - QMap > _mapFact2ParameterName; - int _uasId; ///< Id for uas which this set of Facts are associated with QGCUASParamManagerInterface* _paramMgr; @@ -105,6 +108,10 @@ private: /// Second mapping is parameter name, to Fact* in QVariant QMap _mapParameterName2Variant; + /// First mapping is by component id + /// Second mapping is group name, to Fact + QMap > _mapGroup2ParameterName; + bool _parametersReady; ///< All params received from param mgr int _defaultComponentId; QString _defaultComponentIdParam;