diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 23f04bd924d2ebbdf7e2e7fd308262ffc56b9f0c..f48ea1857f3caa9eeb4ee7153ab95b4e9f094ecb 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -776,7 +776,7 @@ HEADERS += \ src/FactSystem/FactBinder.h \ src/FactSystem/FactMetaData.h \ src/FactSystem/FactValidator.h \ - src/FactSystem/FactLoader.h \ + src/FactSystem/ParameterLoader.h \ SOURCES += \ src/FactSystem/FactSystem.cc \ @@ -784,4 +784,4 @@ SOURCES += \ src/FactSystem/FactBinder.cc \ src/FactSystem/FactMetaData.cc \ src/FactSystem/FactValidator.cc \ - src/FactSystem/FactLoader.cc \ + src/FactSystem/ParameterLoader.cc \ diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index b4b4da38f076d455dd1470766d5103a4c6deb43e..5908a0fbf751afdf03334398f709ad161bcfc3c5 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -34,42 +34,28 @@ AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) : Q_ASSERT(_uas); } -void AutoPilotPlugin::refreshAllParameters(void) +bool AutoPilotPlugin::factExists(FactSystem::Provider_t provider, int componentId, const QString& name) { - Q_ASSERT(_uas); - QGCUASParamManagerInterface* paramMgr = _uas->getParamManager(); - Q_ASSERT(paramMgr); - paramMgr->requestParameterList(); -} - -void AutoPilotPlugin::refreshParameter(const QString& param) -{ - Q_ASSERT(_uas); - QGCUASParamManagerInterface* paramMgr = _uas->getParamManager(); - Q_ASSERT(paramMgr); + switch (provider) { + case FactSystem::ParameterProvider: + return getParameterLoader()->factExists(componentId, name); + + // Other providers will go here once they come online + } - QList compIdList = paramMgr->getComponentForParam(param); - Q_ASSERT(compIdList.count() > 0); - paramMgr->requestParameterUpdate(compIdList[0], param); + Q_ASSERT(false); + return false; } -void AutoPilotPlugin::refreshParametersPrefix(const QString& paramPrefix) +Fact* AutoPilotPlugin::getFact(FactSystem::Provider_t provider, int componentId, const QString& name) { - foreach(QVariant varFact, parameters()) { - Fact* fact = qvariant_cast(varFact); - Q_ASSERT(fact); - if (fact->name().startsWith(paramPrefix)) { - refreshParameter(fact->name()); - } + switch (provider) { + case FactSystem::ParameterProvider: + return getParameterLoader()->getFact(componentId, name); + + // Other providers will go here once they come online } -} - -bool AutoPilotPlugin::factExists(const QString& param) -{ - return parameters().contains(param); -} - -Fact* AutoPilotPlugin::getFact(const QString& name) -{ - return parameters()[name].value(); + + Q_ASSERT(false); + return NULL; } diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index 6e0a0c6945cc4417c19b0377d92c8772e0a9ceee..b25e418ab23726f8a55db36a974f2656dd7f0b7d 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -35,6 +35,7 @@ #include "UASInterface.h" #include "VehicleComponent.h" #include "FactSystem.h" +#include "ParameterLoader.h" /// This is the base class for AutoPilot plugins /// @@ -52,26 +53,43 @@ public: Q_PROPERTY(bool pluginReady READ pluginReady NOTIFY pluginReadyChanged) - Q_PROPERTY(QVariantList components READ components CONSTANT) - Q_PROPERTY(QUrl setupBackgroundImage READ setupBackgroundImage CONSTANT) + /// List of VehicleComponent objects + Q_PROPERTY(QVariantList vehicleComponents READ vehicleComponents CONSTANT) /// Re-request the full set of parameters from the autopilot - Q_INVOKABLE void refreshAllParameters(void); + Q_INVOKABLE void refreshAllParameters(void) { getParameterLoader()->refreshAllParameters(); } /// Request a refresh on the specific parameter - Q_INVOKABLE void refreshParameter(const QString& param); + Q_INVOKABLE void refreshParameter(int componentId, const QString& name) { getParameterLoader()->refreshParameter(componentId, name); } - // Request a refresh on all parameters that begin with the specified prefix - Q_INVOKABLE void refreshParametersPrefix(const QString& paramPrefix); + /// 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 bool factExists(const QString& param); + /// Returns true if the specifed fact exists + Q_INVOKABLE bool factExists(FactSystem::Provider_t provider, ///< fact provider + int componentId, ///< fact component, -1=default component + const QString& name); ///< fact name - Fact* getFact(const QString& name); + /// Returns the specified Fact. + /// WARNING: Will assert if fact does not exists. If that possibility exists, check for existince first with + /// factExists. + Fact* getFact(FactSystem::Provider_t provider, ///< fact provider + int componentId, ///< fact component, -1=default component + const QString& name); ///< fact name - // Property accessors - virtual const QVariantList& components(void) = 0; - virtual const QVariantMap& parameters(void) = 0; - virtual QUrl setupBackgroundImage(void) = 0; + /// 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); } + + // 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); diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc index bf92452a53684c043e9f780c819976fab3d78647..0e2de3f5618c92165541c622e9a4b383013b89b5 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc @@ -34,7 +34,7 @@ GenericAutoPilotPlugin::GenericAutoPilotPlugin(UASInterface* uas, QObject* paren _parameterFacts = new GenericParameterFacts(uas, this); Q_CHECK_PTR(_parameterFacts); - connect(_parameterFacts, &GenericParameterFacts::factsReady, this, &GenericAutoPilotPlugin::_factsReady); + connect(_parameterFacts, &GenericParameterFacts::parametersReady, this, &GenericAutoPilotPlugin::_parametersReady); } QList GenericAutoPilotPlugin::getModes(void) @@ -87,24 +87,14 @@ void GenericAutoPilotPlugin::clearStaticData(void) // No Static data yet } -const QVariantList& GenericAutoPilotPlugin::components(void) +const QVariantList& GenericAutoPilotPlugin::vehicleComponents(void) { static QVariantList emptyList; return emptyList; } -const QVariantMap& GenericAutoPilotPlugin::parameters(void) -{ - return _parameterFacts->factMap(); -} - -QUrl GenericAutoPilotPlugin::setupBackgroundImage(void) -{ - return QUrl::fromUserInput("qrc:/qml/px4fmu_2.x.png"); -} - -void GenericAutoPilotPlugin::_factsReady(void) +void GenericAutoPilotPlugin::_parametersReady(void) { _pluginReady = true; emit pluginReadyChanged(_pluginReady); diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h index f4eb8a71bd1aac454f9b48c7a583948df01bbf34..3840b04387cffd5455e8f0cbb69682abdd92ff27 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.h @@ -41,16 +41,15 @@ public: GenericAutoPilotPlugin(UASInterface* uas, QObject* parent = NULL); // Overrides from AutoPilotPlugin - virtual QUrl setupBackgroundImage(void); - virtual const QVariantList& components(void); - virtual const QVariantMap& parameters(void); + virtual const QVariantList& vehicleComponents(void); + virtual ParameterLoader* getParameterLoader(void) { return _parameterFacts; } static QList getModes(void); static QString getShortModeText(uint8_t baseMode, uint32_t customMode); static void clearStaticData(void); private slots: - void _factsReady(void); + void _parametersReady(void); private: GenericParameterFacts* _parameterFacts; diff --git a/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc b/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc index c37d05dc5fec7d71b7511e5f16188d9a32c77268..d33b143f81fa47db1691e431c5ec85803a487acd 100644 --- a/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc +++ b/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc @@ -25,13 +25,11 @@ /// @author Don Gagne #include "GenericParameterFacts.h" -#include "QGCApplication.h" -#include #include GenericParameterFacts::GenericParameterFacts(UASInterface* uas, QObject* parent) : - FactLoader(uas, parent) + ParameterLoader(uas, parent) { Q_ASSERT(uas); } diff --git a/src/AutoPilotPlugins/Generic/GenericParameterFacts.h b/src/AutoPilotPlugins/Generic/GenericParameterFacts.h index ae363b432494de6efc8492949cacd7485173fc87..5a964eeae7f634dc6d70bd394c7ae9cdbb72c5c9 100644 --- a/src/AutoPilotPlugins/Generic/GenericParameterFacts.h +++ b/src/AutoPilotPlugins/Generic/GenericParameterFacts.h @@ -25,11 +25,8 @@ #define GenericParameterFacts_h #include -#include -#include -#include -#include "FactSystem.h" +#include "ParameterLoader.h" #include "UASInterface.h" /// @file @@ -37,13 +34,16 @@ /// Collection of Parameter Facts for Generic AutoPilot -class GenericParameterFacts : public FactLoader +class GenericParameterFacts : public ParameterLoader { Q_OBJECT public: /// @param uas Uas which this set of facts is associated with GenericParameterFacts(UASInterface* uas, QObject* parent = NULL); + + /// Override from ParameterLoader + virtual QString getDefaultComponentIdParam(void) const { return QString(); } }; #endif \ No newline at end of file diff --git a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc index fffd4400ca34c60ea211021915721c946e37f7e6..018457d26704c219917ba1d44a4c96845c744096 100644 --- a/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc +++ b/src/AutoPilotPlugins/PX4/FlightModesComponentController.cc @@ -69,7 +69,9 @@ void FlightModesComponentController::_validateConfiguration(void) { _validConfiguration = true; - _channelCount = _autoPilotPlugin->factExists("RC_CHAN_CNT") ? _autoPilotPlugin->getFact("RC_CHAN_CNT")->value().toInt() : _chanMax; + _channelCount = _autoPilotPlugin->parameterExists("RC_CHAN_CNT") ? + _autoPilotPlugin->getParameterFact("RC_CHAN_CNT")->value().toInt() : + _chanMax; if (_channelCount <= 0 || _channelCount > _chanMax) { // Parameter exists, but has not yet been set or is invalid. Use default _channelCount = _chanMax; @@ -84,7 +86,7 @@ void FlightModesComponentController::_validateConfiguration(void) switchNames << "Mode Switch" << "Return Switch" << "Loiter Switch" << "PosCtl Switch"; for(int i=0; igetFact(switchParams[i])->value().toInt(); + int map = _autoPilotPlugin->getParameterFact(switchParams[i])->value().toInt(); switchMappings << map; if (map < 0 || map > _channelCount) { @@ -101,7 +103,7 @@ void FlightModesComponentController::_validateConfiguration(void) attitudeNames << "Throttle" << "Yaw" << "Pitch" << "Roll" << "Flaps" << "Aux1" << "Aux2"; for (int i=0; igetFact(attitudeParams[i])->value().toInt(); + int map = _autoPilotPlugin->getParameterFact(attitudeParams[i])->value().toInt(); for (int j=0; jgetFact(rcMinParam)->value().toInt(); - _rgRCMax[i] = _autoPilotPlugin->getFact(rcMaxParam)->value().toInt(); + _rgRCMin[i] = _autoPilotPlugin->getParameterFact(rcMinParam)->value().toInt(); + _rgRCMax[i] = _autoPilotPlugin->getParameterFact(rcMaxParam)->value().toInt(); - float floatReversed = _autoPilotPlugin->getFact(rcRevParam)->value().toFloat(); + float floatReversed = _autoPilotPlugin->getParameterFact(rcRevParam)->value().toFloat(); _rgRCReversed[i] = floatReversed == -1.0f; } @@ -170,7 +172,7 @@ double FlightModesComponentController::_switchLiveRange(const QString& param) { QVariant value; - int channel = _autoPilotPlugin->getFact(param)->value().toInt(); + int channel = _autoPilotPlugin->getParameterFact(param)->value().toInt(); if (channel == 0) { return 1.0; } else { diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index 3ea406a24d8b699d6aa6c5d2a66d76c6de092120..3aca2983231a316a84ff2a80bd0b496e4bf8331d 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -80,7 +80,7 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) : _parameterFacts = new PX4ParameterFacts(uas, this); Q_CHECK_PTR(_parameterFacts); - connect(_parameterFacts, &PX4ParameterFacts::factsReady, this, &PX4AutoPilotPlugin::_pluginReadyPreChecks); + connect(_parameterFacts, &PX4ParameterFacts::parametersReady, this, &PX4AutoPilotPlugin::_pluginReadyPreChecks); PX4ParameterFacts::loadParameterFactMetaData(); } @@ -189,7 +189,7 @@ void PX4AutoPilotPlugin::clearStaticData(void) PX4ParameterFacts::clearStaticData(); } -const QVariantList& PX4AutoPilotPlugin::components(void) +const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void) { if (_components.count() == 0 && !_incorrectParameterVersion) { Q_ASSERT(_uas); @@ -222,29 +222,19 @@ const QVariantList& PX4AutoPilotPlugin::components(void) return _components; } -const QVariantMap& PX4AutoPilotPlugin::parameters(void) -{ - return _parameterFacts->factMap(); -} - -QUrl PX4AutoPilotPlugin::setupBackgroundImage(void) -{ - return QUrl::fromUserInput("qrc:/qml/px4fmu_2.x.png"); -} - /// This will perform various checks prior to signalling that the plug in ready void PX4AutoPilotPlugin::_pluginReadyPreChecks(void) { // Check for older parameter version set // FIXME: Firmware is moving to version stamp parameter set. Once that is complete the version stamp // should be used instead. - if (parameters().contains("SENS_GYRO_XOFF")) { + if (parameterExists("SENS_GYRO_XOFF")) { _incorrectParameterVersion = true; QGCMessageBox::warning(tr("Setup"), tr("This version of GroundControl can only perform vehicle setup on a newer version of firmware. " "Please perform a Firmware Upgrade if you wish to use Vehicle Setup.")); } else { // Check for missing setup complete - foreach(const QVariant componentVariant, components()) { + foreach(const QVariant componentVariant, vehicleComponents()) { VehicleComponent* component = qobject_cast(qvariant_cast(componentVariant)); Q_ASSERT(component); diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h index ad7aead1a919c2e1ac96c4a1ed8184478c283e0e..9db1554ca432af11f03d22f2803aa638aabfa771 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h @@ -50,21 +50,20 @@ public: ~PX4AutoPilotPlugin(); // Overrides from AutoPilotPlugin - virtual const QVariantList& components(void); - virtual const QVariantMap& parameters(void); - virtual QUrl setupBackgroundImage(void); + virtual const QVariantList& vehicleComponents(void); + virtual ParameterLoader* getParameterLoader(void) { return _parameterFacts; } static QList getModes(void); static QString getShortModeText(uint8_t baseMode, uint32_t customMode); static void clearStaticData(void); // These methods should only be used by objects within the plugin - AirframeComponent* airframeComponent(void) { return _airframeComponent; } - RadioComponent* radioComponent(void) { return _radioComponent; } - FlightModesComponent* flightModesComponent(void) { return _flightModesComponent; } - SensorsComponent* sensorsComponent(void) { return _sensorsComponent; } - SafetyComponent* safetyComponent(void) { return _safetyComponent; } - PowerComponent* powerComponent(void) { return _powerComponent; } + AirframeComponent* airframeComponent(void) { return _airframeComponent; } + RadioComponent* radioComponent(void) { return _radioComponent; } + FlightModesComponent* flightModesComponent(void) { return _flightModesComponent; } + SensorsComponent* sensorsComponent(void) { return _sensorsComponent; } + SafetyComponent* safetyComponent(void) { return _safetyComponent; } + PowerComponent* powerComponent(void) { return _powerComponent; } private slots: void _pluginReadyPreChecks(void); diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc index 5a055114869b55bcac6b09e105f73956affea028..1d1272930e3f29baef05f00ba88b51b5723daad6 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc +++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc @@ -37,7 +37,7 @@ bool PX4ParameterFacts::_parameterMetaDataLoaded = false; QMap PX4ParameterFacts::_mapParameterName2FactMetaData; PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) : - FactLoader(uas, parent) + ParameterLoader(uas, parent) { Q_ASSERT(uas); } @@ -273,6 +273,6 @@ void PX4ParameterFacts::_addMetaDataToFact(Fact* fact) fact->setMetaData(_mapParameterName2FactMetaData[fact->name()]); } else { // Use generic meta data - FactLoader::_addMetaDataToFact(fact); + ParameterLoader::_addMetaDataToFact(fact); } } diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h index e39ba6eac9c8ba72a7a4dc9165e9c3a877b46ccb..3b733777620aa717ab23433aea788c3407db20c9 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h +++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h @@ -21,14 +21,15 @@ ======================================================================*/ -#ifndef PX4ParameterFacts_h -#define PX4ParameterFacts_h +#ifndef PX4PARAMETERFACTS_H +#define PX4PARAMETERFACTS_H #include #include #include #include +#include "ParameterLoader.h" #include "FactSystem.h" #include "UASInterface.h" @@ -39,13 +40,16 @@ Q_DECLARE_LOGGING_CATEGORY(PX4ParameterFactsMetaDataLog) /// Collection of Parameter Facts for PX4 AutoPilot -class PX4ParameterFacts : public FactLoader +class PX4ParameterFacts : public ParameterLoader { Q_OBJECT public: /// @param uas Uas which this set of facts is associated with PX4ParameterFacts(UASInterface* uas, QObject* parent = NULL); + + /// Override from ParameterLoader + virtual QString getDefaultComponentIdParam(void) const { return QString("SYS_AUTOSTART"); } static void loadParameterFactMetaData(void); static void deleteParameterFactMetaData(void); diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 6238d683e28c489d6084624d728f59fad194c20a..ecd356595188016546b4dabd861871696ebad393 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -268,15 +268,15 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in void SensorsComponentController::_refreshParams(void) { // Pull specified params first so red/green indicators update quickly - _autopilot->refreshParameter("CAL_MAG0_ID"); - _autopilot->refreshParameter("CAL_GYRO0_ID"); - _autopilot->refreshParameter("CAL_ACC0_ID"); - _autopilot->refreshParameter("SENS_DPRES_OFF"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ID"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_GYRO0_ID"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_ACC0_ID"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF"); - _autopilot->refreshParameter("CAL_MAG0_ROT"); - _autopilot->refreshParameter("CAL_MAG1_ROT"); - _autopilot->refreshParameter("CAL_MAG2_ROT"); - _autopilot->refreshParameter("SENS_BOARD_ROT"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ROT"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG1_ROT"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG2_ROT"); + _autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_BOARD_ROT"); // Pull full set in order to get all cal values back _autopilot->refreshAllParameters(); diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index bed0ddf213786edb2c83b656b22770a117c8944b..d50d429ee02610881f57b94eae4e521ed3df9886 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -28,9 +28,10 @@ #include -Fact::Fact(QString name, FactMetaData::ValueType_t type, QObject* parent) : +Fact::Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent) : QObject(parent), _name(name), + _componentId(componentId), _type(type), _metaData(NULL) { @@ -75,6 +76,11 @@ QString Fact::name(void) const return _name; } +int Fact::componentId(void) const +{ + return _componentId; +} + QVariant Fact::value(void) const { return _value; diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index 2398a98b5e619e984cde7ade23101c243a301792..798b23e61982dc5e631865cc869a64a0b0f36dd8 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -44,6 +44,7 @@ class Fact : public QObject Q_OBJECT Q_PROPERTY(QString name READ name CONSTANT) + Q_PROPERTY(int componentId READ componentId CONSTANT) Q_PROPERTY(QVariant value READ value WRITE setValue NOTIFY valueChanged USER true) Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged) Q_PROPERTY(QVariant defaultValue READ defaultValue CONSTANT) @@ -57,13 +58,17 @@ class Fact : public QObject Q_ENUMS(FactMetaData::ValueType_t) public: - Fact(QString name = "", FactMetaData::ValueType_t type = FactMetaData::valueTypeInt32, QObject* parent = NULL); + //Fact(int componentId, QString name = "", FactMetaData::ValueType_t type = FactMetaData::valueTypeInt32, QObject* parent = NULL); + Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent = NULL); // Property system methods - /// Read accessor or name property + /// 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; @@ -112,6 +117,7 @@ signals: private: QString _name; + int _componentId; QVariant _value; FactMetaData::ValueType_t _type; FactMetaData* _metaData; diff --git a/src/FactSystem/FactBinder.cc b/src/FactSystem/FactBinder.cc index 9d91487e6df526959457b4008eca86ca0f13614f..a5d92b6251ce5681a9116a5522c5d8d5d5ae2a03 100644 --- a/src/FactSystem/FactBinder.cc +++ b/src/FactSystem/FactBinder.cc @@ -31,7 +31,8 @@ FactBinder::FactBinder(void) : _autopilotPlugin(NULL), - _fact(NULL) + _fact(NULL), + _componentId(FactSystem::defaultComponentId) { UASInterface* uas = UASManager::instance()->getActiveUAS(); Q_ASSERT(uas); @@ -58,8 +59,8 @@ void FactBinder::setName(const QString& name) } if (!name.isEmpty()) { - if (_autopilotPlugin->factExists(name)) { - _fact = _autopilotPlugin->getFact(name); + if (_autopilotPlugin->factExists(FactSystem::ParameterProvider, _componentId, name)) { + _fact = _autopilotPlugin->getFact(FactSystem::ParameterProvider, _componentId, name); connect(_fact, &Fact::valueChanged, this, &FactBinder::valueChanged); emit valueChanged(); diff --git a/src/FactSystem/FactBinder.h b/src/FactSystem/FactBinder.h index 897396429b58f9b6093806eaab3558e2e3e91c8a..a91db0d916593d990ab58787b2a4ea118c1f5e74 100644 --- a/src/FactSystem/FactBinder.h +++ b/src/FactSystem/FactBinder.h @@ -38,6 +38,7 @@ class FactBinder : public QObject { Q_OBJECT + Q_PROPERTY(int componentId MEMBER _componentId NOTIFY componentIdChanged) Q_PROPERTY(QString name READ name WRITE setName NOTIFY nameChanged) Q_PROPERTY(QVariant value READ value WRITE setValue NOTIFY valueChanged USER true) Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged) @@ -46,6 +47,9 @@ class FactBinder : public QObject public: FactBinder(void); + int componentId(void) const; + void setComponentId(int componentId); + QString name(void) const; void setName(const QString& name); @@ -58,12 +62,14 @@ public: QString units(void) const; signals: + void componentIdChanged(void); void nameChanged(void); void valueChanged(void); private: AutoPilotPlugin* _autopilotPlugin; Fact* _fact; + int _componentId; }; #endif \ No newline at end of file diff --git a/src/FactSystem/FactLoader.cc b/src/FactSystem/FactLoader.cc deleted file mode 100644 index 3b207542a1b30048e673313226c15d511dc3e3f0..0000000000000000000000000000000000000000 --- a/src/FactSystem/FactLoader.cc +++ /dev/null @@ -1,200 +0,0 @@ -/*===================================================================== - - 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 "FactLoader.h" -#include "QGCApplication.h" -#include "QGCLoggingCategory.h" - -#include -#include - -QGC_LOGGING_CATEGORY(FactLoaderLog, "FactLoaderLog") - -FactLoader::FactLoader(UASInterface* uas, QObject* parent) : - QObject(parent), - _lastSeenComponent(-1), - _paramMgr(NULL), - _factsReady(false) -{ - Q_ASSERT(uas); - - _uasId = uas->getUASID(); - - _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())); - - // We track parameters changes to keep Facts up to date. - connect(uas, &UASInterface::parameterUpdate, this, &FactLoader::_parameterUpdate); -} - -FactLoader::~FactLoader() -{ - -} - -/// Called whenever a parameter is updated or first seen. -void FactLoader::_parameterUpdate(int uas, int component, QString parameterName, int mavType, QVariant value) -{ - // Is this for our uas? - if (uas != _uasId) { - return; - } - - if (_lastSeenComponent == -1) { - _lastSeenComponent = component; - } else { - // Code cannot handle parameters coming form different components yets - Q_ASSERT(component == _lastSeenComponent); - } - - bool setMetaData = false; - if (!_mapParameterName2Variant.contains(parameterName)) { - qCDebug(FactLoaderLog) << "Adding new fact" << parameterName; - - FactMetaData::ValueType_t factType; - switch (mavType) { - case MAV_PARAM_TYPE_UINT8: - factType = FactMetaData::valueTypeUint8; - break; - case MAV_PARAM_TYPE_INT8: - factType = FactMetaData::valueTypeUint8; - break; - case MAV_PARAM_TYPE_UINT16: - factType = FactMetaData::valueTypeUint16; - break; - case MAV_PARAM_TYPE_INT16: - factType = FactMetaData::valueTypeInt16; - break; - case MAV_PARAM_TYPE_UINT32: - factType = FactMetaData::valueTypeUint32; - break; - case MAV_PARAM_TYPE_INT32: - factType = FactMetaData::valueTypeInt32; - break; - case MAV_PARAM_TYPE_REAL32: - factType = FactMetaData::valueTypeFloat; - break; - case MAV_PARAM_TYPE_REAL64: - factType = FactMetaData::valueTypeDouble; - break; - default: - factType = FactMetaData::valueTypeInt32; - qCritical() << "Unsupported fact type" << mavType; - break; - } - - Fact* fact = new Fact(parameterName, factType, this); - setMetaData = true; - - _mapParameterName2Variant[parameterName] = QVariant::fromValue(fact); - _mapFact2ParameterName[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, &FactLoader::_valueUpdated); - } - - Q_ASSERT(_mapParameterName2Variant.contains(parameterName)); - - qCDebug(FactLoaderLog) << "Updating fact value" << parameterName << value; - - Fact* fact = _mapParameterName2Variant[parameterName].value(); - Q_ASSERT(fact); - fact->_containerSetValue(value); - - if (setMetaData) { - _addMetaDataToFact(fact); - } -} - -/// Connected to Fact::valueUpdated -/// -/// Sets the new value into the Parameter Manager. Parameter is persisted after send. -void FactLoader::_valueUpdated(const QVariant& value) -{ - Fact* fact = qobject_cast(sender()); - Q_ASSERT(fact); - - Q_ASSERT(_lastSeenComponent != -1); - Q_ASSERT(_paramMgr); - Q_ASSERT(_mapFact2ParameterName.contains(fact)); - - QVariant typedValue; - switch (fact->type()) { - case FactMetaData::valueTypeInt8: - case FactMetaData::valueTypeInt16: - case FactMetaData::valueTypeInt32: - typedValue.setValue(QVariant(value.toInt())); - break; - - case FactMetaData::valueTypeUint8: - case FactMetaData::valueTypeUint16: - case FactMetaData::valueTypeUint32: - typedValue.setValue(value.toUInt()); - break; - - case FactMetaData::valueTypeFloat: - typedValue.setValue(value.toFloat()); - break; - - case FactMetaData::valueTypeDouble: - typedValue.setValue(value.toDouble()); - break; - } - - qCDebug(FactLoaderLog) << "Set parameter" << fact->name() << typedValue; - - _paramMgr->setParameter(_lastSeenComponent, _mapFact2ParameterName[fact], typedValue); - _paramMgr->sendPendingParameters(true /* persistAfterSend */, false /* forceSend */); -} - -// Called when param mgr list is up to date -void FactLoader::_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 FactLoader::_addMetaDataToFact(Fact* fact) -{ - FactMetaData* metaData = new FactMetaData(this); - metaData->initFromTypeOnly(fact->type()); -} diff --git a/src/FactSystem/FactLoader.h b/src/FactSystem/FactLoader.h deleted file mode 100644 index a61b147543b943d2841ff0f63fcf026265ee8402..0000000000000000000000000000000000000000 --- a/src/FactSystem/FactLoader.h +++ /dev/null @@ -1,86 +0,0 @@ -/*===================================================================== - - 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 . - - ======================================================================*/ - -#ifndef FactLoader_h -#define FactLoader_h - -#include -#include -#include -#include - -#include "Fact.h" -#include "UASInterface.h" - -/// @file -/// @author Don Gagne - -Q_DECLARE_LOGGING_CATEGORY(FactLoaderLog) - -/// Connects to Parameter Manager to load/update Facts -class FactLoader : public QObject -{ - Q_OBJECT - -public: - /// @param uas Uas which this set of facts is associated with - FactLoader(UASInterface* uas, QObject* parent = NULL); - - ~FactLoader(); - - /// Returns true if the full set of facts are ready - bool factsAreReady(void) { return _factsReady; } - - /// Returns the fact QVariantMap - const QVariantMap& factMap(void) { return _mapParameterName2Variant; } - -signals: - /// Signalled when the full set of facts are ready - void factsReady(void); - -protected: - /// Base implementation adds generic meta data based on variant type. Derived class can override to provide - /// more details meta data. - virtual void _addMetaDataToFact(Fact* fact); - -private slots: - void _parameterUpdate(int uas, int component, QString parameterName, int mavType, QVariant value); - void _valueUpdated(const QVariant& value); - void _paramMgrParameterListUpToDate(void); - -private: - static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); - - QMap _mapFact2ParameterName; ///< Maps from a Fact to a parameter name - - int _uasId; ///< Id for uas which this set of Facts are associated with - int _lastSeenComponent; - - QGCUASParamManagerInterface* _paramMgr; - - QVariantMap _mapParameterName2Variant; - - bool _factsReady; ///< All facts received from param mgr -}; - -#endif \ No newline at end of file diff --git a/src/FactSystem/FactSystem.cc b/src/FactSystem/FactSystem.cc index 528f75bdba3fcef5e5539aae4b53de9d8ebc01fb..2ff881016eee2e8bbd7000ff4bf3a6fef1820176 100644 --- a/src/FactSystem/FactSystem.cc +++ b/src/FactSystem/FactSystem.cc @@ -40,8 +40,6 @@ FactSystem::FactSystem(QObject* parent) : QGCSingleton(parent) { qmlRegisterType(_factSystemQmlUri, 1, 0, "Fact"); - - // FIXME: Where should these go? qmlRegisterUncreatableType(_factSystemQmlUri, 1, 0, "VehicleComponent", "Can only reference VehicleComponent"); } diff --git a/src/FactSystem/FactSystem.h b/src/FactSystem/FactSystem.h index 8a9b533ac685fc66adde0cee9c0a46c27ce34c3d..4c1377e988e1bdd9179b548aecfedc4a7abe0d21 100644 --- a/src/FactSystem/FactSystem.h +++ b/src/FactSystem/FactSystem.h @@ -24,15 +24,12 @@ /// @file /// @author Don Gagne -#ifndef FactSystem_h -#define FactSystem_h +#ifndef FACTSYSTEM_H +#define FACTSYSTEM_H #include "Fact.h" -#include "FactLoader.h" #include "FactMetaData.h" -#include "UASInterface.h" #include "QGCSingleton.h" -#include "FactValidator.h" /// FactSystem is a singleton which provides access to the Facts in the system /// @@ -42,12 +39,20 @@ /// settings. Client code can then use this system to expose sets of Facts to QML code. An example /// of this is the PX4ParameterFacts onbject which is part of the PX4 AutoPilot plugin. It exposes /// the firmware parameters to QML such that you can bind QML ui elements directly to parameters. + class FactSystem : public QGCSingleton { Q_OBJECT DECLARE_QGC_SINGLETON(FactSystem, FactSystem) +public: + typedef enum { + ParameterProvider, + } Provider_t; + + static const int defaultComponentId = -1; + private: /// All access to FactSystem is through FactSystem::instance, so constructor is private FactSystem(QObject* parent = NULL); diff --git a/src/FactSystem/FactSystemTest.qml b/src/FactSystem/FactSystemTest.qml index 29048fbbb34257c962d2fd3c76593657c4cb620f..b9e3d1fd7caab165dfb6c3753fcdd88760c731e4 100644 --- a/src/FactSystem/FactSystemTest.qml +++ b/src/FactSystem/FactSystemTest.qml @@ -26,14 +26,19 @@ import QtQuick.Controls 1.2 import QGroundControl.FactSystem 1.0 Item { + // Use default component id TextInput { objectName: "testControl" - Fact { id: fact; name: "RC_MAP_THROTTLE" } - text: fact.value - font.family: "Helvetica" - font.pointSize: 24; - color: "red" - focus: true - onAccepted: { fact.value = text; } + Fact { id: fact1; name: "RC_MAP_THROTTLE" } + text: fact1.value + onAccepted: { fact1.value = text; } + } + + // Use specific component id + TextInput { + objectName: "testControl" + Fact { id: fact2; name: "COMPONENT_51"; componentId: 51 } + text: fact2.value + onAccepted: { fact2.value = text; } } } diff --git a/src/FactSystem/FactSystemTestBase.cc b/src/FactSystem/FactSystemTestBase.cc index 29d459bb8d0548fc68e579a63d4ae6c211c0163e..b4751c8b001f58f6a8658fc09a5f5fe0130ed7cf 100644 --- a/src/FactSystem/FactSystemTestBase.cc +++ b/src/FactSystem/FactSystemTestBase.cc @@ -85,21 +85,45 @@ void FactSystemTestBase::_cleanup(void) } /// Basic test of parameter values in Fact System -void FactSystemTestBase::_parameter_test(void) +void FactSystemTestBase::_parameter_default_component_id_test(void) { - // Get the parameter facts from the AutoPilot + // Compare the value in the Parameter Manager with the value from the FactSystem. - const QVariantMap& parameterFacts = _plugin->parameters(); + QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, FactSystem::defaultComponentId, "RC_MAP_THROTTLE")); + Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, FactSystem::defaultComponentId, "RC_MAP_THROTTLE"); + QVERIFY(fact != NULL); + QVariant factValue = fact->value(); + QCOMPARE(factValue.isValid(), true); + QVariant paramValue; + Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue)); - // Compare the value in the Parameter Manager with the value from the FactSystem + QCOMPARE(factValue.toInt(), paramValue.toInt()); +} + +void FactSystemTestBase::_parameter_specific_component_id_test(void) +{ + // Compare the value in the Parameter Manager with the value from the FactSystem. - Fact* fact = parameterFacts["RC_MAP_THROTTLE"].value(); + QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE")); + Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE"); QVERIFY(fact != NULL); QVariant factValue = fact->value(); QCOMPARE(factValue.isValid(), true); + QVariant paramValue; - Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue)); + Q_ASSERT(_paramMgr->getParameterValue(50, "RC_MAP_THROTTLE", paramValue)); + + QCOMPARE(factValue.toInt(), paramValue.toInt()); + + // Test another component id + QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 51, "COMPONENT_51")); + fact = _plugin->getFact(FactSystem::ParameterProvider, 51, "COMPONENT_51"); + QVERIFY(fact != NULL); + factValue = fact->value(); + QCOMPARE(factValue.isValid(), true); + + Q_ASSERT(_paramMgr->getParameterValue(51, "COMPONENT_51", paramValue)); QCOMPARE(factValue.toInt(), paramValue.toInt()); } @@ -129,9 +153,7 @@ void FactSystemTestBase::_paramMgrSignal_test(void) { // Get the parameter Fact from the AutoPilot - const QVariantMap& parameterFacts = _plugin->parameters(); - - Fact* fact = parameterFacts["RC_MAP_THROTTLE"].value(); + Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, -1, "RC_MAP_THROTTLE"); QVERIFY(fact != NULL); // Setting a new value into the parameter should trigger a valueChanged signal on the Fact diff --git a/src/FactSystem/FactSystemTestBase.h b/src/FactSystem/FactSystemTestBase.h index 818f765cd174d20a51eaa0203e58fc687f043af8..3d0536c909a70c535c260f382d06a347dc0628ee 100644 --- a/src/FactSystem/FactSystemTestBase.h +++ b/src/FactSystem/FactSystemTestBase.h @@ -43,7 +43,8 @@ protected: void _init(MAV_AUTOPILOT autopilot); void _cleanup(void); - void _parameter_test(void); + void _parameter_default_component_id_test(void); + void _parameter_specific_component_id_test(void); void _qml_test(void); void _paramMgrSignal_test(void); void _qmlUpdate_test(void); diff --git a/src/FactSystem/FactSystemTestGeneric.h b/src/FactSystem/FactSystemTestGeneric.h index 0435ccf6510adcaccad8c35343f44bfd064d3d08..d54380e21a34aa313b58466827ed7db315f1696b 100644 --- a/src/FactSystem/FactSystemTestGeneric.h +++ b/src/FactSystem/FactSystemTestGeneric.h @@ -43,7 +43,8 @@ private slots: void init(void); void cleanup(void) { _cleanup(); } - void parameter_test(void) { _parameter_test(); } + void parameter_default_component_id_test(void) { _parameter_default_component_id_test(); } + void parameter_specific_component_id_test(void) { _parameter_specific_component_id_test(); } void qml_test(void) { _qml_test(); } void paramMgrSignal_test(void) { _paramMgrSignal_test(); } void qmlUpdate_test(void) { _qmlUpdate_test(); } diff --git a/src/FactSystem/FactSystemTestPX4.h b/src/FactSystem/FactSystemTestPX4.h index 54118e9070c3861ea8c07cda2ad1bbf3952b39ca..d6024a24b799d8d8e3774286f632166bd0f273b8 100644 --- a/src/FactSystem/FactSystemTestPX4.h +++ b/src/FactSystem/FactSystemTestPX4.h @@ -43,7 +43,8 @@ private slots: void init(void); void cleanup(void) { _cleanup(); } - void parameter_test(void) { _parameter_test(); } + void parameter_default_component_id_test(void) { _parameter_default_component_id_test(); } + void parameter_specific_component_id_test(void) { _parameter_specific_component_id_test(); } void qml_test(void) { _qml_test(); } void paramMgrSignal_test(void) { _paramMgrSignal_test(); } void qmlUpdate_test(void) { _qmlUpdate_test(); } diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc new file mode 100644 index 0000000000000000000000000000000000000000..5d66a9ccbec3101f10881505a6217968602d427c --- /dev/null +++ b/src/FactSystem/ParameterLoader.cc @@ -0,0 +1,282 @@ +/*===================================================================== + + 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 "ParameterLoader.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" + +#include +#include + +QGC_LOGGING_CATEGORY(ParameterLoaderLog, "ParameterLoaderLog") + +ParameterLoader::ParameterLoader(UASInterface* uas, QObject* parent) : + QObject(parent), + _paramMgr(NULL), + _parametersReady(false), + _defaultComponentId(FactSystem::defaultComponentId) +{ + Q_ASSERT(uas); + + _uasId = uas->getUASID(); + + _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())); + + // We track parameters changes to keep Facts up to date. + connect(uas, &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate); +} + +ParameterLoader::~ParameterLoader() +{ + +} + +/// Called whenever a parameter is updated or first seen. +void ParameterLoader::_parameterUpdate(int uas, int componentId, QString parameterName, int mavType, QVariant value) +{ + bool setMetaData = false; + + // Is this for our uas? + if (uas != _uasId) { + return; + } + + // Attempt to determine default component id + if (_defaultComponentId == FactSystem::defaultComponentId && _defaultComponentIdParam.isEmpty()) { + _defaultComponentIdParam = getDefaultComponentIdParam(); + } + if (!_defaultComponentIdParam.isEmpty() && _defaultComponentIdParam == parameterName) { + _defaultComponentId = componentId; + } + + 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; + switch (mavType) { + case MAV_PARAM_TYPE_UINT8: + factType = FactMetaData::valueTypeUint8; + break; + case MAV_PARAM_TYPE_INT8: + factType = FactMetaData::valueTypeUint8; + break; + case MAV_PARAM_TYPE_UINT16: + factType = FactMetaData::valueTypeUint16; + break; + case MAV_PARAM_TYPE_INT16: + factType = FactMetaData::valueTypeInt16; + break; + case MAV_PARAM_TYPE_UINT32: + factType = FactMetaData::valueTypeUint32; + break; + case MAV_PARAM_TYPE_INT32: + factType = FactMetaData::valueTypeInt32; + break; + case MAV_PARAM_TYPE_REAL32: + factType = FactMetaData::valueTypeFloat; + break; + case MAV_PARAM_TYPE_REAL64: + factType = FactMetaData::valueTypeDouble; + break; + default: + factType = FactMetaData::valueTypeInt32; + qCritical() << "Unsupported fact type" << mavType; + break; + } + + Fact* fact = new Fact(componentId, parameterName, factType, this); + 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); + } + + Q_ASSERT(_mapParameterName2Variant[componentId].contains(parameterName)); + + qCDebug(ParameterLoaderLog) << "Updating fact value (component:" << componentId << "name:" << parameterName << value << ")"; + + Fact* fact = _mapParameterName2Variant[componentId][parameterName].value(); + Q_ASSERT(fact); + fact->_containerSetValue(value); + + if (setMetaData) { + _addMetaDataToFact(fact); + } +} + +/// Connected to Fact::valueUpdated +/// +/// Sets the new value into the Parameter Manager. Parameter is persisted after send. +void ParameterLoader::_valueUpdated(const QVariant& value) +{ + Fact* fact = qobject_cast(sender()); + Q_ASSERT(fact); + + int componentId = fact->componentId(); + + Q_ASSERT(_paramMgr); + Q_ASSERT(_mapFact2ParameterName.contains(componentId)); + Q_ASSERT(_mapFact2ParameterName[componentId].contains(fact)); + + QVariant typedValue; + switch (fact->type()) { + case FactMetaData::valueTypeInt8: + case FactMetaData::valueTypeInt16: + case FactMetaData::valueTypeInt32: + typedValue.setValue(QVariant(value.toInt())); + break; + + case FactMetaData::valueTypeUint8: + case FactMetaData::valueTypeUint16: + case FactMetaData::valueTypeUint32: + typedValue.setValue(value.toUInt()); + break; + + case FactMetaData::valueTypeFloat: + typedValue.setValue(value.toFloat()); + break; + + case FactMetaData::valueTypeDouble: + typedValue.setValue(value.toDouble()); + break; + } + + qCDebug(ParameterLoaderLog) << "Set parameter (componentId:" << componentId << "name:" << fact->name() << typedValue << ")"; + + _paramMgr->setParameter(componentId, fact->name(), typedValue); + _paramMgr->sendPendingParameters(true /* persistAfterSend */, false /* forceSend */); +} + +// Called when param mgr list is up to date +void ParameterLoader::_paramMgrParameterListUpToDate(void) +{ + if (!_parametersReady) { + _parametersReady = 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(); + + _determineDefaultComponentId(); + + // We should have all parameters now so we can signal ready + emit parametersReady(); + } +} + +void ParameterLoader::_addMetaDataToFact(Fact* fact) +{ + FactMetaData* metaData = new FactMetaData(this); + metaData->initFromTypeOnly(fact->type()); +} + +void ParameterLoader::refreshAllParameters(void) +{ + Q_ASSERT(_paramMgr); + _paramMgr->requestParameterList(); +} + +void ParameterLoader::_determineDefaultComponentId(void) +{ + if (_defaultComponentId == FactSystem::defaultComponentId) { + // We don't have a default component id yet. That means the plugin can't provide + // the param to trigger off of. Instead we use the most prominent component id in + // the set of parameters. Better than nothing! + + _defaultComponentId = -1; + foreach(int componentId, _mapParameterName2Variant.keys()) { + if (_mapParameterName2Variant[componentId].count() > _defaultComponentId) { + _defaultComponentId = componentId; + } + } + Q_ASSERT(_defaultComponentId != -1); + } +} + +/// Translates FactSystem::defaultComponentId to real component id if needed +int ParameterLoader::_actualComponentId(int componentId) +{ + if (componentId == FactSystem::defaultComponentId) { + componentId = _defaultComponentId; + Q_ASSERT(componentId != FactSystem::defaultComponentId); + } + + return componentId; +} + +void ParameterLoader::refreshParameter(int componentId, const QString& name) +{ + Q_ASSERT(_paramMgr); + + _paramMgr->requestParameterUpdate(_actualComponentId(componentId), name); +} + +void ParameterLoader::refreshParametersPrefix(int componentId, const QString& namePrefix) +{ + Q_ASSERT(_paramMgr); + + componentId = _actualComponentId(componentId); + Q_ASSERT(_mapFact2ParameterName.contains(componentId)); + foreach(QString name, _mapParameterName2Variant[componentId].keys()) { + if (name.startsWith(namePrefix)) { + refreshParameter(componentId, name); + } + } +} + +bool ParameterLoader::factExists(int componentId, const QString& name) +{ + componentId = _actualComponentId(componentId); + if (_mapParameterName2Variant.contains(componentId)) { + return _mapParameterName2Variant[componentId].contains(name); + } + return false; +} + +Fact* ParameterLoader::getFact(int componentId, const QString& name) +{ + componentId = _actualComponentId(componentId); + Q_ASSERT(_mapParameterName2Variant.contains(componentId)); + Q_ASSERT(_mapParameterName2Variant[componentId].contains(name)); + Fact* fact = _mapParameterName2Variant[componentId][name].value(); + Q_ASSERT(fact); + return fact; +} diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h new file mode 100644 index 0000000000000000000000000000000000000000..6c1951059cd89a2db12670b9ed07c601d646e397 --- /dev/null +++ b/src/FactSystem/ParameterLoader.h @@ -0,0 +1,113 @@ +/*===================================================================== + + 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 . + + ======================================================================*/ + +#ifndef PARAMETERLOADER_H +#define PARAMETERLOADER_H + +#include +#include +#include +#include + +#include "FactSystem.h" +#include "UASInterface.h" + +/// @file +/// @author Don Gagne + +Q_DECLARE_LOGGING_CATEGORY(ParameterLoaderLog) + +/// Connects to Parameter Manager to load/update Facts +class ParameterLoader : public QObject +{ + Q_OBJECT + +public: + /// @param uas Uas which this set of facts is associated with + ParameterLoader(UASInterface* uas, QObject* parent = NULL); + + ~ParameterLoader(); + + /// Returns true if the full set of facts are ready + bool parametersAreReady(void) { return _parametersReady; } + + /// Re-request the full set of parameters from the autopilot + void refreshAllParameters(void); + + /// Request a refresh on the specific parameter + void refreshParameter(int componentId, const QString& name); + + /// 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 the specified Fact. + /// WARNING: Will assert if fact does not exists. If that possibily exists, check for existince first with + /// factExists. + Fact* getFact(int componentId, ///< fact component, -1=default component + const QString& name); ///< fact name + + /// 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; + +signals: + /// Signalled when the full set of facts are ready + void parametersReady(void); + +protected: + /// Base implementation adds generic meta data based on variant type. Derived class can override to provide + /// more details meta data. + virtual void _addMetaDataToFact(Fact* fact); + +private slots: + void _parameterUpdate(int uas, int componentId, QString parameterName, int mavType, QVariant value); + void _valueUpdated(const QVariant& value); + void _paramMgrParameterListUpToDate(void); + +private: + static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); + int _actualComponentId(int componentId); + void _determineDefaultComponentId(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; + + /// First mapping id\s by component id + /// Second mapping is parameter name, to Fact* in QVariant + QMap _mapParameterName2Variant; + + bool _parametersReady; ///< All params received from param mgr + int _defaultComponentId; + QString _defaultComponentIdParam; +}; + +#endif \ No newline at end of file diff --git a/src/VehicleSetup/SetupViewButtonsConnected.qml b/src/VehicleSetup/SetupViewButtonsConnected.qml index f8bd4a9158012a1360514cd5148ccd24c1514f7c..df5514e4c861f905795904b40a2a53deade80dd4 100644 --- a/src/VehicleSetup/SetupViewButtonsConnected.qml +++ b/src/VehicleSetup/SetupViewButtonsConnected.qml @@ -39,7 +39,7 @@ Rectangle { } Repeater { - model: autopilot ? autopilot.components : 0 + model: autopilot ? autopilot.vehicleComponents : 0 SubMenuButton { width: parent.width diff --git a/src/VehicleSetup/VehicleSummary.qml b/src/VehicleSetup/VehicleSummary.qml index 55620ffdb0fd97e1a2fbc496438b6429cf9d0375..a7a81c4cc64843ec43b107e42cee181c1f32b70d 100644 --- a/src/VehicleSetup/VehicleSummary.qml +++ b/src/VehicleSetup/VehicleSummary.qml @@ -72,7 +72,7 @@ Rectangle { spacing: 10 Repeater { - model: autopilot.components + model: autopilot.vehicleComponents // Outer summary item rectangle Rectangle { diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index b62811134325fc7ca7d6527b6437e5dcb7b903da..c4af745c401d88910c2021c66a716d43091bdd07 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -223,6 +223,13 @@ void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected) **/ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { + // Since receiveBytes signals cross threads we can end up with signals in the queue + // that come through after the link is disconnected. For these we just drop the data + // since the link is closed. + if (!LinkManager::instance()->containsLink(link)) { + return; + } + // receiveMutex.lock(); mavlink_message_t message; mavlink_status_t status; diff --git a/src/qgcunittest/MockLink.cc b/src/qgcunittest/MockLink.cc index b3dad7a23195a6b5cc17a4304566f2a090a2e0d7..75c60763b75c389b38b2329999c8a810afab13c9 100644 --- a/src/qgcunittest/MockLink.cc +++ b/src/qgcunittest/MockLink.cc @@ -184,6 +184,7 @@ void MockLink::_loadParams(void) QStringList paramData = line.split("\t"); Q_ASSERT(paramData.count() == 5); + int componentId = paramData.at(1).toInt(); QString paramName = paramData.at(2); QString valStr = paramData.at(3); uint paramType = paramData.at(4).toUInt(); @@ -209,7 +210,7 @@ void MockLink::_loadParams(void) qCDebug(MockLinkLog) << "Loading param" << paramName << paramValue; - _mapParamName2Value[paramName] = paramValue; + _mapParamName2Value[componentId][paramName] = paramValue; _mapParamName2MavParamType[paramName] = static_cast(paramType); } } @@ -368,11 +369,12 @@ void MockLink::_handleSetMode(const mavlink_message_t& msg) _mavCustomMode = request.custom_mode; } -void MockLink::_setParamFloatUnionIntoMap(const QString& paramName, float paramFloat) +void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat) { mavlink_param_union_t valueUnion; - Q_ASSERT(_mapParamName2Value.contains(paramName)); + Q_ASSERT(_mapParamName2Value.contains(componentId)); + Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); valueUnion.param_float = paramFloat; @@ -403,19 +405,20 @@ void MockLink::_setParamFloatUnionIntoMap(const QString& paramName, float paramF } qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant; - _mapParamName2Value[paramName] = paramVariant; + _mapParamName2Value[componentId][paramName] = paramVariant; } /// Convert from a parameter variant to the float value from mavlink_param_union_t -float MockLink::_floatUnionForParam(const QString& paramName) +float MockLink::_floatUnionForParam(int componentId, const QString& paramName) { mavlink_param_union_t valueUnion; - Q_ASSERT(_mapParamName2Value.contains(paramName)); + Q_ASSERT(_mapParamName2Value.contains(componentId)); + Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; - QVariant paramVar = _mapParamName2Value[paramName]; + QVariant paramVar = _mapParamName2Value[componentId][paramName]; switch (paramType) { case MAV_PARAM_TYPE_INT8: @@ -443,38 +446,41 @@ float MockLink::_floatUnionForParam(const QString& paramName) 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); - 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; - - 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); - - qCDebug(MockLinkLog) << "Sending msg_param_value" << paramId << paramType; - - 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); + Q_ASSERT(request.target_component == MAV_COMP_ID_ALL); + + foreach (int componentId, _mapParamName2Value.keys()) { + uint16_t paramIndex = 0; + int cParameters = _mapParamName2Value[componentId].count(); + + foreach(QString paramName, _mapParamName2Value[componentId].keys()) { + char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + mavlink_message_t responseMsg; + + Q_ASSERT(_mapParamName2Value[componentId].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); + + qCDebug(MockLinkLog) << "Sending msg_param_value" << paramId << paramType; + + mavlink_msg_param_value_pack(_vehicleSystemId, + componentId, // component id + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramName), // Parameter value + paramType, // MAV_PARAM_TYPE + cParameters, // Total number of parameters + paramIndex++); // Index of this parameter + _emitMavlinkMessage(responseMsg); + } } } @@ -484,27 +490,29 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg) mavlink_msg_param_set_decode(&msg, &request); Q_ASSERT(request.target_system == _vehicleSystemId); + int componentId = request.target_component; // 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(_mapParamName2Value.contains(componentId)); + Q_ASSERT(_mapParamName2Value[componentId].contains(paramId)); Q_ASSERT(request.param_type == _mapParamName2MavParamType[paramId]); // Save the new value - _setParamFloatUnionIntoMap(paramId, request.param_value); + _setParamFloatUnionIntoMap(componentId, 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 + componentId, // component id + &responseMsg, // Outgoing message + paramId, // Parameter name + request.param_value, // Send same value back + request.param_type, // Send same type back + _mapParamName2Value[componentId].count(), // Total number of parameters + _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter _emitMavlinkMessage(responseMsg); } @@ -512,6 +520,9 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) { mavlink_param_request_read_t request; mavlink_msg_param_request_read_decode(&msg, &request); + + int componentId = request.target_component; + Q_ASSERT(_mapParamName2Value.contains(componentId)); char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; paramId[0] = 0; @@ -526,24 +537,24 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value.count()); - QString key = _mapParamName2Value.keys().at(request.param_index); + QString key = _mapParamName2Value[componentId].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(_mapParamName2Value[componentId].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 + componentId, // component id + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramId), // Parameter value + _mapParamName2MavParamType[paramId], // Parameter type + _mapParamName2Value[componentId].count(), // Total number of parameters + _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter _emitMavlinkMessage(responseMsg); } diff --git a/src/qgcunittest/MockLink.h b/src/qgcunittest/MockLink.h index b8c1b20ee65f275fba499db7556cf5b1947f41c7..9c0c0605d961c5dd953950161d0df8eb011ab382 100644 --- a/src/qgcunittest/MockLink.h +++ b/src/qgcunittest/MockLink.h @@ -117,8 +117,8 @@ private: void _handleMissionRequestList(const mavlink_message_t& msg); void _handleMissionRequest(const mavlink_message_t& msg); void _handleMissionItem(const mavlink_message_t& msg); - float _floatUnionForParam(const QString& paramName); - void _setParamFloatUnionIntoMap(const QString& paramName, float paramFloat); + float _floatUnionForParam(int componentId, const QString& paramName); + void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); MockLinkMissionItemHandler* _missionItemHandler; @@ -131,8 +131,8 @@ private: bool _inNSH; bool _mavlinkStarted; - QMap _mapParamName2Value; - QMap _mapParamName2MavParamType; + QMap > _mapParamName2Value; + QMap _mapParamName2MavParamType; typedef QMap MissionList_t; MissionList_t _missionItems; diff --git a/src/qgcunittest/MockLink.param b/src/qgcunittest/MockLink.param index 5ea56e36fb3beb7c1e49f8e326f04b89188815ee..9f9e0c6185c28177ed05f0d180c3dfd9affa34f5 100644 --- a/src/qgcunittest/MockLink.param +++ b/src/qgcunittest/MockLink.param @@ -510,3 +510,4 @@ 1 50 VT_MOT_COUNT 0 6 1 50 VT_POWER_MAX 120 9 1 50 VT_PROP_EFF 0 9 +1 51 COMPONENT_51 51 6