diff --git a/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc b/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc index 3d4c24b18a67ed4dbed393b31bbc5263c1360fd1..aea2f8b9be62afe3d4bf364170a4dee419fec8ff 100644 --- a/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc +++ b/src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc @@ -1,12 +1,307 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + #include "PX4AirframeLoader.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "AirframeComponentAirframes.h" -PX4AirframeLoader::PX4AirframeLoader() -{ +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(PX4AirframeLoaderLog, "PX4AirframeLoaderLog") +bool PX4AirframeLoader::_airframeMetaDataLoaded = false; +//QMap PX4AirframeLoader::_mapParameterName2FactMetaData; + +PX4AirframeLoader::PX4AirframeLoader(AutoPilotPlugin* autopilot, UASInterface* uas, QObject* parent) +{ + Q_ASSERT(uas); } -PX4AirframeLoader::~PX4AirframeLoader() +/// Load Airframe Fact meta data +/// +/// The meta data comes from firmware airframes.xml file. +void PX4AirframeLoader::loadAirframeFactMetaData(void) { + if (_airframeMetaDataLoaded) { + return; + } + _airframeMetaDataLoaded = true; + + qCDebug(PX4AirframeLoaderLog) << "Loading PX4 airframe fact meta data"; + + Q_ASSERT(AirframeComponentAirframes::rgAirframeTypes.count() == 0); + +// QString parameterFilename; + +// // We want unit test builds to always use the resource based meta data to provide repeatable results +// if (!qgcApp()->runningUnitTests()) { +// // 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(); +// parameterFilename = parameterDir.filePath("PX4AirframeFactMetaData.xml"); +// } +// if (parameterFilename.isEmpty() || !QFile(parameterFilename).exists()) { +// parameterFilename = ":/AutoPilotPlugins/PX4/AirframeFactMetaData.xml"; +// } + +// qCDebug(PX4AirframeLoaderLog) << "Loading meta data file:" << parameterFilename; + +// QFile xmlFile(parameterFilename); +// Q_ASSERT(xmlFile.exists()); + +// bool success = xmlFile.open(QIODevice::ReadOnly); +// Q_UNUSED(success); +// Q_ASSERT(success); + +// QXmlStreamReader xml(xmlFile.readAll()); +// xmlFile.close(); +// if (xml.hasError()) { +// qWarning() << "Badly formed XML" << xml.errorString(); +// return; +// } + +// QString factGroup; +// QString errorString; +// FactMetaData* metaData = NULL; +// int xmlState = XmlStateNone; +// bool badMetaData = true; + +// while (!xml.atEnd()) { +// if (xml.isStartElement()) { +// QString elementName = xml.name().toString(); + +// if (elementName == "parameters") { +// if (xmlState != XmlStateNone) { +// qWarning() << "Badly formed XML"; +// return; +// } +// xmlState = XmlStateFoundParameters; + +// } else if (elementName == "version") { +// if (xmlState != XmlStateFoundParameters) { +// qWarning() << "Badly formed XML"; +// return; +// } +// xmlState = XmlStateFoundVersion; + +// bool convertOk; +// QString strVersion = xml.readElementText(); +// int intVersion = strVersion.toInt(&convertOk); +// if (!convertOk) { +// qWarning() << "Badly formed XML"; +// return; +// } +// if (intVersion <= 2) { +// // We can't read these old files +// qDebug() << "Parameter version stamp too old, skipping load. Found:" << intVersion << "Want: 3 File:" << parameterFilename; +// return; +// } + + +// } else if (elementName == "group") { +// if (xmlState != XmlStateFoundVersion) { +// // We didn't get a version stamp, assume older version we can't read +// qDebug() << "Parameter version stamp not found, skipping load" << parameterFilename; +// return; +// } +// xmlState = XmlStateFoundGroup; +// if (!xml.attributes().hasAttribute("name")) { +// qWarning() << "Badly formed XML"; +// return; +// } +// factGroup = xml.attributes().value("name").toString(); +// qCDebug(PX4AirframeLoaderLog) << "Found group: " << factGroup; + +// } else if (elementName == "parameter") { +// if (xmlState != XmlStateFoundGroup) { +// qWarning() << "Badly formed XML"; +// return; +// } +// xmlState = XmlStateFoundParameter; + +// if (!xml.attributes().hasAttribute("name") || !xml.attributes().hasAttribute("type")) { +// qWarning() << "Badly formed XML"; +// return; +// } + +// QString name = xml.attributes().value("name").toString(); +// QString type = xml.attributes().value("type").toString(); +// QString strDefault = xml.attributes().value("default").toString(); + +// qCDebug(PX4AirframeLoaderLog) << "Found parameter name:" << name << " type:" << type << " default:" << strDefault; + +// // 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; +// FactMetaData::ValueType_t foundType; +// for (size_t i=0; istrType) { +// found = true; +// foundType = info->type; +// break; +// } +// } +// if (!found) { +// qWarning() << "Parameter meta data with bad type:" << type << " name:" << name; +// return; +// } + +// // Now that we know type we can create meta data object and add it to the system + +// metaData = new FactMetaData(foundType); +// Q_CHECK_PTR(metaData); +// if (_mapParameterName2FactMetaData.contains(name)) { +// // We can't trust the meta dafa since we have dups +// qCWarning(PX4AirframeLoaderLog) << "Duplicate parameter found:" << name; +// badMetaData = true; +// // Reset to default meta data +// _mapParameterName2FactMetaData[name] = metaData; +// } else { +// _mapParameterName2FactMetaData[name] = metaData; +// metaData->setName(name); +// metaData->setGroup(factGroup); + +// if (xml.attributes().hasAttribute("default") && !strDefault.isEmpty()) { +// QVariant varDefault; + +// if (metaData->convertAndValidate(strDefault, false, varDefault, errorString)) { +// metaData->setDefaultValue(varDefault); +// } else { +// qCWarning(PX4AirframeLoaderLog) << "Invalid default value, name:" << name << " type:" << type << " default:" << strDefault << " error:" << errorString; +// } +// } +// } + +// } else { +// // We should be getting meta data now +// if (xmlState != XmlStateFoundParameter) { +// qWarning() << "Badly formed XML"; +// return; +// } + +// if (!badMetaData) { +// if (elementName == "short_desc") { +// Q_ASSERT(metaData); +// QString text = xml.readElementText(); +// text = text.replace("\n", " "); +// qCDebug(PX4AirframeLoaderLog) << "Short description:" << text; +// metaData->setShortDescription(text); + +// } else if (elementName == "long_desc") { +// Q_ASSERT(metaData); +// QString text = xml.readElementText(); +// text = text.replace("\n", " "); +// qCDebug(PX4AirframeLoaderLog) << "Long description:" << text; +// metaData->setLongDescription(text); + +// } else if (elementName == "min") { +// Q_ASSERT(metaData); +// QString text = xml.readElementText(); +// qCDebug(PX4AirframeLoaderLog) << "Min:" << text; + +// QVariant varMin; +// if (metaData->convertAndValidate(text, true /* convertOnly */, varMin, errorString)) { +// metaData->setMin(varMin); +// } else { +// qCWarning(PX4AirframeLoaderLog) << "Invalid min value, name:" << metaData->name() << " type:" << metaData->type() << " min:" << text << " error:" << errorString; +// } + +// } else if (elementName == "max") { +// Q_ASSERT(metaData); +// QString text = xml.readElementText(); +// qCDebug(PX4AirframeLoaderLog) << "Max:" << text; + +// QVariant varMax; +// if (metaData->convertAndValidate(text, true /* convertOnly */, varMax, errorString)) { +// metaData->setMax(varMax); +// } else { +// qCWarning(PX4AirframeLoaderLog) << "Invalid max value, name:" << metaData->name() << " type:" << metaData->type() << " max:" << text << " error:" << errorString; +// } + +// } else if (elementName == "unit") { +// Q_ASSERT(metaData); +// QString text = xml.readElementText(); +// qCDebug(PX4AirframeLoaderLog) << "Unit:" << text; +// metaData->setUnits(text); + +// } else { +// qDebug() << "Unknown element in XML: " << elementName; +// } +// } +// } +// } else if (xml.isEndElement()) { +// QString elementName = xml.name().toString(); + +// if (elementName == "parameter") { +// // Done loading this parameter, validate default value +// if (metaData->defaultValueAvailable()) { +// QVariant var; + +// if (!metaData->convertAndValidate(metaData->defaultValue(), false /* convertOnly */, var, errorString)) { +// qCWarning(PX4AirframeLoaderLog) << "Invalid default value, name:" << metaData->name() << " type:" << metaData->type() << " default:" << metaData->defaultValue() << " error:" << errorString; +// } +// } + +// // Reset for next parameter +// metaData = NULL; +// badMetaData = false; +// xmlState = XmlStateFoundGroup; +// } else if (elementName == "group") { +// xmlState = XmlStateFoundVersion; +// } else if (elementName == "parameters") { +// xmlState = XmlStateFoundParameters; +// } +// } +// xml.readNext(); +// } } +void PX4AirframeLoader::clearStaticData(void) +{ +// foreach(QString airframeName, AirframeComponentAirframes::rgAirframeTypes.keys()) { +// delete AirframeComponentAirframes::rgAirframeTypes[airframeName]; +// } + AirframeComponentAirframes::rgAirframeTypes.clear(); + _airframeMetaDataLoaded = false; +} diff --git a/src/AutoPilotPlugins/PX4/PX4AirframeLoader.h b/src/AutoPilotPlugins/PX4/PX4AirframeLoader.h index 03737da90cb4c20de984d0a5f4b8d91c8a68db73..84d6e778d29ac10c8e95bf20ed7270a6a650a17a 100644 --- a/src/AutoPilotPlugins/PX4/PX4AirframeLoader.h +++ b/src/AutoPilotPlugins/PX4/PX4AirframeLoader.h @@ -1,13 +1,69 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2015 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + #ifndef PX4AIRFRAMELOADER_H #define PX4AIRFRAMELOADER_H #include +#include +#include +#include -class PX4AirframeLoader +#include "ParameterLoader.h" +#include "FactSystem.h" +#include "UASInterface.h" +#include "AutoPilotPlugin.h" + +/// @file PX4AirframeLoader.h +/// @author Lorenz Meier + +Q_DECLARE_LOGGING_CATEGORY(PX4AirframeLoaderLog) + +/// Collection of Parameter Facts for PX4 AutoPilot + +class PX4AirframeLoader : QObject { + Q_OBJECT + public: - PX4AirframeLoader(); - ~PX4AirframeLoader(); + /// @param uas Uas which this set of facts is associated with + PX4AirframeLoader(AutoPilotPlugin* autpilot,UASInterface* uas, QObject* parent = NULL); + + static void loadAirframeFactMetaData(void); + static void clearStaticData(void); + +private: + enum { + XmlStateNone, + XmlStateFoundAirframes, + XmlStateFoundVersion, + XmlStateFoundGroup, + XmlStateFoundAirframe, + XmlStateDone + }; + + static bool _airframeMetaDataLoaded; ///< true: parameter meta data already loaded + static QMap _mapParameterName2FactMetaData; ///< Maps from a parameter name to FactMetaData }; #endif // PX4AIRFRAMELOADER_H