diff --git a/ChangeLog.md b/ChangeLog.md index 38cbea0195b97024943f3b862ca5580a62acd05d..219dd1519ab84111a268f39df5c6c71eb280142c 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -4,6 +4,8 @@ Note: This file only contains high level features or important fixes. ## 4.1 - Daily build +* Load Parameters From File: Support loading parameters which don't currently existing on the vehicle. +* Load Parameters From File: Add dialog which shows diff of file and vehicle params. Selective param upload from file. * Video Streaming: New camera control supports capturing individual images from the stream * Fly: Press and hold on arm button will change it to Force Arm. Click again to force arm. * VTOL: General setting for transition distance which affects Plan takeoff, landing pattern creation diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index ea152f915f4fe7894cb75628b4ac6b807afb0a6e..0f372e682b87bd1bcd01d8417e600df23436c256 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -116,6 +116,7 @@ src/QmlControls/ModeSwitchDisplay.qml src/QmlControls/MultiRotorMotorDisplay.qml src/QmlControls/OfflineMapButton.qml + src/QmlControls/ParameterDiffDialog.qml src/QmlControls/ParameterEditor.qml src/QmlControls/ParameterEditorDialog.qml src/QmlControls/PIDTuning.qml diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index 561345f206225b158d06997fa08bb686ad9da7c1..7a6d1f3ca7df01d64933fae25bdf6a6b8de360a0 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -7,12 +7,7 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - -#ifndef Fact_H -#define Fact_H +#pragma once #include "FactMetaData.h" @@ -215,5 +210,3 @@ protected: FactValueSliderListModel* _valueSliderModel; bool _ignoreQGCRebootRequired; }; - -#endif diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index a9ed0ce1952d7481f235fb340e235d9ed9670915..7ce64e56ed5dc5971437d1f4b68702c819171bbe 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -36,9 +36,8 @@ const qreal FactMetaData::UnitConsts_s::inchesToCentimeters = 2.54; const qreal FactMetaData::UnitConsts_s::ouncesToGrams = 28.3495; const qreal FactMetaData::UnitConsts_s::poundsToGrams = 453.592; - -static const char* kDefaultCategory = QT_TRANSLATE_NOOP("FactMetaData", "Other"); -static const char* kDefaultGroup = QT_TRANSLATE_NOOP("FactMetaData", "Misc"); +const char* FactMetaData::kDefaultCategory = QT_TRANSLATE_NOOP("FactMetaData", "Other"); +const char* FactMetaData::kDefaultGroup = QT_TRANSLATE_NOOP("FactMetaData", "Misc"); const char* FactMetaData::qgcFileType = "FactMetaData"; const char* FactMetaData::_jsonMetaDataDefinesName = "QGC.MetaData.Defines"; diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 8a5de25f3c4edba8caac925b9fa37709ecd55006..71eec73e31ea917a1efb4e80eac2d7fee0518fd3 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -194,6 +194,8 @@ public: static const int kDefaultDecimalPlaces = 3; ///< Default value for decimal places if not specified/known static const int kUnknownDecimalPlaces = -1; ///< Number of decimal places to specify is not known + static const char* kDefaultCategory; + static const char* kDefaultGroup; static ValueType_t stringToType(const QString& typeString, bool& unknownType); static QString typeToString(ValueType_t type); diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index fce88db73f95e3cc8852f903b76313bde558a4ec..7c3f2290ba3c91cd968ed479fc79a2f5a2517c16 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -82,7 +82,6 @@ ParameterManager::ParameterManager(Vehicle* vehicle) , _saveRequired (false) , _metaDataAddedToFacts (false) , _logReplay (vehicle->vehicleLinkManager()->primaryLink() && vehicle->vehicleLinkManager()->primaryLink()->isLogReplay()) - , _parameterSetMajorVersion (-1) , _prevWaitingReadParamIndexCount (0) , _prevWaitingReadParamNameCount (0) , _prevWaitingWriteParamNameCount (0) @@ -91,8 +90,6 @@ ParameterManager::ParameterManager(Vehicle* vehicle) , _indexBatchQueueActive (false) , _totalParamCount (0) { - _versionParam = vehicle->firmwarePlugin()->getVersionParam(); - if (_vehicle->isOfflineEditingVehicle()) { _loadOfflineEditingParams(); return; @@ -108,8 +105,6 @@ ParameterManager::ParameterManager(Vehicle* vehicle) _waitingParamTimeoutTimer.setInterval(3000); connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterManager::_waitingParamTimeout); - connect(_vehicle->uas(), &UASInterface::parameterUpdate, this, &ParameterManager::_parameterUpdate); - // Ensure the cache directory exists QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache"); } @@ -171,26 +166,70 @@ void ParameterManager::_updateProgressBar(void) } } -/// Called whenever a parameter is updated or first seen. -void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value) + +void ParameterManager::mavlinkMessageReceived(mavlink_message_t message) { - // Is this for our uas? - if (vehicleId != _vehicle->id()) { - return; + if (message.msgid == MAVLINK_MSG_ID_PARAM_VALUE) { + mavlink_param_value_t param_value; + mavlink_msg_param_value_decode(&message, ¶m_value); + + // This will null terminate the name string + QByteArray bytes(param_value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName(bytes); + + mavlink_param_union_t paramUnion; + paramUnion.param_float = param_value.param_value; + paramUnion.type = param_value.param_type; + + QVariant parameterValue; + + switch (paramUnion.type) { + case MAV_PARAM_TYPE_REAL32: + parameterValue = QVariant(paramUnion.param_float); + break; + case MAV_PARAM_TYPE_UINT8: + parameterValue = QVariant(paramUnion.param_uint8); + break; + case MAV_PARAM_TYPE_INT8: + parameterValue = QVariant(paramUnion.param_int8); + break; + case MAV_PARAM_TYPE_UINT16: + parameterValue = QVariant(paramUnion.param_uint16); + break; + case MAV_PARAM_TYPE_INT16: + parameterValue = QVariant(paramUnion.param_int16); + break; + case MAV_PARAM_TYPE_UINT32: + parameterValue = QVariant(paramUnion.param_uint32); + break; + case MAV_PARAM_TYPE_INT32: + parameterValue = QVariant(paramUnion.param_int32); + break; + default: + qCritical() << "ParameterManager::_handleParamValue - unsupported MAV_PARAM_TYPE" << paramUnion.type; + break; + } + + _handleParamValue(message.compid, parameterName, param_value.param_count, param_value.param_index, static_cast(param_value.param_type), parameterValue); } +} + +/// Called whenever a parameter is updated or first seen. +void ParameterManager::_handleParamValue(int componentId, QString parameterName, int parameterCount, int parameterIndex, MAV_PARAM_TYPE mavParamType, QVariant parameterValue) +{ qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "_parameterUpdate" << "name:" << parameterName << "count:" << parameterCount << - "index:" << parameterId << - "mavType:" << mavType << - "value:" << value << + "index:" << parameterIndex << + "mavType:" << mavParamType << + "value:" << parameterValue << ")"; // ArduPilot has this strange behavior of streaming parameters that we didn't ask for. This even happens before it responds to the // PARAM_REQUEST_LIST. We disregard any of this until the initial request is responded to. - if (parameterId == 65535 && parameterName != "_HASH_CHECK" && _initialRequestTimeoutTimer.isActive()) { + if (parameterIndex == 65535 && parameterName != "_HASH_CHECK" && _initialRequestTimeoutTimer.isActive()) { qCDebug(ParameterManagerVerbose1Log) << "Disregarding unrequested param prior to initial list response" << parameterName; return; } @@ -218,7 +257,7 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") { if (!_initialLoadComplete && !_logReplay) { /* we received a cache hash, potentially load from cache */ - _tryCacheHashLoad(vehicleId, componentId, value); + _tryCacheHashLoad(_vehicle->id(), componentId, parameterValue); } return; } @@ -226,14 +265,13 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString // Used to debug cache crc misses (turn on ParameterManagerDebugCacheFailureLog) if (!_initialLoadComplete && !_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) { if (_debugCacheMap[componentId].contains(parameterName)) { - const ParamTypeVal& cacheParamTypeVal = _debugCacheMap[componentId][parameterName]; - size_t dataSize = FactMetaData::typeToSize(static_cast(cacheParamTypeVal.first)); - const void *cacheData = cacheParamTypeVal.second.constData(); - - const void *vehicleData = value.constData(); + const ParamTypeVal& cacheParamTypeVal = _debugCacheMap[componentId][parameterName]; + size_t dataSize = FactMetaData::typeToSize(static_cast(cacheParamTypeVal.first)); + const void* cacheData = cacheParamTypeVal.second.constData(); + const void* vehicleData = parameterValue.constData(); if (memcmp(cacheData, vehicleData, dataSize)) { - qDebug() << "Cache/Vehicle values differ for name:cache:actual" << parameterName << value << cacheParamTypeVal.second; + qDebug() << "Cache/Vehicle values differ for name:cache:actual" << parameterName << parameterValue << cacheParamTypeVal.second; } _debugCacheParamSeen[componentId][parameterName] = true; } else { @@ -244,15 +282,13 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString _initialRequestTimeoutTimer.stop(); _waitingParamTimeoutTimer.stop(); - _dataMutex.lock(); - // 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 we've never seen this component id before, setup the index wait lists. if (!_waitingReadParamIndexMap.contains(componentId)) { // Add all indices to the wait list, parameter index is 0-based for (int waitingIndex=0; waitingIndexdefaultComponentId())) { + if (!_mapCompId2FactMap.contains(_vehicle->defaultComponentId())) { // Still waiting for parameters from default component qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer (still waiting for default component params)"; _waitingParamTimeoutTimer.start(); @@ -342,83 +372,27 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString \ _updateProgressBar(); - // Get parameter set version - if (!_versionParam.isEmpty() && _versionParam == parameterName) { - _parameterSetMajorVersion = value.toInt(); - } - - if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(parameterName)) { + Fact* fact = nullptr; + if (_mapCompId2FactMap.contains(componentId) && _mapCompId2FactMap[componentId].contains(parameterName)) { + fact = _mapCompId2FactMap[componentId][parameterName]; + } else { qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "Adding new fact" << parameterName; - FactMetaData::ValueType_t factType; - switch (mavType) { - case MAV_PARAM_TYPE_UINT8: - factType = FactMetaData::valueTypeUint8; - break; - case MAV_PARAM_TYPE_INT8: - factType = FactMetaData::valueTypeInt8; - break; - case MAV_PARAM_TYPE_UINT16: - factType = FactMetaData::valueTypeUint16; - break; - case MAV_PARAM_TYPE_INT16: - factType = FactMetaData::valueTypeInt16; - break; - case MAV_PARAM_TYPE_UINT32: - factType = FactMetaData::valueTypeUint32; - break; - case MAV_PARAM_TYPE_INT32: - factType = FactMetaData::valueTypeInt32; - break; - case MAV_PARAM_TYPE_UINT64: - factType = FactMetaData::valueTypeUint64; - break; - case MAV_PARAM_TYPE_INT64: - factType = FactMetaData::valueTypeInt64; - break; - case MAV_PARAM_TYPE_REAL32: - factType = FactMetaData::valueTypeFloat; - break; - case MAV_PARAM_TYPE_REAL64: - factType = FactMetaData::valueTypeDouble; - break; - default: - factType = FactMetaData::valueTypeInt32; - qCritical() << "Unsupported fact type" << mavType; - break; - } + fact = new Fact(componentId, parameterName, mavTypeToFactType(mavParamType), this); + FactMetaData* factMetaData = _vehicle->compInfoManager()->compInfoParam(componentId)->factMetaDataForName(parameterName, fact->type()); + fact->setMetaData(factMetaData); - Fact* fact = new Fact(componentId, parameterName, factType, this); + _mapCompId2FactMap[componentId][parameterName] = fact; - _mapParameterName2Variant[componentId][parameterName] = QVariant::fromValue(fact); + // We need to know when the fact value changes so we can update the vehicle + connect(fact, &Fact::_containerRawValueChanged, this, &ParameterManager::_factRawValueUpdated); - // We need to know when the fact changes from QML so that we can send the new value to the parameter manager - connect(fact, &Fact::_containerRawValueChanged, this, &ParameterManager::_valueUpdated); + emit factAdded(componentId, fact); } _dataMutex.unlock(); - Fact* fact = nullptr; - if (_mapParameterName2Variant[componentId].contains(parameterName)) { - fact = _mapParameterName2Variant[componentId][parameterName].value(); - } - if (fact) { - fact->_containerSetRawValue(value); - } else { - qWarning() << "Internal error"; - } - - if (componentParamsComplete) { - if (componentId == _vehicle->defaultComponentId()) { - // Add meta data to default component. We need to do this before we setup the group map since group - // map requires meta data. - _addMetaDataToDefaultComponent(); - } - // When we are getting the very last component param index, reset the group maps to update for the - // new params. By handling this here, we can pick up components which finish up later than the default - // component param set. - _setupComponentCategoryMap(componentId); - } + fact->_containerSetRawValue(parameterValue); // Update param cache. The param cache is only used on PX4 Firmware since ArduPilot and Solo have volatile params // which invalidate the cache. The Solo also streams param updates in flight for things like gimbal values @@ -426,7 +400,7 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString if (!_logReplay && _vehicle->px4Firmware()) { if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0 && readWaitingParamCount == 0) { // All reads just finished, update the cache - _writeLocalParamCache(vehicleId, componentId); + _writeLocalParamCache(_vehicle->id(), componentId); } } @@ -439,20 +413,9 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "_parameterUpdate complete"; } -/// Connected to Fact::valueUpdated -/// -/// Writes the parameter to mavlink, sets up for write wait -void ParameterManager::_valueUpdated(const QVariant& value) +/// Writes the parameter update to mavlink, sets up for write wait +void ParameterManager::_factRawValueUpdateWorker(int componentId, const QString& name, FactMetaData::ValueType_t valueType, const QVariant& rawValue) { - Fact* fact = qobject_cast(sender()); - if (!fact) { - qWarning() << "Internal error"; - return; - } - - int componentId = fact->componentId(); - QString name = fact->name(); - _dataMutex.lock(); if (_waitingWriteParamNameMap.contains(componentId)) { @@ -466,13 +429,24 @@ void ParameterManager::_valueUpdated(const QVariant& value) _waitingParamTimeoutTimer.start(); _saveRequired = true; } else { - qWarning() << "Internal error"; + qWarning() << "Internal error ParameterManager::_factValueUpdateWorker: component id not found" << componentId; } _dataMutex.unlock(); - _writeParameterRaw(componentId, fact->name(), value); - qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Set parameter - name:" << name << value << "(_waitingParamTimeoutTimer started)"; + _sendParamSetToVehicle(componentId, name, valueType, rawValue); + qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Update parameter (_waitingParamTimeoutTimer started) - compId:name:rawValue" << componentId << name << rawValue; +} + +void ParameterManager::_factRawValueUpdated(const QVariant& rawValue) +{ + Fact* fact = qobject_cast(sender()); + if (!fact) { + qWarning() << "Internal error"; + return; + } + + _factRawValueUpdateWorker(fact->componentId(), fact->name(), fact->type(), rawValue); } void ParameterManager::refreshAllParameters(uint8_t componentId) @@ -571,7 +545,7 @@ void ParameterManager::refreshParametersPrefix(int componentId, const QString& n componentId = _actualComponentId(componentId); qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParametersPrefix - name:" << namePrefix << ")"; - for(const QString ¶mName: _mapParameterName2Variant[componentId].keys()) { + for (const QString ¶mName: _mapCompId2FactMap[componentId].keys()) { if (paramName.startsWith(namePrefix)) { refreshParameter(componentId, paramName); } @@ -583,8 +557,8 @@ bool ParameterManager::parameterExists(int componentId, const QString& paramName bool ret = false; componentId = _actualComponentId(componentId); - if (_mapParameterName2Variant.contains(componentId)) { - ret = _mapParameterName2Variant[componentId].contains(_remapParamNameToVersion(paramName)); + if (_mapCompId2FactMap.contains(componentId)) { + ret = _mapCompId2FactMap[componentId].contains(_remapParamNameToVersion(paramName)); } return ret; @@ -595,87 +569,25 @@ Fact* ParameterManager::getParameter(int componentId, const QString& paramName) componentId = _actualComponentId(componentId); QString mappedParamName = _remapParamNameToVersion(paramName); - if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(mappedParamName)) { + if (!_mapCompId2FactMap.contains(componentId) || !_mapCompId2FactMap[componentId].contains(mappedParamName)) { qgcApp()->reportMissingParameter(componentId, mappedParamName); return &_defaultFact; } - return _mapParameterName2Variant[componentId][mappedParamName].value(); + return _mapCompId2FactMap[componentId][mappedParamName]; } QStringList ParameterManager::parameterNames(int componentId) { QStringList names; - for(const QString ¶mName: _mapParameterName2Variant[_actualComponentId(componentId)].keys()) { + for(const QString ¶mName: _mapCompId2FactMap[_actualComponentId(componentId)].keys()) { names << paramName; } return names; } -void ParameterManager::_setupComponentCategoryMap(int componentId) -{ - if (componentId == _vehicle->defaultComponentId()) { - _setupDefaultComponentCategoryMap(); - return; - } - - ComponentCategoryMapType& componentCategoryMap = _componentCategoryMaps[componentId]; - - QString category = getComponentCategory(componentId); - - // Must be able to handle being called multiple times - componentCategoryMap.clear(); - - // Fill parameters into the group determined by param name - for (const QString ¶mName: _mapParameterName2Variant[componentId].keys()) { - int i = paramName.indexOf("_"); - if (i > 0) { - componentCategoryMap[category][paramName.left(i)] += paramName; - } else { - componentCategoryMap[category][tr("Misc")] += paramName; - } - } - - // Memorize category for component ID - if (!_componentCategoryHash.contains(category)) { - _componentCategoryHash.insert(category, componentId); - } -} - -void ParameterManager::_setupDefaultComponentCategoryMap(void) -{ - ComponentCategoryMapType& defaultComponentCategoryMap = _componentCategoryMaps[_vehicle->defaultComponentId()]; - - // Must be able to handle being called multiple times - defaultComponentCategoryMap.clear(); - - for (const QString ¶mName: _mapParameterName2Variant[_vehicle->defaultComponentId()].keys()) { - Fact* fact = _mapParameterName2Variant[_vehicle->defaultComponentId()][paramName].value(); - defaultComponentCategoryMap[fact->category()][fact->group()] += paramName; - } -} - -QString ParameterManager::getComponentCategory(int componentId) -{ - if (_mavlinkCompIdHash.contains(componentId)) { - return tr("Component %1 (%2)").arg(_mavlinkCompIdHash.value(componentId)).arg(componentId); - } - QString componentCategoryPrefix = tr("Component "); - return QString("%1%2").arg(componentCategoryPrefix).arg(componentId); -} - -const QMap >& ParameterManager::getComponentCategoryMap(int componentId) -{ - return _componentCategoryMaps[componentId]; -} - -int ParameterManager::getComponentId(const QString& category) -{ - return (_componentCategoryHash.contains(category)) ? _componentCategoryHash.value(category) : _vehicle->defaultComponentId(); -} - /// Requests missing index based parameters from the vehicle. /// @param waitingParamTimeout: true: being called due to timeout, false: being called to re-fill the batch queue /// return true: Parameters were requested, false: No more requests needed @@ -747,10 +659,10 @@ void ParameterManager::_waitingParamTimeout(void) // First check for any missing parameters from the initial index based load paramsRequested = _fillIndexBatchQueue(true /* waitingParamTimeout */); - if (!paramsRequested && !_waitingForDefaultComponent && !_mapParameterName2Variant.contains(_vehicle->defaultComponentId())) { + if (!paramsRequested && !_waitingForDefaultComponent && !_mapCompId2FactMap.contains(_vehicle->defaultComponentId())) { // Initial load is complete but we still don't have any default component params. Wait one more cycle to see if the // any show up. - qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer - still don't have default component params" << _vehicle->defaultComponentId() << _mapParameterName2Variant.keys(); + qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer - still don't have default component params" << _vehicle->defaultComponentId(); _waitingParamTimeoutTimer.start(); _waitingForDefaultComponent = true; return; @@ -765,7 +677,8 @@ void ParameterManager::_waitingParamTimeout(void) paramsRequested = true; _waitingWriteParamNameMap[componentId][paramName]++; // Bump retry count if (_waitingWriteParamNameMap[componentId][paramName] <= _maxReadWriteRetry) { - _writeParameterRaw(componentId, paramName, getParameter(componentId, paramName)->rawValue()); + Fact* fact = getParameter(componentId, paramName); + _sendParamSetToVehicle(componentId, paramName, fact->type(), fact->rawValue()); qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Write resend for (paramName:" << paramName << "retryCount:" << _waitingWriteParamNameMap[componentId][paramName] << ")"; if (++batchCount > maxBatchSize) { goto Out; @@ -827,17 +740,16 @@ void ParameterManager::_readParameterRaw(int componentId, const QString& paramNa _vehicle->sendMessageOnLinkThreadSafe(_vehicle->vehicleLinkManager()->primaryLink(), msg); } -void ParameterManager::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value) +void ParameterManager::_sendParamSetToVehicle(int componentId, const QString& paramName, FactMetaData::ValueType_t valueType, const QVariant& value) { mavlink_param_set_t p; mavlink_param_union_t union_value; memset(&p, 0, sizeof(p)); - FactMetaData::ValueType_t factType = getParameter(componentId, paramName)->type(); - p.param_type = _factTypeToMavType(factType); + p.param_type = factTypeToMavType(valueType); - switch (factType) { + switch (valueType) { case FactMetaData::valueTypeUint8: union_value.param_uint8 = (uint8_t)value.toUInt(); break; @@ -863,7 +775,7 @@ void ParameterManager::_writeParameterRaw(int componentId, const QString& paramN break; default: - qCritical() << "Unsupported fact type" << factType; + qCritical() << "Unsupported fact falue type" << valueType; // fall through case FactMetaData::valueTypeInt32: @@ -890,8 +802,8 @@ void ParameterManager::_writeLocalParamCache(int vehicleId, int componentId) { CacheMapName2ParamTypeVal cacheMap; - for(const QString& paramName: _mapParameterName2Variant[componentId].keys()) { - const Fact *fact = _mapParameterName2Variant[componentId][paramName].value(); + for (const QString& paramName: _mapCompId2FactMap[componentId].keys()) { + const Fact *fact = _mapCompId2FactMap[componentId][paramName]; cacheMap[paramName] = ParamTypeVal(fact->type(), fact->rawValue()); } @@ -931,13 +843,6 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian QDataStream ds(&cacheFile); ds >> cacheMap; - // Load parameter meta data for the version number stored in cache. - // We need meta data so we have access to the volatile bit - if (cacheMap.contains(_versionParam)) { - _parameterSetMajorVersion = cacheMap[_versionParam].second.toInt(); - } - _vehicle->compInfoManager()->compInfoParam(MAV_COMP_ID_AUTOPILOT1)->_parameterMajorVersionKnown(_parameterSetMajorVersion); - /* compute the crc of the local cache to check against the remote */ for (const QString& name: cacheMap.keys()) { @@ -962,8 +867,8 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian for (const QString& name: cacheMap.keys()) { const ParamTypeVal& paramTypeVal = cacheMap[name]; const FactMetaData::ValueType_t fact_type = static_cast(paramTypeVal.first); - const int mavType = _factTypeToMavType(fact_type); - _parameterUpdate(vehicleId, componentId, name, count, index++, mavType, paramTypeVal.second); + const MAV_PARAM_TYPE mavParamType = factTypeToMavType(fact_type); + _handleParamValue(componentId, name, count, index++, mavParamType, paramTypeVal.second); } // Return the hash value to notify we don't want any more updates @@ -1004,8 +909,6 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian ani->start(QAbstractAnimation::DeleteWhenStopped); } else { - // Cache parameter version may differ from vehicle parameter version so we can't trust information loaded from cache parameter version number - _parameterSetMajorVersion = -1; qCInfo(ParameterManagerLog) << "Parameters cache match failed" << qPrintable(QFileInfo(cacheFile).absoluteFilePath()); if (ParameterManagerDebugCacheFailureLog().isDebugEnabled()) { _debugCacheCRC[componentId] = true; @@ -1015,7 +918,6 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian } qgcApp()->showAppMessage(tr("Parameter cache CRC match failed")); } - _vehicle->compInfoManager()->compInfoParam(MAV_COMP_ID_AUTOPILOT1)->_clearPX4ParameterMetaData(); } } @@ -1048,7 +950,7 @@ QString ParameterManager::readParametersFromStream(QTextStream& stream) } Fact* fact = getParameter(componentId, paramName); - if (fact->type() != _mavTypeToFactType((MAV_PARAM_TYPE)mavType)) { + if (fact->type() != mavTypeToFactType((MAV_PARAM_TYPE)mavType)) { QString error; error = QStringLiteral("%1:%2 ").arg(componentId).arg(paramName); typeErrors += error; @@ -1092,11 +994,11 @@ void ParameterManager::writeParametersToStream(QTextStream& stream) stream << "#\n"; stream << "# Vehicle-Id Component-Id Name Value Type\n"; - for (int componentId: _mapParameterName2Variant.keys()) { - for (const QString ¶mName: _mapParameterName2Variant[componentId].keys()) { - Fact* fact = _mapParameterName2Variant[componentId][paramName].value(); + for (int componentId: _mapCompId2FactMap.keys()) { + for (const QString ¶mName: _mapCompId2FactMap[componentId].keys()) { + Fact* fact = _mapCompId2FactMap[componentId][paramName]; if (fact) { - stream << _vehicle->id() << "\t" << componentId << "\t" << paramName << "\t" << fact->rawValueStringFullPrecision() << "\t" << QString("%1").arg(_factTypeToMavType(fact->type())) << "\n"; + stream << _vehicle->id() << "\t" << componentId << "\t" << paramName << "\t" << fact->rawValueStringFullPrecision() << "\t" << QString("%1").arg(factTypeToMavType(fact->type())) << "\n"; } else { qWarning() << "Internal error: missing fact"; } @@ -1106,7 +1008,7 @@ void ParameterManager::writeParametersToStream(QTextStream& stream) stream.flush(); } -MAV_PARAM_TYPE ParameterManager::_factTypeToMavType(FactMetaData::ValueType_t factType) +MAV_PARAM_TYPE ParameterManager::factTypeToMavType(FactMetaData::ValueType_t factType) { switch (factType) { case FactMetaData::valueTypeUint8: @@ -1145,7 +1047,7 @@ MAV_PARAM_TYPE ParameterManager::_factTypeToMavType(FactMetaData::ValueType_t fa } } -FactMetaData::ValueType_t ParameterManager::_mavTypeToFactType(MAV_PARAM_TYPE mavType) +FactMetaData::ValueType_t ParameterManager::mavTypeToFactType(MAV_PARAM_TYPE mavType) { switch (mavType) { case MAV_PARAM_TYPE_UINT8: @@ -1184,26 +1086,6 @@ FactMetaData::ValueType_t ParameterManager::_mavTypeToFactType(MAV_PARAM_TYPE ma } } -void ParameterManager::_addMetaDataToDefaultComponent(void) -{ - if (_metaDataAddedToFacts) { - return; - } - _metaDataAddedToFacts = true; - - _vehicle->compInfoManager()->compInfoParam(MAV_COMP_ID_AUTOPILOT1)->_parameterMajorVersionKnown(_parameterSetMajorVersion); - - if (_mapParameterName2Variant.contains(MAV_COMP_ID_AUTOPILOT1)) { - // Loop over all parameters in autopilot component adding meta data - QVariantMap& factMap = _mapParameterName2Variant[MAV_COMP_ID_AUTOPILOT1]; - for (const QString& key: factMap.keys()) { - Fact* fact = factMap[key].value(); - FactMetaData* factMetaData = _vehicle->compInfoManager()->compInfoParam(MAV_COMP_ID_AUTOPILOT1)->factMetaDataForName(key, fact->type()); - fact->setMetaData(factMetaData); - } - } -} - void ParameterManager::_checkInitialLoadComplete(void) { // Already processed? @@ -1218,7 +1100,7 @@ void ParameterManager::_checkInitialLoadComplete(void) } } - if (!_mapParameterName2Variant.contains(_vehicle->defaultComponentId())) { + if (!_mapCompId2FactMap.contains(_vehicle->defaultComponentId())) { // No default component params yet, not done yet return; } @@ -1400,16 +1282,14 @@ void ParameterManager::_loadOfflineEditingParams(void) break; } - // Get parameter set version - if (!_versionParam.isEmpty() && _versionParam == paramName) { - _parameterSetMajorVersion = paramValue.toInt(); - } + Fact* fact = new Fact(defaultComponentId, paramName, mavTypeToFactType(paramType), this); + + FactMetaData* factMetaData = _vehicle->compInfoManager()->compInfoParam(defaultComponentId)->factMetaDataForName(paramName, fact->type()); + fact->setMetaData(factMetaData); - Fact* fact = new Fact(defaultComponentId, paramName, _mavTypeToFactType(paramType), this); - _mapParameterName2Variant[defaultComponentId][paramName] = QVariant::fromValue(fact); + _mapCompId2FactMap[defaultComponentId][paramName] = fact; } - _setupDefaultComponentCategoryMap(); _parametersReady = true; _initialLoadComplete = true; _debugCacheCRC.clear(); @@ -1417,7 +1297,7 @@ void ParameterManager::_loadOfflineEditingParams(void) void ParameterManager::resetAllParametersToDefaults() { - _vehicle->sendMavCommand(MAV_COMP_ID_ALL, + _vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_PREFLIGHT_STORAGE, true, // showError 2, // Reset params to default diff --git a/src/FactSystem/ParameterManager.h b/src/FactSystem/ParameterManager.h index 334db9844418c7f04972e8997cd8b4253e5f9d8d..67d20df6881cd83fa2c5e45c8a559dd207c9355a 100644 --- a/src/FactSystem/ParameterManager.h +++ b/src/FactSystem/ParameterManager.h @@ -27,10 +27,14 @@ Q_DECLARE_LOGGING_CATEGORY(ParameterManagerVerbose1Log) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerVerbose2Log) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerDebugCacheFailureLog) +class ParameterEditorController; + class ParameterManager : public QObject { Q_OBJECT + friend class ParameterEditorController; + public: /// @param uas Uas which this set of facts is associated with ParameterManager(Vehicle* vehicle); @@ -50,6 +54,8 @@ public: /// @return Location of parameter cache file static QString parameterCacheFile(int vehicleId, int componentId); + void mavlinkMessageReceived(mavlink_message_t message); + QList componentIds(void); /// Re-request the full set of parameters from the autopilot @@ -78,70 +84,55 @@ public: /// @param name: Parameter name Fact* getParameter(int componentId, const QString& paramName); - int getComponentId(const QString& category); - QString getComponentCategory(int componentId); - const QMap >& getComponentCategoryMap(int componentId); - /// Returns error messages from loading QString readParametersFromStream(QTextStream& stream); void writeParametersToStream(QTextStream& stream); - /// Returns the version number for the parameter set, -1 if not known - int parameterSetVersion(void) { return _parameterSetMajorVersion; } - bool pendingWrites(void); Vehicle* vehicle(void) { return _vehicle; } + static MAV_PARAM_TYPE factTypeToMavType(FactMetaData::ValueType_t factType); + static FactMetaData::ValueType_t mavTypeToFactType(MAV_PARAM_TYPE mavType); + signals: void parametersReadyChanged (bool parametersReady); void missingParametersChanged (bool missingParameters); void loadProgressChanged (float value); void pendingWritesChanged (bool pendingWrites); + void factAdded (int componentId, Fact* fact); -protected: - Vehicle* _vehicle; - MAVLinkProtocol* _mavlink; - - void _parameterUpdate (int vehicleId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value); - void _valueUpdated (const QVariant& value); - void _waitingParamTimeout (void); - void _tryCacheLookup (void); - void _initialRequestTimeout (void); +private slots: + void _factRawValueUpdated (const QVariant& rawValue); private: - static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); - + void _handleParamValue (int componentId, QString parameterName, int parameterCount, int parameterIndex, MAV_PARAM_TYPE mavParamType, QVariant parameterValue); + void _factRawValueUpdateWorker (int componentId, const QString& name, FactMetaData::ValueType_t valueType, const QVariant& rawValue); + void _waitingParamTimeout (void); + void _tryCacheLookup (void); + void _initialRequestTimeout (void); int _actualComponentId (int componentId); - void _setupComponentCategoryMap (int componentId); - void _setupDefaultComponentCategoryMap (void); void _readParameterRaw (int componentId, const QString& paramName, int paramIndex); - void _writeParameterRaw (int componentId, const QString& paramName, const QVariant& value); + void _sendParamSetToVehicle (int componentId, const QString& paramName, FactMetaData::ValueType_t valueType, const QVariant& value); void _writeLocalParamCache (int vehicleId, int componentId); void _tryCacheHashLoad (int vehicleId, int componentId, QVariant hash_value); void _loadMetaData (void); void _clearMetaData (void); - void _addMetaDataToDefaultComponent (void); QString _remapParamNameToVersion (const QString& paramName); void _loadOfflineEditingParams (void); QString _logVehiclePrefix (int componentId); void _setLoadProgress (double loadProgress); bool _fillIndexBatchQueue (bool waitingParamTimeout); void _updateProgressBar (void); + void _checkInitialLoadComplete (void); - MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType); - FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType); - void _checkInitialLoadComplete(void); + static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); - /// First mapping is by component id - /// Second mapping is parameter name, to Fact* in QVariant - QMap _mapParameterName2Variant; + Vehicle* _vehicle; + MAVLinkProtocol* _mavlink; - // List of category map of component parameters - typedef QMap> ComponentCategoryMapType; //> - QMap _componentCategoryMaps; - QHash _componentCategoryHash; + QMap> _mapCompId2FactMap; double _loadProgress; ///< Parameter load progess, [0.0,1.0] bool _parametersReady; ///< true: parameter load complete @@ -151,8 +142,6 @@ private: bool _saveRequired; ///< true: _saveToEEPROM should be called bool _metaDataAddedToFacts; ///< true: FactMetaData has been adde to the default component facts bool _logReplay; ///< true: running with log replay link - QString _versionParam; ///< Parameter which contains parameter set version - int _parameterSetMajorVersion; ///< Version for parameter set, -1 if not known typedef QPair ParamTypeVal; typedef QMap CacheMapName2ParamTypeVal; diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 81226a754a5c788189c84d239adb5e4b659f3b1d..5c4fe94802bd968ca97dc38546b208c2c9d6f1d5 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -98,7 +98,6 @@ public: void initializeVehicle (Vehicle* vehicle) override; bool sendHomePositionToVehicle (void) override; QString missionCommandOverrides (QGCMAVLink::VehicleClass_t vehicleClass) const override; - QString getVersionParam (void) override { return QStringLiteral("SYSID_SW_MREV"); } QString _internalParameterMetaDataFile (Vehicle* vehicle) override; FactMetaData* _getMetaDataForFact (QObject* parameterMetaData, const QString& name, FactMetaData::ValueType_t type, MAV_TYPE vehicleType) override; void _getParameterMetaDataVersionInfo(const QString& metaDataFile, int& majorVersion, int& minorVersion) override { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } diff --git a/src/FirmwarePlugin/APM/APMParameterMetaData.cc b/src/FirmwarePlugin/APM/APMParameterMetaData.cc index b64785c60df8eba6da0b284e278ae6831c5e179b..0f52f4458853d261e88150e8dcf095a5a69815d4 100644 --- a/src/FirmwarePlugin/APM/APMParameterMetaData.cc +++ b/src/FirmwarePlugin/APM/APMParameterMetaData.cc @@ -242,9 +242,6 @@ void APMParameterMetaData::loadParameterFactMetaDataFile(const QString& metaData QString group = _groupFromParameterName(name); QString category = xml.attributes().value("user").toString(); - if (category.isEmpty()) { - category = QStringLiteral("Advanced"); - } QString shortDescription = xml.attributes().value("humanName").toString(); QString longDescription = xml.attributes().value("documentation").toString(); @@ -266,7 +263,9 @@ void APMParameterMetaData::loadParameterFactMetaDataFile(const QString& metaData } qCDebug(APMParameterMetaDataVerboseLog) << "inserting metadata for field" << name; rawMetaData->name = name; - rawMetaData->category = category; + if (!category.isEmpty()) { + rawMetaData->category = category; + } rawMetaData->group = group; rawMetaData->shortDescription = shortDescription; rawMetaData->longDescription = longDescription; @@ -448,7 +447,9 @@ FactMetaData* APMParameterMetaData::getMetaDataForFact(const QString& name, MAV_ } metaData->setName(rawMetaData->name); - metaData->setCategory(rawMetaData->category); + if (!rawMetaData->category.isEmpty()) { + metaData->setCategory(rawMetaData->category); + } metaData->setGroup(rawMetaData->group); metaData->setVehicleRebootRequired(rawMetaData->rebootRequired); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index a5e343196e499e1241c13bd7ab03c7183c4ea22e..07144c56a8e44176d9124292173949c255f06901 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -209,9 +209,6 @@ public: /// false: Do not send first item to vehicle, sequence numbers must be adjusted virtual bool sendHomePositionToVehicle(void); - /// Returns the parameter which is used to identify the version number of parameter set - virtual QString getVersionParam(void) { return QString(); } - /// Returns the parameter set version info pulled from inside the meta data file. -1 if not found. /// Note: The implementation for this must not vary by vehicle type. /// Important: Only CompInfoParam code should use this method diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 5eb347ac5db344acc325757a9452005f40a55e78..9421602e447fbc947c500ccac58d738f51842a1d 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -56,7 +56,6 @@ public: void initializeVehicle (Vehicle* vehicle) override; bool sendHomePositionToVehicle (void) override; QString missionCommandOverrides (QGCMAVLink::VehicleClass_t vehicleClass) const override; - QString getVersionParam (void) override { return QString("SYS_PARAM_VER"); } FactMetaData* _getMetaDataForFact (QObject* parameterMetaData, const QString& name, FactMetaData::ValueType_t type, MAV_TYPE vehicleType) override; QString _internalParameterMetaDataFile (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); } void _getParameterMetaDataVersionInfo(const QString& metaDataFile, int& majorVersion, int& minorVersion) override; diff --git a/src/FirmwarePlugin/PX4/PX4ParameterMetaData.h b/src/FirmwarePlugin/PX4/PX4ParameterMetaData.h index 30bccc3cda20ac6405690bb0e3485feaa2056143..1a33dc8f486b1be47bc7c9578220ff4e97da06ea 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterMetaData.h +++ b/src/FirmwarePlugin/PX4/PX4ParameterMetaData.h @@ -7,9 +7,7 @@ * ****************************************************************************/ - -#ifndef PX4ParameterMetaData_H -#define PX4ParameterMetaData_H +#pragma once #include #include @@ -60,5 +58,3 @@ private: bool _parameterMetaDataLoaded = false; ///< true: parameter meta data already loaded FactMetaData::NameToMetaDataMap_t _mapParameterName2FactMetaData; ///< Maps from a parameter name to FactMetaData }; - -#endif diff --git a/src/QmlControls/ParameterDiffDialog.qml b/src/QmlControls/ParameterDiffDialog.qml new file mode 100644 index 0000000000000000000000000000000000000000..18341835746f5d2a2b9d56774d223e8754d2e2a6 --- /dev/null +++ b/src/QmlControls/ParameterDiffDialog.qml @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.12 +import QtQuick.Layouts 1.2 +import QtQuick.Controls 2.5 +import QtQuick.Dialogs 1.3 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Controllers 1.0 + +QGCPopupDialog { + title: qsTr("Load Parameters") + buttons: StandardButton.Cancel | (paramController.diffList.count ? StandardButton.Ok : 0) + + property var paramController + + function accept() { + hideDialog() + paramController.sendDiff(); + } + + Component.onDestruction: paramController.clearDiff(); + + ColumnLayout { + spacing: ScreenTools.defaultDialogControlSpacing + + QGCLabel { + Layout.preferredWidth: mainGrid.visible ? mainGrid.width : ScreenTools.defaultFontPixelWidth * 40 + wrapMode: Text.WordWrap + text: paramController.diffList.count ? + qsTr("The following parameters from the loaded file differ from what is currently set on the Vehicle. Click 'Ok' to update them on the Vehicle.") : + qsTr("There are no differences between the file loaded and the current settings on the Vehicle.") + } + + GridLayout { + id: mainGrid + rows: paramController.diffList.count + 1 + columns: paramController.diffMultipleComponents ? 5 : 4 + flow: GridLayout.TopToBottom + visible: paramController.diffList.count + + QGCCheckBox { + checked: true + onClicked: { + for (var i=0; i ParameterEditorController::ParameterEditorController(void) - : _currentCategory ("Standard") // FIXME: firmware specific - , _parameters (new QmlObjectListModel(this)) - , _parameterMgr (_vehicle->parameterManager()) - , _showModifiedOnly (false) + : _parameterMgr(_vehicle->parameterManager()) { - const QMap >& categoryMap = _parameterMgr->getComponentCategoryMap(_vehicle->defaultComponentId()); - _categories = categoryMap.keys(); + _buildLists(); - // Move default category to front - _categories.removeOne(_currentCategory); - _categories.prepend(_currentCategory); + connect(this, &ParameterEditorController::currentCategoryChanged, this, &ParameterEditorController::_currentCategoryChanged); + connect(this, &ParameterEditorController::currentGroupChanged, this, &ParameterEditorController::_currentGroupChanged); + connect(this, &ParameterEditorController::searchTextChanged, this, &ParameterEditorController::_searchTextChanged); + connect(this, &ParameterEditorController::showModifiedOnlyChanged, this, &ParameterEditorController::_searchTextChanged); - // There is a category for each non default component - for (int compId: _parameterMgr->componentIds()) { - if (compId != _vehicle->defaultComponentId()) { - _categories.append(_parameterMgr->getComponentCategory(compId)); + connect(_parameterMgr, &ParameterManager::factAdded, this, &ParameterEditorController::_factAdded); + + ParameterEditorCategory* category = _categories.count() ? _categories.value(0) : nullptr; + setCurrentCategory(category); +} + +ParameterEditorController::~ParameterEditorController() +{ + +} + +void ParameterEditorController::_buildListsForComponent(int compId) +{ + for (const QString& factName: _parameterMgr->parameterNames(compId)) { + Fact* fact = _parameterMgr->getParameter(compId, factName); + + ParameterEditorCategory* category = nullptr; + if (_mapCategoryName2Category.contains(fact->category())) { + category = _mapCategoryName2Category[fact->category()]; + } else { + category = new ParameterEditorCategory(this); + category->name = fact->category(); + _mapCategoryName2Category[fact->category()] = category; + _categories.append(category); + } + + ParameterEditorGroup* group = nullptr; + if (category->mapGroupName2Group.contains(fact->group())) { + group = category->mapGroupName2Group[fact->group()]; + } else { + group = new ParameterEditorGroup(this); + group->componentId = compId; + group->name = fact->group(); + category->mapGroupName2Group[fact->group()] = group; + category->groups.append(group); } + + group->facts.append(fact); } +} - // Be careful about no parameters - if (categoryMap.contains(_currentCategory) && categoryMap[_currentCategory].size() != 0) { - _currentGroup = categoryMap[_currentCategory].keys()[0]; +void ParameterEditorController::_buildLists(void) +{ + // Autopilot component should always be first list + _buildListsForComponent(MAV_COMP_ID_AUTOPILOT1); + + // "Standard" category should always be first + for (int i=0; i<_categories.count(); i++) { + ParameterEditorCategory* category = _categories.value(i); + if (category->name == "Standard" && i != 0) { + _categories.removeAt(i); + _categories.insert(0, category); + break; + } + } + + // Default category should always be last + for (int i=0; i<_categories.count(); i++) { + ParameterEditorCategory* category = _categories.value(i); + if (category->name == FactMetaData::kDefaultCategory) { + if (i != _categories.count() - 1) { + _categories.removeAt(i); + _categories.append(category); + } + break; + } } - _updateParameters(); + // Now add other random components + for (int compId: _parameterMgr->componentIds()) { + if (compId != MAV_COMP_ID_AUTOPILOT1) { + _buildListsForComponent(compId); + } + } - connect(this, &ParameterEditorController::searchTextChanged, this, &ParameterEditorController::_updateParameters); - connect(this, &ParameterEditorController::currentCategoryChanged, this, &ParameterEditorController::_updateParameters); - connect(this, &ParameterEditorController::currentGroupChanged, this, &ParameterEditorController::_updateParameters); - connect(this, &ParameterEditorController::showModifiedOnlyChanged, this, &ParameterEditorController::_updateParameters); + // Default group should always be last + for (int i=0; i<_categories.count(); i++) { + ParameterEditorCategory* category = _categories.value(i); + for (int j=0; jgroups.count(); j++) { + ParameterEditorGroup* group = category->groups.value(j); + if (group->name == FactMetaData::kDefaultGroup) { + if (j != _categories.count() - 1) { + category->groups.removeAt(j); + category->groups.append(category); + } + break; + } + } + } } -ParameterEditorController::~ParameterEditorController() +void ParameterEditorController::_factAdded(int compId, Fact* fact) { + bool inserted = false; + ParameterEditorCategory* category = nullptr; -} + if (_mapCategoryName2Category.contains(fact->category())) { + category = _mapCategoryName2Category[fact->category()]; + } else { + category = new ParameterEditorCategory(this); + category->name = fact->category(); + _mapCategoryName2Category[fact->category()] = category; + + // Insert in sorted order + inserted = false; + for (int i=0; i<_categories.count(); i++) { + if (_categories.value(i)->name > category->name) { + _categories.insert(i, category); + inserted = true; + break; + } + } + if (!inserted) { + _categories.append(category); + } + } -QStringList ParameterEditorController::getGroupsForCategory(const QString& category) -{ - int compId = _parameterMgr->getComponentId(category); - const QMap >& categoryMap = _parameterMgr->getComponentCategoryMap(compId); - return categoryMap[category].keys(); + ParameterEditorGroup* group = nullptr; + if (category->mapGroupName2Group.contains(fact->group())) { + group = category->mapGroupName2Group[fact->group()]; + } else { + group = new ParameterEditorGroup(this); + group->componentId = compId; + group->name = fact->group(); + category->mapGroupName2Group[fact->group()] = group; + + // Insert in sorted order + QmlObjectListModel& groups = category->groups; + inserted = false; + for (int i=0; i(i)->name > group->name) { + groups.insert(i, group); + inserted = true; + break; + } + } + if (!inserted) { + groups.append(group); + } + } + + // Insert in sorted order + QmlObjectListModel& facts = group->facts; + inserted = false; + for (int i=0; i(i)->name() > fact->name()) { + facts.insert(i, fact); + inserted = true; + break; + } + } + if (!inserted) { + facts.append(fact); + } } QStringList ParameterEditorController::searchParameters(const QString& searchText, bool searchInName, bool searchInDescriptions) @@ -107,26 +224,115 @@ void ParameterEditorController::saveToFile(const QString& filename) } } -void ParameterEditorController::loadFromFile(const QString& filename) +void ParameterEditorController::clearDiff(void) { - QString errors; + _diffList.clearAndDeleteContents(); + _diffOtherVehicle = false; + _diffMultipleComponents = false; - if (!filename.isEmpty()) { - QFile file(filename); + emit diffOtherVehicleChanged(_diffOtherVehicle); + emit diffMultipleComponentsChanged(_diffMultipleComponents); +} - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - qgcApp()->showAppMessage(tr("Unable to open file: %1").arg(filename)); - return; +void ParameterEditorController::sendDiff(void) +{ + for (int i=0; i<_diffList.count(); i++) { + ParameterEditorDiff* paramDiff = _diffList.value(i); + + if (paramDiff->load) { + if (paramDiff->noVehicleValue) { + _parameterMgr->_factRawValueUpdateWorker(paramDiff->componentId, paramDiff->name, paramDiff->valueType, paramDiff->fileValueVar); + } else { + Fact* fact = _parameterMgr->getParameter(paramDiff->componentId, paramDiff->name); + fact->setRawValue(paramDiff->fileValueVar); + } } + } +} - QTextStream stream(&file); - errors = _parameterMgr->readParametersFromStream(stream); - file.close(); +bool ParameterEditorController::buildDiffFromFile(const QString& filename) +{ + QFile file(filename); + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + qgcApp()->showAppMessage(tr("Unable to open file: %1").arg(filename)); + return false; + } + + clearDiff(); + + QTextStream stream(&file); + + int firstComponentId = -1; + while (!stream.atEnd()) { + QString line = stream.readLine(); + if (!line.startsWith("#")) { + QStringList wpParams = line.split("\t"); + if (wpParams.size() == 5) { + int vehicleId = wpParams.at(0).toInt(); + int componentId = wpParams.at(1).toInt(); + QString paramName = wpParams.at(2); + QString fileValueStr = wpParams.at(3); + int mavParamType = wpParams.at(4).toInt(); + QString vehicleValueStr; + QString units; + QVariant fileValueVar = fileValueStr; + bool noVehicleValue = false; + + if (_vehicle->id() != vehicleId) { + _diffOtherVehicle = true; + } + if (firstComponentId == -1) { + firstComponentId = componentId; + } else if (firstComponentId != componentId) { + _diffMultipleComponents = true; + } + + if (_parameterMgr->parameterExists(componentId, paramName)) { + Fact* vehicleFact = _parameterMgr->getParameter(componentId, paramName); + FactMetaData* vehicleFactMetaData = vehicleFact->metaData(); + Fact* fileFact = new Fact(vehicleFact->componentId(), vehicleFact->name(), vehicleFact->type(), this); - if (!errors.isEmpty()) { - emit showErrorMessage(errors); + // Turn off reboot messaging before setting value in fileFact + bool vehicleRebootRequired = vehicleFactMetaData->vehicleRebootRequired(); + vehicleFactMetaData->setVehicleRebootRequired(false); + fileFact->setMetaData(vehicleFact->metaData()); + fileFact->setRawValue(fileValueStr); + vehicleFactMetaData->setVehicleRebootRequired(vehicleRebootRequired); + + if (vehicleFact->rawValue() == fileFact->rawValue()) { + continue; + } + fileValueStr = fileFact->enumOrValueString(); + fileValueVar = fileFact->rawValue(); + vehicleValueStr = vehicleFact->enumOrValueString(); + units = vehicleFact->cookedUnits(); + } else { + noVehicleValue = true; + } + + ParameterEditorDiff* paramDiff = new ParameterEditorDiff(this); + + paramDiff->componentId = componentId; + paramDiff->name = paramName; + paramDiff->valueType = ParameterManager::mavTypeToFactType(static_cast(mavParamType)); + paramDiff->fileValue = fileValueStr; + paramDiff->fileValueVar = fileValueVar; + paramDiff->vehicleValue = vehicleValueStr; + paramDiff->noVehicleValue = noVehicleValue; + paramDiff->units = units; + + _diffList.append(paramDiff); + } } } + + file.close(); + + emit diffOtherVehicleChanged(_diffOtherVehicle); + emit diffMultipleComponentsChanged(_diffMultipleComponents); + + return true; } void ParameterEditorController::refresh(void) @@ -146,60 +352,93 @@ void ParameterEditorController::resetAllToVehicleConfiguration(void) refresh(); } -void ParameterEditorController::setRCToParam(const QString& paramName) -{ -#ifdef __mobile__ - Q_UNUSED(paramName) -#else - if (_uas) { - Q_UNUSED(paramName) - //-- TODO QGCMapRCToParamDialog * d = new QGCMapRCToParamDialog(paramName, _uas, qgcApp()->toolbox()->multiVehicleManager(), MainWindow::instance()); - //d->exec(); - } -#endif -} - bool ParameterEditorController::_shouldShow(Fact* fact) { bool show = _showModifiedOnly ? (fact->defaultValueAvailable() ? (fact->valueEqualsDefault() ? false : true) : false) : true; return show; } -void ParameterEditorController::_updateParameters(void) +void ParameterEditorController::_searchTextChanged(void) { QObjectList newParameterList; #if QT_VERSION < QT_VERSION_CHECK(5, 15, 0) - QStringList searchItems = _searchText.split(' ', QString::SkipEmptyParts); + QStringList rgSearchStrings = _searchText.split(' ', QString::SkipEmptyParts); #else - QStringList searchItems = _searchText.split(' ', Qt::SkipEmptyParts); + QStringList rgSearchStrings = _searchText.split(' ', Qt::SkipEmptyParts); #endif - if (searchItems.isEmpty() && !_showModifiedOnly) { - int compId = _parameterMgr->getComponentId(_currentCategory); - const QMap >& categoryMap = _parameterMgr->getComponentCategoryMap(compId); - for (const QString& paramName: categoryMap[_currentCategory][_currentGroup]) { - newParameterList.append(_parameterMgr->getParameter(compId, paramName)); - } + + if (rgSearchStrings.isEmpty() && !_showModifiedOnly) { + ParameterEditorCategory* category = _categories.count() ? _categories.value(0) : nullptr; + setCurrentCategory(category); + _searchParameters.clear(); } else { - for(const QString ¶Name: _parameterMgr->parameterNames(_vehicle->defaultComponentId())) { + _searchParameters.beginReset(); + _searchParameters.clear(); + + for (const QString ¶Name: _parameterMgr->parameterNames(_vehicle->defaultComponentId())) { Fact* fact = _parameterMgr->getParameter(_vehicle->defaultComponentId(), paraName); bool matched = _shouldShow(fact); // All of the search items must match in order for the parameter to be added to the list - if(matched) { - for (const auto& searchItem : searchItems) { + if (matched) { + for (const auto& searchItem : rgSearchStrings) { if (!fact->name().contains(searchItem, Qt::CaseInsensitive) && - !fact->shortDescription().contains(searchItem, Qt::CaseInsensitive) && - !fact->longDescription().contains(searchItem, Qt::CaseInsensitive)) { + !fact->shortDescription().contains(searchItem, Qt::CaseInsensitive) && + !fact->longDescription().contains(searchItem, Qt::CaseInsensitive)) { matched = false; } } } if (matched) { - newParameterList.append(fact); + _searchParameters.append(fact); } } + + _searchParameters.endReset(); + + if (_parameters != &_searchParameters) { + _parameters = &_searchParameters; + emit parametersChanged(); + + _currentCategory = nullptr; + _currentGroup = nullptr; + } + } +} + +void ParameterEditorController::_currentCategoryChanged(void) +{ + ParameterEditorGroup* group = nullptr; + if (_currentCategory) { + // Select first group when category changes + group = _currentCategory->groups.value(0); + } else { + group = nullptr; } + setCurrentGroup(group); +} + +void ParameterEditorController::_currentGroupChanged(void) +{ + _parameters = _currentGroup ? &_currentGroup->facts : nullptr; + emit parametersChanged(); +} - _parameters->swapObjectList(newParameterList); +void ParameterEditorController::setCurrentCategory(QObject* currentCategory) +{ + ParameterEditorCategory* category = qobject_cast(currentCategory); + if (category != _currentCategory) { + _currentCategory = category; + emit currentCategoryChanged(); + } +} + +void ParameterEditorController::setCurrentGroup(QObject* currentGroup) +{ + ParameterEditorGroup* group = qobject_cast(currentGroup); + if (group != _currentGroup) { + _currentGroup = group; + emit currentGroupChanged(); + } } diff --git a/src/QmlControls/ParameterEditorController.h b/src/QmlControls/ParameterEditorController.h index 41c262c135c230fab3d2a3a07c0303b89ae67a45..717af92b2ddedbee42f13175ce7b14f9cb714f43 100644 --- a/src/QmlControls/ParameterEditorController.h +++ b/src/QmlControls/ParameterEditorController.h @@ -7,12 +7,7 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - -#ifndef PARAMETEREDITORCONTROLLER_H -#define PARAMETEREDITORCONTROLLER_H +#pragma once #include #include @@ -23,6 +18,69 @@ #include "QmlObjectListModel.h" #include "ParameterManager.h" +class ParameterEditorGroup : public QObject +{ + Q_OBJECT + +public: + ParameterEditorGroup(QObject* parent) : QObject(parent) { } + + Q_PROPERTY(QString name MEMBER name CONSTANT) + Q_PROPERTY(QmlObjectListModel* facts READ getFacts CONSTANT) + + QmlObjectListModel* getFacts(void) { return &facts; } + + int componentId; + QString name; + QmlObjectListModel facts; +}; + +class ParameterEditorCategory : public QObject +{ + Q_OBJECT + +public: + ParameterEditorCategory(QObject* parent) : QObject(parent) { } + + Q_PROPERTY(QString name MEMBER name CONSTANT) + Q_PROPERTY(QmlObjectListModel* groups READ getGroups CONSTANT) + + QmlObjectListModel* getGroups(void) { return &groups; } + + QString name; + QmlObjectListModel groups; + QMap mapGroupName2Group; +}; + +class ParameterEditorDiff : public QObject +{ + Q_OBJECT + +public: + ParameterEditorDiff(QObject* parent) : QObject(parent) { } + + Q_PROPERTY(int componentId MEMBER componentId CONSTANT) + Q_PROPERTY(QString name MEMBER name CONSTANT) + Q_PROPERTY(QString fileValue MEMBER fileValue CONSTANT) + Q_PROPERTY(QString vehicleValue MEMBER vehicleValue CONSTANT) + Q_PROPERTY(bool noVehicleValue MEMBER noVehicleValue CONSTANT) + Q_PROPERTY(QString units MEMBER units CONSTANT) + Q_PROPERTY(bool load MEMBER load NOTIFY loadChanged) + + int componentId; + QString name; + FactMetaData::ValueType_t valueType; + QString fileValue; + QVariant fileValueVar; + QString vehicleValue; + bool noVehicleValue = false; + QString units; + bool load = true; + +signals: + void loadChanged(bool load); +}; + class ParameterEditorController : public FactPanelController { Q_OBJECT @@ -31,46 +89,67 @@ public: ParameterEditorController(void); ~ParameterEditorController(); - Q_PROPERTY(QString searchText MEMBER _searchText NOTIFY searchTextChanged) - Q_PROPERTY(QString currentCategory MEMBER _currentCategory NOTIFY currentCategoryChanged) - Q_PROPERTY(QString currentGroup MEMBER _currentGroup NOTIFY currentGroupChanged) - Q_PROPERTY(QmlObjectListModel* parameters MEMBER _parameters CONSTANT) - Q_PROPERTY(QStringList categories MEMBER _categories CONSTANT) - Q_PROPERTY(bool showModifiedOnly MEMBER _showModifiedOnly NOTIFY showModifiedOnlyChanged) + Q_PROPERTY(QString searchText MEMBER _searchText NOTIFY searchTextChanged) + Q_PROPERTY(QmlObjectListModel* categories READ categories CONSTANT) + Q_PROPERTY(QObject* currentCategory READ currentCategory WRITE setCurrentCategory NOTIFY currentCategoryChanged) + Q_PROPERTY(QObject* currentGroup READ currentGroup WRITE setCurrentGroup NOTIFY currentGroupChanged) + Q_PROPERTY(QmlObjectListModel* parameters MEMBER _parameters NOTIFY parametersChanged) + Q_PROPERTY(bool showModifiedOnly MEMBER _showModifiedOnly NOTIFY showModifiedOnlyChanged) - Q_INVOKABLE QStringList getGroupsForCategory(const QString& category); - Q_INVOKABLE QStringList searchParameters(const QString& searchText, bool searchInName=true, bool searchInDescriptions=true); + // These property are related to the diff associated with a load from file + Q_PROPERTY(bool diffOtherVehicle MEMBER _diffOtherVehicle NOTIFY diffOtherVehicleChanged) + Q_PROPERTY(bool diffMultipleComponents MEMBER _diffMultipleComponents NOTIFY diffMultipleComponentsChanged) + Q_PROPERTY(QmlObjectListModel* diffList READ diffList CONSTANT) - Q_INVOKABLE void saveToFile(const QString& filename); - Q_INVOKABLE void loadFromFile(const QString& filename); - Q_INVOKABLE void refresh(void); - Q_INVOKABLE void resetAllToDefaults(void); - Q_INVOKABLE void resetAllToVehicleConfiguration(void); - Q_INVOKABLE void setRCToParam(const QString& paramName); + Q_INVOKABLE QStringList searchParameters(const QString& searchText, bool searchInName=true, bool searchInDescriptions=true); - QList model(void); + Q_INVOKABLE void saveToFile (const QString& filename); + Q_INVOKABLE bool buildDiffFromFile (const QString& filename); + Q_INVOKABLE void clearDiff (void); + Q_INVOKABLE void sendDiff (void); + Q_INVOKABLE void refresh (void); + Q_INVOKABLE void resetAllToDefaults (void); + Q_INVOKABLE void resetAllToVehicleConfiguration (void); + + QObject* currentCategory (void) { return _currentCategory; } + QObject* currentGroup (void) { return _currentGroup; } + QmlObjectListModel* categories (void) { return &_categories; } + QmlObjectListModel* diffList (void) { return &_diffList; } + void setCurrentCategory (QObject* currentCategory); + void setCurrentGroup (QObject* currentGroup); signals: - void searchTextChanged(QString searchText); - void currentCategoryChanged(QString category); - void currentGroupChanged(QString group); - void showErrorMessage(const QString& errorMsg); - void showModifiedOnlyChanged(); + void searchTextChanged (QString searchText); + void currentCategoryChanged (void); + void currentGroupChanged (void); + void showModifiedOnlyChanged (void); + void diffOtherVehicleChanged (bool diffOtherVehicle); + void diffMultipleComponentsChanged (bool diffMultipleComponents); + void parametersChanged (void); private slots: - void _updateParameters(void); + void _currentCategoryChanged(void); + void _currentGroupChanged (void); + void _searchTextChanged (void); + void _buildLists (void); + void _buildListsForComponent(int compId); + void _factAdded (int compId, Fact* fact); private: bool _shouldShow(Fact *fact); private: - QStringList _categories; - QString _searchText; - QString _currentCategory; - QString _currentGroup; - QmlObjectListModel* _parameters; - ParameterManager* _parameterMgr; - bool _showModifiedOnly; + ParameterManager* _parameterMgr = nullptr; + QString _searchText; + ParameterEditorCategory* _currentCategory = nullptr; + ParameterEditorGroup* _currentGroup = nullptr; + bool _showModifiedOnly = false; + bool _diffOtherVehicle = false; + bool _diffMultipleComponents = false; + + QmlObjectListModel _categories; + QmlObjectListModel _diffList; + QmlObjectListModel _searchParameters; + QmlObjectListModel* _parameters = nullptr; + QMap _mapCategoryName2Category; }; - -#endif diff --git a/src/QmlControls/QGCTabButton.qml b/src/QmlControls/QGCTabButton.qml index cdda14730b268fff695545267529032208bbdd04..88e56b99351b26688a7042646dd3b6ab80ff75b8 100644 --- a/src/QmlControls/QGCTabButton.qml +++ b/src/QmlControls/QGCTabButton.qml @@ -12,6 +12,7 @@ TabButton { id: control font.pointSize: ScreenTools.defaultFontPointSize font.family: ScreenTools.normalFontFamily + icon.color: _showHighlight ? qgcPal.buttonHighlightText : qgcPal.buttonText property bool _showHighlight: (pressed | hovered | checked) @@ -22,7 +23,6 @@ TabButton { mirrored: control.mirrored display: control.display icon: control.icon - text: control.text font: control.font color: _showHighlight ? qgcPal.buttonHighlightText : qgcPal.buttonText } diff --git a/src/QmlControls/QGroundControl/Controls/qmldir b/src/QmlControls/QGroundControl/Controls/qmldir index 713ef49f5b815d63b67f169c83e0cae06e64abbe..3d5c4ad9d1e2772c7b6bfcebc54ff3ed83850a63 100644 --- a/src/QmlControls/QGroundControl/Controls/qmldir +++ b/src/QmlControls/QGroundControl/Controls/qmldir @@ -43,6 +43,7 @@ MissionItemStatus 1.0 MissionItemStatus.qml ModeSwitchDisplay 1.0 ModeSwitchDisplay.qml MultiRotorMotorDisplay 1.0 MultiRotorMotorDisplay.qml OfflineMapButton 1.0 OfflineMapButton.qml +ParameterDiffDialog 1.0 ParameterDiffDialog.qml ParameterEditor 1.0 ParameterEditor.qml ParameterEditorDialog 1.0 ParameterEditorDialog.qml PIDTuning 1.0 PIDTuning.qml diff --git a/src/Vehicle/CompInfoParam.cc b/src/Vehicle/CompInfoParam.cc index 2a3380a07010ecab00e1a7383663f5191863a8f6..53a09cbf7bff755c02797305946c1db2ca011122 100644 --- a/src/Vehicle/CompInfoParam.cc +++ b/src/Vehicle/CompInfoParam.cc @@ -89,9 +89,14 @@ FactMetaData* CompInfoParam::factMetaDataForName(const QString& name, FactMetaDa { FactMetaData* factMetaData = nullptr; - if (_opaqueParameterMetaData) { - factMetaData = vehicle->firmwarePlugin()->_getMetaDataForFact(_opaqueParameterMetaData, name, type, vehicle->vehicleType()); - } else { + if (_noJsonMetadata) { + QObject* opaqueMetaData = _getOpaqueParameterMetaData(); + if (opaqueMetaData) { + factMetaData = vehicle->firmwarePlugin()->_getMetaDataForFact(opaqueMetaData, name, type, vehicle->vehicleType()); + } + } + + if (!factMetaData) { if (_nameToMetaDataMap.contains(name)) { factMetaData = _nameToMetaDataMap[name]; } else { @@ -122,22 +127,31 @@ FactMetaData* CompInfoParam::factMetaDataForName(const QString& name, FactMetaDa if (!factMetaData) { factMetaData = new FactMetaData(type, this); + int i = name.indexOf("_"); + if (i > 0) { + factMetaData->setGroup(name.left(i)); + } + if (compId != MAV_COMP_ID_AUTOPILOT1) { + factMetaData->setCategory(tr("Component %1").arg(compId)); + } } _nameToMetaDataMap[name] = factMetaData; } } - return factMetaData; } bool CompInfoParam::_isParameterVolatile(const QString& name) { - if (_opaqueParameterMetaData) { - return vehicle->firmwarePlugin()->_isParameterVolatile(_opaqueParameterMetaData, name, vehicle->vehicleType()); - } else { - return _nameToMetaDataMap.contains(name) ? _nameToMetaDataMap[name]->volatileValue() : false; + if (_noJsonMetadata) { + QObject* opaqueMetaData = _getOpaqueParameterMetaData(); + if (opaqueMetaData) { + return vehicle->firmwarePlugin()->_isParameterVolatile(opaqueMetaData, name, vehicle->vehicleType()); + } } + + return _nameToMetaDataMap.contains(name) ? _nameToMetaDataMap[name]->volatileValue() : false; } FirmwarePlugin* CompInfoParam::_anyVehicleTypeFirmwarePlugin(MAV_AUTOPILOT firmwareType) @@ -148,13 +162,14 @@ FirmwarePlugin* CompInfoParam::_anyVehicleTypeFirmwarePlugin(MAV_AUTOPILOT firmw return pluginMgr->firmwarePluginForAutopilot(firmwareType, anySupportedVehicleType); } -QString CompInfoParam::_parameterMetaDataFile(Vehicle* vehicle, MAV_AUTOPILOT firmwareType, int wantedMajorVersion, int& majorVersion, int& minorVersion) +QString CompInfoParam::_parameterMetaDataFile(Vehicle* vehicle, MAV_AUTOPILOT firmwareType, int& majorVersion, int& minorVersion) { - bool cacheHit = false; - FirmwarePlugin* plugin = _anyVehicleTypeFirmwarePlugin(firmwareType); + bool cacheHit = false; + int wantedMajorVersion = 1; + FirmwarePlugin* fwPlugin = _anyVehicleTypeFirmwarePlugin(firmwareType); if (firmwareType != MAV_AUTOPILOT_PX4) { - return plugin->_internalParameterMetaDataFile(vehicle); + return fwPlugin->_internalParameterMetaDataFile(vehicle); } else { // Only PX4 support the old style cached metadata QSettings settings; @@ -164,7 +179,7 @@ QString CompInfoParam::_parameterMetaDataFile(Vehicle* vehicle, MAV_AUTOPILOT fi int cacheMinorVersion, cacheMajorVersion; QFile cacheFile(cacheDir.filePath(QString("%1.%2.%3.xml").arg(_cachedMetaDataFilePrefix).arg(firmwareType).arg(wantedMajorVersion))); if (cacheFile.exists()) { - plugin->_getParameterMetaDataVersionInfo(cacheFile.fileName(), cacheMajorVersion, cacheMinorVersion); + fwPlugin->_getParameterMetaDataVersionInfo(cacheFile.fileName(), cacheMajorVersion, cacheMinorVersion); if (wantedMajorVersion != cacheMajorVersion) { qWarning() << "Parameter meta data cache corruption:" << cacheFile.fileName() << "major version does not match file name" << "actual:excepted" << cacheMajorVersion << wantedMajorVersion; } else { @@ -196,7 +211,7 @@ QString CompInfoParam::_parameterMetaDataFile(Vehicle* vehicle, MAV_AUTOPILOT fi // We have a cache hit on a lower major version, read minor version as well int majorVersion; cacheFile.setFileName(cacheDir.filePath(cacheHits[cacheHitIndex])); - plugin->_getParameterMetaDataVersionInfo(cacheFile.fileName(), majorVersion, cacheMinorVersion); + fwPlugin->_getParameterMetaDataVersionInfo(cacheFile.fileName(), majorVersion, cacheMinorVersion); if (majorVersion != cacheMajorVersion) { qWarning() << "Parameter meta data cache corruption:" << cacheFile.fileName() << "major version does not match file name" << "actual:excepted" << majorVersion << cacheMajorVersion; cacheHit = false; @@ -208,8 +223,8 @@ QString CompInfoParam::_parameterMetaDataFile(Vehicle* vehicle, MAV_AUTOPILOT fi } int internalMinorVersion, internalMajorVersion; - QString internalMetaDataFile = plugin->_internalParameterMetaDataFile(vehicle); - plugin->_getParameterMetaDataVersionInfo(internalMetaDataFile, internalMajorVersion, internalMinorVersion); + QString internalMetaDataFile = fwPlugin->_internalParameterMetaDataFile(vehicle); + fwPlugin->_getParameterMetaDataVersionInfo(internalMetaDataFile, internalMajorVersion, internalMinorVersion); qCDebug(CompInfoParamLog) << "Internal metadata file:major:minor" << internalMetaDataFile << internalMajorVersion << internalMinorVersion; if (cacheHit) { // Cache hit is available, we need to check if internal meta data is a better match, if so use internal version @@ -254,11 +269,15 @@ void CompInfoParam::_cachePX4MetaDataFile(const QString& metaDataFile) int newMajorVersion, newMinorVersion; plugin->_getParameterMetaDataVersionInfo(metaDataFile, newMajorVersion, newMinorVersion); + if (newMajorVersion != 1) { + newMajorVersion = 1; + qgcApp()->showAppMessage(tr("Internal Error: Parameter MetaData major must be 1")); + } qCDebug(CompInfoParamLog) << "ParameterManager::cacheMetaDataFile file:major;minor" << metaDataFile << newMajorVersion << newMinorVersion; // Find the cache hit closest to this new file int cacheMajorVersion, cacheMinorVersion; - QString cacheHit = _parameterMetaDataFile(nullptr, MAV_AUTOPILOT_PX4, newMajorVersion, cacheMajorVersion, cacheMinorVersion); + QString cacheHit = _parameterMetaDataFile(nullptr, MAV_AUTOPILOT_PX4, cacheMajorVersion, cacheMinorVersion); qCDebug(CompInfoParamLog) << "ParameterManager::cacheMetaDataFile cacheHit file:firmware:major;minor" << cacheHit << cacheMajorVersion << cacheMinorVersion; bool cacheNewFile = false; @@ -290,28 +309,19 @@ void CompInfoParam::_cachePX4MetaDataFile(const QString& metaDataFile) } } -void CompInfoParam::_parameterMajorVersionKnown(int wantedMajorVersion) +QObject* CompInfoParam::_getOpaqueParameterMetaData(void) { - if (_noJsonMetadata) { - if (_opaqueParameterMetaData) { - return; - } - - QString metaDataFile; - int majorVersion, minorVersion; + if (!_noJsonMetadata) { + qWarning() << "CompInfoParam::_getOpaqueParameterMetaData _noJsonMetadata == false"; + } + if (!_opaqueParameterMetaData && compId == MAV_COMP_ID_AUTOPILOT1) { // Load best parameter meta data set - metaDataFile = _parameterMetaDataFile(vehicle, vehicle->firmwareType(), wantedMajorVersion, majorVersion, minorVersion); - qCDebug(CompInfoParamLog) << "Loading meta data the old way file:major:minor" << metaDataFile << majorVersion << minorVersion; + int majorVersion, minorVersion; + QString metaDataFile = _parameterMetaDataFile(vehicle, vehicle->firmwareType(), majorVersion, minorVersion); + qCDebug(CompInfoParamLog) << "Loading meta data the old way file" << metaDataFile; _opaqueParameterMetaData = vehicle->firmwarePlugin()->_loadParameterMetaData(metaDataFile); } -} -void CompInfoParam::_clearPX4ParameterMetaData (void) -{ - if (_opaqueParameterMetaData) { - qCDebug(CompInfoParamLog) << "_clearPX4ParameterMetaData"; - _opaqueParameterMetaData->deleteLater(); - _opaqueParameterMetaData = nullptr; - } + return _opaqueParameterMetaData; } diff --git a/src/Vehicle/CompInfoParam.h b/src/Vehicle/CompInfoParam.h index 118cba9362ad0a2c955c2922b16d83ea380819a6..f7f0cc5364e264f2c8e0eb7dc14d678a03b2c300 100644 --- a/src/Vehicle/CompInfoParam.h +++ b/src/Vehicle/CompInfoParam.h @@ -36,13 +36,14 @@ public: // The following methods are used to support the old non-COMPONENT_INFORMATION based mechanism to get parameter meta data bool _isParameterVolatile (const QString& name); - void _parameterMajorVersionKnown(int wantedMajorVersion); - void _clearPX4ParameterMetaData (void); + static void _cachePX4MetaDataFile(const QString& metaDataFile); private: + QObject* _getOpaqueParameterMetaData(void); + static FirmwarePlugin* _anyVehicleTypeFirmwarePlugin (MAV_AUTOPILOT firmwareType); - static QString _parameterMetaDataFile (Vehicle* vehicle, MAV_AUTOPILOT firmwareType, int wantedMajorVersion, int& majorVersion, int& minorVersion); + static QString _parameterMetaDataFile (Vehicle* vehicle, MAV_AUTOPILOT firmwareType, int& majorVersion, int& minorVersion); typedef QPair RegexFactMetaDataPair_t; diff --git a/src/Vehicle/ComponentInformationManager.cc b/src/Vehicle/ComponentInformationManager.cc index 77397f2e16321d2e476fa139bf9aaa61d6656d5c..b547a489d18a97ee7141997e02e53e06d3cfefaa 100644 --- a/src/Vehicle/ComponentInformationManager.cc +++ b/src/Vehicle/ComponentInformationManager.cc @@ -102,7 +102,11 @@ bool ComponentInformationManager::_isCompTypeSupported(COMP_METADATA_TYPE type) CompInfoParam* ComponentInformationManager::compInfoParam(uint8_t compId) { - return _compInfoMap.contains(compId) && _compInfoMap[compId].contains(COMP_METADATA_TYPE_PARAMETER) ? qobject_cast(_compInfoMap[compId][COMP_METADATA_TYPE_PARAMETER]) : nullptr; + if (!_compInfoMap.contains(compId)) { + // Create default info + _compInfoMap[compId][COMP_METADATA_TYPE_PARAMETER] = new CompInfoParam(compId, _vehicle, this); + } + return qobject_cast(_compInfoMap[compId][COMP_METADATA_TYPE_PARAMETER]); } CompInfoVersion* ComponentInformationManager::compInfoVersion(uint8_t compId) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index d9f98063364deccc2f9efc75990ea2e3d1e58b6b..cd7089de75a688b26a95fb8f75b6e968c9a2ced7 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -601,6 +601,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes return; } _ftpManager->mavlinkMessageReceived(message); + _parameterManager->mavlinkMessageReceived(message); _waitForMavlinkMessageMessageReceived(message); @@ -2616,7 +2617,7 @@ void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool s emit mavCommandResult(_id, targetCompId, command, MAV_RESULT_FAILED, failureCode); } if (showError) { - qgcApp()->showAppMessage(tr("Unabled to send command: %1. %2").arg(compIdAll ? tr("Internal error") : tr("Waiting on previous response to same command."))); + qgcApp()->showAppMessage(tr("Unable to send command: %1.").arg(compIdAll ? tr("Internal error - MAV_COMP_ID_ALL not supported") : tr("Waiting on previous response to same command."))); } return; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index b306fc81670dea1ecf5badbbdc46be3003f3c4b8..71e785363a2846053c6563efe2143c4a87ddf718 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -633,7 +633,7 @@ public: typedef enum { MavCmdResultCommandResultOnly, ///< commandResult specifies full success/fail info MavCmdResultFailureNoResponseToCommand, ///< No response from vehicle to command - MavCmdResultFailureDuplicateCommand, ///< Unabled to send command since duplicate is already being waited on for response + MavCmdResultFailureDuplicateCommand, ///< Unable to send command since duplicate is already being waited on for response } MavCmdResultFailureCode_t; /// Callback for sendMavCommandWithHandler diff --git a/src/VehicleSetup/FirmwareImage.cc b/src/VehicleSetup/FirmwareImage.cc index 33c00a4a370be3fa327d0280680cd2bda8e2118d..e833bbcf1aa94df0a1f245b62aed5d68b68c38c7 100644 --- a/src/VehicleSetup/FirmwareImage.cc +++ b/src/VehicleSetup/FirmwareImage.cc @@ -238,7 +238,7 @@ bool FirmwareImage::_px4Load(const QString& imageFilename) QStringList requiredKeys; requiredKeys << _jsonBoardIdKey << _jsonImageKey << _jsonImageSizeKey; if (!JsonHelper::validateRequiredKeys(px4Json, requiredKeys, errorString)) { - emit statusMessage(tr("Firmware file mission required key: %1").arg(errorString)); + emit statusMessage(tr("Firmware file missing required key: %1").arg(errorString)); return false; } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 59a61209dcf35c0da8c79c7ad2fc2da0f8b41a35..d09fb1c758ad0f56329d18ffc31be9294e93cc63 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -1567,13 +1567,15 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request, bool switch ((int)request.param1) { case MAVLINK_MSG_ID_COMPONENT_INFORMATION: - switch (static_cast(request.param2)) { - case COMP_METADATA_TYPE_VERSION: - _sendVersionMetaData(); - return true; - case COMP_METADATA_TYPE_PARAMETER: - _sendParameterMetaData(); - return true; + if (_firmwareType == MAV_AUTOPILOT_PX4) { + switch (static_cast(request.param2)) { + case COMP_METADATA_TYPE_VERSION: + _sendVersionMetaData(); + return true; + case COMP_METADATA_TYPE_PARAMETER: + _sendParameterMetaData(); + return true; + } } break; case MAVLINK_MSG_ID_DEBUG: diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 0a7928cfc6ba071f8d3aa835a6e61cfa106de731..c1d948d276cee444f94f24409a498f398fb77ce1 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -145,21 +145,6 @@ void UAS::receiveMessage(mavlink_message_t message) { switch (message.msgid) { - case MAVLINK_MSG_ID_PARAM_VALUE: - { - mavlink_param_value_t rawValue; - mavlink_msg_param_value_decode(&message, &rawValue); - QByteArray bytes(rawValue.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - // Construct a string stopping at the first NUL (0) character, else copy the whole - // byte array (max MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN, so safe) - QString parameterName(bytes); - mavlink_param_union_t paramVal; - paramVal.param_float = rawValue.param_value; - paramVal.type = rawValue.param_type; - - processParamValueMsg(message, parameterName,rawValue,paramVal); - } - break; case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: { @@ -524,61 +509,6 @@ quint64 UAS::getUptime() const } } -//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; - - QVariant paramValue; - - // Insert with correct type - - switch (rawValue.param_type) { - case MAV_PARAM_TYPE_REAL32: - paramValue = QVariant(paramUnion.param_float); - break; - - case MAV_PARAM_TYPE_UINT8: - paramValue = QVariant(paramUnion.param_uint8); - break; - - case MAV_PARAM_TYPE_INT8: - paramValue = QVariant(paramUnion.param_int8); - break; - - case MAV_PARAM_TYPE_UINT16: - paramValue = QVariant(paramUnion.param_uint16); - break; - - case MAV_PARAM_TYPE_INT16: - paramValue = QVariant(paramUnion.param_int16); - break; - - case MAV_PARAM_TYPE_UINT32: - paramValue = QVariant(paramUnion.param_uint32); - break; - - case MAV_PARAM_TYPE_INT32: - paramValue = QVariant(paramUnion.param_int32); - break; - - //-- Note: These are not handled above: - // - // MAV_PARAM_TYPE_UINT64 - // MAV_PARAM_TYPE_INT64 - // MAV_PARAM_TYPE_REAL64 - // - // No space in message (the only storage allocation is a "float") and not present in mavlink_param_union_t - - default: - qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; - } - - qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type; - - emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); -} - /** * Order the robot to start receiver pairing */ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 7504a0b56136d883aa4e2f2190b6ee1c3f44f927..969feb2dc529efdbb8cd469a8e078e58fe8035c0 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -196,8 +196,6 @@ protected: /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ quint64 getUnixReferenceTime(quint64 time); - virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); - bool connectionLost; ///< Flag indicates a timed out connection quint64 connectionLossTime; ///< Time the connection was interrupted quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 601a15497ef404b68b7bce53537d434fb0c21afc..0bfbc45642edbd0249a56bbab7474aea74efb65f 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -49,8 +49,6 @@ signals: /** @brief The robot is disconnected **/ void disconnected(); - 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/ui/QGCMapRCToParamDialog.cpp b/src/ui/QGCMapRCToParamDialog.cpp deleted file mode 100644 index 61291b738d56a6603e79e63ac021b716cfde813c..0000000000000000000000000000000000000000 --- a/src/ui/QGCMapRCToParamDialog.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/// @file -/// @author Thomas Gubler - -#include "QGCMapRCToParamDialog.h" -#include "ui_QGCMapRCToParamDialog.h" -#include "ParameterManager.h" - -#include -#include -#include -#include -#include - -QGCMapRCToParamDialog::QGCMapRCToParamDialog(QString param_id, UASInterface *mav, MultiVehicleManager* multiVehicleManager, QWidget *parent) - : QDialog(parent) - , param_id(param_id) - , mav(mav) - , _multiVehicleManager(multiVehicleManager) - , ui(new Ui::QGCMapRCToParamDialog) -{ - ui->setupUi(this); - - // refresh the parameter from onboard to make sure the current value is used - Vehicle* vehicle = _multiVehicleManager->getVehicleById(mav->getUASID()); - Fact* paramFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, param_id); - - ui->minValueDoubleSpinBox->setValue(paramFact->rawMin().toDouble()); - ui->maxValueDoubleSpinBox->setValue(paramFact->rawMax().toDouble()); - - // only enable ok button when param was refreshed - QPushButton *okButton = ui->buttonBox->button(QDialogButtonBox::Ok); - okButton->setEnabled(false); - - ui->paramIdLabel->setText(param_id); - - connect(paramFact, &Fact::vehicleUpdated, this, &QGCMapRCToParamDialog::_parameterUpdated); - vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, param_id); -} - -QGCMapRCToParamDialog::~QGCMapRCToParamDialog() -{ - delete ui; -} - -void QGCMapRCToParamDialog::accept() { - emit mapRCToParamDialogResult(param_id, - (float)ui->scaleDoubleSpinBox->value(), - (float)ui->value0DoubleSpinBox->value(), - (quint8)ui->rcParamChannelComboBox->currentIndex(), - (float)ui->minValueDoubleSpinBox->value(), - (float)ui->maxValueDoubleSpinBox->value()); - - QDialog::accept(); -} - -void QGCMapRCToParamDialog::_parameterUpdated(QVariant value) -{ - Q_UNUSED(value); - - ui->infoLabel->setText("Parameter value is up to date"); - ui->value0DoubleSpinBox->setValue(value.toDouble()); - ui->value0DoubleSpinBox->setEnabled(true); - - connect(this, &QGCMapRCToParamDialog::mapRCToParamDialogResult, mav, &UASInterface::sendMapRCToParam); - QPushButton *okButton = ui->buttonBox->button(QDialogButtonBox::Ok); - okButton->setEnabled(true); -} diff --git a/src/ui/QGCMapRCToParamDialog.h b/src/ui/QGCMapRCToParamDialog.h deleted file mode 100644 index 56a46bc3d2641add668032bb81c0ecf2c42dd83e..0000000000000000000000000000000000000000 --- a/src/ui/QGCMapRCToParamDialog.h +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2018 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/// @file -/// @brief Dialog to configure RC to parameter mapping -/// @author Thomas Gubler - -#pragma once - -#include -#include - -#include "UASInterface.h" -#include "AutoPilotPlugin.h" -#include "MultiVehicleManager.h" - -namespace Ui { -class QGCMapRCToParamDialog; -} - - - -class QGCMapRCToParamDialog : public QDialog -{ - Q_OBJECT - QThread paramLoadThread; - -public: - explicit QGCMapRCToParamDialog(QString param_id, UASInterface *mav, MultiVehicleManager* multiVehicleManager, QWidget *parent = 0); - ~QGCMapRCToParamDialog(); - -signals: - void mapRCToParamDialogResult(QString param_id, float scale, float value0, - quint8 param_rc_channel_index, float valueMin, float valueMax); - -public slots: - void accept(); - -protected: - // void showEvent(QShowEvent * event ); - QString param_id; - UASInterface *mav; - -private slots: - void _parameterUpdated(QVariant value); - -private: - MultiVehicleManager* _multiVehicleManager; - Ui::QGCMapRCToParamDialog* ui; -}; - diff --git a/src/ui/QGCMapRCToParamDialog.ui b/src/ui/QGCMapRCToParamDialog.ui deleted file mode 100644 index c6fbb1efd3067fe04fdb2d83a0f74ef26d88288d..0000000000000000000000000000000000000000 --- a/src/ui/QGCMapRCToParamDialog.ui +++ /dev/null @@ -1,241 +0,0 @@ - - - QGCMapRCToParamDialog - - - - 0 - 0 - 400 - 358 - - - - Dialog - - - - - - QFormLayout::AllNonFixedFieldsGrow - - - - - Bind - - - - - - - Parameter Tuning ID - - - - - - - false - - - 1 - - - - 1 - - - - - 2 - - - - - 3 - - - - - - - - Parameter - - - - - - - TextLabel - - - - - - - with - - - - - - - Scale (keep default) - - - - - - - 1.000000000000000 - - - - - - - Center value - - - - - - - false - - - 8 - - - -100000.000000000000000 - - - 100000.000000000000000 - - - - - - - Minimum Value - - - - - - - 8 - - - -100000.000000000000000 - - - 100000.000000000000000 - - - - - - - Maximum Value - - - - - - - 8 - - - -10000.000000000000000 - - - 100000.000000000000000 - - - 1.000000000000000 - - - 10.000000000000000 - - - - - - - Waiting for parameter refresh,,, - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - Tuning IDs can be mapped to channels in the RC settings - - - - - - - - - Qt::Horizontal - - - QDialogButtonBox::Cancel|QDialogButtonBox::Ok - - - - - - - - - buttonBox - accepted() - QGCMapRCToParamDialog - accept() - - - 248 - 254 - - - 157 - 274 - - - - - buttonBox - rejected() - QGCMapRCToParamDialog - reject() - - - 316 - 260 - - - 286 - 274 - - - - -