Commit 8dd7b72d authored by Don Gagne's avatar Don Gagne

Merge pull request #1400 from DonLakeFlyer/ComponentIDs

Add component id support through FactSystem
parents 59856660 cc427cdc
...@@ -776,7 +776,7 @@ HEADERS += \ ...@@ -776,7 +776,7 @@ HEADERS += \
src/FactSystem/FactBinder.h \ src/FactSystem/FactBinder.h \
src/FactSystem/FactMetaData.h \ src/FactSystem/FactMetaData.h \
src/FactSystem/FactValidator.h \ src/FactSystem/FactValidator.h \
src/FactSystem/FactLoader.h \ src/FactSystem/ParameterLoader.h \
SOURCES += \ SOURCES += \
src/FactSystem/FactSystem.cc \ src/FactSystem/FactSystem.cc \
...@@ -784,4 +784,4 @@ SOURCES += \ ...@@ -784,4 +784,4 @@ SOURCES += \
src/FactSystem/FactBinder.cc \ src/FactSystem/FactBinder.cc \
src/FactSystem/FactMetaData.cc \ src/FactSystem/FactMetaData.cc \
src/FactSystem/FactValidator.cc \ src/FactSystem/FactValidator.cc \
src/FactSystem/FactLoader.cc \ src/FactSystem/ParameterLoader.cc \
...@@ -34,42 +34,28 @@ AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) : ...@@ -34,42 +34,28 @@ AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) :
Q_ASSERT(_uas); Q_ASSERT(_uas);
} }
void AutoPilotPlugin::refreshAllParameters(void) bool AutoPilotPlugin::factExists(FactSystem::Provider_t provider, int componentId, const QString& name)
{ {
Q_ASSERT(_uas); switch (provider) {
QGCUASParamManagerInterface* paramMgr = _uas->getParamManager(); case FactSystem::ParameterProvider:
Q_ASSERT(paramMgr); return getParameterLoader()->factExists(componentId, name);
paramMgr->requestParameterList();
} // Other providers will go here once they come online
}
void AutoPilotPlugin::refreshParameter(const QString& param)
{
Q_ASSERT(_uas);
QGCUASParamManagerInterface* paramMgr = _uas->getParamManager();
Q_ASSERT(paramMgr);
QList<int> compIdList = paramMgr->getComponentForParam(param); Q_ASSERT(false);
Q_ASSERT(compIdList.count() > 0); return false;
paramMgr->requestParameterUpdate(compIdList[0], param);
} }
void AutoPilotPlugin::refreshParametersPrefix(const QString& paramPrefix) Fact* AutoPilotPlugin::getFact(FactSystem::Provider_t provider, int componentId, const QString& name)
{ {
foreach(QVariant varFact, parameters()) { switch (provider) {
Fact* fact = qvariant_cast<Fact*>(varFact); case FactSystem::ParameterProvider:
Q_ASSERT(fact); return getParameterLoader()->getFact(componentId, name);
if (fact->name().startsWith(paramPrefix)) {
refreshParameter(fact->name()); // Other providers will go here once they come online
}
} }
}
Q_ASSERT(false);
bool AutoPilotPlugin::factExists(const QString& param) return NULL;
{
return parameters().contains(param);
}
Fact* AutoPilotPlugin::getFact(const QString& name)
{
return parameters()[name].value<Fact*>();
} }
...@@ -35,6 +35,7 @@ ...@@ -35,6 +35,7 @@
#include "UASInterface.h" #include "UASInterface.h"
#include "VehicleComponent.h" #include "VehicleComponent.h"
#include "FactSystem.h" #include "FactSystem.h"
#include "ParameterLoader.h"
/// This is the base class for AutoPilot plugins /// This is the base class for AutoPilot plugins
/// ///
...@@ -52,26 +53,43 @@ public: ...@@ -52,26 +53,43 @@ public:
Q_PROPERTY(bool pluginReady READ pluginReady NOTIFY pluginReadyChanged) Q_PROPERTY(bool pluginReady READ pluginReady NOTIFY pluginReadyChanged)
Q_PROPERTY(QVariantList components READ components CONSTANT) /// List of VehicleComponent objects
Q_PROPERTY(QUrl setupBackgroundImage READ setupBackgroundImage CONSTANT) Q_PROPERTY(QVariantList vehicleComponents READ vehicleComponents CONSTANT)
/// Re-request the full set of parameters from the autopilot /// 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 /// 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 /// Request a refresh on all parameters that begin with the specified prefix
Q_INVOKABLE void refreshParametersPrefix(const QString& paramPrefix); 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 /// Returns true if the specifed parameter exists from the default component
virtual const QVariantList& components(void) = 0; Q_INVOKABLE bool parameterExists(const QString& name) { return getParameterLoader()->factExists(FactSystem::defaultComponentId, name); }
virtual const QVariantMap& parameters(void) = 0;
virtual QUrl setupBackgroundImage(void) = 0; /// 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 /// FIXME: Kind of hacky
static void clearStaticData(void); static void clearStaticData(void);
......
...@@ -34,7 +34,7 @@ GenericAutoPilotPlugin::GenericAutoPilotPlugin(UASInterface* uas, QObject* paren ...@@ -34,7 +34,7 @@ GenericAutoPilotPlugin::GenericAutoPilotPlugin(UASInterface* uas, QObject* paren
_parameterFacts = new GenericParameterFacts(uas, this); _parameterFacts = new GenericParameterFacts(uas, this);
Q_CHECK_PTR(_parameterFacts); Q_CHECK_PTR(_parameterFacts);
connect(_parameterFacts, &GenericParameterFacts::factsReady, this, &GenericAutoPilotPlugin::_factsReady); connect(_parameterFacts, &GenericParameterFacts::parametersReady, this, &GenericAutoPilotPlugin::_parametersReady);
} }
QList<AutoPilotPluginManager::FullMode_t> GenericAutoPilotPlugin::getModes(void) QList<AutoPilotPluginManager::FullMode_t> GenericAutoPilotPlugin::getModes(void)
...@@ -87,24 +87,14 @@ void GenericAutoPilotPlugin::clearStaticData(void) ...@@ -87,24 +87,14 @@ void GenericAutoPilotPlugin::clearStaticData(void)
// No Static data yet // No Static data yet
} }
const QVariantList& GenericAutoPilotPlugin::components(void) const QVariantList& GenericAutoPilotPlugin::vehicleComponents(void)
{ {
static QVariantList emptyList; static QVariantList emptyList;
return emptyList; return emptyList;
} }
const QVariantMap& GenericAutoPilotPlugin::parameters(void) void GenericAutoPilotPlugin::_parametersReady(void)
{
return _parameterFacts->factMap();
}
QUrl GenericAutoPilotPlugin::setupBackgroundImage(void)
{
return QUrl::fromUserInput("qrc:/qml/px4fmu_2.x.png");
}
void GenericAutoPilotPlugin::_factsReady(void)
{ {
_pluginReady = true; _pluginReady = true;
emit pluginReadyChanged(_pluginReady); emit pluginReadyChanged(_pluginReady);
......
...@@ -41,16 +41,15 @@ public: ...@@ -41,16 +41,15 @@ public:
GenericAutoPilotPlugin(UASInterface* uas, QObject* parent = NULL); GenericAutoPilotPlugin(UASInterface* uas, QObject* parent = NULL);
// Overrides from AutoPilotPlugin // Overrides from AutoPilotPlugin
virtual QUrl setupBackgroundImage(void); virtual const QVariantList& vehicleComponents(void);
virtual const QVariantList& components(void); virtual ParameterLoader* getParameterLoader(void) { return _parameterFacts; }
virtual const QVariantMap& parameters(void);
static QList<AutoPilotPluginManager::FullMode_t> getModes(void); static QList<AutoPilotPluginManager::FullMode_t> getModes(void);
static QString getShortModeText(uint8_t baseMode, uint32_t customMode); static QString getShortModeText(uint8_t baseMode, uint32_t customMode);
static void clearStaticData(void); static void clearStaticData(void);
private slots: private slots:
void _factsReady(void); void _parametersReady(void);
private: private:
GenericParameterFacts* _parameterFacts; GenericParameterFacts* _parameterFacts;
......
...@@ -25,13 +25,11 @@ ...@@ -25,13 +25,11 @@
/// @author Don Gagne <don@thegagnes.com> /// @author Don Gagne <don@thegagnes.com>
#include "GenericParameterFacts.h" #include "GenericParameterFacts.h"
#include "QGCApplication.h"
#include <QFile>
#include <QDebug> #include <QDebug>
GenericParameterFacts::GenericParameterFacts(UASInterface* uas, QObject* parent) : GenericParameterFacts::GenericParameterFacts(UASInterface* uas, QObject* parent) :
FactLoader(uas, parent) ParameterLoader(uas, parent)
{ {
Q_ASSERT(uas); Q_ASSERT(uas);
} }
...@@ -25,11 +25,8 @@ ...@@ -25,11 +25,8 @@
#define GenericParameterFacts_h #define GenericParameterFacts_h
#include <QObject> #include <QObject>
#include <QMap>
#include <QXmlStreamReader>
#include <QLoggingCategory>
#include "FactSystem.h" #include "ParameterLoader.h"
#include "UASInterface.h" #include "UASInterface.h"
/// @file /// @file
...@@ -37,13 +34,16 @@ ...@@ -37,13 +34,16 @@
/// Collection of Parameter Facts for Generic AutoPilot /// Collection of Parameter Facts for Generic AutoPilot
class GenericParameterFacts : public FactLoader class GenericParameterFacts : public ParameterLoader
{ {
Q_OBJECT Q_OBJECT
public: public:
/// @param uas Uas which this set of facts is associated with /// @param uas Uas which this set of facts is associated with
GenericParameterFacts(UASInterface* uas, QObject* parent = NULL); GenericParameterFacts(UASInterface* uas, QObject* parent = NULL);
/// Override from ParameterLoader
virtual QString getDefaultComponentIdParam(void) const { return QString(); }
}; };
#endif #endif
\ No newline at end of file
...@@ -69,7 +69,9 @@ void FlightModesComponentController::_validateConfiguration(void) ...@@ -69,7 +69,9 @@ void FlightModesComponentController::_validateConfiguration(void)
{ {
_validConfiguration = true; _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) { if (_channelCount <= 0 || _channelCount > _chanMax) {
// Parameter exists, but has not yet been set or is invalid. Use default // Parameter exists, but has not yet been set or is invalid. Use default
_channelCount = _chanMax; _channelCount = _chanMax;
...@@ -84,7 +86,7 @@ void FlightModesComponentController::_validateConfiguration(void) ...@@ -84,7 +86,7 @@ void FlightModesComponentController::_validateConfiguration(void)
switchNames << "Mode Switch" << "Return Switch" << "Loiter Switch" << "PosCtl Switch"; switchNames << "Mode Switch" << "Return Switch" << "Loiter Switch" << "PosCtl Switch";
for(int i=0; i<switchParams.count(); i++) { for(int i=0; i<switchParams.count(); i++) {
int map = _autoPilotPlugin->getFact(switchParams[i])->value().toInt(); int map = _autoPilotPlugin->getParameterFact(switchParams[i])->value().toInt();
switchMappings << map; switchMappings << map;
if (map < 0 || map > _channelCount) { if (map < 0 || map > _channelCount) {
...@@ -101,7 +103,7 @@ void FlightModesComponentController::_validateConfiguration(void) ...@@ -101,7 +103,7 @@ void FlightModesComponentController::_validateConfiguration(void)
attitudeNames << "Throttle" << "Yaw" << "Pitch" << "Roll" << "Flaps" << "Aux1" << "Aux2"; attitudeNames << "Throttle" << "Yaw" << "Pitch" << "Roll" << "Flaps" << "Aux1" << "Aux2";
for (int i=0; i<attitudeParams.count(); i++) { for (int i=0; i<attitudeParams.count(); i++) {
int map = _autoPilotPlugin->getFact(attitudeParams[i])->value().toInt(); int map = _autoPilotPlugin->getParameterFact(attitudeParams[i])->value().toInt();
for (int j=0; j<switchParams.count(); j++) { for (int j=0; j<switchParams.count(); j++) {
if (map != 0 && map == switchMappings[j]) { if (map != 0 && map == switchMappings[j]) {
...@@ -126,10 +128,10 @@ void FlightModesComponentController::setSendLiveRCSwitchRanges(bool start) ...@@ -126,10 +128,10 @@ void FlightModesComponentController::setSendLiveRCSwitchRanges(bool start)
QVariant value; QVariant value;
_rgRCMin[i] = _autoPilotPlugin->getFact(rcMinParam)->value().toInt(); _rgRCMin[i] = _autoPilotPlugin->getParameterFact(rcMinParam)->value().toInt();
_rgRCMax[i] = _autoPilotPlugin->getFact(rcMaxParam)->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; _rgRCReversed[i] = floatReversed == -1.0f;
} }
...@@ -170,7 +172,7 @@ double FlightModesComponentController::_switchLiveRange(const QString& param) ...@@ -170,7 +172,7 @@ double FlightModesComponentController::_switchLiveRange(const QString& param)
{ {
QVariant value; QVariant value;
int channel = _autoPilotPlugin->getFact(param)->value().toInt(); int channel = _autoPilotPlugin->getParameterFact(param)->value().toInt();
if (channel == 0) { if (channel == 0) {
return 1.0; return 1.0;
} else { } else {
......
...@@ -80,7 +80,7 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) : ...@@ -80,7 +80,7 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) :
_parameterFacts = new PX4ParameterFacts(uas, this); _parameterFacts = new PX4ParameterFacts(uas, this);
Q_CHECK_PTR(_parameterFacts); Q_CHECK_PTR(_parameterFacts);
connect(_parameterFacts, &PX4ParameterFacts::factsReady, this, &PX4AutoPilotPlugin::_pluginReadyPreChecks); connect(_parameterFacts, &PX4ParameterFacts::parametersReady, this, &PX4AutoPilotPlugin::_pluginReadyPreChecks);
PX4ParameterFacts::loadParameterFactMetaData(); PX4ParameterFacts::loadParameterFactMetaData();
} }
...@@ -189,7 +189,7 @@ void PX4AutoPilotPlugin::clearStaticData(void) ...@@ -189,7 +189,7 @@ void PX4AutoPilotPlugin::clearStaticData(void)
PX4ParameterFacts::clearStaticData(); PX4ParameterFacts::clearStaticData();
} }
const QVariantList& PX4AutoPilotPlugin::components(void) const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void)
{ {
if (_components.count() == 0 && !_incorrectParameterVersion) { if (_components.count() == 0 && !_incorrectParameterVersion) {
Q_ASSERT(_uas); Q_ASSERT(_uas);
...@@ -222,29 +222,19 @@ const QVariantList& PX4AutoPilotPlugin::components(void) ...@@ -222,29 +222,19 @@ const QVariantList& PX4AutoPilotPlugin::components(void)
return _components; 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 /// This will perform various checks prior to signalling that the plug in ready
void PX4AutoPilotPlugin::_pluginReadyPreChecks(void) void PX4AutoPilotPlugin::_pluginReadyPreChecks(void)
{ {
// Check for older parameter version set // Check for older parameter version set
// FIXME: Firmware is moving to version stamp parameter set. Once that is complete the version stamp // FIXME: Firmware is moving to version stamp parameter set. Once that is complete the version stamp
// should be used instead. // should be used instead.
if (parameters().contains("SENS_GYRO_XOFF")) { if (parameterExists("SENS_GYRO_XOFF")) {
_incorrectParameterVersion = true; _incorrectParameterVersion = true;
QGCMessageBox::warning(tr("Setup"), tr("This version of GroundControl can only perform vehicle setup on a newer version of firmware. " 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.")); "Please perform a Firmware Upgrade if you wish to use Vehicle Setup."));
} else { } else {
// Check for missing setup complete // Check for missing setup complete
foreach(const QVariant componentVariant, components()) { foreach(const QVariant componentVariant, vehicleComponents()) {
VehicleComponent* component = qobject_cast<VehicleComponent*>(qvariant_cast<QObject *>(componentVariant)); VehicleComponent* component = qobject_cast<VehicleComponent*>(qvariant_cast<QObject *>(componentVariant));
Q_ASSERT(component); Q_ASSERT(component);
......
...@@ -50,21 +50,20 @@ public: ...@@ -50,21 +50,20 @@ public:
~PX4AutoPilotPlugin(); ~PX4AutoPilotPlugin();
// Overrides from AutoPilotPlugin // Overrides from AutoPilotPlugin
virtual const QVariantList& components(void); virtual const QVariantList& vehicleComponents(void);
virtual const QVariantMap& parameters(void); virtual ParameterLoader* getParameterLoader(void) { return _parameterFacts; }
virtual QUrl setupBackgroundImage(void);
static QList<AutoPilotPluginManager::FullMode_t> getModes(void); static QList<AutoPilotPluginManager::FullMode_t> getModes(void);
static QString getShortModeText(uint8_t baseMode, uint32_t customMode); static QString getShortModeText(uint8_t baseMode, uint32_t customMode);
static void clearStaticData(void); static void clearStaticData(void);
// These methods should only be used by objects within the plugin // These methods should only be used by objects within the plugin
AirframeComponent* airframeComponent(void) { return _airframeComponent; } AirframeComponent* airframeComponent(void) { return _airframeComponent; }
RadioComponent* radioComponent(void) { return _radioComponent; } RadioComponent* radioComponent(void) { return _radioComponent; }
FlightModesComponent* flightModesComponent(void) { return _flightModesComponent; } FlightModesComponent* flightModesComponent(void) { return _flightModesComponent; }
SensorsComponent* sensorsComponent(void) { return _sensorsComponent; } SensorsComponent* sensorsComponent(void) { return _sensorsComponent; }
SafetyComponent* safetyComponent(void) { return _safetyComponent; } SafetyComponent* safetyComponent(void) { return _safetyComponent; }
PowerComponent* powerComponent(void) { return _powerComponent; } PowerComponent* powerComponent(void) { return _powerComponent; }
private slots: private slots:
void _pluginReadyPreChecks(void); void _pluginReadyPreChecks(void);
......
...@@ -37,7 +37,7 @@ bool PX4ParameterFacts::_parameterMetaDataLoaded = false; ...@@ -37,7 +37,7 @@ bool PX4ParameterFacts::_parameterMetaDataLoaded = false;
QMap<QString, FactMetaData*> PX4ParameterFacts::_mapParameterName2FactMetaData; QMap<QString, FactMetaData*> PX4ParameterFacts::_mapParameterName2FactMetaData;
PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) : PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) :
FactLoader(uas, parent) ParameterLoader(uas, parent)
{ {
Q_ASSERT(uas); Q_ASSERT(uas);
} }
...@@ -273,6 +273,6 @@ void PX4ParameterFacts::_addMetaDataToFact(Fact* fact) ...@@ -273,6 +273,6 @@ void PX4ParameterFacts::_addMetaDataToFact(Fact* fact)
fact->setMetaData(_mapParameterName2FactMetaData[fact->name()]); fact->setMetaData(_mapParameterName2FactMetaData[fact->name()]);
} else { } else {
// Use generic meta data // Use generic meta data
FactLoader::_addMetaDataToFact(fact); ParameterLoader::_addMetaDataToFact(fact);
} }
} }
...@@ -21,14 +21,15 @@ ...@@ -21,14 +21,15 @@
======================================================================*/ ======================================================================*/
#ifndef PX4ParameterFacts_h #ifndef PX4PARAMETERFACTS_H
#define PX4ParameterFacts_h #define PX4PARAMETERFACTS_H
#include <QObject> #include <QObject>
#include <QMap> #include <QMap>
#include <QXmlStreamReader> #include <QXmlStreamReader>
#include <QLoggingCategory> #include <QLoggingCategory>
#include "ParameterLoader.h"
#include "FactSystem.h" #include "FactSystem.h"
#include "UASInterface.h" #include "UASInterface.h"
...@@ -39,13 +40,16 @@ Q_DECLARE_LOGGING_CATEGORY(PX4ParameterFactsMetaDataLog) ...@@ -39,13 +40,16 @@ Q_DECLARE_LOGGING_CATEGORY(PX4ParameterFactsMetaDataLog)
/// Collection of Parameter Facts for PX4 AutoPilot /// Collection of Parameter Facts for PX4 AutoPilot
class PX4ParameterFacts : public FactLoader class PX4ParameterFacts : public ParameterLoader
{ {
Q_OBJECT Q_OBJECT
public: public:
/// @param uas Uas which this set of facts is associated with /// @param uas Uas which this set of facts is associated with
PX4ParameterFacts(UASInterface* uas, QObject* parent = NULL); PX4ParameterFacts(UASInterface* uas, QObject* parent = NULL);
/// Override from ParameterLoader
virtual QString getDefaultComponentIdParam(void) const { return QString("SYS_AUTOSTART"); }
static void loadParameterFactMetaData(void); static void loadParameterFactMetaData(void);
static void deleteParameterFactMetaData(void); static void deleteParameterFactMetaData(void);
......
...@@ -268,15 +268,15 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in ...@@ -268,15 +268,15 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
void SensorsComponentController::_refreshParams(void) void SensorsComponentController::_refreshParams(void)
{ {
// Pull specified params first so red/green indicators update quickly // Pull specified params first so red/green indicators update quickly
_autopilot->refreshParameter("CAL_MAG0_ID"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ID");
_autopilot->refreshParameter("CAL_GYRO0_ID"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_GYRO0_ID");
_autopilot->refreshParameter("CAL_ACC0_ID"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_ACC0_ID");
_autopilot->refreshParameter("SENS_DPRES_OFF"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF");
_autopilot->refreshParameter("CAL_MAG0_ROT"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG0_ROT");
_autopilot->refreshParameter("CAL_MAG1_ROT"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG1_ROT");
_autopilot->refreshParameter("CAL_MAG2_ROT"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "CAL_MAG2_ROT");
_autopilot->refreshParameter("SENS_BOARD_ROT"); _autopilot->refreshParameter(FactSystem::defaultComponentId, "SENS_BOARD_ROT");
// Pull full set in order to get all cal values back // Pull full set in order to get all cal values back
_autopilot->refreshAllParameters(); _autopilot->refreshAllParameters();
......
...@@ -28,9 +28,10 @@ ...@@ -28,9 +28,10 @@
#include <QtQml> #include <QtQml>
Fact::Fact(QString name, FactMetaData::ValueType_t type, QObject* parent) : Fact::Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObject* parent) :
QObject(parent), QObject(parent),
_name(name), _name(name),
_componentId(componentId),
_type(type), _type(type),
_metaData(NULL) _metaData(NULL)
{ {
...@@ -75,6 +76,11 @@ QString Fact::name(void) const ...@@ -75,6 +76,11 @@ QString Fact::name(void) const
return _name; return _name;
} }
int Fact::componentId(void) const
{
return _componentId;
}
QVariant Fact::value(void) const QVariant Fact::value(void) const
{ {
return _value; return _value;
......
...@@ -44,6 +44,7 @@ class Fact : public QObject ...@@ -44,6 +44,7 @@ class Fact : public QObject
Q_OBJECT Q_OBJECT
Q_PROPERTY(QString name READ name CONSTANT) 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 value READ value WRITE setValue NOTIFY valueChanged USER true)
Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged) Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged)
Q_PROPERTY(QVariant defaultValue READ defaultValue CONSTANT) Q_PROPERTY(QVariant defaultValue READ defaultValue CONSTANT)
...@@ -57,13 +58,17 @@ class Fact : public QObject ...@@ -57,13 +58,17 @@ class Fact : public QObject
Q_ENUMS(FactMetaData::ValueType_t) Q_ENUMS(FactMetaData::ValueType_t)
public: 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 // Property system methods
/// Read accessor or name property /// Read accessor for name property
QString name(void) const; QString name(void) const;
/// Read accessor for componentId property
int componentId(void) const;
/// Read accessor for value property /// Read accessor for value property
QVariant value(void) const; QVariant value(void) const;
...@@ -112,6 +117,7 @@ signals: ...@@ -112,6 +117,7 @@ signals:
private: private:
QString _name; QString _name;
int _componentId;
QVariant _value; QVariant _value;
FactMetaData::ValueType_t _type; FactMetaData::ValueType_t _type;
FactMetaData* _metaData; FactMetaData* _metaData;
......
...@@ -31,7 +31,8 @@ ...@@ -31,7 +31,8 @@
FactBinder::FactBinder(void) : FactBinder::FactBinder(void) :
_autopilotPlugin(NULL), _autopilotPlugin(NULL),
_fact(NULL) _fact(NULL),
_componentId(FactSystem::defaultComponentId)
{ {
UASInterface* uas = UASManager::instance()->getActiveUAS(); UASInterface* uas = UASManager::instance()->getActiveUAS();
Q_ASSERT(uas); Q_ASSERT(uas);
...@@ -58,8 +59,8 @@ void FactBinder::setName(const QString& name) ...@@ -58,8 +59,8 @@ void FactBinder::setName(const QString& name)
} }
if (!name.isEmpty()) { if (!name.isEmpty()) {
if (_autopilotPlugin->factExists(name)) { if (_autopilotPlugin->factExists(FactSystem::ParameterProvider, _componentId, name)) {
_fact = _autopilotPlugin->getFact(name); _fact = _autopilotPlugin->getFact(FactSystem::ParameterProvider, _componentId, name);
connect(_fact, &Fact::valueChanged, this, &FactBinder::valueChanged); connect(_fact, &Fact::valueChanged, this, &FactBinder::valueChanged);
emit valueChanged(); emit valueChanged();
......
...@@ -38,6 +38,7 @@ class FactBinder : public QObject ...@@ -38,6 +38,7 @@ class FactBinder : public QObject
{ {
Q_OBJECT Q_OBJECT
Q_PROPERTY(int componentId MEMBER _componentId NOTIFY componentIdChanged)
Q_PROPERTY(QString name READ name WRITE setName NOTIFY nameChanged) 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 value READ value WRITE setValue NOTIFY valueChanged USER true)
Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged) Q_PROPERTY(QVariant valueString READ valueString NOTIFY valueChanged)
...@@ -46,6 +47,9 @@ class FactBinder : public QObject ...@@ -46,6 +47,9 @@ class FactBinder : public QObject
public: public:
FactBinder(void); FactBinder(void);
int componentId(void) const;
void setComponentId(int componentId);
QString name(void) const; QString name(void) const;
void setName(const QString& name); void setName(const QString& name);
...@@ -58,12 +62,14 @@ public: ...@@ -58,12 +62,14 @@ public:
QString units(void) const; QString units(void) const;
signals: signals:
void componentIdChanged(void);
void nameChanged(void); void nameChanged(void);
void valueChanged(void); void valueChanged(void);
private: private:
AutoPilotPlugin* _autopilotPlugin; AutoPilotPlugin* _autopilotPlugin;
Fact* _fact; Fact* _fact;
int _componentId;
}; };
#endif #endif
\ No newline at end of file
...@@ -40,8 +40,6 @@ FactSystem::FactSystem(QObject* parent) : ...@@ -40,8 +40,6 @@ FactSystem::FactSystem(QObject* parent) :
QGCSingleton(parent) QGCSingleton(parent)
{ {
qmlRegisterType<FactBinder>(_factSystemQmlUri, 1, 0, "Fact"); qmlRegisterType<FactBinder>(_factSystemQmlUri, 1, 0, "Fact");
// FIXME: Where should these go?
qmlRegisterUncreatableType<VehicleComponent>(_factSystemQmlUri, 1, 0, "VehicleComponent", "Can only reference VehicleComponent"); qmlRegisterUncreatableType<VehicleComponent>(_factSystemQmlUri, 1, 0, "VehicleComponent", "Can only reference VehicleComponent");
} }
......
...@@ -24,15 +24,12 @@ ...@@ -24,15 +24,12 @@
/// @file /// @file
/// @author Don Gagne <don@thegagnes.com> /// @author Don Gagne <don@thegagnes.com>
#ifndef FactSystem_h #ifndef FACTSYSTEM_H
#define FactSystem_h #define FACTSYSTEM_H
#include "Fact.h" #include "Fact.h"
#include "FactLoader.h"
#include "FactMetaData.h" #include "FactMetaData.h"
#include "UASInterface.h"
#include "QGCSingleton.h" #include "QGCSingleton.h"
#include "FactValidator.h"
/// FactSystem is a singleton which provides access to the Facts in the system /// FactSystem is a singleton which provides access to the Facts in the system
/// ///
...@@ -42,12 +39,20 @@ ...@@ -42,12 +39,20 @@
/// settings. Client code can then use this system to expose sets of Facts to QML code. An example /// 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 /// 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. /// the firmware parameters to QML such that you can bind QML ui elements directly to parameters.
class FactSystem : public QGCSingleton class FactSystem : public QGCSingleton
{ {
Q_OBJECT Q_OBJECT
DECLARE_QGC_SINGLETON(FactSystem, FactSystem) DECLARE_QGC_SINGLETON(FactSystem, FactSystem)
public:
typedef enum {
ParameterProvider,
} Provider_t;
static const int defaultComponentId = -1;
private: private:
/// All access to FactSystem is through FactSystem::instance, so constructor is private /// All access to FactSystem is through FactSystem::instance, so constructor is private
FactSystem(QObject* parent = NULL); FactSystem(QObject* parent = NULL);
......
...@@ -26,14 +26,19 @@ import QtQuick.Controls 1.2 ...@@ -26,14 +26,19 @@ import QtQuick.Controls 1.2
import QGroundControl.FactSystem 1.0 import QGroundControl.FactSystem 1.0
Item { Item {
// Use default component id
TextInput { TextInput {
objectName: "testControl" objectName: "testControl"
Fact { id: fact; name: "RC_MAP_THROTTLE" } Fact { id: fact1; name: "RC_MAP_THROTTLE" }
text: fact.value text: fact1.value
font.family: "Helvetica" onAccepted: { fact1.value = text; }
font.pointSize: 24; }
color: "red"
focus: true // Use specific component id
onAccepted: { fact.value = text; } TextInput {
objectName: "testControl"
Fact { id: fact2; name: "COMPONENT_51"; componentId: 51 }
text: fact2.value
onAccepted: { fact2.value = text; }
} }
} }
...@@ -85,21 +85,45 @@ void FactSystemTestBase::_cleanup(void) ...@@ -85,21 +85,45 @@ void FactSystemTestBase::_cleanup(void)
} }
/// Basic test of parameter values in Fact System /// 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<Fact*>(); QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE"));
Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE");
QVERIFY(fact != NULL); QVERIFY(fact != NULL);
QVariant factValue = fact->value(); QVariant factValue = fact->value();
QCOMPARE(factValue.isValid(), true); QCOMPARE(factValue.isValid(), true);
QVariant paramValue; 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()); QCOMPARE(factValue.toInt(), paramValue.toInt());
} }
...@@ -129,9 +153,7 @@ void FactSystemTestBase::_paramMgrSignal_test(void) ...@@ -129,9 +153,7 @@ void FactSystemTestBase::_paramMgrSignal_test(void)
{ {
// Get the parameter Fact from the AutoPilot // Get the parameter Fact from the AutoPilot
const QVariantMap& parameterFacts = _plugin->parameters(); Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, -1, "RC_MAP_THROTTLE");
Fact* fact = parameterFacts["RC_MAP_THROTTLE"].value<Fact*>();
QVERIFY(fact != NULL); QVERIFY(fact != NULL);
// Setting a new value into the parameter should trigger a valueChanged signal on the Fact // Setting a new value into the parameter should trigger a valueChanged signal on the Fact
......
...@@ -43,7 +43,8 @@ protected: ...@@ -43,7 +43,8 @@ protected:
void _init(MAV_AUTOPILOT autopilot); void _init(MAV_AUTOPILOT autopilot);
void _cleanup(void); 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 _qml_test(void);
void _paramMgrSignal_test(void); void _paramMgrSignal_test(void);
void _qmlUpdate_test(void); void _qmlUpdate_test(void);
......
...@@ -43,7 +43,8 @@ private slots: ...@@ -43,7 +43,8 @@ private slots:
void init(void); void init(void);
void cleanup(void) { _cleanup(); } 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 qml_test(void) { _qml_test(); }
void paramMgrSignal_test(void) { _paramMgrSignal_test(); } void paramMgrSignal_test(void) { _paramMgrSignal_test(); }
void qmlUpdate_test(void) { _qmlUpdate_test(); } void qmlUpdate_test(void) { _qmlUpdate_test(); }
......
...@@ -43,7 +43,8 @@ private slots: ...@@ -43,7 +43,8 @@ private slots:
void init(void); void init(void);
void cleanup(void) { _cleanup(); } 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 qml_test(void) { _qml_test(); }
void paramMgrSignal_test(void) { _paramMgrSignal_test(); } void paramMgrSignal_test(void) { _paramMgrSignal_test(); }
void qmlUpdate_test(void) { _qmlUpdate_test(); } void qmlUpdate_test(void) { _qmlUpdate_test(); }
......
...@@ -24,20 +24,20 @@ ...@@ -24,20 +24,20 @@
/// @file /// @file
/// @author Don Gagne <don@thegagnes.com> /// @author Don Gagne <don@thegagnes.com>
#include "FactLoader.h" #include "ParameterLoader.h"
#include "QGCApplication.h" #include "QGCApplication.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
#include <QFile> #include <QFile>
#include <QDebug> #include <QDebug>
QGC_LOGGING_CATEGORY(FactLoaderLog, "FactLoaderLog") QGC_LOGGING_CATEGORY(ParameterLoaderLog, "ParameterLoaderLog")
FactLoader::FactLoader(UASInterface* uas, QObject* parent) : ParameterLoader::ParameterLoader(UASInterface* uas, QObject* parent) :
QObject(parent), QObject(parent),
_lastSeenComponent(-1),
_paramMgr(NULL), _paramMgr(NULL),
_factsReady(false) _parametersReady(false),
_defaultComponentId(FactSystem::defaultComponentId)
{ {
Q_ASSERT(uas); Q_ASSERT(uas);
...@@ -53,32 +53,37 @@ FactLoader::FactLoader(UASInterface* uas, QObject* parent) : ...@@ -53,32 +53,37 @@ FactLoader::FactLoader(UASInterface* uas, QObject* parent) :
connect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate())); connect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate()));
// We track parameters changes to keep Facts up to date. // We track parameters changes to keep Facts up to date.
connect(uas, &UASInterface::parameterUpdate, this, &FactLoader::_parameterUpdate); connect(uas, &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate);
} }
FactLoader::~FactLoader() ParameterLoader::~ParameterLoader()
{ {
} }
/// Called whenever a parameter is updated or first seen. /// Called whenever a parameter is updated or first seen.
void FactLoader::_parameterUpdate(int uas, int component, QString parameterName, int mavType, QVariant value) void ParameterLoader::_parameterUpdate(int uas, int componentId, QString parameterName, int mavType, QVariant value)
{ {
bool setMetaData = false;
// Is this for our uas? // Is this for our uas?
if (uas != _uasId) { if (uas != _uasId) {
return; return;
} }
if (_lastSeenComponent == -1) { // Attempt to determine default component id
_lastSeenComponent = component; if (_defaultComponentId == FactSystem::defaultComponentId && _defaultComponentIdParam.isEmpty()) {
} else { _defaultComponentIdParam = getDefaultComponentIdParam();
// Code cannot handle parameters coming form different components yets }
Q_ASSERT(component == _lastSeenComponent); if (!_defaultComponentIdParam.isEmpty() && _defaultComponentIdParam == parameterName) {
_defaultComponentId = componentId;
} }
bool setMetaData = false; if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(parameterName)) {
if (!_mapParameterName2Variant.contains(parameterName)) { // These should not get our of sync
qCDebug(FactLoaderLog) << "Adding new fact" << parameterName; Q_ASSERT(_mapParameterName2Variant.contains(componentId) == _mapFact2ParameterName.contains(componentId));
qCDebug(ParameterLoaderLog) << "Adding new fact (component:" << componentId << "name:" << parameterName << ")";
FactMetaData::ValueType_t factType; FactMetaData::ValueType_t factType;
switch (mavType) { switch (mavType) {
...@@ -112,21 +117,21 @@ void FactLoader::_parameterUpdate(int uas, int component, QString parameterName, ...@@ -112,21 +117,21 @@ void FactLoader::_parameterUpdate(int uas, int component, QString parameterName,
break; break;
} }
Fact* fact = new Fact(parameterName, factType, this); Fact* fact = new Fact(componentId, parameterName, factType, this);
setMetaData = true; setMetaData = true;
_mapParameterName2Variant[parameterName] = QVariant::fromValue(fact); _mapParameterName2Variant[componentId][parameterName] = QVariant::fromValue(fact);
_mapFact2ParameterName[fact] = parameterName; _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 // 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); connect(fact, &Fact::_containerValueChanged, this, &ParameterLoader::_valueUpdated);
} }
Q_ASSERT(_mapParameterName2Variant.contains(parameterName)); Q_ASSERT(_mapParameterName2Variant[componentId].contains(parameterName));
qCDebug(FactLoaderLog) << "Updating fact value" << parameterName << value; qCDebug(ParameterLoaderLog) << "Updating fact value (component:" << componentId << "name:" << parameterName << value << ")";
Fact* fact = _mapParameterName2Variant[parameterName].value<Fact*>(); Fact* fact = _mapParameterName2Variant[componentId][parameterName].value<Fact*>();
Q_ASSERT(fact); Q_ASSERT(fact);
fact->_containerSetValue(value); fact->_containerSetValue(value);
...@@ -138,14 +143,16 @@ void FactLoader::_parameterUpdate(int uas, int component, QString parameterName, ...@@ -138,14 +143,16 @@ void FactLoader::_parameterUpdate(int uas, int component, QString parameterName,
/// Connected to Fact::valueUpdated /// Connected to Fact::valueUpdated
/// ///
/// Sets the new value into the Parameter Manager. Parameter is persisted after send. /// Sets the new value into the Parameter Manager. Parameter is persisted after send.
void FactLoader::_valueUpdated(const QVariant& value) void ParameterLoader::_valueUpdated(const QVariant& value)
{ {
Fact* fact = qobject_cast<Fact*>(sender()); Fact* fact = qobject_cast<Fact*>(sender());
Q_ASSERT(fact); Q_ASSERT(fact);
Q_ASSERT(_lastSeenComponent != -1); int componentId = fact->componentId();
Q_ASSERT(_paramMgr); Q_ASSERT(_paramMgr);
Q_ASSERT(_mapFact2ParameterName.contains(fact)); Q_ASSERT(_mapFact2ParameterName.contains(componentId));
Q_ASSERT(_mapFact2ParameterName[componentId].contains(fact));
QVariant typedValue; QVariant typedValue;
switch (fact->type()) { switch (fact->type()) {
...@@ -170,17 +177,17 @@ void FactLoader::_valueUpdated(const QVariant& value) ...@@ -170,17 +177,17 @@ void FactLoader::_valueUpdated(const QVariant& value)
break; break;
} }
qCDebug(FactLoaderLog) << "Set parameter" << fact->name() << typedValue; qCDebug(ParameterLoaderLog) << "Set parameter (componentId:" << componentId << "name:" << fact->name() << typedValue << ")";
_paramMgr->setParameter(_lastSeenComponent, _mapFact2ParameterName[fact], typedValue); _paramMgr->setParameter(componentId, fact->name(), typedValue);
_paramMgr->sendPendingParameters(true /* persistAfterSend */, false /* forceSend */); _paramMgr->sendPendingParameters(true /* persistAfterSend */, false /* forceSend */);
} }
// Called when param mgr list is up to date // Called when param mgr list is up to date
void FactLoader::_paramMgrParameterListUpToDate(void) void ParameterLoader::_paramMgrParameterListUpToDate(void)
{ {
if (!_factsReady) { if (!_parametersReady) {
_factsReady = true; _parametersReady = true;
// We don't need this any more // We don't need this any more
disconnect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate())); disconnect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate()));
...@@ -188,13 +195,88 @@ void FactLoader::_paramMgrParameterListUpToDate(void) ...@@ -188,13 +195,88 @@ void FactLoader::_paramMgrParameterListUpToDate(void)
// There may be parameterUpdated signals still in our queue. Flush them out. // There may be parameterUpdated signals still in our queue. Flush them out.
qgcApp()->processEvents(); qgcApp()->processEvents();
// We should have all paramters now so we can signal ready _determineDefaultComponentId();
emit factsReady();
// We should have all parameters now so we can signal ready
emit parametersReady();
} }
} }
void FactLoader::_addMetaDataToFact(Fact* fact) void ParameterLoader::_addMetaDataToFact(Fact* fact)
{ {
FactMetaData* metaData = new FactMetaData(this); FactMetaData* metaData = new FactMetaData(this);
metaData->initFromTypeOnly(fact->type()); 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<Fact*>();
Q_ASSERT(fact);
return fact;
}
...@@ -21,42 +21,62 @@ ...@@ -21,42 +21,62 @@
======================================================================*/ ======================================================================*/
#ifndef FactLoader_h #ifndef PARAMETERLOADER_H
#define FactLoader_h #define PARAMETERLOADER_H
#include <QObject> #include <QObject>
#include <QMap> #include <QMap>
#include <QXmlStreamReader> #include <QXmlStreamReader>
#include <QLoggingCategory> #include <QLoggingCategory>
#include "Fact.h" #include "FactSystem.h"
#include "UASInterface.h" #include "UASInterface.h"
/// @file /// @file
/// @author Don Gagne <don@thegagnes.com> /// @author Don Gagne <don@thegagnes.com>
Q_DECLARE_LOGGING_CATEGORY(FactLoaderLog) Q_DECLARE_LOGGING_CATEGORY(ParameterLoaderLog)
/// Connects to Parameter Manager to load/update Facts /// Connects to Parameter Manager to load/update Facts
class FactLoader : public QObject class ParameterLoader : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
/// @param uas Uas which this set of facts is associated with /// @param uas Uas which this set of facts is associated with
FactLoader(UASInterface* uas, QObject* parent = NULL); ParameterLoader(UASInterface* uas, QObject* parent = NULL);
~FactLoader(); ~ParameterLoader();
/// Returns true if the full set of facts are ready /// Returns true if the full set of facts are ready
bool factsAreReady(void) { return _factsReady; } bool parametersAreReady(void) { return _parametersReady; }
/// Returns the fact QVariantMap /// Re-request the full set of parameters from the autopilot
const QVariantMap& factMap(void) { return _mapParameterName2Variant; } 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: signals:
/// Signalled when the full set of facts are ready /// Signalled when the full set of facts are ready
void factsReady(void); void parametersReady(void);
protected: protected:
/// Base implementation adds generic meta data based on variant type. Derived class can override to provide /// Base implementation adds generic meta data based on variant type. Derived class can override to provide
...@@ -64,23 +84,30 @@ protected: ...@@ -64,23 +84,30 @@ protected:
virtual void _addMetaDataToFact(Fact* fact); virtual void _addMetaDataToFact(Fact* fact);
private slots: private slots:
void _parameterUpdate(int uas, int component, QString parameterName, int mavType, QVariant value); void _parameterUpdate(int uas, int componentId, QString parameterName, int mavType, QVariant value);
void _valueUpdated(const QVariant& value); void _valueUpdated(const QVariant& value);
void _paramMgrParameterListUpToDate(void); void _paramMgrParameterListUpToDate(void);
private: private:
static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false);
int _actualComponentId(int componentId);
void _determineDefaultComponentId(void);
QMap<Fact*, QString> _mapFact2ParameterName; ///< Maps from a Fact to a parameter name /// First mapping is by component id
/// Second mapping is parameter name, to Fact
QMap<int, QMap<Fact*, QString> > _mapFact2ParameterName;
int _uasId; ///< Id for uas which this set of Facts are associated with int _uasId; ///< Id for uas which this set of Facts are associated with
int _lastSeenComponent;
QGCUASParamManagerInterface* _paramMgr; QGCUASParamManagerInterface* _paramMgr;
QVariantMap _mapParameterName2Variant; /// First mapping id\s by component id
/// Second mapping is parameter name, to Fact* in QVariant
QMap<int, QVariantMap> _mapParameterName2Variant;
bool _factsReady; ///< All facts received from param mgr bool _parametersReady; ///< All params received from param mgr
int _defaultComponentId;
QString _defaultComponentIdParam;
}; };
#endif #endif
\ No newline at end of file
...@@ -39,7 +39,7 @@ Rectangle { ...@@ -39,7 +39,7 @@ Rectangle {
} }
Repeater { Repeater {
model: autopilot ? autopilot.components : 0 model: autopilot ? autopilot.vehicleComponents : 0
SubMenuButton { SubMenuButton {
width: parent.width width: parent.width
......
...@@ -72,7 +72,7 @@ Rectangle { ...@@ -72,7 +72,7 @@ Rectangle {
spacing: 10 spacing: 10
Repeater { Repeater {
model: autopilot.components model: autopilot.vehicleComponents
// Outer summary item rectangle // Outer summary item rectangle
Rectangle { Rectangle {
......
...@@ -223,6 +223,13 @@ void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected) ...@@ -223,6 +223,13 @@ void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected)
**/ **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) 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(); // receiveMutex.lock();
mavlink_message_t message; mavlink_message_t message;
mavlink_status_t status; mavlink_status_t status;
......
This diff is collapsed.
...@@ -117,8 +117,8 @@ private: ...@@ -117,8 +117,8 @@ private:
void _handleMissionRequestList(const mavlink_message_t& msg); void _handleMissionRequestList(const mavlink_message_t& msg);
void _handleMissionRequest(const mavlink_message_t& msg); void _handleMissionRequest(const mavlink_message_t& msg);
void _handleMissionItem(const mavlink_message_t& msg); void _handleMissionItem(const mavlink_message_t& msg);
float _floatUnionForParam(const QString& paramName); float _floatUnionForParam(int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap(const QString& paramName, float paramFloat); void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
MockLinkMissionItemHandler* _missionItemHandler; MockLinkMissionItemHandler* _missionItemHandler;
...@@ -131,8 +131,8 @@ private: ...@@ -131,8 +131,8 @@ private:
bool _inNSH; bool _inNSH;
bool _mavlinkStarted; bool _mavlinkStarted;
QMap<QString, QVariant> _mapParamName2Value; QMap<int, QMap<QString, QVariant> > _mapParamName2Value;
QMap<QString, MAV_PARAM_TYPE> _mapParamName2MavParamType; QMap<QString, MAV_PARAM_TYPE> _mapParamName2MavParamType;
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t; typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
MissionList_t _missionItems; MissionList_t _missionItems;
......
...@@ -510,3 +510,4 @@ ...@@ -510,3 +510,4 @@
1 50 VT_MOT_COUNT 0 6 1 50 VT_MOT_COUNT 0 6
1 50 VT_POWER_MAX 120 9 1 50 VT_POWER_MAX 120 9
1 50 VT_PROP_EFF 0 9 1 50 VT_PROP_EFF 0 9
1 51 COMPONENT_51 51 6
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment