diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index dc79a8fb7dd74172080e8e320cc7b8977946361d..b1c3161cb7b52d58529a5c0007345ab6a363f8d0 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 1a1c92a2372b512976147e34408c6693f697d8dd..41c7d264b5b91e829af9b11aca18bcf5443cca77 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 74152027a3bd455869e9566399a406e75136d923..47cc29a4b3d3954e05da2c16a4be37003382c4fa 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 7c28709e1f3ca520e2ed046277314b9e95628e3b..cdd71b0f7d6dca05ebad66c5bd72d4ab695682ec 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 72bf6b06c8b1c89951fd0d1ecf6249fc5a6cbdb7..ca6bdc36487176108f7a0986585e8b14cd87d949 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());