From 80ef4da8297e8e9856353f5ed603c1c5543f4297 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Mon, 15 Feb 2016 18:10:59 -0500 Subject: [PATCH 1/3] New parameter cache loading spec - Request list of all parameters, expecting device to lead each component with _HASH_CHECK cache value - Test for hash collision, if so our cache is valid and respond with _HASH_CHECK to stop listing - Else: let listing continue as normal to get updated param values - Store each cache by systemID + componentID For #2586 --- src/FactSystem/FactMetaData.cc | 25 +++++++++++ src/FactSystem/FactMetaData.h | 1 + src/FactSystem/ParameterLoader.cc | 90 ++++++++++++++++++++++----------------- src/FactSystem/ParameterLoader.h | 9 ++-- src/QGCApplication.cc | 4 +- 5 files changed, 86 insertions(+), 43 deletions(-) diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index dc79a8f..b1c3161 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -434,3 +434,28 @@ FactMetaData::ValueType_t FactMetaData::stringToType(const QString& typeString, return valueTypeDouble; } + +size_t FactMetaData::typeToSize(ValueType_t type) +{ + switch (type) { + case valueTypeUint8: + case valueTypeInt8: + return 1; + + case valueTypeUint16: + case valueTypeInt16: + return 2; + + case valueTypeUint32: + case valueTypeInt32: + case valueTypeFloat: + return 4; + + case valueTypeDouble: + return 8; + + default: + qWarning() << "Unsupported fact value type" << type; + return 4; + } +} diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 1a1c92a..41c7d26 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -121,6 +121,7 @@ public: static const int defaultDecimalPlaces = 3; static ValueType_t stringToType(const QString& typeString, bool& unknownType); + static size_t typeToSize(ValueType_t); private: QVariant _minForType(void) const; diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 7415202..47cc29a 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -78,8 +78,9 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q connect(_vehicle->uas(), &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate); - /* Initially attempt a local cache load, refresh over the link if it fails */ - _tryCacheLookup(); + // Ensure the cache directory exists + QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache"); + refreshAllParameters(); } ParameterLoader::~ParameterLoader() @@ -120,7 +121,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param if (parameterName == "_HASH_CHECK") { /* we received a cache hash, potentially load from cache */ _cacheTimeoutTimer.stop(); - _tryCacheHashLoad(uasId, value); + _tryCacheHashLoad(uasId, componentId, value); return; } _dataMutex.lock(); @@ -269,7 +270,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param if (waitingParamCount == 0) { // Now that we know vehicle is up to date persist _saveToEEPROM(); - _writeLocalParamCache(); + _writeLocalParamCache(uasId, componentId); } _checkInitialLoadComplete(); @@ -604,40 +605,42 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa _vehicle->sendMessageOnLink(_dedicatedLink, msg); } -void ParameterLoader::_writeLocalParamCache() +void ParameterLoader::_writeLocalParamCache(int uasId, int componentId) { - QMap cache_map; + MapID2NamedParam cache_map; - foreach(int component, _mapParameterId2Name.keys()) { - foreach(int id, _mapParameterId2Name[component].keys()) { - const QString name(_mapParameterId2Name[component][id]); - const Fact *fact = _mapParameterName2Variant[component][name].value(); - cache_map[component][id] = NamedParam(name, ParamTypeVal(fact->type(), fact->rawValue())); - } + foreach(int id, _mapParameterId2Name[componentId].keys()) { + const QString name(_mapParameterId2Name[componentId][id]); + const Fact *fact = _mapParameterName2Variant[componentId][name].value(); + cache_map[id] = NamedParam(name, ParamTypeVal(fact->type(), fact->rawValue())); } - QFile cache_file(QFileInfo(QSettings().fileName()).path() + QDir::separator() + "param_cache"); + QFile cache_file(parameterCacheFile(uasId, componentId)); cache_file.open(QIODevice::WriteOnly | QIODevice::Truncate); QDataStream ds(&cache_file); ds << cache_map; } -QString ParameterLoader::parameterCacheFile(void) +QDir ParameterLoader::parameterCacheDir() +{ + const QString spath(QFileInfo(QSettings().fileName()).dir().absolutePath()); + return spath + QDir::separator() + "ParamCache"; +} + +QString ParameterLoader::parameterCacheFile(int uasId, int componentId) { - const QDir settingsDir(QFileInfo(QSettings().fileName()).dir()); - return settingsDir.filePath("param_cache"); + return parameterCacheDir().filePath(QString("%1_%2").arg(uasId).arg(componentId)); } -void ParameterLoader::_tryCacheHashLoad(int uasId, QVariant hash_value) +void ParameterLoader::_tryCacheHashLoad(int uasId, int componentId, QVariant hash_value) { uint32_t crc32_value = 0; /* The datastructure of the cache table */ - QMap cache_map; - QFile cache_file(parameterCacheFile()); + MapID2NamedParam cache_map; + QFile cache_file(parameterCacheFile(uasId, componentId)); if (!cache_file.exists()) { - /* no local cache, immediately refresh all params */ - refreshAllParameters(); + /* no local cache, just wait for them to come in*/ return; } cache_file.open(QIODevice::ReadOnly); @@ -647,29 +650,38 @@ void ParameterLoader::_tryCacheHashLoad(int uasId, QVariant hash_value) ds >> cache_map; /* compute the crc of the local cache to check against the remote */ - foreach(int component, cache_map.keys()) { - foreach(int id, cache_map[component].keys()) { - const QString name(cache_map[component][id].first); - const void *vdat = cache_map[component][id].second.second.constData(); - crc32_value = QGC::crc32((const uint8_t *)qPrintable(name), name.length(), crc32_value); - crc32_value = QGC::crc32((const uint8_t *)vdat, sizeof(uint32_t), crc32_value); - } + + foreach(int id, cache_map.keys()) { + const QString name(cache_map[id].first); + const void *vdat = cache_map[id].second.second.constData(); + const FactMetaData::ValueType_t fact_type = static_cast(cache_map[id].second.first); + crc32_value = QGC::crc32((const uint8_t *)qPrintable(name), name.length(), crc32_value); + crc32_value = QGC::crc32((const uint8_t *)vdat, FactMetaData::typeToSize(fact_type), crc32_value); } if (crc32_value == hash_value.toUInt()) { + qCInfo(ParameterLoaderLog) << "Parameters loaded from cache" << qPrintable(QFileInfo(cache_file).absoluteFilePath()); /* if the two param set hashes match, just load from the disk */ - foreach(int component, cache_map.keys()) { - int count = cache_map[component].count(); - foreach(int id, cache_map[component].keys()) { - const QString &name = cache_map[component][id].first; - const QVariant &value = cache_map[component][id].second.second; - const int mavType = _factTypeToMavType(static_cast(cache_map[component][id].second.first)); - _parameterUpdate(uasId, component, name, count, id, mavType, value); - } + int count = cache_map.count(); + foreach(int id, cache_map.keys()) { + const QString &name = cache_map[id].first; + const QVariant &value = cache_map[id].second.second; + const FactMetaData::ValueType_t fact_type = static_cast(cache_map[id].second.first); + const int mavType = _factTypeToMavType(fact_type); + _parameterUpdate(uasId, componentId, name, count, id, mavType, value); } - } else { - /* cache and remote hashes differ. Immediately request all params */ - refreshAllParameters(); + // Return the hash value to notify we don't want any more updates + mavlink_param_set_t p; + mavlink_param_union_t union_value; + p.param_type = MAV_PARAM_TYPE_UINT32; + strncpy(p.param_id, "_HASH_CHECK", sizeof(p.param_id)); + union_value.param_uint32 = crc32_value; + p.param_value = union_value.param_float; + p.target_system = (uint8_t)_vehicle->id(); + p.target_component = (uint8_t)componentId; + mavlink_message_t msg; + mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } } diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 7c28709..cdd71b0 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -53,8 +53,11 @@ public: ~ParameterLoader(); + /// @return Directory of parameter caches + static QDir parameterCacheDir(); + /// @return Location of parameter cache file - static QString parameterCacheFile(void); + static QString parameterCacheFile(int uasId, int componentId); /// Returns true if the full set of facts are ready bool parametersAreReady(void) { return _parametersReady; } @@ -120,8 +123,8 @@ private: void _setupGroupMap(void); void _readParameterRaw(int componentId, const QString& paramName, int paramIndex); void _writeParameterRaw(int componentId, const QString& paramName, const QVariant& value); - void _writeLocalParamCache(); - void _tryCacheHashLoad(int uasId, QVariant hash_value); + void _writeLocalParamCache(int uasId, int componentId); + void _tryCacheHashLoad(int uasId, int componentId, QVariant hash_value); MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType); FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType); diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 72bf6b0..ca6bdc3 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -374,7 +374,9 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) QFile::remove(PX4AirframeLoader::aiframeMetaDataFile()); QFile::remove(PX4ParameterMetaData::parameterMetaDataFile()); - QFile::remove(ParameterLoader::parameterCacheFile()); + QDir paramDir(ParameterLoader::parameterCacheDir()); + paramDir.removeRecursively(); + paramDir.mkpath(paramDir.absolutePath()); } _lastKnownHomePosition.setLatitude(settings.value(_lastKnownHomePositionLatKey, 37.803784).toDouble()); From 823b9a6debde88da38cca4f3d9af2572cde25984 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Thu, 18 Feb 2016 00:47:09 -0500 Subject: [PATCH 2/3] Include type arg in method prototype --- src/FactSystem/FactMetaData.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 41c7d26..9a3661f 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -121,7 +121,7 @@ public: static const int defaultDecimalPlaces = 3; static ValueType_t stringToType(const QString& typeString, bool& unknownType); - static size_t typeToSize(ValueType_t); + static size_t typeToSize(ValueType_t type); private: QVariant _minForType(void) const; From 708096b590d6d7d4d6d0bff8077ceb78f5f73da5 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Thu, 18 Feb 2016 10:50:49 -0500 Subject: [PATCH 3/3] Remove dead parameter cache code Conflicts: src/FactSystem/ParameterLoader.cc --- src/FactSystem/ParameterLoader.cc | 24 ------------------------ src/FactSystem/ParameterLoader.h | 4 ---- 2 files changed, 28 deletions(-) diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 47cc29a..c07f40e 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -72,10 +72,6 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q _waitingParamTimeoutTimer.setInterval(1000); connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_waitingParamTimeout); - _cacheTimeoutTimer.setSingleShot(true); - _cacheTimeoutTimer.setInterval(2500); - connect(&_cacheTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_timeoutRefreshAll); - connect(_vehicle->uas(), &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate); // Ensure the cache directory exists @@ -120,7 +116,6 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param if (parameterName == "_HASH_CHECK") { /* we received a cache hash, potentially load from cache */ - _cacheTimeoutTimer.stop(); _tryCacheHashLoad(uasId, componentId, value); return; } @@ -523,19 +518,6 @@ Out: } } -void ParameterLoader::_tryCacheLookup() -{ - /* Start waiting for 2.5 seconds to get a cache hit and avoid loading all params over the radio */ - _cacheTimeoutTimer.start(); - - MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - Q_ASSERT(mavlink); - - mavlink_message_t msg; - mavlink_msg_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL, "_HASH_CHECK", -1); - _vehicle->sendMessageOnLink(_dedicatedLink, msg); -} - void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex) { mavlink_message_t msg; @@ -913,9 +895,3 @@ void ParameterLoader::_initialRequestTimeout(void) refreshAllParameters(); _initialRequestTimeoutTimer.start(); } - -void ParameterLoader::_timeoutRefreshAll() -{ - refreshAllParameters(); -} - diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index cdd71b0..48638d5 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -112,9 +112,6 @@ protected: void _waitingParamTimeout(void); void _tryCacheLookup(void); void _initialRequestTimeout(void); - -private slots: - void _timeoutRefreshAll(); private: static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); @@ -160,7 +157,6 @@ private: QTimer _initialRequestTimeoutTimer; QTimer _waitingParamTimeoutTimer; - QTimer _cacheTimeoutTimer; QMutex _dataMutex;