diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index d43488fa5959eb5ddc311661098ba6b11c03c3fa..a69b25831813093008c547a51e3f297f7fe245f3 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -134,7 +134,7 @@ QString Fact::valueString(void) const return _value.toString(); } -QVariant Fact::defaultValue(void) +QVariant Fact::defaultValue(void) const { if (_metaData) { if (!_metaData->defaultValueAvailable()) { @@ -147,12 +147,12 @@ QVariant Fact::defaultValue(void) } } -FactMetaData::ValueType_t Fact::type(void) +FactMetaData::ValueType_t Fact::type(void) const { return _type; } -QString Fact::shortDescription(void) +QString Fact::shortDescription(void) const { if (_metaData) { return _metaData->shortDescription(); @@ -162,7 +162,7 @@ QString Fact::shortDescription(void) } } -QString Fact::longDescription(void) +QString Fact::longDescription(void) const { if (_metaData) { return _metaData->longDescription(); @@ -172,7 +172,7 @@ QString Fact::longDescription(void) } } -QString Fact::units(void) +QString Fact::units(void) const { if (_metaData) { return _metaData->units(); @@ -182,7 +182,7 @@ QString Fact::units(void) } } -QVariant Fact::min(void) +QVariant Fact::min(void) const { if (_metaData) { return _metaData->min(); @@ -192,7 +192,7 @@ QVariant Fact::min(void) } } -QVariant Fact::max(void) +QVariant Fact::max(void) const { if (_metaData) { return _metaData->max(); @@ -202,7 +202,7 @@ QVariant Fact::max(void) } } -bool Fact::minIsDefaultForType(void) +bool Fact::minIsDefaultForType(void) const { if (_metaData) { return _metaData->minIsDefaultForType(); @@ -212,7 +212,7 @@ bool Fact::minIsDefaultForType(void) } } -bool Fact::maxIsDefaultForType(void) +bool Fact::maxIsDefaultForType(void) const { if (_metaData) { return _metaData->maxIsDefaultForType(); @@ -222,7 +222,7 @@ bool Fact::maxIsDefaultForType(void) } } -QString Fact::group(void) +QString Fact::group(void) const { if (_metaData) { return _metaData->group(); @@ -237,7 +237,7 @@ void Fact::setMetaData(FactMetaData* metaData) _metaData = metaData; } -bool Fact::valueEqualsDefault(void) +bool Fact::valueEqualsDefault(void) const { if (_metaData) { if (_metaData->defaultValueAvailable()) { @@ -251,7 +251,7 @@ bool Fact::valueEqualsDefault(void) } } -bool Fact::defaultValueAvailable(void) +bool Fact::defaultValueAvailable(void) const { if (_metaData) { return _metaData->defaultValueAvailable(); diff --git a/src/FactSystem/Fact.h b/src/FactSystem/Fact.h index 90be94d4a41d842b045a899633ff8adbdda4c844..da8a85efebd290f8e8ffc5f1ea16ce1085354358 100644 --- a/src/FactSystem/Fact.h +++ b/src/FactSystem/Fact.h @@ -74,18 +74,18 @@ public: QVariant value(void) const; QString valueString(void) const; void setValue(const QVariant& value); - QVariant defaultValue(void); - bool defaultValueAvailable(void); - bool valueEqualsDefault(void); - FactMetaData::ValueType_t type(void); - QString shortDescription(void); - QString longDescription(void); - QString units(void); - QVariant min(void); - bool minIsDefaultForType(void); - QVariant max(void); - bool maxIsDefaultForType(void); - QString group(void); + QVariant defaultValue(void) const; + bool defaultValueAvailable(void) const; + bool valueEqualsDefault(void) const; + FactMetaData::ValueType_t type(void) const; + QString shortDescription(void) const; + QString longDescription(void) const; + QString units(void) const; + QVariant min(void) const; + bool minIsDefaultForType(void) const; + QVariant max(void) const; + bool maxIsDefaultForType(void) const; + QString group(void) const; /// Sets and sends new value to vehicle even if value is the same void forceSetValue(const QVariant& value); @@ -120,4 +120,4 @@ private: FactMetaData* _metaData; }; -#endif \ No newline at end of file +#endif diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index 3d63c41c944272fbca43957f0ff143ce1d7462bd..7360e65a7912a0417476a8d8dbc59c8bf2d7704a 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -80,7 +80,7 @@ const FactMetaData& FactMetaData::operator=(const FactMetaData& other) return *this; } -QVariant FactMetaData::defaultValue(void) +QVariant FactMetaData::defaultValue(void) const { if (_defaultValueAvailable) { return _defaultValue; @@ -122,7 +122,7 @@ void FactMetaData::setMax(const QVariant& max) } } -QVariant FactMetaData::_minForType(void) +QVariant FactMetaData::_minForType(void) const { switch (_type) { case valueTypeUint8: @@ -147,7 +147,7 @@ QVariant FactMetaData::_minForType(void) return QVariant(); } -QVariant FactMetaData::_maxForType(void) +QVariant FactMetaData::_maxForType(void) const { switch (_type) { case valueTypeUint8: diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 8ad7c11182404e87f6667791b2257fec12fe5626..49c72d8784332c2a3e4ad27ac79f2af15a7cbe86 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -57,28 +57,28 @@ public: FactMetaData(const FactMetaData& other, QObject* parent = NULL); const FactMetaData& operator=(const FactMetaData& other); - - // Property accessors - QString name(void) { return _name; } - QString group(void) { return _group; } - ValueType_t type(void) { return _type; } - QVariant defaultValue(void); - bool defaultValueAvailable(void) { return _defaultValueAvailable; } - QString shortDescription(void) { return _shortDescription; } - QString longDescription(void) { return _longDescription;} - QString units(void) { return _units; } - QVariant min(void) { return _min; } - QVariant max(void) { return _max; } - bool minIsDefaultForType(void) { return _minIsDefaultForType; } - bool maxIsDefaultForType(void) { return _maxIsDefaultForType; } - - // Property setters + + // Property accessors + QString name(void) const { return _name; } + QString group(void) const { return _group; } + ValueType_t type(void) const { return _type; } + QVariant defaultValue(void) const; + bool defaultValueAvailable(void) const { return _defaultValueAvailable; } + QString shortDescription(void) const { return _shortDescription; } + QString longDescription(void) const { return _longDescription;} + QString units(void) const { return _units; } + QVariant min(void) const { return _min; } + QVariant max(void) const { return _max; } + bool minIsDefaultForType(void) const { return _minIsDefaultForType; } + bool maxIsDefaultForType(void) const { return _maxIsDefaultForType; } + + // Property setters void setName(const QString& name) { _name = name; } - void setGroup(const QString& group) { _group = group; } - void setDefaultValue(const QVariant& defaultValue); - void setShortDescription(const QString& shortDescription) { _shortDescription = shortDescription; } - void setLongDescription(const QString& longDescription) { _longDescription = longDescription;} - void setUnits(const QString& units) { _units = units; } + void setGroup(const QString& group) { _group = group; } + void setDefaultValue(const QVariant& defaultValue); + void setShortDescription(const QString& shortDescription) { _shortDescription = shortDescription; } + void setLongDescription(const QString& longDescription) { _longDescription = longDescription;} + void setUnits(const QString& units) { _units = units; } void setMin(const QVariant& max); void setMax(const QVariant& max); @@ -91,8 +91,8 @@ public: bool convertAndValidate(const QVariant& value, bool convertOnly, QVariant& typedValue, QString& errorString); private: - QVariant _minForType(void); - QVariant _maxForType(void); + QVariant _minForType(void) const; + QVariant _maxForType(void) const; QString _name; QString _group; @@ -108,4 +108,4 @@ private: bool _maxIsDefaultForType; }; -#endif \ No newline at end of file +#endif diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 9fad146c4a6e3544ce0ee77260a521417fa30116..f46e38488013d391e0d93f983374ca8d4c6a8933 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -36,6 +36,11 @@ #include #include +/* types for local parameter cache */ +typedef QPair ParamTypeVal; +typedef QPair NamedParam; +typedef QMap MapID2NamedParam; + QGC_LOGGING_CATEGORY(ParameterLoaderLog, "ParameterLoaderLog") QGC_LOGGING_CATEGORY(ParameterLoaderVerboseLog, "ParameterLoaderVerboseLog") @@ -61,12 +66,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() @@ -101,7 +110,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 @@ -112,6 +127,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)) { @@ -246,6 +263,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(); @@ -484,6 +502,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; @@ -553,6 +584,70 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa _vehicle->sendMessage(msg); } +void ParameterLoader::_writeLocalParamCache() +{ + QMap 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->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 cache_map; + const QDir settingsDir(QFileInfo(QSettings().fileName()).dir()); + QFile cache_file(settingsDir.filePath("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 = _factTypeToMavType(static_cast(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)) { diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index 4e32711a3b4061dde3e66f79e8350924bd245cbc..9502e901dba40d05fcddf4799a0316da59f94f6d 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/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 _mapParameterName2Variant; + QMap _mapParameterName2Variant; + QMap > _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 \ No newline at end of file +#endif diff --git a/src/QGC.cc b/src/QGC.cc index dd4b8d0fb3b947bb4c75091a96cb285c978bba7e..5f86eae91d1461339a96275395c02cd538445bf7 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -94,4 +94,48 @@ double limitAngleToPMPId(double angle) return angle; } +static const quint32 crctab[] = +{ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + +quint32 crc32(const quint8 *src, unsigned len, unsigned state) +{ + for (unsigned i = 0; i < len; i++) { + state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); + } + return state; +} + } diff --git a/src/QGC.h b/src/QGC.h index 35f58e14ede8136a4d21ff2edb64c3ca60bfa14e..1c394adf6049b38046bdcb4a5f57aa6cb6933741 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -125,6 +125,8 @@ public: } }; +quint32 crc32(const quint8 *src, unsigned len, unsigned state); + } #define QGC_EVENTLOOP_DEBUG 0 diff --git a/src/VehicleSetup/Bootloader.cc b/src/VehicleSetup/Bootloader.cc index 0b85c2deadeb3a97a5d59540cce62176fcbe8fe6..ec2d01745ceee46150af30608a5520f712d367ca 100644 --- a/src/VehicleSetup/Bootloader.cc +++ b/src/VehicleSetup/Bootloader.cc @@ -35,50 +35,6 @@ #include "QGC.h" -static const quint32 crctab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -static quint32 crc32(const uint8_t *src, unsigned len, unsigned state) -{ - for (unsigned i = 0; i < len; i++) { - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); - } - return state; -} - Bootloader::Bootloader(QObject *parent) : QObject(parent) { @@ -286,7 +242,7 @@ bool Bootloader::_binProgram(QextSerialPort* port, const FirmwareImage* image) bytesSent += bytesToSend; // Calculate the CRC now so we can test it after the board is flashed. - _imageCRC = crc32((uint8_t *)imageBuf, bytesToSend, _imageCRC); + _imageCRC = QGC::crc32((uint8_t *)imageBuf, bytesToSend, _imageCRC); emit updateProgress(bytesSent, imageSize); } @@ -295,7 +251,7 @@ bool Bootloader::_binProgram(QextSerialPort* port, const FirmwareImage* image) // We calculate the CRC using the entire flash size, filling the remainder with 0xFF. while (bytesSent < _boardFlashSize) { const uint8_t fill = 0xFF; - _imageCRC = crc32(&fill, 1, _imageCRC); + _imageCRC = QGC::crc32(&fill, 1, _imageCRC); bytesSent++; } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 53d2518828e9649e9122186ef75896f122128796..ded26addf3559afe7885a1352c980eb6e2004848 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -571,10 +571,31 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg) void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) { + mavlink_message_t responseMsg; mavlink_param_request_read_t request; mavlink_msg_param_request_read_decode(&msg, &request); - + + const QString param_name(QString::fromLocal8Bit(request.param_id, strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))); int componentId = request.target_component; + + // special case for magic _HASH_CHECK value + if (request.target_component == MAV_COMP_ID_ALL && param_name == "_HASH_CHECK") { + mavlink_param_union_t valueUnion; + valueUnion.type = MAV_PARAM_TYPE_UINT32; + valueUnion.param_uint32 = 0; + // Special case of magic hash check value + mavlink_msg_param_value_pack(_vehicleSystemId, + componentId, + &responseMsg, + request.param_id, + valueUnion.param_float, + MAV_PARAM_TYPE_UINT32, + 0, + -1); + respondWithMavlinkMessage(responseMsg); + return; + } + Q_ASSERT(_mapParamName2Value.contains(componentId)); char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; @@ -598,8 +619,6 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) Q_ASSERT(_mapParamName2Value[componentId].contains(paramId)); Q_ASSERT(_mapParamName2MavParamType.contains(paramId)); - mavlink_message_t responseMsg; - mavlink_msg_param_value_pack(_vehicleSystemId, componentId, // component id &responseMsg, // Outgoing message