Introduce locally cached parameter sync to speed up initial connection

src/FactSystem/ParameterLoader.cc

@@ -35,6 +35,11 @@
 #include <QFile>
 #include <QDebug>
 
+/* types for local parameter cache */
+typedef QPair<int, QVariant> ParamTypeVal;
+typedef QPair<QString, ParamTypeVal> NamedParam;
+typedef QMap<int, NamedParam> MapID2NamedParam;
+
 QGC_LOGGING_CATEGORY(ParameterLoaderLog, "ParameterLoaderLog")
 QGC_LOGGING_CATEGORY(ParameterLoaderVerboseLog, "ParameterLoaderVerboseLog")
 
@@ -60,12 +65,16 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q
     _waitingParamTimeoutTimer.setSingleShot(true);
     _waitingParamTimeoutTimer.setInterval(1000);
     connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_waitingParamTimeout);
+
+    _cacheTimeoutTimer.setSingleShot(true);
+    _cacheTimeoutTimer.setInterval(2500);
+    connect(&_cacheTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::refreshAllParameters);
     
     // FIXME: Why not direct connect?
     connect(_vehicle->uas(), SIGNAL(parameterUpdate(int, int, QString, int, int, int, QVariant)), this, SLOT(_parameterUpdate(int, int, QString, int, int, int, QVariant)));
-    
-    // Request full param list
-    refreshAllParameters();
+
+    /* Initially attempt a local cache load, refresh over the link if it fails */
+    _tryCacheLookup();
 }
 
 ParameterLoader::~ParameterLoader()
@@ -100,7 +109,13 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
         return;
     }
 #endif
-    
+
+    if (parameterName == "_HASH_CHECK") {
+        /* we received a cache hash, potentially load from cache */
+        _cacheTimeoutTimer.stop();
+        _tryCacheHashLoad(uasId, value);
+        return;
+    }
     _dataMutex.lock();
     
     // Restart our waiting for param timer
@@ -111,6 +126,8 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
         _paramCountMap[componentId] = parameterCount;
         _totalParamCount += parameterCount;
     }
+
+    _mapParameterId2Name[componentId][parameterId] = parameterName;
     
     // If we've never seen this component id before, setup the wait lists.
     if (!_waitingReadParamIndexMap.contains(componentId)) {
@@ -245,6 +262,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();
     }
     
     _checkInitialLoadComplete();
@@ -483,6 +501,19 @@ 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 = MAVLinkProtocol::instance();
+    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->sendMessage(msg);
+}
+
 void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex)
 {
     mavlink_message_t msg;
@@ -552,6 +583,70 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
     _vehicle->sendMessage(msg);
 }
 
+void ParameterLoader::_writeLocalParamCache()
+{
+    QMap<int, 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<Fact*>();
+            cache_map[component][id] = NamedParam(name, ParamTypeVal(fact->type(), fact->value()));
+        }
+    }
+
+    QFile cache_file(QFileInfo(QSettings().fileName()).path() + QDir::separator() + "param_cache");
+    cache_file.open(QIODevice::WriteOnly | QIODevice::Truncate);
+
+    QDataStream ds(&cache_file);
+    ds << cache_map;
+}
+
+void ParameterLoader::_tryCacheHashLoad(int uasId, QVariant hash_value)
+{
+    uint32_t crc32_value = 0;
+    /* The datastructure of the cache table */
+    QMap<int, MapID2NamedParam> cache_map;
+
+    QFile cache_file(QFileInfo(QSettings().fileName()).path() + QDir::separator() + "param_cache");
+    if (!cache_file.exists()) {
+        /* no local cache, immediately refresh all params */
+        refreshAllParameters();
+        return;
+    }
+    cache_file.open(QIODevice::ReadOnly);
+
+    /* Deserialize the parameter cache table */
+    QDataStream ds(&cache_file);
+    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);
+        }
+    }
+
+    if (crc32_value == hash_value.toUInt()) {
+        /* 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 = cache_map[component][id].second.first;
+                _parameterUpdate(uasId, component, name, count, id, mavType, value);
+            }
+        }
+    } else {
+        /* cache and remote hashes differ. Immediately request all params */
+        refreshAllParameters();
+    }
+}
+
 void ParameterLoader::_saveToEEPROM(void)
 {
     if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) {

src/FactSystem/ParameterLoader.h

@@ -55,10 +55,12 @@ public:
     
     /// Returns true if the full set of facts are ready
     bool parametersAreReady(void) { return _parametersReady; }
-    
+
+public slots:
     /// Re-request the full set of parameters from the autopilot
     void refreshAllParameters(void);
-    
+
+public:
     /// Request a refresh on the specific parameter
     void refreshParameter(int componentId, const QString& name);
     
@@ -109,6 +111,7 @@ private slots:
     void _valueUpdated(const QVariant& value);
     void _restartWaitingParamTimer(void);
     void _waitingParamTimeout(void);
+    void _tryCacheLookup(void);
     
 private:
     static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false);
@@ -117,6 +120,9 @@ 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);
+
     MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType);
     FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType);
     void _saveToEEPROM(void);
@@ -128,7 +134,8 @@ private:
     
     /// First mapping is by component id
     /// Second mapping is parameter name, to Fact* in QVariant
-    QMap<int, QVariantMap> _mapParameterName2Variant;
+    QMap<int, QVariantMap>            _mapParameterName2Variant;
+    QMap<int, QMap<int, QString> >    _mapParameterId2Name;
     
     /// First mapping is by component id
     /// Second mapping is group name, to Fact
@@ -150,10 +157,11 @@ private:
     int _totalParamCount;   ///< Number of parameters across all components
     
     QTimer _waitingParamTimeoutTimer;
+    QTimer _cacheTimeoutTimer;
     
     QMutex _dataMutex;
     
     static Fact _defaultFact;   ///< Used to return default fact, when parameter not found
 };
 
-#endif
+#endif