Commit 97c8b6dc authored by Don Gagne's avatar Don Gagne

FactSystem now only source for parameters

QGCUASParamManager and friends are replaced by FactSystem
parent 2d2ed7a7
......@@ -25,11 +25,11 @@
/// @author Don Gagne <don@thegagnes.com>
#include "AutoPilotPlugin.h"
#include "QGCUASParamManagerInterface.h"
#include "SetupView.h"
#include "QGCApplication.h"
#include "QGCMessageBox.h"
#include "MainWindow.h"
#include "ParameterLoader.h"
AutoPilotPlugin::AutoPilotPlugin(UASInterface* uas, QObject* parent) :
QObject(parent),
......@@ -160,27 +160,10 @@ const QMap<int, QMap<QString, QStringList> >& AutoPilotPlugin::getGroupMap(void)
void AutoPilotPlugin::writeParametersToStream(QTextStream &stream)
{
Q_ASSERT(_uas);
_uas->getParamManager()->writeOnboardParamsToStream(stream, _uas->getUASName());
_getParameterLoader()->writeParametersToStream(stream, _uas->getUASName());
}
void AutoPilotPlugin::readParametersFromStream(QTextStream &stream)
{
Q_ASSERT(_uas);
Fact* autoSaveFact = NULL;
int previousAutoSave = 0;
if (parameterExists("COM_AUTOS_PAR")) {
autoSaveFact = getParameterFact("COM_AUTOS_PAR");
previousAutoSave = autoSaveFact->value().toInt();
autoSaveFact->setValue(1);
}
_uas->getParamManager()->readPendingParamsFromStream(stream);
if (autoSaveFact) {
autoSaveFact->setValue(previousAutoSave);
}
_getParameterLoader()->readParametersFromStream(stream);
}
......@@ -35,7 +35,8 @@
#include "UASInterface.h"
#include "VehicleComponent.h"
#include "FactSystem.h"
#include "ParameterLoader.h"
class ParameterLoader;
/// This is the base class for AutoPilot plugins
///
......@@ -115,6 +116,7 @@ public:
signals:
void pluginReadyChanged(bool pluginReady);
void setupCompleteChanged(bool setupComplete);
void parameterListProgress(float value);
protected:
/// All access to AutoPilotPugin objects is through getInstanceForAutoPilotPlugin
......
......@@ -31,10 +31,11 @@ GenericAutoPilotPlugin::GenericAutoPilotPlugin(UASInterface* uas, QObject* paren
{
Q_ASSERT(uas);
_parameterFacts = new GenericParameterFacts(uas, this);
_parameterFacts = new GenericParameterFacts(this, uas, this);
Q_CHECK_PTR(_parameterFacts);
connect(_parameterFacts, &GenericParameterFacts::parametersReady, this, &GenericAutoPilotPlugin::_parametersReady);
connect(_parameterFacts, &GenericParameterFacts::parameterListProgress, this, &GenericAutoPilotPlugin::parameterListProgress);
}
QList<AutoPilotPluginManager::FullMode_t> GenericAutoPilotPlugin::getModes(void)
......
......@@ -28,8 +28,8 @@
#include <QDebug>
GenericParameterFacts::GenericParameterFacts(UASInterface* uas, QObject* parent) :
ParameterLoader(uas, parent)
GenericParameterFacts::GenericParameterFacts(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) :
ParameterLoader(autopilot, uas, parent)
{
Q_ASSERT(uas);
}
......@@ -28,6 +28,7 @@
#include "ParameterLoader.h"
#include "UASInterface.h"
#include "AutoPilotPlugin.h"
/// @file
/// @author Don Gagne <don@thegagnes.com>
......@@ -40,7 +41,7 @@ class GenericParameterFacts : public ParameterLoader
public:
/// @param uas Uas which this set of facts is associated with
GenericParameterFacts(UASInterface* uas, QObject* parent = NULL);
GenericParameterFacts(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent = NULL);
/// Override from ParameterLoader
virtual QString getDefaultComponentIdParam(void) const { return QString(); }
......
......@@ -81,7 +81,7 @@ AirframeComponentController::AirframeComponentController(QObject* parent) :
_airframeTypes.append(QVariant::fromValue(airframeType));
}
if (_autostartId != 0) {
// FIXME: Should be a user error
Q_UNUSED(autostartFound);
......@@ -104,33 +104,14 @@ void AirframeComponentController::changeAutostart(void)
_autoPilotPlugin->getParameterFact("SYS_AUTOSTART")->setValue(_autostartId);
_autoPilotPlugin->getParameterFact("SYS_AUTOCONFIG")->setValue(1);
// Wait for the parameters to come back to us
qgcApp()->setOverrideCursor(Qt::WaitCursor);
int waitSeconds = 10;
bool success = false;
QGCUASParamManagerInterface* paramMgr = _uas->getParamManager();
while (true) {
if (paramMgr->countPendingParams() == 0) {
success = true;
break;
}
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
QGC::SLEEP::sleep(1);
if (--waitSeconds == 0) {
break;
}
}
// Wait for the parameters to flow through system
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
QGC::SLEEP::sleep(1);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
if (!success) {
qgcApp()->restoreOverrideCursor();
QGCMessageBox::critical("Airframe Config", "Airframe Config parameters not received back from vehicle. Config has not been set.");
return;
}
// Reboot board and reconnect
_uas->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
......@@ -173,4 +154,3 @@ Airframe::~Airframe()
{
}
......@@ -24,7 +24,6 @@
#include "PX4AutoPilotPlugin.h"
#include "AutoPilotPluginManager.h"
#include "UASManager.h"
#include "QGCUASParamManagerInterface.h"
#include "PX4ParameterLoader.h"
#include "FlightModesComponentController.h"
#include "AirframeComponentController.h"
......@@ -79,10 +78,11 @@ PX4AutoPilotPlugin::PX4AutoPilotPlugin(UASInterface* uas, QObject* parent) :
qmlRegisterType<FlightModesComponentController>("QGroundControl.Controllers", 1, 0, "FlightModesComponentController");
qmlRegisterType<AirframeComponentController>("QGroundControl.Controllers", 1, 0, "AirframeComponentController");
_parameterFacts = new PX4ParameterLoader(uas, this);
_parameterFacts = new PX4ParameterLoader(this, uas, this);
Q_CHECK_PTR(_parameterFacts);
connect(_parameterFacts, &PX4ParameterLoader::parametersReady, this, &PX4AutoPilotPlugin::_pluginReadyPreChecks);
connect(_parameterFacts, &PX4ParameterLoader::parameterListProgress, this, &PX4AutoPilotPlugin::parameterListProgress);
PX4ParameterLoader::loadParameterFactMetaData();
}
......@@ -197,26 +197,32 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void)
_airframeComponent = new AirframeComponent(_uas, this);
Q_CHECK_PTR(_airframeComponent);
_airframeComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent));
_radioComponent = new RadioComponent(_uas, this);
Q_CHECK_PTR(_radioComponent);
_radioComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_radioComponent));
_flightModesComponent = new FlightModesComponent(_uas, this);
Q_CHECK_PTR(_flightModesComponent);
_flightModesComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent));
_sensorsComponent = new SensorsComponent(_uas, this);
Q_CHECK_PTR(_sensorsComponent);
_sensorsComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent));
_powerComponent = new PowerComponent(_uas, this);
Q_CHECK_PTR(_powerComponent);
_powerComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_powerComponent));
_safetyComponent = new SafetyComponent(_uas, this);
Q_CHECK_PTR(_safetyComponent);
_safetyComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent));
}
......
......@@ -25,32 +25,29 @@
/// @author Don Gagne <don@thegagnes.com>
#include "PX4Component.h"
#include "Fact.h"
#include "AutoPilotPlugin.h"
PX4Component::PX4Component(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) :
VehicleComponent(uas, autopilot, parent)
{
Q_ASSERT(uas);
Q_ASSERT(autopilot);
_paramMgr = _uas->getParamManager();
Q_ASSERT(_paramMgr);
}
bool fSuccess = connect(_paramMgr, SIGNAL(parameterUpdated(int, QString, QVariant)), this, SLOT(_parameterUpdated(int, QString, QVariant)));
Q_ASSERT(fSuccess);
Q_UNUSED(fSuccess);
void PX4Component::setupTriggerSignals(void)
{
// Watch for changed on trigger list params
foreach (QString paramName, setupCompleteChangedTriggerList()) {
Fact* fact = _autopilot->getParameterFact(paramName);
connect(fact, &Fact::valueChanged, this, &PX4Component::_triggerUpdated);
}
}
void PX4Component::_parameterUpdated(int compId, QString paramName, QVariant value)
void PX4Component::_triggerUpdated(QVariant value)
{
Q_UNUSED(value);
if (compId == _paramMgr->getDefaultComponentId()) {
QStringList triggerList = setupCompleteChangedTriggerList();
foreach(QString triggerParam, triggerList) {
if (paramName == triggerParam) {
emit setupCompleteChanged(setupComplete());
return;
}
}
}
emit setupCompleteChanged(setupComplete());
}
......@@ -43,13 +43,12 @@ public:
/// signal to be emitted. Last element is signalled by NULL.
virtual QStringList setupCompleteChangedTriggerList(void) const = 0;
private slots:
/// @brief Connected to QGCUASParamManagerInterface::parameterUpdated signal in order to signal
/// setupCompleteChanged at appropriate times.
void _parameterUpdated(int compId, QString paramName, QVariant value);
/// Should be called after the component is created (but not in constructor) to setup the
/// signals which are used to track parameter changes which affect setupComplete state.
void setupTriggerSignals(void);
private:
QGCUASParamManagerInterface* _paramMgr;
private slots:
void _triggerUpdated(QVariant value);
};
#endif
......@@ -38,8 +38,8 @@ QGC_LOGGING_CATEGORY(PX4ParameterLoaderLog, "PX4ParameterLoaderLog")
bool PX4ParameterLoader::_parameterMetaDataLoaded = false;
QMap<QString, FactMetaData*> PX4ParameterLoader::_mapParameterName2FactMetaData;
PX4ParameterLoader::PX4ParameterLoader(UASInterface* uas, QObject* parent) :
ParameterLoader(uas, parent)
PX4ParameterLoader::PX4ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) :
ParameterLoader(autopilot, uas, parent)
{
Q_ASSERT(uas);
}
......@@ -221,7 +221,7 @@ void PX4ParameterLoader::loadParameterFactMetaData(void)
Q_CHECK_PTR(metaData);
if (_mapParameterName2FactMetaData.contains(name)) {
// We can't trust the meta dafa since we have dups
qDebug() << "Duplicate parameter found:" << name;
qCDebug(PX4ParameterLoaderLog) << "Duplicate parameter found:" << name;
badMetaData = true;
// Reset to default meta data
_mapParameterName2FactMetaData[name] = metaData;
......@@ -236,7 +236,7 @@ void PX4ParameterLoader::loadParameterFactMetaData(void)
metaData->setDefaultValue(varDefault);
} else {
// Non-fatal
qDebug() << "Parameter meta data with bad default value, name:" << name << " type:" << type << " default:" << strDefault;
qCDebug(PX4ParameterLoaderLog) << "Parameter meta data with bad default value, name:" << name << " type:" << type << " default:" << strDefault;
}
}
}
......
......@@ -32,6 +32,7 @@
#include "ParameterLoader.h"
#include "FactSystem.h"
#include "UASInterface.h"
#include "AutoPilotPlugin.h"
/// @file
/// @author Don Gagne <don@thegagnes.com>
......@@ -46,7 +47,7 @@ class PX4ParameterLoader : public ParameterLoader
public:
/// @param uas Uas which this set of facts is associated with
PX4ParameterLoader(UASInterface* uas, QObject* parent = NULL);
PX4ParameterLoader(AutoPilotPlugin* autpilot,UASInterface* uas, QObject* parent = NULL);
/// Override from ParameterLoader
virtual QString getDefaultComponentIdParam(void) const { return QString("SYS_AUTOSTART"); }
......
......@@ -228,7 +228,6 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
}
_appendStatusLog(text);
qDebug() << text;
if (_unknownFirmwareVersion) {
// We don't know how to do visual cal with the version of firwmare
......@@ -401,8 +400,8 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
void SensorsComponentController::_refreshParams(void)
{
// Pull full set in order to get all cal values back
_autopilot->refreshAllParameters();
_autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "CAL_");
_autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "SENS_");
}
bool SensorsComponentController::fixedWing(void)
......
......@@ -40,29 +40,35 @@ Fact::Fact(int componentId, QString name, FactMetaData::ValueType_t type, QObjec
void Fact::setValue(const QVariant& value)
{
QVariant newValue;
switch (type()) {
case FactMetaData::valueTypeInt8:
case FactMetaData::valueTypeInt16:
case FactMetaData::valueTypeInt32:
_value.setValue(QVariant(value.toInt()));
newValue = QVariant(value.toInt());
break;
case FactMetaData::valueTypeUint8:
case FactMetaData::valueTypeUint16:
case FactMetaData::valueTypeUint32:
_value.setValue(value.toUInt());
newValue = QVariant(value.toUInt());
break;
case FactMetaData::valueTypeFloat:
_value.setValue(value.toFloat());
newValue = QVariant(value.toFloat());
break;
case FactMetaData::valueTypeDouble:
_value.setValue(value.toDouble());
newValue = QVariant(value.toDouble());
break;
}
emit valueChanged(_value);
emit _containerValueChanged(_value);
if (newValue != _value) {
_value.setValue(newValue);
emit valueChanged(_value);
emit _containerValueChanged(_value);
}
}
void Fact::_containerSetValue(const QVariant& value)
......
......@@ -59,9 +59,6 @@ void FactSystemTestBase::_init(MAV_AUTOPILOT autopilot)
_uas = UASManager::instance()->getActiveUAS();
Q_ASSERT(_uas);
_paramMgr = _uas->getParamManager();
Q_ASSERT(_paramMgr);
// Get the plugin for the uas
AutoPilotPluginManager* pluginMgr = AutoPilotPluginManager::instance();
......@@ -74,7 +71,7 @@ void FactSystemTestBase::_init(MAV_AUTOPILOT autopilot)
QSignalSpy spyPlugin(_plugin, SIGNAL(pluginReadyChanged(bool)));
if (!_plugin->pluginReady()) {
QCOMPARE(spyPlugin.wait(5000), true);
QCOMPARE(spyPlugin.wait(10000), true);
}
Q_ASSERT(_plugin->pluginReady());
}
......@@ -87,23 +84,17 @@ void FactSystemTestBase::_cleanup(void)
/// Basic test of parameter values in Fact System
void FactSystemTestBase::_parameter_default_component_id_test(void)
{
// Compare the value in the Parameter Manager with the value from the FactSystem.
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));
QCOMPARE(factValue.toInt(), paramValue.toInt());
QCOMPARE(factValue.toInt(), 3);
}
void FactSystemTestBase::_parameter_specific_component_id_test(void)
{
// Compare the value in the Parameter Manager with the value from the FactSystem.
QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE"));
Fact* fact = _plugin->getFact(FactSystem::ParameterProvider, 50, "RC_MAP_THROTTLE");
QVERIFY(fact != NULL);
......@@ -111,10 +102,7 @@ void FactSystemTestBase::_parameter_specific_component_id_test(void)
QCOMPARE(factValue.isValid(), true);
QVariant paramValue;
Q_ASSERT(_paramMgr->getParameterValue(50, "RC_MAP_THROTTLE", paramValue));
QCOMPARE(factValue.toInt(), paramValue.toInt());
QCOMPARE(factValue.toInt(), 3);
// Test another component id
QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 51, "COMPONENT_51"));
......@@ -123,9 +111,7 @@ void FactSystemTestBase::_parameter_specific_component_id_test(void)
factValue = fact->value();
QCOMPARE(factValue.isValid(), true);
Q_ASSERT(_paramMgr->getParameterValue(51, "COMPONENT_51", paramValue));
QCOMPARE(factValue.toInt(), paramValue.toInt());
QCOMPARE(factValue.toInt(), 51);
}
/// Test that QML can reference a Fact
......@@ -142,38 +128,7 @@ void FactSystemTestBase::_qml_test(void)
QVERIFY(control != NULL);
QVariant qmlValue = control->property("text").toInt();
QVariant paramMgrValue;
Q_ASSERT(_paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramMgrValue));
QCOMPARE(qmlValue.toInt(), paramMgrValue.toInt());
}
// Test correct behavior when the Param Manager gets a parameter update
void FactSystemTestBase::_paramMgrSignal_test(void)
{
// Get the parameter Fact from the AutoPilot
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
QSignalSpy spyFact(fact, SIGNAL(valueChanged(QVariant)));
QVariant paramValue = 12;
_paramMgr->setParameter(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue);
_paramMgr->sendPendingParameters(true, false);
// Wait for the Fact::valueChanged signal to come through
QCOMPARE(spyFact.wait(5000), true);
// Make sure the signal has the right value
QList<QVariant> arguments = spyFact.takeFirst();
qDebug() << arguments.at(0).type();
QCOMPARE(arguments.at(0).toInt(), 12);
// Make sure the Fact has the new value
QCOMPARE(fact->value().toInt(), 12);
QCOMPARE(qmlValue.toInt(), 3);
}
/// Test QML getting an updated Fact value
......@@ -185,11 +140,10 @@ void FactSystemTestBase::_qmlUpdate_test(void)
widget->setSource(QUrl::fromUserInput("qrc:unittest/FactSystemTest.qml"));
// Change the value using param manager
// Change the value
QVariant paramValue = 12;
_paramMgr->setParameter(_paramMgr->getDefaultComponentId(), "RC_MAP_THROTTLE", paramValue);
_paramMgr->sendPendingParameters(true, false);
_plugin->getParameterFact("RC_MAP_THROTTLE")->setValue(paramValue);
QTest::qWait(500); // Let the signals flow through
......
......@@ -46,11 +46,9 @@ protected:
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);
UASInterface* _uas;
QGCUASParamManagerInterface* _paramMgr;
AutoPilotPlugin* _plugin;
LinkManager* _linkMgr;
};
......
......@@ -46,7 +46,6 @@ private slots:
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(); }
};
......
......@@ -46,7 +46,6 @@ private slots:
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(); }
};
......
This diff is collapsed.
......@@ -28,9 +28,13 @@
#include <QMap>
#include <QXmlStreamReader>
#include <QLoggingCategory>
#include <QMutex>
#include "FactSystem.h"
#include "UASInterface.h"
#include "MAVLinkProtocol.h"
#include "AutoPilotPlugin.h"
#include "QGCMavlink.h"
/// @file
/// @author Don Gagne <don@thegagnes.com>
......@@ -44,7 +48,7 @@ class ParameterLoader : public QObject
public:
/// @param uas Uas which this set of facts is associated with
ParameterLoader(UASInterface* uas, QObject* parent = NULL);
ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent = NULL);
~ParameterLoader();
......@@ -76,6 +80,9 @@ public:
const QMap<int, QMap<QString, QStringList> >& getGroupMap(void);
void readParametersFromStream(QTextStream& stream);
void writeParametersToStream(QTextStream &stream, const QString& 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;
......@@ -83,6 +90,12 @@ public:
signals:
/// Signalled when the full set of facts are ready
void parametersReady(void);
/// Signalled to update progress of full parameter list request
void parameterListProgress(float value);
/// Signalled to ourselves in order to get call on our own thread
void restartWaitingParamTimer(void);
protected:
/// Base implementation adds generic meta data based on variant type. Derived class can override to provide
......@@ -90,21 +103,27 @@ protected:
virtual void _addMetaDataToFact(Fact* fact);
private slots:
void _parameterUpdate(int uas, int componentId, QString parameterName, int mavType, QVariant value);
void _parameterUpdate(int uasId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value);
void _valueUpdated(const QVariant& value);
void _paramMgrParameterListUpToDate(void);
void _restartWaitingParamTimer(void);
void _waitingParamTimeout(void);
private:
static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false);
int _actualComponentId(int componentId);
void _determineDefaultComponentId(void);
void _setupGroupMap(void);
int _uasId; ///< Id for uas which this set of Facts are associated with
void _readParameterRaw(int componentId, const QString& paramName, int paramIndex);
void _writeParameterRaw(int componentId, const QString& paramName, const QVariant& value);
MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType);
FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType);
void _saveToEEPROM(void);
QGCUASParamManagerInterface* _paramMgr;
AutoPilotPlugin* _autopilot;
UASInterface* _uas;
MAVLinkProtocol* _mavlink;
/// First mapping id\s by component id
/// First mapping is by component id
/// Second mapping is parameter name, to Fact* in QVariant
QMap<int, QVariantMap> _mapParameterName2Variant;
......@@ -115,6 +134,19 @@ private:
bool _parametersReady; ///< All params received from param mgr
int _defaultComponentId;
QString _defaultComponentIdParam;
QMap<int, int> _paramCountMap; ///< Map of total known parameter count, keyed by component id
QMap<int, QList<int> > _waitingReadParamIndexMap; ///< Map of param indices waiting for initial first time read, keyed by component id
QMap<int, QStringList> _waitingReadParamNameMap; ///< Map of param names we are waiting to hear a read response from, keyed by component id
QMap<int, QStringList> _waitingWriteParamNameMap; ///< Map of param names we are waiting to hear a write response from, keyed by component id
int _totalParamCount; ///< Number of parameters across all components
QTimer _waitingParamTimeoutTimer;
bool _fullRefresh;
QMutex _dataMutex;
};
#endif
\ No newline at end of file
......@@ -39,9 +39,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
// Make UAS aware that this link can be used to communicate with the actual robot
uasInterface->addLink(link);
// First thing we do with a new UAS is get the parameters
uasInterface->getParamManager()->requestParameterList();
// Now add UAS to "official" list, which makes the whole application aware of it
UASManager::instance()->addUAS(uasInterface);
......
#include "QGCUASParamManager.h"
#include <QApplication>
#include <QDir>
#include <QDebug>
#include "UASInterface.h"
#include "QGCMessageBox.h"
QGCUASParamManager::QGCUASParamManager(QObject *parent) :
QGCUASParamManagerInterface(parent),
mav(NULL),
paramDataModel(this),
_parametersReady(false)
{
}
QGCUASParamManager* QGCUASParamManager::initWithUAS(UASInterface* uas)
{
mav = uas;
paramDataModel.setUASID(mav->getUASID());
paramCommsMgr.initWithUAS(uas);
connectToModelAndComms();
return this;
}
void QGCUASParamManager::connectToModelAndComms()
{
// Pass along comms mgr status msgs
connect(&paramCommsMgr, SIGNAL(parameterStatusMsgUpdated(QString,int)),
this, SIGNAL(parameterStatusMsgUpdated(QString,int)));
connect(&paramCommsMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_parameterListUpToDate()));
connect(&paramCommsMgr, SIGNAL(parameterListProgress(float)), this, SIGNAL(parameterListProgress(float)));
// Pass along data model updates
connect(&paramDataModel, SIGNAL(parameterUpdated(int, QString , QVariant )),
this, SIGNAL(parameterUpdated(int, QString , QVariant )));
connect(&paramDataModel, SIGNAL(pendingParamUpdate(int , const QString& , QVariant , bool )),
this, SIGNAL(pendingParamUpdate(int , const QString& , QVariant , bool )));
}
void QGCUASParamManager::clearAllPendingParams()
{
paramDataModel.clearAllPendingParams();
emit parameterStatusMsgUpdated(tr("Cleared all pending params"), UASParameterCommsMgr::ParamCommsStatusLevel_OK);
}
int QGCUASParamManager::getDefaultComponentId()
{
return paramDataModel.getDefaultComponentId();