diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc index e74d0b282696f46bbc255124d66c11127a55cb75..ccebd8b2437d5ad2299599651c4b5864a3b89488 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc +++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.cc @@ -29,6 +29,8 @@ #include "QGCLoggingCategory.h" #include +#include +#include #include QGC_LOGGING_CATEGORY(PX4ParameterFactsMetaDataLog, "PX4ParameterFactsMetaDataLog") @@ -42,63 +44,6 @@ PX4ParameterFacts::PX4ParameterFacts(UASInterface* uas, QObject* parent) : Q_ASSERT(uas); } -/// Parse the Parameter element of parameter xml meta data -/// @param[in] xml stream reader -/// @param[in] group fact group associated with this Param element -/// @return Returns the meta data object for this parameter -FactMetaData* PX4ParameterFacts::_parseParameter(QXmlStreamReader& xml, const QString& group) -{ - Q_UNUSED(group); - - QString name = xml.attributes().value("name").toString(); - QString type = xml.attributes().value("type").toString(); - - qCDebug(PX4ParameterFactsMetaDataLog) << "Parsed parameter: " << name << type; - - Q_ASSERT(!name.isEmpty()); - Q_ASSERT(!type.isEmpty()); - - FactMetaData* metaData = new FactMetaData(); - Q_CHECK_PTR(metaData); - - metaData->group = group; - - // Convert type from string to FactMetaData::ValueType_t - - struct String2Type { - const char* strType; - FactMetaData::ValueType_t type; - }; - - static const struct String2Type rgString2Type[] = { - { "FLOAT", FactMetaData::valueTypeFloat }, - { "INT32", FactMetaData::valueTypeInt32 }, - }; - static const size_t crgString2Type = sizeof(rgString2Type) / sizeof(rgString2Type[0]); - - bool found = false; - for (size_t i=0; istrType) { - found = true; - metaData->type = info->type; - break; - } - } - Q_UNUSED(found); - Q_ASSERT(found); - - _initMetaData(metaData); - - // FIXME: Change to change the parameter build scheme in Firmware to get rid of ifdef dup problem - if (!_mapParameterName2FactMetaData.contains(name)) { - _mapParameterName2FactMetaData[name] = metaData; - } - - return metaData; -} - /// This will fill in missing meta data such as range info void PX4ParameterFacts::_initMetaData(FactMetaData* metaData) { @@ -163,6 +108,7 @@ QVariant PX4ParameterFacts::_stringToTypedVariant(const QString& string, FactMet case FactMetaData::valueTypeInt32: convertOk = var.convert(QVariant::Int); if (!failOk) { + qDebug() << string; Q_ASSERT(convertOk); } break; @@ -170,6 +116,7 @@ QVariant PX4ParameterFacts::_stringToTypedVariant(const QString& string, FactMet case FactMetaData::valueTypeDouble: convertOk = var.convert(QVariant::Double); if (!failOk) { + qDebug() << string; Q_ASSERT(convertOk); } break; @@ -191,9 +138,16 @@ void PX4ParameterFacts::loadParameterFactMetaData(void) qCDebug(PX4ParameterFactsMetaDataLog) << "Loading PX4 parameter fact meta data"; Q_ASSERT(_mapParameterName2FactMetaData.count() == 0); - - QFile xmlFile(":/AutoPilotPlugins/PX4/ParameterFactMetaData.xml"); - + + // First look for meta data that comes from a firmware download. Fall back to resource if not there. + QSettings settings; + QDir parameterDir = QFileInfo(settings.fileName()).dir(); + QString parameterFilename = parameterDir.filePath("PX4ParameterFactMetaData.xml"); + if (!QFile(parameterFilename).exists()) { + parameterFilename = ":/AutoPilotPlugins/PX4/ParameterFactMetaData.xml"; + } + + QFile xmlFile(parameterFilename); Q_ASSERT(xmlFile.exists()); bool success = xmlFile.open(QIODevice::ReadOnly); @@ -215,7 +169,49 @@ void PX4ParameterFacts::loadParameterFactMetaData(void) factGroup = xml.attributes().value("name").toString(); qCDebug(PX4ParameterFactsMetaDataLog) << "Found group: " << factGroup; } else if (elementName == "parameter") { - metaData = _parseParameter(xml, factGroup); + metaData = new FactMetaData(); + Q_CHECK_PTR(metaData); + metaData->group = factGroup; + } else if (elementName == "code") { + Q_ASSERT(metaData); + QString name = xml.readElementText(); + qCDebug(PX4ParameterFactsMetaDataLog) << "Found parameter: " << name; + // FIXME: Change the parameter build scheme in Firmware to get rid of ifdef dup problem + if (!_mapParameterName2FactMetaData.contains(name)) { + _mapParameterName2FactMetaData[name] = metaData; + } + } else if (elementName == "type") { + Q_ASSERT(metaData); + QString type = xml.readElementText(); + + // Convert type from string to FactMetaData::ValueType_t + + struct String2Type { + const char* strType; + FactMetaData::ValueType_t type; + }; + + static const struct String2Type rgString2Type[] = { + { "FLOAT", FactMetaData::valueTypeFloat }, + { "INT32", FactMetaData::valueTypeInt32 }, + }; + static const size_t crgString2Type = sizeof(rgString2Type) / sizeof(rgString2Type[0]); + + bool found = false; + for (size_t i=0; istrType) { + found = true; + metaData->type = info->type; + break; + } + } + Q_UNUSED(found); + Q_ASSERT(found); + + qCDebug(PX4ParameterFactsMetaDataLog) << "Type:" << type << metaData->type; + _initMetaData(metaData); } else if (elementName == "short_desc") { Q_ASSERT(metaData); QString text = xml.readElementText(); diff --git a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h index 018f4b5aa84f64e562869dc17a89517f14b3ed25..eb2b89b0117e3996a64b72c8955e46ada0fe3645 100644 --- a/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h +++ b/src/AutoPilotPlugins/PX4/PX4ParameterFacts.h @@ -59,7 +59,6 @@ private: virtual void _addMetaDataToFact(Fact* fact); // Class methods - static FactMetaData* _parseParameter(QXmlStreamReader& xml, const QString& group); static void _initMetaData(FactMetaData* metaData); static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false); diff --git a/src/VehicleSetup/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc index 0af15f33e0d70628eb7d7459192db34117f0cfbb..94d518ba8bca31babf7c72d1c42f470f4c4d018a 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.cc +++ b/src/VehicleSetup/FirmwareUpgradeController.cc @@ -344,55 +344,51 @@ void FirmwareUpgradeController::_downloadFinished(void) return; } - _imageSize = px4Json.value(QString("image_size")).toInt(); - if (_imageSize == 0) { - _appendStatusLog(tr("Image size of 0 in .px4 file %1").arg(downloadFilename)); - _cancel(); - return; + // Decompress the parameter xml and save to file + QByteArray decompressedBytes; + bool success = _decompressJsonValue(px4Json, // JSON object + bytes, // Raw bytes of JSON document + "parameter_xml_size", // key which holds byte size + "parameter_xml", // key which holds compress bytes + decompressedBytes); // Returned decompressed bytes + if (success) { + QSettings settings; + QDir parameterDir = QFileInfo(settings.fileName()).dir(); + QString parameterFilename = parameterDir.filePath("PX4ParameterFactMetaData.xml"); + qDebug() << parameterFilename; + QFile parameterFile(parameterFilename); + if (parameterFile.open(QIODevice::WriteOnly | QIODevice::Truncate)) { + qint64 bytesWritten = parameterFile.write(decompressedBytes); + if (bytesWritten != decompressedBytes.count()) { + _appendStatusLog(tr("Write failed for parameter meta data file, error: %1").arg(parameterFile.errorString())); + parameterFile.close(); + QFile::remove(parameterFilename); + } else { + parameterFile.close(); + } + } else { + _appendStatusLog(tr("Unable to open parameter meta data file %1 for writing, error: %2").arg(parameterFilename).arg(parameterFile.errorString())); + } + } - qDebug() << "Image size from px4:" << _imageSize; - - // Convert image from base-64 and decompress - - // XXX Qt's JSON string handling is terribly broken, strings - // with some length (18K / 25K) are just weirdly cut. - // The code below works around this by manually 'parsing' - // for the image string. Since its compressed / checksummed - // this should be fine. - QStringList list = QString(bytes).split("\"image\": \""); - list = list.last().split("\""); + // FIXME: Save NYI - // Convert String to QByteArray and unzip it - QByteArray raw; - - // Store image size - raw.append((unsigned char)((_imageSize >> 24) & 0xFF)); - raw.append((unsigned char)((_imageSize >> 16) & 0xFF)); - raw.append((unsigned char)((_imageSize >> 8) & 0xFF)); - raw.append((unsigned char)((_imageSize >> 0) & 0xFF)); - - QByteArray raw64 = list.first().toUtf8(); - - raw.append(QByteArray::fromBase64(raw64)); - QByteArray uncompressed = qUncompress(raw); - - QByteArray b = uncompressed; - - if (b.count() == 0) { - _appendStatusLog(tr("Firmware file has 0 length image")); - _cancel(); - return; - } - if (b.count() != (int)_imageSize) { - _appendStatusLog(tr("Image size for decompressed image does not match stored image size: Expected(%1) Actual(%2)").arg(_imageSize).arg(b.count())); + // Decompress the image and save to file + _imageSize = px4Json.value(QString("image_size")).toInt(); + success = _decompressJsonValue(px4Json, // JSON object + bytes, // Raw bytes of JSON document + "image_size", // key which holds byte size + "image", // key which holds compress bytes + decompressedBytes); // Returned decompressed bytes + if (!success) { _cancel(); return; } - + // Pad image to 4-byte boundary - while ((b.count() % 4) != 0) { - b.append(static_cast(static_cast(0xFF))); + while ((decompressedBytes.count() % 4) != 0) { + decompressedBytes.append(static_cast(static_cast(0xFF))); } // Store decompressed image file in same location as original download file @@ -406,8 +402,8 @@ void FirmwareUpgradeController::_downloadFinished(void) return; } - qint64 bytesWritten = decompressFile.write(b); - if (bytesWritten != b.count()) { + qint64 bytesWritten = decompressFile.write(decompressedBytes); + if (bytesWritten != decompressedBytes.count()) { _appendStatusLog(tr("Write failed for decompressed image file, error: %1").arg(decompressFile.errorString())); _cancel(); return; @@ -459,6 +455,65 @@ void FirmwareUpgradeController::_downloadFinished(void) _erase(); } +/// Decompress a set of bytes stored in a Json document. +bool FirmwareUpgradeController::_decompressJsonValue(const QJsonObject& jsonObject, ///< JSON object + const QByteArray& jsonDocBytes, ///< Raw bytes of JSON document + const QString& sizeKey, ///< key which holds byte size + const QString& bytesKey, ///< key which holds compress bytes + QByteArray& decompressedBytes) ///< Returned decompressed bytes +{ + // Validate decompressed size key + if (!jsonObject.contains(sizeKey)) { + _appendStatusLog(QString("Firmware file missing %1 key").arg(sizeKey)); + return false; + } + int decompressedSize = jsonObject.value(QString(sizeKey)).toInt(); + if (decompressedSize == 0) { + _appendStatusLog(QString("Firmware file has invalid decompressed size for %1").arg(sizeKey)); + return false; + } + + // XXX Qt's JSON string handling is terribly broken, strings + // with some length (18K / 25K) are just weirdly cut. + // The code below works around this by manually 'parsing' + // for the image string. Since its compressed / checksummed + // this should be fine. + + QStringList parts = QString(jsonDocBytes).split(QString("\"%1\": \"").arg(bytesKey)); + if (parts.count() == 1) { + _appendStatusLog(QString("Could not find compressed bytes for %1 in Firmware file").arg(bytesKey)); + return false; + } + parts = parts.last().split("\""); + if (parts.count() == 1) { + _appendStatusLog(QString("Incorrectly formed compressed bytes section for %1 in Firmware file").arg(bytesKey)); + return false; + } + + // Store decompressed size as first four bytes. This is required by qUncompress routine. + QByteArray raw; + raw.append((unsigned char)((decompressedSize >> 24) & 0xFF)); + raw.append((unsigned char)((decompressedSize >> 16) & 0xFF)); + raw.append((unsigned char)((decompressedSize >> 8) & 0xFF)); + raw.append((unsigned char)((decompressedSize >> 0) & 0xFF)); + + QByteArray raw64 = parts.first().toUtf8(); + raw.append(QByteArray::fromBase64(raw64)); + decompressedBytes = qUncompress(raw); + + if (decompressedBytes.count() == 0) { + _appendStatusLog(QString("Firmware file has 0 length %1").arg(bytesKey)); + return false; + } + if (decompressedBytes.count() != decompressedSize) { + _appendStatusLog(QString("Size for decompressed %1 does not match stored size: Expected(%1) Actual(%2)").arg(decompressedSize).arg(decompressedBytes.count())); + return false; + } + + _appendStatusLog(QString("Succesfully decompressed %1").arg(bytesKey)); + return true; +} + /// @brief Called when an error occurs during download void FirmwareUpgradeController::_downloadError(QNetworkReply::NetworkError code) { diff --git a/src/VehicleSetup/FirmwareUpgradeController.h b/src/VehicleSetup/FirmwareUpgradeController.h index d605143b7f1bbeea3f660aa2eb91d7b55e1a82d3..de90c18369768dcdec9a434e6e6ca844989dcfeb 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.h +++ b/src/VehicleSetup/FirmwareUpgradeController.h @@ -105,12 +105,14 @@ private: void _findBootloader(void); void _cancel(void); void _getFirmwareFile(void); - void _downloadFirmware(void); - void _erase(void); - void _appendStatusLog(const QString& text); + bool _decompressJsonValue(const QJsonObject& jsonObject, + const QByteArray& jsonDocBytes, + const QString& sizeKey, + const QString& bytesKey, + QByteArray& decompressedBytes); QString _portName; QString _portDescription;