diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index e34572c79caff97103f23e47bca74d74f4f78b6f..932b430d9c0793cf25186f70e55d0eacf93fc84c 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -25,11 +25,11 @@ /// @author Don Gagne #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 >& 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); } diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index 05a97e227732b1550e8f9c386d8abf1a1b1ca63e..4f635afb96528d743f2736de63686d8c7d0e282a 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -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 diff --git a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc index 0e2de3f5618c92165541c622e9a4b383013b89b5..f2ddfac47d791dfdd2bfa41789ddd7dfc87c0446 100644 --- a/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/Generic/GenericAutoPilotPlugin.cc @@ -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 GenericAutoPilotPlugin::getModes(void) diff --git a/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc b/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc index d33b143f81fa47db1691e431c5ec85803a487acd..fb1e1d18a0b633b41f18ade6c58515dc2e0bc440 100644 --- a/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc +++ b/src/AutoPilotPlugins/Generic/GenericParameterFacts.cc @@ -28,8 +28,8 @@ #include -GenericParameterFacts::GenericParameterFacts(UASInterface* uas, QObject* parent) : - ParameterLoader(uas, parent) +GenericParameterFacts::GenericParameterFacts(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) : + ParameterLoader(autopilot, uas, parent) { Q_ASSERT(uas); } diff --git a/src/AutoPilotPlugins/Generic/GenericParameterFacts.h b/src/AutoPilotPlugins/Generic/GenericParameterFacts.h index 5a964eeae7f634dc6d70bd394c7ae9cdbb72c5c9..dd5d10ed8b86468936a6d4dde180c0f9e95b373d 100644 --- a/src/AutoPilotPlugins/Generic/GenericParameterFacts.h +++ b/src/AutoPilotPlugins/Generic/GenericParameterFacts.h @@ -28,6 +28,7 @@ #include "ParameterLoader.h" #include "UASInterface.h" +#include "AutoPilotPlugin.h" /// @file /// @author Don Gagne @@ -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(); } diff --git a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc index 6b198e823b290a57dae6ab216f3c926c9c12d8ba..6e367f175a4c5ade2c3ee906876584c6c15e98a7 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponentController.cc +++ b/src/AutoPilotPlugins/PX4/AirframeComponentController.cc @@ -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() { } - diff --git a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc index b0409a7dda20d30346064a747ce8b8a5f2e54633..41204218075490efc67c74d40934e51ce2479ce7 100644 --- a/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc @@ -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("QGroundControl.Controllers", 1, 0, "FlightModesComponentController"); qmlRegisterType("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)); } diff --git a/src/AutoPilotPlugins/PX4/PX4Component.cc b/src/AutoPilotPlugins/PX4/PX4Component.cc index 25cd36eb6bfc05546abb9915372d41303e3bd038..81072833ab7a6ffb213d915c8122416ce480b7b4 100644 --- a/src/AutoPilotPlugins/PX4/PX4Component.cc +++ b/src/AutoPilotPlugins/PX4/PX4Component.cc @@ -25,32 +25,29 @@ /// @author Don Gagne #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()); } diff --git a/src/AutoPilotPlugins/PX4/PX4Component.h b/src/AutoPilotPlugins/PX4/PX4Component.h index 8efdb3f3354bcebdf271c78236d09e499c364970..74455ceda59946efb651c0b80d0fc01644abb4e7 100644 --- a/src/AutoPilotPlugins/PX4/PX4Component.h +++ b/src/AutoPilotPlugins/PX4/PX4Component.h @@ -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 diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc index daa2d44f71cd8aaec0e1f6e6229cdeca0987e504..66ee86fdc04b0b1b10435ccf22e09875d19188cd 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc +++ b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.cc @@ -38,8 +38,8 @@ QGC_LOGGING_CATEGORY(PX4ParameterLoaderLog, "PX4ParameterLoaderLog") bool PX4ParameterLoader::_parameterMetaDataLoaded = false; QMap 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; } } } diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h index 026e9c4e73bf54a85bc6b1498cdc1cbe38e5d42e..c90631c0ec2a4286fa347cddd4fd3ea341eba0bf 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h +++ b/src/AutoPilotPlugins/PX4/PX4ParameterLoader.h @@ -32,6 +32,7 @@ #include "ParameterLoader.h" #include "FactSystem.h" #include "UASInterface.h" +#include "AutoPilotPlugin.h" /// @file /// @author Don Gagne @@ -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"); } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 9825238057fd3d26c02191493be7a1c1be987f97..c3d8ac7575943defd72853d9d8bf4f56b6986f58 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -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) diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index 688dfde5b091b8ff169e63abf1329b7bf9d450ee..356b3493e797cb80142520a3ed707bc07e4d74ce 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -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) diff --git a/src/FactSystem/FactSystemTestBase.cc b/src/FactSystem/FactSystemTestBase.cc index b4751c8b001f58f6a8658fc09a5f5fe0130ed7cf..b0944b722e4e2b0561c6010bcad274343c16ed48 100644 --- a/src/FactSystem/FactSystemTestBase.cc +++ b/src/FactSystem/FactSystemTestBase.cc @@ -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 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 diff --git a/src/FactSystem/FactSystemTestBase.h b/src/FactSystem/FactSystemTestBase.h index 3d0536c909a70c535c260f382d06a347dc0628ee..0a6cf311939d81143bf7f9a238523ae9f67d9bdb 100644 --- a/src/FactSystem/FactSystemTestBase.h +++ b/src/FactSystem/FactSystemTestBase.h @@ -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; }; diff --git a/src/FactSystem/FactSystemTestGeneric.h b/src/FactSystem/FactSystemTestGeneric.h index d54380e21a34aa313b58466827ed7db315f1696b..df0c1199b973a6772e1b9f6af6194b71d4b6ebe8 100644 --- a/src/FactSystem/FactSystemTestGeneric.h +++ b/src/FactSystem/FactSystemTestGeneric.h @@ -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(); } }; diff --git a/src/FactSystem/FactSystemTestPX4.h b/src/FactSystem/FactSystemTestPX4.h index d6024a24b799d8d8e3774286f632166bd0f273b8..929e6a306eccdfcf4329be84fa94f59844f64da6 100644 --- a/src/FactSystem/FactSystemTestPX4.h +++ b/src/FactSystem/FactSystemTestPX4.h @@ -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(); } }; diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index c1a5da25196843d187fb3eba41e5bb4e40a7f551..cbea9627b787d6c45e78b6438298bd21c9ef63bd 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -28,33 +28,39 @@ #include "QGCApplication.h" #include "QGCLoggingCategory.h" #include "QGCApplication.h" +#include "QGCMessageBox.h" #include #include QGC_LOGGING_CATEGORY(ParameterLoaderLog, "ParameterLoaderLog") -ParameterLoader::ParameterLoader(UASInterface* uas, QObject* parent) : +ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) : QObject(parent), - _paramMgr(NULL), + _autopilot(autopilot), + _uas(uas), + _mavlink(MAVLinkProtocol::instance()), _parametersReady(false), - _defaultComponentId(FactSystem::defaultComponentId) + _defaultComponentId(FactSystem::defaultComponentId), + _totalParamCount(0), + _fullRefresh(false) { - Q_ASSERT(uas); - - _uasId = uas->getUASID(); + Q_ASSERT(_autopilot); + Q_ASSERT(_uas); + Q_ASSERT(_mavlink); - _paramMgr = uas->getParamManager(); - Q_ASSERT(_paramMgr); + // We signal this to ouselves in order to start timer on our thread + connect(this, &ParameterLoader::restartWaitingParamTimer, this, &ParameterLoader::_restartWaitingParamTimer); - // We need to be initialized before param mgr starts sending parameters so we catch each one - Q_ASSERT(!_paramMgr->parametersReady()); + _waitingParamTimeoutTimer.setSingleShot(true); + _waitingParamTimeoutTimer.setInterval(100); + connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_waitingParamTimeout); - // We need to know when the param mgr is done sending the initial set of paramters - connect(_paramMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_paramMgrParameterListUpToDate())); + // FIXME: Why not direct connect? + connect(_uas, SIGNAL(parameterUpdate(int, int, QString, int, int, int, QVariant)), this, SLOT(_parameterUpdate(int, int, QString, int, int, int, QVariant))); - // We track parameters changes to keep Facts up to date. - connect(uas, &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate); + // Request full param list + refreshAllParameters(); } ParameterLoader::~ParameterLoader() @@ -63,15 +69,111 @@ ParameterLoader::~ParameterLoader() } /// Called whenever a parameter is updated or first seen. -void ParameterLoader::_parameterUpdate(int uas, int componentId, QString parameterName, int mavType, QVariant value) +void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value) { bool setMetaData = false; // Is this for our uas? - if (uas != _uasId) { + if (uasId != _uas->getUASID()) { return; } + qCDebug(ParameterLoaderLog) << "_parameterUpdate (usaId:" << uasId << + "componentId:" << componentId << + "name:" << parameterName << + "count:" << parameterCount << + "index:" << parameterId << + "mavType:" << mavType << + "value:" << value << + ")"; + +#if 1 + // This code is handy for testing re-request logic. It drops ever other param update. + static int increment = 0; + if (increment++ & 1) { + return; + } +#endif + + _dataMutex.lock(); + + // Restart our waiting for param timer + _waitingParamTimeoutTimer.start(); + + // Update our total parameter counts + if (!_paramCountMap.contains(componentId)) { + _paramCountMap[componentId] = parameterCount; + _totalParamCount += parameterCount; + } + + // If we've never seen this component id before, setup the wait lists. + if (!_waitingReadParamIndexMap.contains(componentId)) { + QStringList paramNameList; + QList paramIndexList; + + // Parameter index is 0-based + for (int i=0; i(); Q_ASSERT(fact); fact->_containerSetValue(value); @@ -135,67 +235,50 @@ void ParameterLoader::_parameterUpdate(int uas, int componentId, QString paramet if (setMetaData) { _addMetaDataToFact(fact); } + + _dataMutex.unlock(); + + if (waitingParamCount == 0) { + // Now that we know vehicle is up to date persist + _saveToEEPROM(); + } + + // Check to see if we have the full param list for the first time + + if (_fullRefresh) { + if (waitingParamCount == 0) { + if (!_parametersReady) { + _parametersReady = true; + _determineDefaultComponentId(); + _setupGroupMap(); + emit parametersReady(); + } + } + } } /// Connected to Fact::valueUpdated /// -/// Sets the new value into the Parameter Manager. Parameter is persisted after send. +/// Writes the parameter to mavlink, sets up for write wait void ParameterLoader::_valueUpdated(const QVariant& value) { Fact* fact = qobject_cast(sender()); Q_ASSERT(fact); int componentId = fact->componentId(); + QString name = fact->name(); - Q_ASSERT(_paramMgr); + _dataMutex.lock(); - 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; - } + Q_ASSERT(_waitingWriteParamNameMap.contains(componentId)); + _waitingWriteParamNameMap[componentId].removeOne(name); + _waitingWriteParamNameMap[componentId] << name; + _waitingParamTimeoutTimer.start(); - 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(); - _setupGroupMap(); - - // We should have all parameters now so we can signal ready - emit parametersReady(); - } + _dataMutex.unlock(); + + _writeParameterRaw(componentId, fact->name(), value); + qCDebug(ParameterLoaderLog) << "Set parameter (componentId:" << componentId << "name:" << name << value << ")"; } void ParameterLoader::_addMetaDataToFact(Fact* fact) @@ -206,8 +289,32 @@ void ParameterLoader::_addMetaDataToFact(Fact* fact) void ParameterLoader::refreshAllParameters(void) { - Q_ASSERT(_paramMgr); - _paramMgr->requestParameterList(); + _dataMutex.lock(); + + _fullRefresh = true; + + // Reset index wait lists + foreach (int componentId, _paramCountMap.keys()) { + QList paramIndexList; + + // Parameter index is 0-based + for (int i=0; i<_paramCountMap[componentId]; i++) { + paramIndexList << i; + } + + _waitingReadParamIndexMap[componentId] = paramIndexList; + } + + _dataMutex.unlock(); + + MAVLinkProtocol* mavlink = MAVLinkProtocol::instance(); + Q_ASSERT(mavlink); + + mavlink_message_t msg; + mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _uas->getUASID(), MAV_COMP_ID_ALL); + _uas->sendMessage(msg); + + qCDebug(ParameterLoaderLog) << "Request to refresh all parameters"; } void ParameterLoader::_determineDefaultComponentId(void) @@ -240,16 +347,29 @@ int ParameterLoader::_actualComponentId(int componentId) void ParameterLoader::refreshParameter(int componentId, const QString& name) { - Q_ASSERT(_paramMgr); + componentId = _actualComponentId(componentId); + qCDebug(ParameterLoaderLog) << "refreshParameter (component id:" << componentId << "name:" << name << ")"; + + _dataMutex.lock(); + + Q_ASSERT(_waitingReadParamNameMap.contains(componentId)); + + if (_waitingReadParamNameMap.contains(componentId)) { + _waitingReadParamNameMap[componentId].removeOne(name); + _waitingReadParamNameMap[componentId] << name; + emit restartWaitingParamTimer(); + } - _paramMgr->requestParameterUpdate(_actualComponentId(componentId), name); + _dataMutex.unlock(); + + _readParameterRaw(componentId, name, -1); } void ParameterLoader::refreshParametersPrefix(int componentId, const QString& namePrefix) { - Q_ASSERT(_paramMgr); - componentId = _actualComponentId(componentId); + qCDebug(ParameterLoaderLog) << "refreshParametersPrefix (component id:" << componentId << "name:" << namePrefix << ")"; + foreach(QString name, _mapParameterName2Variant[componentId].keys()) { if (name.startsWith(namePrefix)) { refreshParameter(componentId, name); @@ -259,11 +379,14 @@ void ParameterLoader::refreshParametersPrefix(int componentId, const QString& na bool ParameterLoader::parameterExists(int componentId, const QString& name) { + bool ret = false; + componentId = _actualComponentId(componentId); if (_mapParameterName2Variant.contains(componentId)) { - return _mapParameterName2Variant[componentId].contains(name); + ret = _mapParameterName2Variant[componentId].contains(name); } - return false; + + return ret; } Fact* ParameterLoader::getFact(int componentId, const QString& name) @@ -277,6 +400,7 @@ Fact* ParameterLoader::getFact(int componentId, const QString& name) Fact* fact = _mapParameterName2Variant[componentId][name].value(); Q_ASSERT(fact); + return fact; } @@ -305,3 +429,289 @@ const QMap >& ParameterLoader::getGroupMap(void) { return _mapGroup2ParameterName; } + +void ParameterLoader::_waitingParamTimeout(void) +{ + bool paramsRequested = false; + const int maxBatchSize = 10; + int batchCount = 0; + + MAVLinkProtocol* mavlink = MAVLinkProtocol::instance(); + Q_ASSERT(mavlink); + + // We timed out waiting for some parameters from the initial set. Re-request those. + + batchCount = 0; + foreach(int componentId, _waitingReadParamIndexMap.keys()) { + foreach(int paramIndex, _waitingReadParamIndexMap[componentId]) { + paramsRequested = true; + _readParameterRaw(componentId, "", paramIndex); + qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramIndex:" << paramIndex << ")"; + + if (++batchCount > maxBatchSize) { + goto Out; + } + } + } + + if (!paramsRequested) { + foreach(int componentId, _waitingWriteParamNameMap.keys()) { + foreach(QString paramName, _waitingWriteParamNameMap[componentId]) { + paramsRequested = true; + _writeParameterRaw(componentId, paramName, _autopilot->getFact(FactSystem::ParameterProvider, componentId, paramName)->value()); + qCDebug(ParameterLoaderLog) << "Write resend for (componentId:" << componentId << "paramName:" << paramName << ")"; + + if (++batchCount > maxBatchSize) { + goto Out; + } + } + } + } + + if (!paramsRequested) { + foreach(int componentId, _waitingReadParamNameMap.keys()) { + foreach(QString paramName, _waitingReadParamNameMap[componentId]) { + paramsRequested = true; + _readParameterRaw(componentId, paramName, -1); + qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramName:" << paramName << ")"; + + if (++batchCount > maxBatchSize) { + goto Out; + } + } + } + } + +Out: + if (paramsRequested) { + _waitingParamTimeoutTimer.start(); + } +} + +void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex) +{ + mavlink_message_t msg; + char fixedParamName[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN]; + + strncpy(fixedParamName, paramName.toStdString().c_str(), sizeof(fixedParamName)); + mavlink_msg_param_request_read_pack(_mavlink->getSystemId(), // Our system id + _mavlink->getComponentId(), // Our component id + &msg, // Pack into this mavlink_message_t + _uas->getUASID(), // Target system id + componentId, // Target component id + fixedParamName, // Named parameter being requested + paramIndex); // Parameter index being requested, -1 for named + _uas->sendMessage(msg); +} + +void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value) +{ + bool floatHack = _uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA; + + mavlink_param_set_t p; + mavlink_param_union_t union_value; + + FactMetaData::ValueType_t factType = _autopilot->getFact(FactSystem::ParameterProvider, componentId, paramName)->type(); + p.param_type = _factTypeToMavType(factType); + + switch (factType) { + case FactMetaData::valueTypeUint8: + if (floatHack) { + union_value.param_float = (uint8_t)value.toUInt(); + } else { + union_value.param_uint8 = (uint8_t)value.toUInt(); + } + break; + + case FactMetaData::valueTypeInt8: + if (floatHack) { + union_value.param_float = (int8_t)value.toInt(); + } else { + union_value.param_int8 = (int8_t)value.toInt(); + } + break; + + case FactMetaData::valueTypeUint16: + if (floatHack) { + union_value.param_float = (uint16_t)value.toUInt(); + } else { + union_value.param_uint16 = (uint16_t)value.toUInt(); + } + break; + + case FactMetaData::valueTypeInt16: + if (floatHack) { + union_value.param_float = (int16_t)value.toInt(); + } else { + union_value.param_int16 = (int16_t)value.toInt(); + } + break; + + case FactMetaData::valueTypeUint32: + if (floatHack) { + union_value.param_float = (uint32_t)value.toUInt(); + } else { + union_value.param_uint32 = (uint32_t)value.toUInt(); + } + break; + + case FactMetaData::valueTypeFloat: + union_value.param_float = value.toFloat(); + break; + + default: + qCritical() << "Unsupported fact type" << factType; + // fall through + + case FactMetaData::valueTypeInt32: + if (floatHack) { + union_value.param_float = (int32_t)value.toInt(); + } else { + union_value.param_int32 = (int32_t)value.toInt(); + } + break; + } + + p.param_value = union_value.param_float; + p.target_system = (uint8_t)_uas->getUASID(); + p.target_component = (uint8_t)componentId; + + strncpy(p.param_id, paramName.toStdString().c_str(), sizeof(p.param_id)); + + mavlink_message_t msg; + mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); + _uas->sendMessage(msg); +} + +void ParameterLoader::_saveToEEPROM(void) +{ + mavlink_message_t msg; + mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _uas->getUASID(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); + _uas->sendMessage(msg); + qCDebug(ParameterLoaderLog) << "_saveToEEPROM"; +} + +void ParameterLoader::readParametersFromStream(QTextStream& stream) +{ + bool userWarned = false; + + while (!stream.atEnd()) { + QString line = stream.readLine(); + if (!line.startsWith("#")) { + QStringList wpParams = line.split("\t"); + int lineMavId = wpParams.at(0).toInt(); + if (wpParams.size() == 5) { + if (!userWarned && (_uas->getUASID() != lineMavId)) { + userWarned = true; + QString msg("The parameters in the stream have been saved from System Id %1, but the current vehicle has the System Id %2."); + QGCMessageBox::StandardButton button = QGCMessageBox::warning("Parameter Load", + msg.arg(lineMavId).arg(_uas->getUASID()), + QGCMessageBox::Ok | QGCMessageBox::Cancel, + QGCMessageBox::Cancel); + if (button == QGCMessageBox::Cancel) { + return; + } + } + + int componentId = wpParams.at(1).toInt(); + QString paramName = wpParams.at(2); + QString valStr = wpParams.at(3); + uint mavType = wpParams.at(4).toUInt(); + + if (!_autopilot->factExists(FactSystem::ParameterProvider, componentId, paramName)) { + continue; + } + + Fact* fact = _autopilot->getFact(FactSystem::ParameterProvider, componentId, paramName); + if (fact->type() != _mavTypeToFactType((MAV_PARAM_TYPE)mavType)) { + continue; + } + + fact->setValue(valStr); + } + } + } +} + +void ParameterLoader::writeParametersToStream(QTextStream &stream, const QString& name) +{ + stream << "# Onboard parameters for system " << name << "\n"; + stream << "#\n"; + stream << "# MAV ID COMPONENT ID PARAM NAME VALUE (FLOAT)\n"; + + foreach (int componentId, _mapParameterName2Variant.keys()) { + foreach (QString paramName, _mapParameterName2Variant[componentId].keys()) { + Fact* fact = _mapParameterName2Variant[componentId][paramName].value(); + Q_ASSERT(fact); + + stream << _uas->getUASID() << "\t" << componentId << "\t" << paramName << "\t" << fact->valueString() << "\t" << QString("%1").arg(_factTypeToMavType(fact->type())) << "\n"; + } + } + + stream.flush(); +} + +MAV_PARAM_TYPE ParameterLoader::_factTypeToMavType(FactMetaData::ValueType_t factType) +{ + switch (factType) { + case FactMetaData::valueTypeUint8: + return MAV_PARAM_TYPE_UINT8; + + case FactMetaData::valueTypeInt8: + return MAV_PARAM_TYPE_INT8; + + case FactMetaData::valueTypeUint16: + return MAV_PARAM_TYPE_UINT16; + + case FactMetaData::valueTypeInt16: + return MAV_PARAM_TYPE_INT16; + + case FactMetaData::valueTypeUint32: + return MAV_PARAM_TYPE_UINT32; + + case FactMetaData::valueTypeFloat: + return MAV_PARAM_TYPE_REAL32; + + default: + qWarning() << "Unsupported fact type" << factType; + // fall through + + case FactMetaData::valueTypeInt32: + return MAV_PARAM_TYPE_INT32; + } +} + +FactMetaData::ValueType_t ParameterLoader::_mavTypeToFactType(MAV_PARAM_TYPE mavType) +{ + switch (mavType) { + case MAV_PARAM_TYPE_UINT8: + return FactMetaData::valueTypeUint8; + + case MAV_PARAM_TYPE_INT8: + return FactMetaData::valueTypeInt8; + + case MAV_PARAM_TYPE_UINT16: + return FactMetaData::valueTypeUint16; + + case MAV_PARAM_TYPE_INT16: + return FactMetaData::valueTypeInt16; + + case MAV_PARAM_TYPE_UINT32: + return FactMetaData::valueTypeUint32; + + case MAV_PARAM_TYPE_REAL32: + return FactMetaData::valueTypeFloat; + + default: + qWarning() << "Unsupported mav param type" << mavType; + // fall through + + case MAV_PARAM_TYPE_INT32: + return FactMetaData::valueTypeInt32; + } +} + +void ParameterLoader::_restartWaitingParamTimer(void) +{ + _waitingParamTimeoutTimer.start(); +} diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 0237e757ea9e9b79da42fc525bbe3196d816c40b..a94921fcddfa0dd741c1b5a3c8945b2acfc17001 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -28,9 +28,13 @@ #include #include #include +#include #include "FactSystem.h" #include "UASInterface.h" +#include "MAVLinkProtocol.h" +#include "AutoPilotPlugin.h" +#include "QGCMavlink.h" /// @file /// @author Don Gagne @@ -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 >& 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 _mapParameterName2Variant; @@ -115,6 +134,19 @@ private: bool _parametersReady; ///< All params received from param mgr int _defaultComponentId; QString _defaultComponentIdParam; + + QMap _paramCountMap; ///< Map of total known parameter count, keyed by component id + QMap > _waitingReadParamIndexMap; ///< Map of param indices waiting for initial first time read, keyed by component id + QMap _waitingReadParamNameMap; ///< Map of param names we are waiting to hear a read response from, keyed by component id + QMap _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 diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index 88f5aefac59ef36fd7768ead59e0764ae4fc01c7..34c1a697f744e20029785c509edbb7bb871e4475 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -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); diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc deleted file mode 100644 index bec6cfae559024fa32d839bd9cb63ce057c558b0..0000000000000000000000000000000000000000 --- a/src/uas/QGCUASParamManager.cc +++ /dev/null @@ -1,199 +0,0 @@ -#include "QGCUASParamManager.h" - -#include -#include -#include - -#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(¶mCommsMgr, SIGNAL(parameterStatusMsgUpdated(QString,int)), - this, SIGNAL(parameterStatusMsgUpdated(QString,int))); - - connect(¶mCommsMgr, SIGNAL(parameterListUpToDate()), this, SLOT(_parameterListUpToDate())); - - connect(¶mCommsMgr, SIGNAL(parameterListProgress(float)), this, SIGNAL(parameterListProgress(float))); - - // Pass along data model updates - connect(¶mDataModel, SIGNAL(parameterUpdated(int, QString , QVariant )), - this, SIGNAL(parameterUpdated(int, QString , QVariant ))); - - connect(¶mDataModel, 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(); -} - -QList QGCUASParamManager::getComponentForParam(const QString& parameter) const -{ - return paramDataModel.getComponentForOnboardParam(parameter); -} - - -bool QGCUASParamManager::getParameterValue(int component, const QString& parameter, QVariant& value) const -{ - return paramDataModel.getOnboardParamValue(component,parameter,value); -} - - -void QGCUASParamManager::requestParameterUpdate(int component, const QString& parameter) -{ - if (mav) { - paramCommsMgr.requestParameterUpdate(component,parameter); - } -} - - - -/** - * Send a request to deliver the list of onboard parameters - * to the MAV. - */ -void QGCUASParamManager::requestParameterList() -{ - if (mav) { - emit parameterStatusMsgUpdated(tr("Requested param list.. waiting"), UASParameterCommsMgr::ParamCommsStatusLevel_OK); - paramCommsMgr.requestParameterList(); - } -} - -void QGCUASParamManager::requestParameterListIfEmpty() -{ - if (mav) { - int totalOnboard = paramDataModel.countOnboardParams(); - if (totalOnboard < 2) { //TODO arbitrary constant, maybe 0 is OK? - requestParameterList(); - } - } -} - - -void QGCUASParamManager::setParamDescriptions(const QMap& paramInfo) { - paramDataModel.setParamDescriptions(paramInfo); -} - - -void QGCUASParamManager::setParameter(int compId, QString paramName, QVariant value) -{ - if ((0 == compId) || (-1 == compId)) { - //attempt to get an actual component ID - compId = paramDataModel.getDefaultComponentId(); - } - paramDataModel.updatePendingParamWithValue(compId,paramName,value); -} - -void QGCUASParamManager::sendPendingParameters(bool persistAfterSend, bool forceSend) -{ - paramCommsMgr.sendPendingParameters(persistAfterSend, forceSend); -} - - - - -void QGCUASParamManager::setPendingParam(int compId, const QString& paramName, const QVariant& value, bool forceSend) -{ - if ((0 == compId) || (-1 == compId)) { - //attempt to get an actual component ID - compId = paramDataModel.getDefaultComponentId(); - } - paramDataModel.updatePendingParamWithValue(compId,paramName,value, forceSend); -} - - - -UASInterface* QGCUASParamManager::getUAS() -{ - return mav; -} - -UASParameterDataModel* QGCUASParamManager::dataModel() -{ - return ¶mDataModel; -} - - - -void QGCUASParamManager::writeOnboardParamsToStream(QTextStream &stream, const QString& uasName) -{ - paramDataModel.writeOnboardParamsToStream(stream,uasName); -} - -void QGCUASParamManager::readPendingParamsFromStream(QTextStream &stream) -{ - paramDataModel.readUpdateParamsFromStream(stream); -} - - - -void QGCUASParamManager::copyVolatileParamsToPersistent() -{ - int changedParamCount = paramDataModel.countPendingParams(); - - if (changedParamCount > 0) { - QGCMessageBox::warning(tr("Warning"), - tr("There are locally changed parameters. Please transmit them first () or update them with the onboard values () before storing onboard from RAM to ROM.")); - } - else { - paramCommsMgr.writeParamsToPersistentStorage(); - } -} - - -void QGCUASParamManager::copyPersistentParamsToVolatile() -{ - if (mav) { - mav->readParametersFromStorage(); //TODO use comms mgr instead? - } -} - - -void QGCUASParamManager::requestRcCalibrationParamsUpdate() { - paramCommsMgr.requestRcCalibrationParamsUpdate(); -} - -void QGCUASParamManager::_parameterListUpToDate(void) -{ - //qDebug() << "Emitting parameters ready, count:" << paramDataModel.countOnboardParams(); - - _parametersReady = true; - emit parameterListUpToDate(); -} diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h deleted file mode 100644 index 421f754ca8c54dc19098ffe55d7a8b036d5938ce..0000000000000000000000000000000000000000 --- a/src/uas/QGCUASParamManager.h +++ /dev/null @@ -1,137 +0,0 @@ -#ifndef QGCUASPARAMMANAGER_H -#define QGCUASPARAMMANAGER_H - -#include -#include -#include -#include - -#include "UASParameterDataModel.h" -#include "QGCUASParamManagerInterface.h" -#include "UASParameterCommsMgr.h" - -//forward declarations -class QTextStream; -class UASInterface; - -class QGCUASParamManager : public QGCUASParamManagerInterface -{ - Q_OBJECT -public: - QGCUASParamManager(QObject* parent = 0); - QGCUASParamManager* initWithUAS(UASInterface* uas); - - /** @brief Get the known, confirmed value of a parameter */ - virtual bool getParameterValue(int component, const QString& parameter, QVariant& value) const; - - /** @brief determine which component is the root component for the UAS and return its ID or 0 if unknown */ - virtual int getDefaultComponentId(); - - /** - * @brief Get a list of all component IDs using this parameter name - * @param parameter The string encoding the parameter name - * @return A list with all components using this parameter name. Can be empty. - */ - virtual QList getComponentForParam(const QString& parameter) const; - - /** @brief Provide tooltips / user-visible descriptions for parameters */ - virtual void setParamDescriptions(const QMap& paramDescs); - - /** - * @brief Count the pending parameters in the current transmission - * @return The number of pending parameters - */ - virtual int countPendingParams() { - return paramDataModel.countPendingParams(); - } - - /** - * @brief Count the number of onboard parameters - * @return The number of onboard parameters - */ - virtual int countOnboardParams() { - return paramDataModel.countOnboardParams(); - } - - /** @brief Get the UAS of this widget - * @return The MAV of this mgr. Unless the MAV object has been destroyed, this is never null. - */ - UASInterface* getUAS(); - - /** @return The data model managed by this class */ - virtual UASParameterDataModel* dataModel(); - - /// @return true: first full set of parameters received - virtual bool parametersReady(void) { return _parametersReady; } - -protected: - void connectToModelAndComms(); - - -signals: - - /** @brief We updated the parameter status message */ - void parameterStatusMsgUpdated(QString msg, int level); - /** @brief We have received a complete list of all parameters onboard the MAV */ - void parameterListUpToDate(); - void parameterListProgress(float percentComplete); - - /** @brief We've received an update of a parameter's value */ - void parameterUpdated(int compId, QString paramName, QVariant value); - - /** @brief Notifies listeners that a param was added to or removed from the pending list */ - void pendingParamUpdate(int compId, const QString& paramName, QVariant value, bool isPending); - - - -public slots: - /** @brief Send one parameter to the MAV: changes value in transient memory of MAV */ - virtual void setParameter(int component, QString parameterName, QVariant value); - - /** @brief Send all pending parameters to the MAV, for storage in transient (RAM) memory - * @param persistAfterSend If true, all parameters will be written to persistent storage as well - */ - virtual void sendPendingParameters(bool persistAfterSend = false, bool forceSend = false); - - - /** @brief Request list of parameters from MAV */ - virtual void requestParameterList(); - - /** @brief Request a list of params onboard the MAV if the onboard param list we have is empty */ - virtual void requestParameterListIfEmpty(); - - /** @brief queue a pending parameter for sending to the MAV */ - virtual void setPendingParam(int componentId, const QString& key, const QVariant& value, bool forceSend = false); - - /** @brief remove all params from the pending list */ - virtual void clearAllPendingParams(); - - /** @brief Request a single parameter by name from the MAV */ - virtual void requestParameterUpdate(int component, const QString& parameter); - - - virtual void writeOnboardParamsToStream(QTextStream &stream, const QString& uasName); - virtual void readPendingParamsFromStream(QTextStream &stream); - - virtual void requestRcCalibrationParamsUpdate(); - - /** @brief Copy the current parameters in volatile RAM to persistent storage (EEPROM/HDD) */ - virtual void copyVolatileParamsToPersistent(); - /** @brief Copy the parameters from persistent storage to volatile RAM */ - virtual void copyPersistentParamsToVolatile(); - -private slots: - void _parameterListUpToDate(void); - -protected: - - // Parameter data model - UASInterface* mav; ///< The MAV this manager is controlling - UASParameterDataModel paramDataModel;///< Shared data model of parameters - UASParameterCommsMgr paramCommsMgr; ///< Shared comms mgr for parameters - - bool _parametersReady; - -}; - -#endif // QGCUASPARAMMANAGER_H diff --git a/src/uas/QGCUASParamManagerInterface.h b/src/uas/QGCUASParamManagerInterface.h deleted file mode 100644 index a704fe6afc8d10af684312c79513beed31932724..0000000000000000000000000000000000000000 --- a/src/uas/QGCUASParamManagerInterface.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef QGCUASPARAMMANAGERINTERFACE_H -#define QGCUASPARAMMANAGERINTERFACE_H - -#include -#include -#include -#include -#include - -#include "UASParameterDataModel.h" - -/** - * @brief This is the abstract base class for QGCUASParamManager. Although there is - * normally only a single QGCUASParamManager we still use an abstract base interface - * since this allows us to create mock versions. - * - * See QGCUASParamManager.h for method documentation - **/ - -//forward declarations -class QTextStream; -class UASInterface; -class UASParameterCommsMgr; - -class QGCUASParamManagerInterface : public QObject -{ -public: - QGCUASParamManagerInterface(QObject* parent = NULL) : QObject(parent) { } - virtual bool getParameterValue(int component, const QString& parameter, QVariant& value) const = 0; - virtual int getDefaultComponentId() = 0; - virtual QList getComponentForParam(const QString& parameter) const = 0; - virtual void setParamDescriptions(const QMap& paramDescs) = 0; - virtual int countPendingParams() = 0; - virtual int countOnboardParams() = 0; - virtual UASParameterDataModel* dataModel() = 0; - virtual bool parametersReady(void) = 0; - -public slots: - virtual void setParameter(int component, QString parameterName, QVariant value) = 0; - virtual void sendPendingParameters(bool persistAfterSend = false, bool forceSend = false) = 0; - virtual void requestParameterList() = 0; - virtual void requestParameterListIfEmpty() = 0; - virtual void setPendingParam(int componentId, const QString& key, const QVariant& value, bool forceSend = false) = 0; - virtual void clearAllPendingParams() = 0; - virtual void requestParameterUpdate(int component, const QString& parameter) = 0; - virtual void writeOnboardParamsToStream(QTextStream &stream, const QString& uasName) = 0; - virtual void readPendingParamsFromStream(QTextStream &stream) = 0; - virtual void requestRcCalibrationParamsUpdate() = 0; - virtual void copyVolatileParamsToPersistent() = 0; - virtual void copyPersistentParamsToVolatile() = 0; - -signals: - void parameterStatusMsgUpdated(QString msg, int level); - void parameterListUpToDate(); - void parameterListProgress(float percentComplete); - void parameterUpdated(int compId, QString paramName, QVariant value); - void pendingParamUpdate(int compId, const QString& paramName, QVariant value, bool isPending); -}; - -#endif // QGCUASPARAMMANAGER_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index ae97ec0186d82459ea3f3715a77422e0b01a98c8..d5f380b2bab4a4c47d37a1d0dd35a81bbc550806 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -27,7 +27,6 @@ #include "QGCMAVLink.h" #include "LinkManager.h" #include "SerialLink.h" -#include "UASParameterCommsMgr.h" #include #include "AutoPilotPluginManager.h" #include "QGCMessageBox.h" @@ -143,8 +142,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), receivedMode(false), - paramsOnceRequested(false), - paramMgr(this), simulation(0), // The protected members. @@ -230,8 +227,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); statusTimeout.start(500); readSettings(); - //need to init paramMgr after readSettings have been loaded, to properly set autopilot and so forth - paramMgr.initWithUAS(this); + // Initial signals emit disarmed(); emit armingChanged(false); @@ -1645,26 +1641,6 @@ quint64 UAS::getUnixTime(quint64 time) return ret; } -/** -* @param component that will be searched for in the map of parameters. -*/ -QList UAS::getParameterNames(int component) -{ - if (parameters.contains(component)) - { - return parameters.value(component)->keys(); - } - else - { - return QList(); - } -} - -QList UAS::getComponentIds() -{ - return parameters.keys(); -} - /** * @param newBaseMode that UAS is to be set to. * @param newCustomMode that UAS is to be set to. @@ -1914,21 +1890,6 @@ quint64 UAS::getUptime() const } } -void UAS::writeParametersToStorage() -{ - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); - qDebug() << "SENT COMMAND" << MAV_CMD_PREFLIGHT_STORAGE; - sendMessage(msg); -} - -void UAS::readParametersFromStorage() -{ - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 0, -1, -1, -1, 0, 0, 0); - sendMessage(msg); -} - bool UAS::isRotaryWing() { switch (type) { @@ -2205,116 +2166,11 @@ void UAS::enableExtra3Transmission(int rate) sendMessage(msg); } -/** - * Set a parameter value onboard - * - * @param component The component to set the parameter - * @param id Name of the parameter - */ -void UAS::setParameter(const int compId, const QString& paramId, const QVariant& value) -{ - if (!paramId.isNull()) - { - mavlink_message_t msg; - mavlink_param_set_t p; - mavlink_param_union_t union_value; - - // Assign correct value based on QVariant - // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling. - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) - { - switch ((int)value.type()) - { - case QVariant::Char: - union_value.param_float = (unsigned char)value.toChar().toLatin1(); - p.param_type = MAV_PARAM_TYPE_INT8; - break; - case QVariant::Int: - union_value.param_float = value.toInt(); - p.param_type = MAV_PARAM_TYPE_INT32; - break; - case QVariant::UInt: - union_value.param_float = value.toUInt(); - p.param_type = MAV_PARAM_TYPE_UINT32; - break; - case QMetaType::Float: - union_value.param_float = value.toFloat(); - p.param_type = MAV_PARAM_TYPE_REAL32; - break; - default: - qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; - return; - } - } - else - { - switch ((int)value.type()) - { - case QVariant::Char: - union_value.param_int8 = (unsigned char)value.toChar().toLatin1(); - p.param_type = MAV_PARAM_TYPE_INT8; - break; - case QVariant::Int: - union_value.param_int32 = value.toInt(); - p.param_type = MAV_PARAM_TYPE_INT32; - break; - case QVariant::UInt: - union_value.param_uint32 = value.toUInt(); - p.param_type = MAV_PARAM_TYPE_UINT32; - break; - case QMetaType::Float: - union_value.param_float = value.toFloat(); - p.param_type = MAV_PARAM_TYPE_REAL32; - break; - default: - qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE"; - return; - } - } - - p.param_value = union_value.param_float; - p.target_system = (uint8_t)uasId; - p.target_component = (uint8_t)compId; - - //qDebug() << "SENT PARAM:" << value; - - // Copy string into buffer, ensuring not to exceed the buffer size - for (unsigned int i = 0; i < sizeof(p.param_id); i++) - { - // String characters - if ((int)i < paramId.length()) - { - p.param_id[i] = paramId.toLatin1()[i]; - } - else - { - // Fill rest with zeros - p.param_id[i] = 0; - } - } - mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p); - sendMessage(msg); - } -} - - - - //TODO update this to use the parameter manager / param data model instead void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramUnion) { int compId = msg.compid; - // Insert component if necessary - if (!parameters.contains(compId)) { - parameters.insert(compId, new QMap()); - } - - // Insert parameter into registry - if (parameters.value(compId)->contains(paramName)) { - parameters.value(compId)->remove(paramName); - } - QVariant paramValue; // Insert with correct type @@ -2374,50 +2230,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type; - parameters.value(compId)->insert(paramName, paramValue); - emit parameterChanged(uasId, compId, paramName, paramValue); - emit parameterUpdate(uasId, compId, paramName, rawValue.param_type, paramValue); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, paramValue); -} - -/** -* Request parameter, use parameter name to request it. -*/ -void UAS::requestParameter(int component, int id) -{ - // Request parameter, use parameter name to request it - mavlink_message_t msg; - mavlink_param_request_read_t read; - read.param_index = id; - read.param_id[0] = '\0'; // Enforce null termination - read.target_system = uasId; - read.target_component = component; - mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read); - sendMessage(msg); - //qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM ID" << id; -} - -/** -* Request a parameter, use parameter name to request it. -*/ -void UAS::requestParameter(int component, const QString& parameter) -{ - // Request parameter, use parameter name to request it - mavlink_message_t msg; - mavlink_param_request_read_t read; - read.param_index = -1; - // Copy full param name or maximum max field size - if (parameter.length() > MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN) - { - emit textMessageReceived(uasId, 0, MAV_SEVERITY_WARNING, QString("QGC WARNING: Parameter name %1 is more than %2 bytes long. This might lead to errors and mishaps!").arg(parameter).arg(MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN-1)); - } - strncpy(read.param_id, parameter.toStdString().c_str(), sizeof(read.param_id)); - read.param_id[sizeof(read.param_id) - 1] = '\0'; // Enforce null termination - read.target_system = uasId; - read.target_component = component; - mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read); - sendMessage(msg); - //qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM NAME" << parameter; + emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); } /** diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 3a689d033a48a754a31ad1e458491fc7ca833acc..9d115fc602e6994e1968abf36d50006b645ddbd3 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -40,7 +40,6 @@ This file is part of the QGROUNDCONTROL project #include "QGCFlightGearLink.h" #include "QGCJSBSimLink.h" #include "QGCXPlaneLink.h" -#include "QGCUASParamManager.h" #include "QGCUASFileManager.h" Q_DECLARE_LOGGING_CATEGORY(UASLog) @@ -462,11 +461,6 @@ protected: //COMMENTS FOR TEST UNIT bool blockHomePositionChanges; ///< Block changes to the home position bool receivedMode; ///< True if mode was retrieved from current conenction to UAS - /// PARAMETERS - QMap* > parameters; ///< All parameters - bool paramsOnceRequested; ///< If the parameter list has been read at least once - QGCUASParamManager paramMgr; ///< Parameter manager for this UAS - /// SIMULATION QGCHilLink* simulation; ///< Hardware in the loop simulation link @@ -493,11 +487,6 @@ public: return &waypointManager; } - /** @brief Get reference to the param manager **/ - virtual QGCUASParamManagerInterface* getParamManager() { - return ¶mMgr; - } - virtual QGCUASFileManager* getFileManager() { return &fileManager; } @@ -821,25 +810,6 @@ public slots: /** @brief Set current mode of operation, e.g. auto or manual, does not check the arming status, for anything else than arming/disarming operations use setMode instead */ void setModeArm(uint8_t newBaseMode, uint32_t newCustomMode); - /** @brief Request a single parameter by name */ - void requestParameter(int component, const QString& parameter); - /** @brief Request a single parameter by index */ - void requestParameter(int component, int id); - - /** @brief Set a system parameter */ - void setParameter(const int compId, const QString& paramId, const QVariant& value); - - /** @brief Write parameters to permanent storage */ - void writeParametersToStorage(); - /** @brief Read parameters from permanent storage */ - void readParametersFromStorage(); - - /** @brief Get the names of all parameters */ - QList getParameterNames(int component); - - /** @brief Get the ids of all components */ - QList getComponentIds(); - void enableAllDataTransmission(int rate); void enableRawSensorDataTransmission(int rate); void enableExtendedSystemStatusTransmission(int rate); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 6a69e5423dd88bbdd06ad376c6c794e72d3ad58c..51dae58984a3b6f3bcc2cf9112196d7b24838964 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -40,9 +40,7 @@ This file is part of the QGROUNDCONTROL project #include "LinkInterface.h" #include "ProtocolInterface.h" -#include "UASParameterDataModel.h" #include "UASWaypointManager.h" -#include "QGCUASParamManagerInterface.h" class QGCUASFileManager; @@ -134,9 +132,6 @@ public: /** @brief Get reference to the waypoint manager **/ virtual UASWaypointManager* getWaypointManager(void) = 0; - /** @brief Get reference to the param manager **/ - virtual QGCUASParamManagerInterface* getParamManager() = 0; - virtual QGCUASFileManager* getFileManager() = 0; /** @brief Send a message over this link (to this or to all UAS on this link) */ @@ -314,19 +309,6 @@ public slots: virtual void setLocalOriginAtCurrentGPSPosition() = 0; /** @brief Set world frame origin / home position at this GPS position */ virtual void setHomePosition(double lat, double lon, double alt) = 0; - /** @brief Request one specific onboard parameter */ - virtual void requestParameter(int component, const QString& parameter) = 0; - /** @brief Write parameter to permanent storage */ - virtual void writeParametersToStorage() = 0; - /** @brief Read parameter from permanent storage */ - virtual void readParametersFromStorage() = 0; - /** @brief Set a system parameter - * @param component ID of the system component to write the parameter to - * @param id String identifying the parameter - * @warning The length of the ID string is limited by the MAVLink format! Take care to not exceed it - * @param value Value of the parameter, IEEE 754 single precision floating point - */ - virtual void setParameter(const int component, const QString& id, const QVariant& value) = 0; /** * @brief Add a link to the list of current links @@ -496,11 +478,7 @@ signals: void waypointSelected(int uasId, int id); void waypointReached(UASInterface* uas, int id); void autoModeChanged(bool autoMode); - void parameterChanged(int uas, int component, QString parameterName, QVariant value); - void parameterChanged(int uas, int component, int parameterCount, int parameterId, QString parameterName, QVariant value); - void parameterUpdate(int uas, int component, QString parameterName, int type, QVariant value); - void patternDetected(int uasId, QString patternPath, float confidence, bool detected); - void letterDetected(int uasId, QString letter, float confidence, bool detected); + void parameterUpdate(int uas, int component, QString parameterName, int parameterCount, int parameterId, int type, QVariant value); /** * @brief The battery status has been updated * diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc deleted file mode 100644 index 91d4ea4253e9311d723b0cbcdb80bddbb77b11c5..0000000000000000000000000000000000000000 --- a/src/uas/UASParameterCommsMgr.cc +++ /dev/null @@ -1,586 +0,0 @@ -#include "UASParameterCommsMgr.h" - -#include -#include - -#include "QGCUASParamManagerInterface.h" -#include "UASInterface.h" -#include "MAVLinkProtocol.h" -#include "MainWindow.h" -#include "QGCLoggingCategory.h" - -#define RC_CAL_CHAN_MAX 8 - -QGC_LOGGING_CATEGORY(UASParameterCommsMgrLog, "UASParameterCommsMgrLog") - -UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent) : - QObject(parent), - lastReceiveTime(0), - mav(NULL), - maxSilenceTimeout(30000), - paramDataModel(NULL), - retransmitBurstLimit(5), - silenceTimeout(1000), - transmissionListMode(false) -{ - // We signal to ourselves to start/stop timer on our own thread - connect(this, SIGNAL(_startSilenceTimer(void)), this, SLOT(_startSilenceTimerOnThisThread(void))); - connect(this, SIGNAL(_stopSilenceTimer(void)), this, SLOT(_stopSilenceTimerOnThisThread(void))); -} - -UASParameterCommsMgr* UASParameterCommsMgr::initWithUAS(UASInterface* uas) -{ - mav = uas; - paramDataModel = mav->getParamManager()->dataModel(); - loadParamCommsSettings(); - - //Requesting parameters one-by-one from mav - connect(this, SIGNAL(parameterUpdateRequestedById(int,int)), - mav, SLOT(requestParameter(int,int))); - - // Sending params to the UAS - connect(this, SIGNAL(commitPendingParameter(int,QString,QVariant)), - mav, SLOT(setParameter(int,QString,QVariant))); - - // Received parameter updates from UAS - connect(mav, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), - this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant))); - - connect(&silenceTimer, SIGNAL(timeout()), - this,SLOT(silenceTimerExpired())); - - return this; -} - - - - -void UASParameterCommsMgr::loadParamCommsSettings() -{ - QSettings settings; - //TODO these are duplicates of MAVLinkProtocol settings...seems wrong to use them in two places - settings.beginGroup("QGC_MAVLINK_PROTOCOL"); - bool ok; - int val = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", 1000).toInt(&ok); - if (ok) { - silenceTimeout = val; - qDebug() << "silenceTimeout" << silenceTimeout; - } - - settings.endGroup(); -} - -void UASParameterCommsMgr::_sendParamRequestListMsg(void) -{ - MAVLinkProtocol* mavlink = MAVLinkProtocol::instance(); - Q_ASSERT(mavlink); - - mavlink_message_t msg; - mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, mav->getUASID(), MAV_COMP_ID_ALL); - mav->sendMessage(msg); -} - -/** - * Send a request to deliver the list of onboard parameters - * from the MAV. - */ -void UASParameterCommsMgr::requestParameterList() -{ - if (!mav) { - return; - } - - - if (!transmissionListMode) { - qCDebug(UASParameterCommsMgrLog) << "Requesting full parameter list"; - transmissionListMode = true;//TODO eliminate? - //we use (compId 0, paramId 0) as indicating all params for the system - markReadParamWaiting(0,0); - - _sendParamRequestListMsg(); - - updateSilenceTimer(); - } - else { - qCDebug(UASParameterCommsMgrLog) << "Ignoring requestParameterList because we're receiving params list"; - } - -} - - -void UASParameterCommsMgr::markReadParamWaiting(int compId, int paramId) -{ - if (!readsWaiting.contains(compId)) { - readsWaiting.insert(compId, new QSet()); - } - - readsWaiting.value(compId)->insert(paramId); -} - -void UASParameterCommsMgr::markWriteParamWaiting(int compId, QString paramName, QVariant value) -{ - //ensure we have a map for this compId - if (!writesWaiting.contains(compId)) { - writesWaiting.insert(compId, new QMap()); - } - - // Insert it in missing write ACK list - writesWaiting.value(compId)->insert(paramName, value); -} - -/* - Empty read retransmission list - Empty write retransmission list -*/ -void UASParameterCommsMgr::clearRetransmissionLists(int& missingReadCount, int& missingWriteCount ) -{ - qCDebug(UASParameterCommsMgrLog) << "Clearing re-transmission lists"; - - missingReadCount = 0; - QList compIds = readsWaiting.keys(); - foreach (int compId, compIds) { - missingReadCount += readsWaiting.value(compId)->count(); - readsWaiting.value(compId)->clear(); - } - - missingWriteCount = 0; - compIds = writesWaiting.keys(); - foreach (int compId, compIds) { - missingWriteCount += writesWaiting.value(compId)->count(); - writesWaiting.value(compId)->clear(); - } - -} - - -void UASParameterCommsMgr::emitPendingParameterCommit(int compId, const QString& key, QVariant& value) -{ - int paramType = (int)value.type(); - switch (paramType) - { - case QVariant::Char: - { - QVariant fixedValue(QChar((unsigned char)value.toInt())); - emit commitPendingParameter(compId, key, fixedValue); - } - break; - case QVariant::Int: - { - QVariant fixedValue(value.toInt()); - emit commitPendingParameter(compId, key, fixedValue); - } - break; - case QVariant::UInt: - { - QVariant fixedValue(value.toUInt()); - emit commitPendingParameter(compId, key, fixedValue); - } - break; - case QMetaType::Float: - { - QVariant fixedValue(value.toFloat()); - emit commitPendingParameter(compId, key, fixedValue); - } - break; - default: - qCritical() << "ABORTED PARAM SEND, INVALID QVARIANT TYPE" << paramType; - return; - } - - setParameterStatusMsg(tr("Writing %1: %2 for comp. %3").arg(key).arg(value.toDouble()).arg(compId)); - -} - - -void UASParameterCommsMgr::resendReadWriteRequests() -{ - int compId; - QList compIds; - - // Re-request at maximum retransmitBurstLimit parameters at once - // to prevent link flooding' - int requestedReadCount = 0; - compIds = readsWaiting.keys(); - foreach (compId, compIds) { - // Request n parameters from this component (at maximum) - QSet* missingReadParams = readsWaiting.value(compId, NULL); - qDebug() << "compId " << compId << "readsWaiting:" << missingReadParams->count(); - foreach (int paramId, *missingReadParams) { - if (0 == paramId && 0 == compId) { - _sendParamRequestListMsg(); - //don't request any other params individually for this component - break; - } - if (requestedReadCount < retransmitBurstLimit) { - //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << paramId << "FROM COMPONENT #" << compId; - emit parameterUpdateRequestedById(compId, paramId); - setParameterStatusMsg(tr("Requested retransmission of #%1").arg(paramId+1)); - requestedReadCount++; - } - else { - qCDebug(UASParameterCommsMgrLog) << "Throttling read retransmit requests at" << requestedReadCount; - break; - } - } - } - - // Re-request at maximum retransmitBurstLimit parameters at once - // to prevent write-request link flooding - int requestedWriteCount = 0; - compIds = writesWaiting.keys(); - foreach (compId, compIds) { - QMap * missingWriteParams = writesWaiting.value(compId); - foreach (QString key, missingWriteParams->keys()) { - if (requestedWriteCount < retransmitBurstLimit) { - // Re-request write operation - QVariant value = missingWriteParams->value(key); - emitPendingParameterCommit(compId, key, value); - requestedWriteCount++; - } - else { - qCDebug(UASParameterCommsMgrLog) << "Throttling write retransmit requests at" << requestedWriteCount; - break; - } - } - } - - updateSilenceTimer(); - -} - -void UASParameterCommsMgr::resetAfterListReceive() -{ - transmissionListMode = false; - knownParamListSize.clear(); -} - -void UASParameterCommsMgr::silenceTimerExpired() -{ - quint64 curTime = QGC::groundTimeMilliseconds(); - int elapsed = (int)(curTime - lastSilenceTimerReset); - qCDebug(UASParameterCommsMgrLog) << "silenceTimerExpired elapsed:" << elapsed; - - if (elapsed < silenceTimeout) { - //reset the guard timer: it fired prematurely - updateSilenceTimer(); - return; - } - - int totalElapsed = (int)(curTime - lastReceiveTime); - if (totalElapsed > maxSilenceTimeout) { - qCDebug(UASParameterCommsMgrLog) << "maxSilenceTimeout exceeded: " << totalElapsed; - int missingReads, missingWrites; - clearRetransmissionLists(missingReads,missingWrites); - emit _stopSilenceTimer(); // Stop timer on our thread; - lastReceiveTime = 0; - lastSilenceTimerReset = curTime; - setParameterStatusMsg(tr("TIMEOUT: Abandoning %1 reads %2 writes after %3 seconds").arg(missingReads).arg(missingWrites).arg(totalElapsed/1000)); - } - else { - resendReadWriteRequests(); - } -} - - -void UASParameterCommsMgr::requestParameterUpdate(int compId, const QString& paramName) -{ - if (mav) { - mav->requestParameter(compId, paramName); - qCDebug(UASParameterCommsMgrLog) << "Requested update for" << compId << paramName; - //TODO track these read requests with a paramName but no param ID : use index in getOnboardParamsForComponent? - //ensure we keep track of every single read request - } -} - -void UASParameterCommsMgr::requestRcCalibrationParamsUpdate() -{ - if (!transmissionListMode) { - QString minTpl("RC%1_MIN"); - QString maxTpl("RC%1_MAX"); - QString trimTpl("RC%1_TRIM"); - QString revTpl("RC%1_REV"); - - // Do not request the RC type, as these values depend on this - // active onboard parameter - - - int defCompId = paramDataModel->getDefaultComponentId(); - for (unsigned int i = 1; i < (RC_CAL_CHAN_MAX+1); ++i) { - qDebug() << "Request RC " << i; - requestParameterUpdate(defCompId, minTpl.arg(i)); - requestParameterUpdate(defCompId, trimTpl.arg(i)); - requestParameterUpdate(defCompId, maxTpl.arg(i)); - requestParameterUpdate(defCompId, revTpl.arg(i)); - QGC::SLEEP::usleep(5000); - } - } - else { - qCDebug(UASParameterCommsMgrLog) << "Ignoring requestRcCalibrationParamsUpdate because we're receiving params list"; - } -} - - -/** - * @param component the subsystem which has the parameter - * @param parameterName name of the parameter, as delivered by the system - * @param value value of the parameter - */ -void UASParameterCommsMgr::setParameter(int compId, QString paramName, QVariant value, bool forceSend) -{ - if (paramName.isEmpty()) { - return; - } - - double dblValue = value.toDouble(); - - if (paramDataModel->isValueLessThanParamMin(paramName,dblValue)) { - setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(paramName).arg(dblValue), - ParamCommsStatusLevel_Error - ); - return; - } - if (paramDataModel->isValueGreaterThanParamMax(paramName,dblValue)) { - setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(paramName).arg(dblValue), - ParamCommsStatusLevel_Error - ); - return; - } - - if (!forceSend) { - QVariant onboardVal; - paramDataModel->getOnboardParamValue(compId,paramName,onboardVal); - if (onboardVal == value) { - setParameterStatusMsg(tr("REJ. %1 already %2").arg(paramName).arg(dblValue), - ParamCommsStatusLevel_Warning - ); - return; - } - } - - emitPendingParameterCommit(compId, paramName, value); - - //Add this request to list of writes not yet ack'd - - markWriteParamWaiting( compId, paramName, value); - updateSilenceTimer(); - - -} - -void UASParameterCommsMgr::updateSilenceTimer() -{ - //if there are pending reads or writes, ensure we timeout in a little while - //if we hear nothing but silence from our partner - - int missReadCount = 0; - foreach (int key, readsWaiting.keys()) { - missReadCount += readsWaiting.value(key)->count(); - } - - int missWriteCount = 0; - foreach (int key, writesWaiting.keys()) { - missWriteCount += writesWaiting.value(key)->count(); - } - - - if (missReadCount > 0 || missWriteCount > 0) { - lastSilenceTimerReset = QGC::groundTimeMilliseconds(); - if (0 == lastReceiveTime) { - lastReceiveTime = lastSilenceTimerReset; - } - // We signal this to ourselves so timer is started on the right thread - emit _startSilenceTimer(); - } - else { - //all parameters have been received, broadcast to UI - qCDebug(UASParameterCommsMgrLog) << "emitting parameterListUpToDate"; - emit parameterListProgress(0.0f); - emit parameterListUpToDate(); - resetAfterListReceive(); - emit _stopSilenceTimer(); // Stop timer on our thread; - lastReceiveTime = 0; - } - - - -} - - -void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level) -{ - //qDebug() << "parameterStatusMsg: " << msg; - emit parameterStatusMsgUpdated(msg,level); -} - -void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value) -{ - qCDebug(UASParameterCommsMgrLog) << "Received parameter update for:" << paramName << "count" << paramCount << "index" << paramId << "value" << value; - - Q_UNUSED(uas); //this object is assigned to one UAS only - lastReceiveTime = QGC::groundTimeMilliseconds(); - // qDebug() << "compId" << compId << "receivedParameterUpdate:" << paramName; - - //notify the data model that we have an updated param - paramDataModel->handleParamUpdate(compId,paramName,value); - - - // Ensure we have missing read/write lists for this compId - if (!readsWaiting.contains(compId)) { - readsWaiting.insert(compId, new QSet()); - } - if (!writesWaiting.contains(compId) ) { - writesWaiting.insert(compId,new QMap()); - } - - QSet* compMissingReads = readsWaiting.value(compId); - // List mode is different from single parameter transfers - if (transmissionListMode) { - // Only accept the list size once on the first packet from each component - if (!knownParamListSize.contains(compId)) { - // Mark list size as known - knownParamListSize.insert(compId,paramCount); - - //remove our placeholder read request for all params - readsWaiting.value(0)->remove(0); - - qCDebug(UASParameterCommsMgrLog) << "receivedParameterUpdate: Mark all parameters as missing: " << paramCount; - for (int i = 1; i < paramCount; ++i) { //param Id 0 is "all parameters" and not valid - compMissingReads->insert(i); - } - } - - emit parameterListProgress((float)paramId / (float)paramCount); - if (paramId == paramCount) { - emit parameterListProgress(0.0f); - } - } - - - // Mark this parameter as received in read list - compMissingReads->remove(paramId); - - - bool justWritten = false; - bool writeMismatch = false; - - // Mark this parameter as received in write ACK list - QMap* compMissingWrites = writesWaiting.value(compId); - if (!compMissingWrites) { - //we sometimes send a write request on compId 0 and get a response on a nonzero compId eg 50 - compMissingWrites = writesWaiting.value(0); - } - if (compMissingWrites && compMissingWrites->contains(paramName)) { - justWritten = true; - if (compMissingWrites->value(paramName) != value) { - writeMismatch = true; - } - compMissingWrites->remove(paramName); - } - - - if (justWritten) { - int waitingWritesCount = compMissingWrites->count(); - if (!writeMismatch) { - setParameterStatusMsg(tr("SUCCESS: Wrote %2 (#%1): %3").arg(paramId+1).arg(paramName).arg(value.toDouble())); - } - - if (!writeMismatch) { - if (0 == waitingWritesCount) { - setParameterStatusMsg(tr("SUCCESS: Wrote all params for component %1").arg(compId)); - if (persistParamsAfterSend) { - writeParamsToPersistentStorage(); - persistParamsAfterSend = false; - } - } - } - else { - // Mismatch, tell user - setParameterStatusMsg(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(paramName).arg(compMissingWrites->value(paramName).toDouble()).arg(value.toDouble()), - ParamCommsStatusLevel_Warning); - } - } - else { - int waitingReadsCount = compMissingReads->count(); - - if (0 == waitingReadsCount) { - // Transmission done - QTime time = QTime::currentTime(); - QString timeString = time.toString(); - setParameterStatusMsg(tr("All received. (updated at %1)").arg(timeString)); - } - else { - // Waiting to receive more - QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' ')); - setParameterStatusMsg(tr("OK: %1 %2 (%3/%4)").arg(paramName).arg(val).arg(paramCount-waitingReadsCount).arg(paramCount), - ParamCommsStatusLevel_OK); - } - } - - updateSilenceTimer(); - - -} - - -void UASParameterCommsMgr::writeParamsToPersistentStorage() -{ - if (mav) { - mav->writeParametersToStorage(); //TODO track timeout, retransmit etc? - persistParamsAfterSend = false; //done - } -} - - -void UASParameterCommsMgr::sendPendingParameters(bool copyToPersistent, bool forceSend) -{ - persistParamsAfterSend |= copyToPersistent; - - // Iterate through all components, through all pending parameters and send them to UAS - int parametersSent = 0; - QMap*>* changedValues = paramDataModel->getAllPendingParams(); - QMap*>::iterator i; - for (i = changedValues->begin(); i != changedValues->end(); ++i) { - // Iterate through the parameters of the component - int compId = i.key(); - QMap* paramList = i.value(); - QMap::iterator j; - setParameterStatusMsg(tr("%1 pending params for component %2").arg(paramList->count()).arg(compId)); - - for (j = paramList->begin(); j != paramList->end(); ++j) { - setParameter(compId, j.key(), j.value(), forceSend); - parametersSent++; - } - } - - // Change transmission status if necessary - if (0 == parametersSent) { - setParameterStatusMsg(tr("No transmission: No changed values."),ParamCommsStatusLevel_Warning); - } - else { - setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent)); - qCDebug(UASParameterCommsMgrLog) << "Pending parameters now:" << paramDataModel->countPendingParams(); - } - - - updateSilenceTimer(); -} - -UASParameterCommsMgr::~UASParameterCommsMgr() -{ - silenceTimer.stop(); - - QString ptrStr; - ptrStr.sprintf("%8p", this); - qCDebug(UASParameterCommsMgrLog) << "UASParameterCommsMgr destructor: " << ptrStr ; - -} - -void UASParameterCommsMgr::_startSilenceTimerOnThisThread(void) -{ - silenceTimer.start(silenceTimeout); -} - -void UASParameterCommsMgr::_stopSilenceTimerOnThisThread(void) -{ - silenceTimer.stop(); -} diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h deleted file mode 100644 index 8721f51e31381832325aadec7dc0fc957d8ddad7..0000000000000000000000000000000000000000 --- a/src/uas/UASParameterCommsMgr.h +++ /dev/null @@ -1,134 +0,0 @@ -#ifndef UASPARAMETERCOMMSMGR_H -#define UASPARAMETERCOMMSMGR_H - -#include -#include -#include -#include -#include -#include - -class UASInterface; -class UASParameterDataModel; - -Q_DECLARE_LOGGING_CATEGORY(UASParameterCommsMgrLog) - -class UASParameterCommsMgr : public QObject -{ - Q_OBJECT - - -public: - explicit UASParameterCommsMgr(QObject *parent = 0); - UASParameterCommsMgr* initWithUAS(UASInterface* model);///< Two-stage constructor - - ~UASParameterCommsMgr(); - - typedef enum ParamCommsStatusLevel { - ParamCommsStatusLevel_OK = 0, - ParamCommsStatusLevel_Warning = 2, - ParamCommsStatusLevel_Error = 4, - ParamCommsStatusLevel_Count - } ParamCommsStatusLevel_t; - - -protected: - - /** @brief activate the silence timer if there are unack'd reads or writes */ - virtual void updateSilenceTimer(); - - virtual void setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level=ParamCommsStatusLevel_OK); - - /** @brief Load settings that control eg retransmission timeouts */ - void loadParamCommsSettings(); - - /** @brief clear transmissionMissingPackets and transmissionMissingWriteAckPackets */ - void clearRetransmissionLists(int& missingReadCount, int& missingWriteCount ); - - /** @brief we are waiting for a response to this read param request */ - virtual void markReadParamWaiting(int compId, int paramId); - - /** @brief we are waiting for a response to this write param request */ - void markWriteParamWaiting(int compId, QString paramName, QVariant value); - - void resendReadWriteRequests(); - void resetAfterListReceive(); - - void emitPendingParameterCommit(int compId, const QString& key, QVariant& value); - -signals: - void commitPendingParameter(int component, QString parameter, QVariant value); - void parameterChanged(int component, int parameterIndex, QVariant value); - void parameterValueConfirmed(int uas, int component,int paramCount, int paramId, QString parameter, QVariant value); - - /** @brief We have received a complete list of all parameters onboard the MAV */ - void parameterListUpToDate(); - - void parameterListProgress(float percentComplete); - - void parameterUpdateRequested(int component, const QString& parameter); - void parameterUpdateRequestedById(int componentId, int paramId); - - /** @brief We updated the parameter status message */ - void parameterStatusMsgUpdated(QString msg, int level); - - - /// @brief We signal this to ourselves in order to get timer started/stopped on our own thread. - void _startSilenceTimer(void); - void _stopSilenceTimer(void); - -public slots: - /** @brief Iterate through all components, through all pending parameters and send them to UAS */ - virtual void sendPendingParameters(bool copyToPersistent = false, bool forceSend = false); - - /** @brief Write the current onboard parameters from transient RAM into persistent storage, e.g. EEPROM or harddisk */ - virtual void writeParamsToPersistentStorage(); - - /** @brief Write one parameter to the MAV */ - virtual void setParameter(int component, QString parameterName, QVariant value, bool forceSend = false); - - /** @brief Request list of parameters from MAV */ - virtual void requestParameterList(); - - /** @brief The max silence time expired */ - virtual void silenceTimerExpired(); - - /** @brief Request a single parameter update by name */ - virtual void requestParameterUpdate(int component, const QString& parameter); - - /** @brief Request an update of RC parameters */ - virtual void requestRcCalibrationParamsUpdate(); - - virtual void receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value); - -protected: - - - QMap knownParamListSize;///< The known param list size for each component, by component ID - quint64 lastReceiveTime; ///< The last time we received anything from our partner - quint64 lastSilenceTimerReset; - UASInterface* mav; ///< The MAV we're talking to - - int maxSilenceTimeout; ///< If nothing received within this period of time, abandon resends - - UASParameterDataModel* paramDataModel; - - bool persistParamsAfterSend; ///< Copy all parameters to persistent storage after sending - QMap*> readsWaiting; ///< All reads that have not yet been received, by component ID - int retransmitBurstLimit; ///< Number of packets requested for retransmission per burst - int silenceTimeout; ///< If nothing received within this period of time, start resends - QTimer silenceTimer; ///< Timer handling parameter retransmission - bool transmissionListMode; ///< Currently requesting list - QMap* > writesWaiting; ///< All writes that have not yet been ack'd, by component ID - -private slots: - /// @brief We signal this to ourselves in order to get timer started/stopped on our own thread. - void _startSilenceTimerOnThisThread(void); - void _stopSilenceTimerOnThisThread(void); - -private: - void _sendParamRequestListMsg(void); -}; - - -#endif // UASPARAMETERCOMMSMGR_H diff --git a/src/uas/UASParameterDataModel.cc b/src/uas/UASParameterDataModel.cc deleted file mode 100644 index 760e6303c8379050ca0c84766132600c1bcd22cc..0000000000000000000000000000000000000000 --- a/src/uas/UASParameterDataModel.cc +++ /dev/null @@ -1,515 +0,0 @@ -#include "UASParameterDataModel.h" - -#include - -#include -#include -#include - -#include "QGCMAVLink.h" - -UASParameterDataModel::UASParameterDataModel(QObject *parent) : - QObject(parent), - defaultComponentId(-1) -{ - onboardParameters.clear(); - pendingParameters.clear(); - -} - - - -int UASParameterDataModel::countPendingParams() -{ - int total = 0; - QMap*>::iterator i; - for (i = pendingParameters.begin(); i != pendingParameters.end(); ++i) { - // Iterate through the parameters of the component - QMap* paramList = i.value(); - total += paramList->count(); - } - - return total; -} - -int UASParameterDataModel::countOnboardParams() -{ - int total = 0; - QMap*>::iterator i; - for (i = onboardParameters.begin(); i != onboardParameters.end(); ++i) { - // Iterate through the parameters of the component - QMap* paramList = i.value(); - total += paramList->count(); - } - - return total; -} - - -bool UASParameterDataModel::updatePendingParamWithValue(int compId, const QString& key, const QVariant& value, bool forceSend) -{ - bool pending = true; - //ensure we have this component in our onboard and pending lists already - addComponent(compId); - - if (!forceSend) { - QMap* existParams = getOnboardParamsForComponent(compId); - if (existParams->contains(key)) { - QVariant existValue = existParams->value(key); - if (existValue == value) { - pending = false; - } - } - } - - if (pending) { - setPendingParam(compId,key,value); - } - else { - removePendingParam(compId,key); - } - - return pending; -} - - -bool UASParameterDataModel::isParamChangePending(int compId, const QString& key) -{ - QMap* pendingParms = getPendingParamsForComponent(compId); - return ((NULL != pendingParms) && pendingParms->contains(key)); -} - -void UASParameterDataModel::setPendingParam(int compId, const QString& key, const QVariant &value) -{ - //ensure we have a placeholder map for this component - addComponent(compId); - setParamWithTypeInMap(compId,key,value,pendingParameters); - emit pendingParamUpdate(compId, key, value, true); -} - -void UASParameterDataModel::removePendingParam(int compId, const QString& key) -{ - qDebug() << "removePendingParam:" << key; - - QMap *pendParams = getPendingParamsForComponent(compId); - if (pendParams) { - pendParams->remove(key); - //broadcast the existing value - QVariant existVal; - getOnboardParamValue(compId,key,existVal); - emit pendingParamUpdate(compId, key,existVal, false); - } -} - -void UASParameterDataModel::setOnboardParam(int compId, const QString &key, const QVariant& value) -{ - //ensure we have a placeholder map for this component - addComponent(compId); - //TODO use setParamWithTypeInMap instead and verify - QMap *params = getOnboardParamsForComponent(compId); - params->insert(key,value); -} - -void UASParameterDataModel::setParamWithTypeInMap(int compId, const QString& key, const QVariant &value, QMap* >& map) -{ - - switch ((int)value.type()) - { - case QVariant::Int: - { - QVariant fixedValue(value.toInt()); - map.value(compId)->insert(key, fixedValue); - } - break; - case QVariant::UInt: - { - QVariant fixedValue(value.toUInt()); - map.value(compId)->insert(key, fixedValue); - } - break; - case QMetaType::Float: - { - QVariant fixedValue(value.toFloat()); - map.value(compId)->insert(key, fixedValue); - } - break; - case QMetaType::QChar: - { - QVariant fixedValue(QChar((unsigned char)value.toUInt())); - map.value(compId)->insert(key, fixedValue); - } - break; - case QMetaType::QString: - { - QString strVal = value.toString(); - float floatVal = strVal.toFloat(); - QVariant fixedValue( floatVal ); - //TODO track down WHY we're getting unexpected QString values here...this is a workaround - qDebug() << "Unexpected string QVariant:" << key << " val:" << value << "fixedVal:" << fixedValue; - map.value(compId)->insert(key, fixedValue); - } - break; - default: - qCritical() << "ABORTED PARAM UPDATE, NO VALID QVARIANT TYPE"; - return; - } -} - -void UASParameterDataModel::addComponent(int compId) -{ - if (!onboardParameters.contains(compId)) { - onboardParameters.insert(compId, new QMap()); - } - if (!pendingParameters.contains(compId)) { - pendingParameters.insert(compId, new QMap()); - } -} - - -void UASParameterDataModel::handleParamUpdate(int compId, const QString ¶mName, const QVariant &value) -{ - //verify that the value requested by the user matches the set value - //if it doesn't match, leave the pending parameter in the pending list! - if (pendingParameters.contains(compId)) { - QMap *pendingParams = pendingParameters.value(compId); - if ((NULL != pendingParams) && pendingParams->contains(paramName)) { - QVariant reqVal = pendingParams->value(paramName); - if (reqVal == value) { - //notify everyone that this item is being removed from the pending parameters list since it's now confirmed - removePendingParam(compId,paramName); - } - else { - qDebug() << "Pending commit for " << paramName << " want: " << reqVal << " got: " << value; - } - } - } - - setOnboardParam(compId,paramName,value); - emit parameterUpdated(compId,paramName,value); -} - -bool UASParameterDataModel::getOnboardParamValue(int componentId, const QString& key, QVariant& value) const -{ - - if (onboardParameters.contains(componentId)) { - if (onboardParameters.value(componentId)->contains(key)) { - value = onboardParameters.value(componentId)->value(key); - return true; - } - } - - return false; -} - -int UASParameterDataModel::getDefaultComponentId() -{ - int result = 0; - - if (-1 != defaultComponentId) - return defaultComponentId; - - QList components = getComponentForOnboardParam("SYS_AUTOSTART");//TODO is this the best way to find the right component? - - // Guard against multiple components responding - this will never show in practice - if (1 == components.count()) { - result = components.first(); - defaultComponentId = result; - } - - qDebug() << "Default compId: " << result; - - return result; -} - -QList UASParameterDataModel::getComponentForOnboardParam(const QString& parameter) const -{ - QList components; - // Iterate through all components - foreach (int comp, onboardParameters.keys()) - { - if (onboardParameters.value(comp)->contains(parameter)) - components.append(comp); - } - - return components; -} - -void UASParameterDataModel::forgetAllOnboardParams() -{ - onboardParameters.clear(); -} - -void UASParameterDataModel::clearAllPendingParams() -{ - QList compIds = pendingParameters.keys(); - foreach (int compId , compIds) { - QMap* compParams = pendingParameters.value(compId); - QList paramNames = compParams->keys(); - foreach (QString paramName, paramNames) { - //remove this item from pending status and broadcast update - removePendingParam(compId,paramName); - } - } - -} - -void UASParameterDataModel::readUpdateParamsFromStream( QTextStream& stream) -{ - bool userWarned = false; - - while (!stream.atEnd()) { - QString line = stream.readLine(); - if (!line.startsWith("#")) { - QStringList wpParams = line.split("\t"); - int lineMavId = wpParams.at(0).toInt(); - if (wpParams.size() == 5) { - // Only load parameters for right mav - if (!userWarned && (uasId != lineMavId)) { - //TODO warn the user somehow ?? Appears these are saved currently with mav ID 0 but mav ID is often nonzero? - QString msg = tr("The parameters in the stream have been saved from system %1, but the currently selected system has the ID %2.").arg(lineMavId).arg(uasId); - qDebug() << msg ; - //MainWindow::instance()->showCriticalMessage( - // tr("Parameter loading warning"), - // tr("The parameters from the file %1 have been saved from system %2, but the currently selected system has the ID %3. If this is unintentional, please click on to revert to the parameters that are currently onboard").arg(fileName).arg(wpParams.at(0).toInt()).arg(mav->getUASID())); - userWarned = true; - } - - bool changed = false; - int componentId = wpParams.at(1).toInt(); - QString key = wpParams.at(2); - QString valStr = wpParams.at(3); - double dblVal = wpParams.at(3).toDouble(); - uint paramType = wpParams.at(4).toUInt(); - - - if (!onboardParameters.contains(componentId)) { - addComponent(componentId); - changed = true; - } - else { - QMap* compParams = onboardParameters.value(componentId); - if (!compParams->contains(key) || - (fabs((static_cast(compParams->value(key).toDouble())) - (dblVal)) > 2.0f * FLT_EPSILON)) { - changed = true; - qDebug() << "Changed" << key << "VAL" << dblVal; - } - } - - - if (changed) { - switch (paramType) - { - case MAV_PARAM_TYPE_REAL32: - updatePendingParamWithValue(componentId,key,QVariant(valStr.toFloat())); - break; - case MAV_PARAM_TYPE_UINT32: - updatePendingParamWithValue(componentId,key, QVariant(valStr.toUInt())); - break; - case MAV_PARAM_TYPE_INT32: - updatePendingParamWithValue(componentId,key,QVariant(valStr.toInt())); - break; - case MAV_PARAM_TYPE_INT8: - updatePendingParamWithValue(componentId,key,QVariant((unsigned char) valStr.toUInt())); - break; - default: - qDebug() << "FAILED LOADING PARAM" << key << "UNKNOWN DATA TYPE"; - } - } - - - } - } - } - -} - -void UASParameterDataModel::writeOnboardParamsToStream( QTextStream &stream, const QString& name) -{ - stream << "# Onboard parameters for system " << name << "\n"; - stream << "#\n"; - stream << "# MAV ID COMPONENT ID PARAM NAME VALUE (FLOAT)\n"; - - // Iterate through all components, through all parameters and emit them - QMap*>::iterator i; - for (i = onboardParameters.begin(); i != onboardParameters.end(); ++i) { - // Iterate through the parameters of the component - int compid = i.key(); - QMap* comp = i.value(); - { - QMap::iterator j; - for (j = comp->begin(); j != comp->end(); ++j) - { - QString paramValue("%1"); - QString paramType("%1"); - switch ((int)j.value().type()) - { - case QVariant::Int: - paramValue = paramValue.arg(j.value().toInt()); - paramType = paramType.arg(MAV_PARAM_TYPE_INT32); - break; - case QVariant::UInt: - paramValue = paramValue.arg(j.value().toUInt()); - paramType = paramType.arg(MAV_PARAM_TYPE_UINT32); - break; - case QMetaType::Float: - // We store parameters as floats, with only 6 digits of precision guaranteed for decimal string conversion - // (see IEEE 754, 32 bit single-precision) - paramValue = paramValue.arg((double)j.value().toFloat(), 25, 'g', 6); - paramType = paramType.arg(MAV_PARAM_TYPE_REAL32); - break; - case QMetaType::QChar: - case QMetaType::Char: - // see UAS::setParameter() - paramValue = paramValue.arg((unsigned char)j.value().toUInt()); - paramType = paramType.arg(MAV_PARAM_TYPE_INT8); - break; - default: - qCritical() << "ABORTED PARAM WRITE TO FILE, PARAM '" << j.key() << "' NO VALID QVARIANT TYPE" << j.value(); - return; - } - stream << this->uasId << "\t" << compid << "\t" << j.key() << "\t" << paramValue << "\t" << paramType << "\n"; - stream.flush(); - } - } - } -} - - -void UASParameterDataModel::loadParamMetaInfoFromStream(QTextStream& stream) -{ - - // First line is header - // there might be more lines, but the first - // line is assumed to be at least header - QString header = stream.readLine(); - - // Ignore top-level comment lines - while (header.startsWith('#') || header.startsWith('/') - || header.startsWith('=') || header.startsWith('^')) - { - header = stream.readLine(); - } - - bool charRead = false; - QString separator = ""; - QList sepCandidates; - sepCandidates << '\t'; - sepCandidates << ','; - sepCandidates << ';'; - //sepCandidates << ' '; - sepCandidates << '~'; - sepCandidates << '|'; - - // Iterate until separator is found - // or full header is parsed - for (int i = 0; i < header.length(); i++) - { - if (sepCandidates.contains(header.at(i))) - { - // Separator found - if (charRead) - { - separator += header[i]; - } - } - else - { - // Char found - charRead = true; - // If the separator is not empty, this char - // has been read after a separator, so detection - // is now complete - if (separator != "") break; - } - } - - bool stripFirstSeparator = false; - bool stripLastSeparator = false; - - // Figure out if the lines start with the separator (e.g. wiki syntax) - if (header.startsWith(separator)) stripFirstSeparator = true; - - // Figure out if the lines end with the separator (e.g. wiki syntax) - if (header.endsWith(separator)) stripLastSeparator = true; - - QString out = separator; - out.replace("\t", ""); - //qDebug() << " Separator: \"" << out << "\""; - //qDebug() << "READING CSV:" << header; - - - // Read data - while (!stream.atEnd()) - { - QString line = stream.readLine(); - - //qDebug() << "LINE PRE-STRIP" << line; - - // Strip separtors if necessary - if (stripFirstSeparator) line.remove(0, separator.length()); - if (stripLastSeparator) line.remove(line.length()-separator.length(), line.length()-1); - - //qDebug() << "LINE POST-STRIP" << line; - - // Keep empty parts here - we still have to act on them - QStringList parts = line.split(separator, QString::KeepEmptyParts); - - // Each line is: - // variable name, Min, Max, Default, Multiplier, Enabled (0 = no, 1 = yes), Comment - - - // Fill in min, max and default values - if (parts.count() > 1) - { - // min - paramMin.insert(parts.at(0).trimmed(), parts.at(1).toDouble()); - } - if (parts.count() > 2) - { - // max - paramMax.insert(parts.at(0).trimmed(), parts.at(2).toDouble()); - } - if (parts.count() > 3) - { - // default - paramDefault.insert(parts.at(0).trimmed(), parts.at(3).toDouble()); - } - // IGNORING 4 and 5 for now - if (parts.count() > 6) - { - // tooltip - paramDescriptions.insert(parts.at(0).trimmed(), parts.at(6).trimmed()); - //qDebug() << "PARAM META:" << parts.at(0).trimmed(); - } - } -} - - void UASParameterDataModel::setParamDescriptions(const QMap& paramInfo) -{ - if (paramInfo.isEmpty()) { - qDebug() << __FILE__ << ":" << __LINE__ << "setParamDescriptions with empty"; - } - - paramDescriptions = paramInfo; -} - -bool UASParameterDataModel::isValueGreaterThanParamMax(const QString& paramName, double dblVal) -{ - if (paramMax.contains(paramName)) { - if (dblVal > paramMax.value(paramName)) - return true; - } - - return false; -} - -bool UASParameterDataModel::isValueLessThanParamMin(const QString& paramName, double dblVal) -{ - if (paramMin.contains(paramName)) { - if (dblVal < paramMin.value(paramName)) - return true; - } - - return false; - } - diff --git a/src/uas/UASParameterDataModel.h b/src/uas/UASParameterDataModel.h deleted file mode 100644 index c0b5b76379ce2185252310ae74b761c834dac60e..0000000000000000000000000000000000000000 --- a/src/uas/UASParameterDataModel.h +++ /dev/null @@ -1,136 +0,0 @@ -#ifndef UASPARAMETERDATAMODEL_H -#define UASPARAMETERDATAMODEL_H - -#include -#include -#include - -class QTextStream; - -class UASParameterDataModel : public QObject -{ - Q_OBJECT -public: - explicit UASParameterDataModel(QObject *parent = 0); - - - //Parameter meta info - bool isParamMinKnown(const QString& param) { return paramMin.contains(param); } - virtual bool isValueLessThanParamMin(const QString& param, double dblVal); - - bool isParamMaxKnown(const QString& param) { return paramMax.contains(param); } - virtual bool isValueGreaterThanParamMax(const QString& param, double dblVal); - - bool isParamDefaultKnown(const QString& param) { return paramDefault.contains(param); } - double getParamMin(const QString& param) { return paramMin.value(param, 0.0f); } - double getParamMax(const QString& param) { return paramMax.value(param, 0.0f); } - double getParamDefault(const QString& param) { return paramDefault.value(param, 0.0f); } - virtual QString getParamDescription(const QString& param) { return paramDescriptions.value(param, ""); } - virtual void setParamDescriptions(const QMap& paramInfo); - - /** @brief Get the default component ID for the UAS */ - virtual int getDefaultComponentId(); - - //TODO make this method protected? - /** @brief Ensure that the data model is aware of this component - * @param compId Id of the component - */ - virtual void addComponent(int compId); - - /** - * @brief Return a list of all components for this parameter name - * @param parameter The parameter string to search for - * @return A list with all components, can be potentially empty - */ - virtual QList getComponentForOnboardParam(const QString& parameter) const; - - - /** @brief clears every parameter for every loaded component */ - virtual void forgetAllOnboardParams(); - - - - /** @brief add this parameter to pending list iff it has changed from onboard value - * @return true if the parameter is now pending - */ - virtual bool updatePendingParamWithValue(int componentId, const QString &key, const QVariant &value, bool forceSend = false); - virtual void handleParamUpdate(int componentId, const QString& key, const QVariant& value); - virtual bool getOnboardParamValue(int componentId, const QString& key, QVariant& value) const; - - virtual bool isParamChangePending(int componentId,const QString& key); - - QMap* getPendingParamsForComponent(int componentId) { - return pendingParameters.value(componentId); - } - - QMap* getOnboardParamsForComponent(int componentId) { - return onboardParameters.value(componentId); - } - - QMap* >* getAllPendingParams() { - return &pendingParameters; - } - - QMap* >* getAllOnboardParams() { - return &onboardParameters; - } - - /** @brief return a count of all pending parameters */ - virtual int countPendingParams(); - - /** @brief return a count of all onboard parameters we've received */ - virtual int countOnboardParams(); - - virtual void writeOnboardParamsToStream(QTextStream &stream, const QString& uasName); - virtual void readUpdateParamsFromStream(QTextStream &stream); - - virtual void loadParamMetaInfoFromStream(QTextStream& stream); - - void setUASID(int anId) { this->uasId = anId; } - -protected: - /** @brief set the confirmed value of a parameter in the onboard params list */ - virtual void setOnboardParam(int componentId, const QString& key, const QVariant& value); - - /** @brief Save the parameter with a the type specified in the QVariant as fixed */ - void setParamWithTypeInMap(int compId, const QString& key, const QVariant &value, QMap* >& map); - - /** @brief Write a new pending parameter value that may be eventually sent to the UAS */ - virtual void setPendingParam(int componentId, const QString &key, const QVariant& value); - /** @brief remove a parameter from the pending list */ - virtual void removePendingParam(int compId, const QString &key); - - -signals: - - /** @brief We've received an update of a parameter's value */ - void parameterUpdated(int compId, QString paramName, QVariant value); - - /** @brief Notifies listeners that a param was added to or removed from the pending list */ - void pendingParamUpdate(int compId, const QString& paramName, QVariant value, bool isPending); - - void allPendingParamsCommitted(); ///< All pending params have been committed to the MAV - -public slots: - - virtual void clearAllPendingParams(); - -protected: - int defaultComponentId; ///< Cached default component ID - - int uasId; ///< The UAS / MAV to which this data model pertains - QMap* > pendingParameters; ///< Changed values that have not yet been transmitted to the UAS, by component ID - QMap* > onboardParameters; ///< All parameters confirmed to be stored onboard the UAS, by component ID - - // Tooltip data structures - QMap paramDescriptions; ///< Tooltip values - - // Min / Default / Max data structures - QMap paramMin; ///< Minimum param values - QMap paramDefault; ///< Default param values - QMap paramMax; ///< Minimum param values - - -}; - -#endif // UASPARAMETERDATAMODEL_H diff --git a/src/ui/QGCMapRCToParamDialog.cpp b/src/ui/QGCMapRCToParamDialog.cpp index 8ac5bacb90368c3acc99cadee0179a5bff01ba86..e061058e5cf04bd0547937043ed796a9ddb1275d 100644 --- a/src/ui/QGCMapRCToParamDialog.cpp +++ b/src/ui/QGCMapRCToParamDialog.cpp @@ -26,6 +26,7 @@ #include "QGCMapRCToParamDialog.h" #include "ui_QGCMapRCToParamDialog.h" +#include "AutoPilotPluginManager.h" #include #include @@ -41,6 +42,8 @@ QGCMapRCToParamDialog::QGCMapRCToParamDialog(QString param_id, ui(new Ui::QGCMapRCToParamDialog) { ui->setupUi(this); + + // only enable ok button when param was refreshed QPushButton *okButton = ui->buttonBox->button(QDialogButtonBox::Ok); @@ -92,44 +95,27 @@ void QGCMapRCToParamDialog::paramLoaded(bool success, float value, QString messa } } +ParamLoader::ParamLoader(QString paramName, UASInterface* uas, QObject* parent) : + QObject(parent), + _uas(uas), + _paramName(paramName), + _paramReceived(false) +{ + _autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_uas); + Q_ASSERT(_autopilot); +} + void ParamLoader::load() { - // refresh the parameter from onboard to make sure the current value is used - paramMgr->requestParameterUpdate(paramMgr->getDefaultComponentId(), param_id); - - // wait until parameter update is received - QEventLoop loop; - QTimer timer; - connect(&timer, &QTimer::timeout, &loop, &QEventLoop::quit); - timer.start(10 * 1e3); - // overloaded signal: - connect(mav, static_cast(&UASInterface::parameterChanged), - this, &ParamLoader::handleParameterChanged); - connect(this, &ParamLoader::correctParameterChanged, - &loop, &QEventLoop::quit); - loop.exec(); - - if (!param_received == true) { - // timeout - emit paramLoaded(false, 0.0f, "Timeout"); - return; - } - - QVariant current_param_value; - bool got_param = paramMgr->getParameterValue(paramMgr->getDefaultComponentId(), - param_id, current_param_value); - - QString message = got_param ? "" : "param manager Error"; - emit paramLoaded(got_param, current_param_value.toFloat(), message); + connect(_autopilot->getParameterFact(_paramName), &Fact::valueChanged, this, &ParamLoader::_parameterUpdated); + + // refresh the parameter from onboard to make sure the current value is used + _autopilot->refreshParameter(FactSystem::defaultComponentId, _paramName); } -void ParamLoader::handleParameterChanged(int uas, int component, QString parameterName, QVariant value) +void ParamLoader::_parameterUpdated(QVariant value) { - Q_UNUSED(component); Q_UNUSED(value); - if (uas == mav->getUASID() && parameterName == param_id) { - param_received = true; - emit correctParameterChanged(); - } + + emit paramLoaded(true, _autopilot->getParameterFact(_paramName)->value().toFloat(), ""); } diff --git a/src/ui/QGCMapRCToParamDialog.h b/src/ui/QGCMapRCToParamDialog.h index 82f0d90307e43aa44fce0ec635c55ebd6527d302..0bd3d8615ff671a3ad706e5e661b12ee4fadb3cb 100644 --- a/src/ui/QGCMapRCToParamDialog.h +++ b/src/ui/QGCMapRCToParamDialog.h @@ -30,7 +30,9 @@ #include #include + #include "UASInterface.h" +#include "AutoPilotPlugin.h" namespace Ui { class QGCMapRCToParamDialog; @@ -42,27 +44,23 @@ class ParamLoader : public QObject Q_OBJECT public: - ParamLoader(QString param_id, UASInterface *mav, QObject * parent = 0): - QObject(parent), - mav(mav), - paramMgr(mav->getParamManager()), - param_id(param_id), - param_received(false) -{} + ParamLoader(QString paramName, UASInterface* uas, QObject* parent = NULL); public slots: void load(); - void handleParameterChanged(int uas, int component, QString parameterName, QVariant value); signals: void paramLoaded(bool success, float value, QString message = ""); void correctParameterChanged(); + +private slots: + void _parameterUpdated(QVariant value); -protected: - UASInterface *mav; - QGCUASParamManagerInterface* paramMgr; - QString param_id; - bool param_received; +private: + UASInterface* _uas; + AutoPilotPlugin* _autopilot; + QString _paramName; + bool _paramReceived; }; class QGCMapRCToParamDialog : public QDialog diff --git a/src/ui/px4_configuration/PX4RCCalibration.cc b/src/ui/px4_configuration/PX4RCCalibration.cc index 9bf76c54bad71451bf9cf73bb0afad9e723e28c0..7959c76f5b15593616010f4a43a157da72a1eeaa 100644 --- a/src/ui/px4_configuration/PX4RCCalibration.cc +++ b/src/ui/px4_configuration/PX4RCCalibration.cc @@ -30,6 +30,7 @@ #include "PX4RCCalibration.h" #include "UASManager.h" #include "QGCMessageBox.h" +#include "AutoPilotPluginManager.h" QGC_LOGGING_CATEGORY(PX4RCCalibrationLog, "PX4RCCalibrationLog") @@ -86,8 +87,7 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) : _transmitterMode(2), _chanCount(0), _rcCalState(rcCalStateChannelWait), - _mav(NULL), - _paramMgr(NULL), + _uas(NULL), _ui(new Ui::PX4RCCalibration), _unitTestMode(false) { @@ -122,19 +122,19 @@ PX4RCCalibration::PX4RCCalibration(QWidget *parent) : _rgAttitudeControl[4].valueWidget = _ui->flapsValue; // This code assume we already have an active UAS with ready parameters - _mav = UASManager::instance()->getActiveUAS(); - Q_ASSERT(_mav); + _uas = UASManager::instance()->getActiveUAS(); + Q_ASSERT(_uas); // Connect new signals bool fSucceeded; Q_UNUSED(fSucceeded); - fSucceeded = connect(_mav, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(_remoteControlChannelRawChanged(int,float))); + fSucceeded = connect(_uas, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(_remoteControlChannelRawChanged(int,float))); Q_ASSERT(fSucceeded); - _paramMgr = _mav->getParamManager(); - Q_ASSERT(_paramMgr); - Q_ASSERT(_paramMgr->parametersReady()); + _autopilot = AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_uas); + Q_ASSERT(_autopilot); + Q_ASSERT(_autopilot->pluginReady()); connect(_ui->spektrumBind, &QPushButton::clicked, this, &PX4RCCalibration::_spektrumBind); @@ -701,19 +701,13 @@ void PX4RCCalibration::_resetInternalCalibrationValues(void) rcCalFunctionReturnSwitch }; static const size_t crgFlightModeFunctions = sizeof(rgFlightModeFunctions) / sizeof(rgFlightModeFunctions[0]); - int componentId = _paramMgr->getDefaultComponentId(); for (size_t i=0; i < crgFlightModeFunctions; i++) { QVariant value; enum rcCalFunctions curFunction = rgFlightModeFunctions[i]; - bool paramFound = _paramMgr->getParameterValue(componentId, _rgFunctionInfo[curFunction].parameterName, value); - Q_ASSERT(paramFound); - Q_UNUSED(paramFound); - bool ok; - int switchChannel = value.toInt(&ok); + int switchChannel = _autopilot->getParameterFact(_rgFunctionInfo[curFunction].parameterName)->value().toInt(&ok); Q_ASSERT(ok); - Q_UNUSED(ok); // Parameter: 1-based channel, 0=not mapped // _rgFunctionChannelMapping: 0-based channel, _chanMax=not mapped @@ -729,8 +723,6 @@ void PX4RCCalibration::_resetInternalCalibrationValues(void) /// @brief Sets internal calibration values from the stored parameters void PX4RCCalibration::_setInternalCalibrationValuesFromParameters(void) { - Q_ASSERT(_paramMgr); - // Initialize all function mappings to not set for (size_t i=0; i<_chanMax; i++) { @@ -750,58 +742,36 @@ void PX4RCCalibration::_setInternalCalibrationValuesFromParameters(void) QString maxTpl("RC%1_MAX"); QString trimTpl("RC%1_TRIM"); QString revTpl("RC%1_REV"); - QVariant value; - bool paramFound; + bool convertOk; - int componentId = _paramMgr->getDefaultComponentId(); for (int i = 0; i < _chanMax; ++i) { struct ChannelInfo* info = &_rgChannelInfo[i]; - paramFound = _paramMgr->getParameterValue(componentId, trimTpl.arg(i+1), value); - Q_ASSERT(paramFound); - if (paramFound) { - info->rcTrim = value.toInt(&convertOk); - Q_ASSERT(convertOk); - } + info->rcTrim = _autopilot->getParameterFact(trimTpl.arg(i+1))->value().toInt(&convertOk); + Q_ASSERT(convertOk); - paramFound = _paramMgr->getParameterValue(componentId, minTpl.arg(i+1), value); - Q_ASSERT(paramFound); - if (paramFound) { - info->rcMin = value.toInt(&convertOk); - Q_ASSERT(convertOk); - } + info->rcMin = _autopilot->getParameterFact(minTpl.arg(i+1))->value().toInt(&convertOk); + Q_ASSERT(convertOk); - paramFound = _paramMgr->getParameterValue(componentId, maxTpl.arg(i+1), value); - Q_ASSERT(paramFound); - if (paramFound) { - info->rcMax = value.toInt(&convertOk); - Q_ASSERT(convertOk); - } + info->rcMax = _autopilot->getParameterFact(maxTpl.arg(i+1))->value().toInt(&convertOk); + Q_ASSERT(convertOk); - paramFound = _paramMgr->getParameterValue(componentId, revTpl.arg(i+1), value); - Q_ASSERT(paramFound); - if (paramFound) { - float floatReversed = value.toFloat(&convertOk); - Q_ASSERT(convertOk); - Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f); - info->reversed = floatReversed == -1.0f; - } + float floatReversed = _autopilot->getParameterFact(revTpl.arg(i+1))->value().toFloat(&convertOk); + Q_ASSERT(convertOk); + Q_ASSERT(floatReversed == 1.0f || floatReversed == -1.0f); + info->reversed = floatReversed == -1.0f; } for (int i=0; igetParameterValue(componentId, _rgFunctionInfo[i].parameterName, value); - Q_ASSERT(paramFound); - if (paramFound) { - paramChannel = value.toInt(&convertOk); - Q_ASSERT(convertOk); - - if (paramChannel != 0) { - _rgFunctionChannelMapping[i] = paramChannel - 1; - _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i; - } + paramChannel = _autopilot->getParameterFact(_rgFunctionInfo[i].parameterName)->value().toInt(&convertOk); + Q_ASSERT(convertOk); + + if (paramChannel != 0) { + _rgFunctionChannelMapping[i] = paramChannel - 1; + _rgChannelInfo[paramChannel - 1].function = (enum rcCalFunctions)i; } } @@ -813,7 +783,7 @@ void PX4RCCalibration::_setInternalCalibrationValuesFromParameters(void) void PX4RCCalibration::_spektrumBind(void) { - Q_ASSERT(_mav); + Q_ASSERT(_uas); QMessageBox bindTypeMsg(this); @@ -837,7 +807,7 @@ void PX4RCCalibration::_spektrumBind(void) } else { Q_ASSERT(false); } - _mav->pairRX(0, bindType); + _uas->pairRX(0, bindType); } } @@ -890,15 +860,12 @@ void PX4RCCalibration::_validateCalibration(void) /// @brief Saves the rc calibration values to the board parameters. void PX4RCCalibration::_writeCalibration(void) { - if (!_mav) return; + if (!_uas) return; - _mav->stopCalibration(); + _uas->stopCalibration(); _validateCalibration(); - QGCUASParamManagerInterface* paramMgr = _mav->getParamManager(); - Q_ASSERT(paramMgr); - QString minTpl("RC%1_MIN"); QString maxTpl("RC%1_MAX"); QString trimTpl("RC%1_TRIM"); @@ -907,11 +874,12 @@ void PX4RCCalibration::_writeCalibration(void) // Note that the rc parameters are all float, so you must cast to float in order to get the right QVariant for (int chan = 0; chan<_chanMax; chan++) { struct ChannelInfo* info = &_rgChannelInfo[chan]; - int oneBasedChannel = chan + 1; - paramMgr->setPendingParam(0, trimTpl.arg(oneBasedChannel), (float)info->rcTrim); - paramMgr->setPendingParam(0, minTpl.arg(oneBasedChannel), (float)info->rcMin); - paramMgr->setPendingParam(0, maxTpl.arg(oneBasedChannel), (float)info->rcMax); - paramMgr->setPendingParam(0, revTpl.arg(oneBasedChannel), info->reversed ? -1.0f : 1.0f); + int oneBasedChannel = chan + 1; + + _autopilot->getParameterFact(trimTpl.arg(oneBasedChannel))->setValue((float)info->rcTrim); + _autopilot->getParameterFact(minTpl.arg(oneBasedChannel))->setValue((float)info->rcMin); + _autopilot->getParameterFact(maxTpl.arg(oneBasedChannel))->setValue((float)info->rcMax); + _autopilot->getParameterFact(revTpl.arg(oneBasedChannel))->setValue(info->reversed ? -1.0f : 1.0f); } // Write function mapping parameters @@ -924,16 +892,13 @@ void PX4RCCalibration::_writeCalibration(void) // Note that the channel value is 1-based paramChannel = _rgFunctionChannelMapping[i] + 1; } - paramMgr->setPendingParam(0, _rgFunctionInfo[i].parameterName, paramChannel); + _autopilot->getParameterFact(_rgFunctionInfo[i].parameterName)->setValue(paramChannel); } // If the RC_CHAN_COUNT parameter is available write the channel count - if (paramMgr->getComponentForParam("RC_CHAN_CNT").count() != 0) { - paramMgr->setPendingParam(0, "RC_CHAN_CNT", _chanCount); + if (_autopilot->parameterExists("RC_CHAN_CNT")) { + _autopilot->getParameterFact("RC_CHAN_CNT")->setValue(_chanCount); } - - //let the param mgr manage sending all the pending RC_foo updates and persisting after - paramMgr->sendPendingParameters(true, true); _stopCalibration(); _setInternalCalibrationValuesFromParameters(); @@ -986,7 +951,7 @@ void PX4RCCalibration::_startCalibration(void) _resetInternalCalibrationValues(); // Let the mav known we are starting calibration. This should turn off motors and so forth. - _mav->startCalibration(UASInterface::StartCalibrationRadio); + _uas->startCalibration(UASInterface::StartCalibrationRadio); _ui->rcCalNext->setText(tr("Next")); _ui->rcCalCancel->setEnabled(true); @@ -1000,8 +965,8 @@ void PX4RCCalibration::_stopCalibration(void) { _currentStep = -1; - if (_mav) { - _mav->stopCalibration(); + if (_uas) { + _uas->stopCalibration(); _setInternalCalibrationValuesFromParameters(); } else { _resetInternalCalibrationValues(); diff --git a/src/ui/px4_configuration/PX4RCCalibration.h b/src/ui/px4_configuration/PX4RCCalibration.h index 60a3b1e0c50a34b4b5f7bbeb6c05003a9d89bedd..25ae3e4865d97b66200a734bd473a3ab3a98fdd7 100644 --- a/src/ui/px4_configuration/PX4RCCalibration.h +++ b/src/ui/px4_configuration/PX4RCCalibration.h @@ -35,6 +35,7 @@ #include "UASInterface.h" #include "RCValueWidget.h" #include "QGCLoggingCategory.h" +#include "AutoPilotPlugin.h" #include "ui_PX4RCCalibration.h" @@ -254,8 +255,8 @@ private: RCValueWidget* _rgRCValueMonitorWidget[_chanMax]; ///< Array of radio channel value widgets QLabel* _rgRCValueMonitorLabel[_chanMax]; ///< Array of radio channel value labels - UASInterface* _mav; ///< The current MAV - QGCUASParamManagerInterface* _paramMgr; + UASInterface* _uas; + AutoPilotPlugin* _autopilot; Ui::PX4RCCalibration* _ui; diff --git a/src/ui/toolbar/MainToolBar.cc b/src/ui/toolbar/MainToolBar.cc index 5f2ef58544b63aca6bb669efc97e0aac159f7971..ff09c755987d0f02c80ecb17d0afdb8b154d4561 100644 --- a/src/ui/toolbar/MainToolBar.cc +++ b/src/ui/toolbar/MainToolBar.cc @@ -305,18 +305,18 @@ void MainToolBar::_setActiveUAS(UASInterface* active) disconnect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged); disconnect(_mav, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*,QString,QString))); disconnect(_mav, SIGNAL(armingChanged(bool)), this, SLOT(_updateArmingState(bool))); - if (_mav->getWaypointManager()) - { + + if (_mav->getWaypointManager()) { disconnect(_mav->getWaypointManager(), &UASWaypointManager::currentWaypointChanged, this, &MainToolBar::_updateCurrentWaypoint); disconnect(_mav->getWaypointManager(), &UASWaypointManager::waypointDistanceChanged, this, &MainToolBar::_updateWaypointDistance); } + UAS* pUas = dynamic_cast(_mav); if(pUas) { disconnect(pUas, &UAS::satelliteCountChanged, this, &MainToolBar::_setSatelliteCount); } - QGCUASParamManagerInterface* paramMgr = _mav->getParamManager(); - Q_ASSERT(paramMgr); - disconnect(paramMgr, SIGNAL(parameterListProgress(float)), this, SLOT(_setProgressBarValue(float))); + + disconnect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue); } // Connect new system _mav = active; @@ -334,19 +334,20 @@ void MainToolBar::_setActiveUAS(UASInterface* active) connect(_mav, &UASInterface::remoteControlRSSIChanged, this, &MainToolBar::_remoteControlRSSIChanged); connect(_mav, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*, QString,QString))); connect(_mav, SIGNAL(armingChanged(bool)), this, SLOT(_updateArmingState(bool))); - if (_mav->getWaypointManager()) - { + + if (_mav->getWaypointManager()) { connect(_mav->getWaypointManager(), &UASWaypointManager::currentWaypointChanged, this, &MainToolBar::_updateCurrentWaypoint); connect(_mav->getWaypointManager(), &UASWaypointManager::waypointDistanceChanged, this, &MainToolBar::_updateWaypointDistance); } + UAS* pUas = dynamic_cast(_mav); if(pUas) { _setSatelliteCount(pUas->getSatelliteCount(), QString("")); connect(pUas, &UAS::satelliteCountChanged, this, &MainToolBar::_setSatelliteCount); } - QGCUASParamManagerInterface* paramMgr = _mav->getParamManager(); - Q_ASSERT(paramMgr); - connect(paramMgr, SIGNAL(parameterListProgress(float)), this, SLOT(_setProgressBarValue(float))); + + connect(AutoPilotPluginManager::instance()->getInstanceForAutoPilotPlugin(_mav), &AutoPilotPlugin::parameterListProgress, this, &MainToolBar::_setProgressBarValue); + // Reset connection lost (if any) _currentHeartbeatTimeout = 0; emit heartbeatTimeoutChanged(_currentHeartbeatTimeout);