5#include <QtCore/QRegularExpression>
6#include <QtCore/QRegularExpressionMatch>
7#include <QtCore/QStack>
15 qCDebug(APMParameterMetaDataLog) <<
this;
20 qCDebug(APMParameterMetaDataLog) <<
this;
23QString APMParameterMetaData::_mavTypeToString(MAV_TYPE vehicleTypeEnum)
27 switch(vehicleTypeEnum) {
28 case MAV_TYPE_FIXED_WING:
29 case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR:
30 case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR:
31 case MAV_TYPE_VTOL_TILTROTOR:
32 case MAV_TYPE_VTOL_FIXEDROTOR:
33 case MAV_TYPE_VTOL_TAILSITTER:
34 case MAV_TYPE_VTOL_TILTWING:
35 case MAV_TYPE_VTOL_RESERVED5:
36 vehicleName =
"ArduPlane";
38 case MAV_TYPE_QUADROTOR:
39 case MAV_TYPE_COAXIAL:
40 case MAV_TYPE_HELICOPTER:
41 case MAV_TYPE_HEXAROTOR:
42 case MAV_TYPE_OCTOROTOR:
43 case MAV_TYPE_TRICOPTER:
44 vehicleName =
"ArduCopter";
46 case MAV_TYPE_ANTENNA_TRACKER:
47 vehicleName =
"Antenna Tracker";
49 case MAV_TYPE_GENERIC:
51 case MAV_TYPE_AIRSHIP:
52 case MAV_TYPE_FREE_BALLOON:
55 case MAV_TYPE_GROUND_ROVER:
56 case MAV_TYPE_SURFACE_BOAT:
57 vehicleName =
"Rover";
59 case MAV_TYPE_SUBMARINE:
60 vehicleName =
"ArduSub";
62 case MAV_TYPE_FLAPPING_WING:
64 case MAV_TYPE_ONBOARD_CONTROLLER:
66 case MAV_TYPE_ENUM_END:
73QString APMParameterMetaData::_groupFromParameterName(
const QString &name)
75 static const QRegularExpression regex = QRegularExpression(
"[0-9]*$");
76 QString group = name.split(
'_').first();
77 return group.remove(regex);
82 if (_parameterMetaDataLoaded) {
85 _parameterMetaDataLoaded =
true;
87 qCDebug(APMParameterMetaDataLog) <<
"Loading parameter meta data:" << metaDataFile;
89 QFile xmlFile(metaDataFile);
90 Q_ASSERT(xmlFile.exists());
92 const bool success = xmlFile.open(QIODevice::ReadOnly);
96 QXmlStreamReader xml(xmlFile.readAll());
99 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, reading failed:" << xml.errorString();
103 bool badMetaData =
true;
105 QString currentCategory;
107 QStack<int> xmlState;
108 xmlState.push(XmlState::None);
110 QMap<QString,QStringList> groupMembers;
112 while (!xml.atEnd()) {
113 if (xml.isStartElement()) {
114 const QString elementName = xml.
name().toString();
116 if (elementName.isEmpty()) {
118 }
else if (elementName ==
"paramfile") {
119 if (xmlState.top() != XmlState::None) {
120 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, paramfile matched";
122 xmlState.push(XmlState::ParamFileFound);
124 }
else if (elementName ==
"vehicles") {
125 if (xmlState.top() != XmlState::ParamFileFound) {
126 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, vehicles matched";
129 xmlState.push(XmlState::FoundVehicles);
130 }
else if (elementName ==
"libraries") {
131 if (xmlState.top() != XmlState::ParamFileFound) {
132 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, libraries matched";
135 currentCategory =
"libraries";
136 xmlState.push(XmlState::FoundLibraries);
137 }
else if (elementName ==
"parameters") {
138 if (xmlState.top() != XmlState::FoundVehicles && xmlState.top() != XmlState::FoundLibraries) {
139 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, parameters matched"
140 <<
"but we don't have proper vehicle or libraries yet";
144 if (xml.attributes().hasAttribute(
"name")) {
146 const QString nameValue = xml.attributes().value(
"name").toString();
147 static const QRegularExpression parameterCategories = QRegularExpression(
"ArduCopter|ArduPlane|APMrover2|Rover|ArduSub|AntennaTracker");
148 if (nameValue.contains(parameterCategories)) {
149 xmlState.push(XmlState::FoundParameters);
150 currentCategory = nameValue;
151 }
else if(xmlState.top() == XmlState::FoundLibraries) {
154 xmlState.push(XmlState::FoundParameters);
156 qCDebug(APMParameterMetaDataVerboseLog) <<
"not interested in this block of parameters, skipping:" << nameValue;
157 if (_skipXMLBlock(xml,
"parameters")) {
158 qCWarning(APMParameterMetaDataLog) <<
"something wrong with the xml, skip of the xml failed";
161 (void) xml.readNext();
165 }
else if (elementName ==
"param") {
166 if (xmlState.top() != XmlState::FoundParameters) {
167 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, element param matched"
168 <<
"while we are not yet in parameters";
171 xmlState.push(XmlState::FoundParameter);
173 if (!xml.attributes().hasAttribute(
"name")) {
174 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, parameter attribute name missing";
178 QString name = xml.attributes().value(
"name").toString();
179 if (name.contains(
':')) {
180 name = name.split(
':').last();
182 const QString group = _groupFromParameterName(name);
184 const QString category = xml.attributes().value(
"user").toString();
186 const QString shortDescription = xml.attributes().value(
"humanName").toString();
187 const QString longDescription = xml.attributes().value(
"documentation").toString();
189 qCDebug(APMParameterMetaDataVerboseLog) <<
"Found parameter name:" << name
190 <<
"short Desc:" << shortDescription
191 <<
"longDescription:" << longDescription
192 <<
"category:" << category
193 <<
"group:" << group;
195 Q_ASSERT(!rawMetaData);
196 if (_vehicleTypeToParametersMap[currentCategory].contains(name)) {
197 qCDebug(APMParameterMetaDataLog) <<
"Duplicate parameter found:" << name;
198 rawMetaData = _vehicleTypeToParametersMap[currentCategory][name];
201 _vehicleTypeToParametersMap[currentCategory][name] = rawMetaData;
202 groupMembers[group] << name;
204 qCDebug(APMParameterMetaDataVerboseLog) <<
"inserting metadata for field" << name;
205 rawMetaData->
name = name;
206 if (!category.isEmpty()) {
209 rawMetaData->
group = group;
214 if (xmlState.top() != XmlState::FoundParameter) {
215 qCWarning(APMParameterMetaDataLog) <<
"Badly formed XML, while reading parameter fields wrong state";
219 if (!_parseParameterAttributes(xml, rawMetaData)) {
220 qCDebug(APMParameterMetaDataLog) <<
"Badly formed XML, failed to read parameter attributes";
226 }
else if (xml.isEndElement()) {
227 const QString elementName = xml.name().toString();
229 if ((elementName ==
"param") && (xmlState.top() == XmlState::FoundParameter)) {
232 qCDebug(APMParameterMetaDataVerboseLog) <<
"done loading parameter";
233 rawMetaData =
nullptr;
236 }
else if (elementName ==
"parameters") {
237 qCDebug(APMParameterMetaDataVerboseLog) <<
"end of parameters for category: " << currentCategory;
238 _correctGroupMemberships(_vehicleTypeToParametersMap[currentCategory], groupMembers);
239 groupMembers.clear();
241 }
else if (elementName ==
"vehicles") {
242 qCDebug(APMParameterMetaDataVerboseLog) <<
"vehicles end here, libraries will follow";
246 (void) xml.readNext();
250void APMParameterMetaData::_correctGroupMemberships(
ParameterNametoFactMetaDataMap ¶meterToFactMetaDataMap, QMap<QString,QStringList> &groupMembers)
252 for (
const QString &groupName : groupMembers.keys()) {
253 if (groupMembers[groupName].count() == 1) {
254 for (
const QString ¶meter : groupMembers.value(groupName)) {
261bool APMParameterMetaData::_skipXMLBlock(QXmlStreamReader &xml,
const QString &blockName)
265 (void) xml.readNext();
266 elementName = xml.name().toString();
267 }
while ((elementName != blockName) && (xml.isEndElement()));
268 return !xml.isEndDocument();
271bool APMParameterMetaData::_parseParameterAttributes(QXmlStreamReader &xml,
APMFactMetaDataRaw *rawMetaData)
273 QString elementName = xml.name().toString();
274 QList<QPair<QString,QString>> values;
275 bool bitmaskElementSeen =
false;
278 while (!((elementName ==
"param") && xml.isEndElement())) {
279 if (elementName.isEmpty()) {
281 }
else if (elementName ==
"field") {
282 const QString attributeName = xml.attributes().value(
"name").toString();
284 if (attributeName ==
"Range") {
285 const QString range = xml.readElementText().trimmed();
286 QStringList rangeList = range.split(
' ');
287 if (rangeList.count() != 2) {
288 qCDebug(APMParameterMetaDataVerboseLog) <<
"space separator didn't work, trying 'to' separator";
289 rangeList = range.split(
"to");
290 if (rangeList.count() != 2) {
291 qCDebug(APMParameterMetaDataVerboseLog) <<
"'to' separator didn't work, trying '-' as separator";
292 rangeList = range.split(
'-');
293 if (rangeList.count() != 2) {
294 qCDebug(APMParameterMetaDataLog) <<
"something wrong with range, all three separators have failed" << range;
300 if (rangeList.count() == 2) {
301 rawMetaData->
min = rangeList.first().trimmed();
302 rawMetaData->
max = rangeList.last().trimmed();
305 if (rawMetaData->
min.contains(
' ')) {
306 rawMetaData->
min = rawMetaData->
min.split(
' ').first();
308 if (rawMetaData->
max.contains(
' ')) {
309 rawMetaData->
max = rawMetaData->
max.split(
' ').first();
311 qCDebug(APMParameterMetaDataVerboseLog) <<
"read field parameter"
312 <<
"min:" << rawMetaData->
min
313 <<
"max:" << rawMetaData->
max;
315 }
else if (attributeName ==
"Increment") {
316 const QString increment = xml.readElementText();
317 qCDebug(APMParameterMetaDataVerboseLog) <<
"read Increment:" << increment;
319 }
else if (attributeName ==
"Units") {
320 const QString units = xml.readElementText();
321 qCDebug(APMParameterMetaDataVerboseLog) <<
"read Units:" << units;
322 rawMetaData->
units = units;
323 }
else if (attributeName ==
"ReadOnly") {
324 const QString strValue = xml.readElementText().trimmed();
325 if (strValue.compare(
"true", Qt::CaseInsensitive) == 0) {
328 qCDebug(APMParameterMetaDataVerboseLog) <<
"read ReadOnly:" << rawMetaData->
readOnly;
329 }
else if (attributeName ==
"Bitmask") {
330 if (bitmaskElementSeen) {
332 (void) xml.readElementText();
334 bool parseError =
false;
336 const QString bitmaskString = xml.readElementText();
337 qCDebug(APMParameterMetaDataVerboseLog) <<
"read Bitmask:" << bitmaskString;
338 const QStringList bitmaskList = bitmaskString.split(
",");
339 if (!bitmaskList.isEmpty()) {
340 for (
const QString &bitmask : bitmaskList) {
341 const QStringList pair = bitmask.split(
":");
342 if (pair.count() == 2) {
343 rawMetaData->
bitmask << QPair<QString, QString>(pair[0].trimmed(), pair[1].trimmed());
345 qCDebug(APMParameterMetaDataLog) <<
"parse error: bitmask:" << bitmaskString <<
"pair count:" << pair.count();
356 }
else if (attributeName ==
"RebootRequired") {
357 const QString strValue = xml.readElementText().trimmed();
358 if (strValue.compare(
"true", Qt::CaseInsensitive) == 0) {
362 }
else if (elementName ==
"values") {
364 }
else if (elementName ==
"value") {
365 const QString valueValue = xml.attributes().value(
"code").toString();
366 const QString valueName = xml.readElementText();
367 qCDebug(APMParameterMetaDataVerboseLog) <<
"read value parameter" <<
"value desc:"
368 << valueName <<
"code:" << valueValue;
369 values << QPair<QString,QString>(valueValue, valueName);
370 rawMetaData->
values = values;
371 }
else if (elementName ==
"bitmask") {
372 QList<QPair<QString,QString>> parsed;
373 bool parseError =
false;
375 while (!(xml.isEndElement() && (xml.name() == QStringLiteral(
"bitmask")))) {
376 (void) xml.readNext();
377 if (xml.isStartElement()) {
378 if (xml.name() == QStringLiteral(
"bit")) {
379 const QString code = xml.attributes().value(
"code").toString().trimmed();
380 const QString desc = xml.readElementText().trimmed();
381 if (code.isEmpty()) {
383 qCDebug(APMParameterMetaDataLog) <<
"parse error: <bit> missing code attribute";
386 parsed << qMakePair(code, desc);
388 qCWarning(APMParameterMetaDataLog) <<
"Unknown element inside <bitmask>:" << xml.name().toString();
390 }
else if (xml.hasError() || xml.atEnd()) {
396 if (!parseError && !parsed.isEmpty()) {
398 bitmaskElementSeen =
true;
399 qCDebug(APMParameterMetaDataVerboseLog) <<
"parsed bitmask from <bitmask> tag with" << parsed.size() <<
"entries";
400 }
else if (parseError) {
401 qCDebug(APMParameterMetaDataLog) <<
"parse error while reading <bitmask> block";
404 qCWarning(APMParameterMetaDataLog) <<
"Unknown parameter element in XML: " << elementName;
407 (void) xml.readNext();
408 elementName = xml.name().toString();
415 bool keepTrying =
true;
416 QString mavTypeString = _mavTypeToString(vehicleType);
421 if (_vehicleTypeToParametersMap[mavTypeString].contains(name)) {
422 rawMetaData = _vehicleTypeToParametersMap[mavTypeString][name];
423 }
else if (_vehicleTypeToParametersMap[
"libraries"].contains(name)) {
424 rawMetaData = _vehicleTypeToParametersMap[
"libraries"][name];
426 if (!rawMetaData && (mavTypeString ==
"Rover")) {
428 mavTypeString =
"APMrover2";
439 metaData->
setGroup(_groupFromParameterName(name));
440 qCDebug(APMParameterMetaDataLog) <<
"No metaData for" << name <<
"using generic metadata";
445 if (!rawMetaData->
category.isEmpty()) {
460 if (!rawMetaData->
units.isEmpty()) {
464 if (!rawMetaData->
min.isEmpty()) {
470 qCDebug(APMParameterMetaDataLog) <<
"Invalid min value, name:" << metaData->
name()
471 <<
"type:" << metaData->
type()
472 <<
"min:" << rawMetaData->
min
477 if (!rawMetaData->
max.isEmpty()) {
483 qCDebug(APMParameterMetaDataLog) <<
"Invalid max value, name:" << metaData->
name()
484 <<
"type:" << metaData->
type()
485 <<
"max:" << rawMetaData->
max
490 if (!rawMetaData->
min.isEmpty() && !rawMetaData->
max.isEmpty()) {
495 if (!rawMetaData->
values.isEmpty()) {
496 QStringList enumStrings;
497 QVariantList enumValues;
499 for (
int i = 0; i < rawMetaData->
values.count(); i++) {
502 const QPair<QString, QString> enumPair = rawMetaData->
values[i];
505 enumValues << enumValue;
506 enumStrings << enumPair.second;
508 qCDebug(APMParameterMetaDataLog) <<
"Invalid enum value, name:" << metaData->
name()
509 <<
"type:" << metaData->
type()
510 <<
"value:" << enumPair.first
518 if (!enumStrings.isEmpty()) {
523 if (!rawMetaData->
bitmask.isEmpty()) {
524 QStringList bitmaskStrings;
525 QVariantList bitmaskValues;
527 for (
int i = 0; i < rawMetaData->
bitmask.count(); i++) {
529 const QPair<QString, QString> bitmaskPair = rawMetaData->
bitmask[i];
532 const unsigned int bitIndex = bitmaskPair.first.toUInt(&ok);
533 const quint64 bitSet = ok ? (1ull << bitIndex) : 0ull;
535 QVariant typedBitSet;
539 typedBitSet = QVariant(
static_cast<qint8
>(bitSet));
542 typedBitSet = QVariant(
static_cast<qint16
>(bitSet));
545 typedBitSet = QVariant(
static_cast<qint32
>(bitSet));
548 typedBitSet = QVariant(
static_cast<qint64
>(bitSet));
551 typedBitSet = QVariant(
static_cast<quint8
>(bitSet));
554 typedBitSet = QVariant(
static_cast<quint16
>(bitSet));
557 typedBitSet = QVariant(
static_cast<quint32
>(bitSet));
560 typedBitSet = QVariant(
static_cast<quint64
>(bitSet));
566 if (typedBitSet.isNull()) {
567 qCDebug(APMParameterMetaDataLog) <<
"Invalid type for bitmask, name:" << metaData->
name()
568 <<
"type:" << metaData->
type();
572 qCDebug(APMParameterMetaDataLog) <<
"Invalid bitmask value, name:" << metaData->
name()
573 <<
"type:" << metaData->
type()
574 <<
"value:" << bitmaskPair.first
575 <<
"error: toUInt failed";
576 bitmaskStrings.clear();
577 bitmaskValues.clear();
581 QVariant bitmaskValue;
583 bitmaskValues << bitmaskValue;
584 bitmaskStrings << bitmaskPair.second;
586 qCDebug(APMParameterMetaDataLog) <<
"Invalid bitmask value, name:" << metaData->
name()
587 <<
"type:" << metaData->
type()
588 <<
"value:" << typedBitSet
590 bitmaskStrings.clear();
591 bitmaskValues.clear();
596 if (!bitmaskStrings.isEmpty()) {
603 const double increment = rawMetaData->
incrementSize.toDouble(&ok);
607 qCDebug(APMParameterMetaDataLog) <<
"Invalid value for increment, name:" << metaData->
name() <<
"increment:" << rawMetaData->
incrementSize;
612 if ((name.endsWith(QStringLiteral(
"_P")) || name.endsWith(QStringLiteral(
"_I")) || name.endsWith(QStringLiteral(
"_D"))) &&
622 static const QRegularExpression regex(
".*\\.(\\d)\\.(\\d)\\.xml$");
623 const QRegularExpressionMatch match = regex.match(metaDataFile);
624 if (match.hasMatch() && (match.lastCapturedIndex() == 2)) {
625 majorVersion = match.captured(1).toInt();
626 minorVersion = match.captured(2).toInt();
630 qCWarning(APMParameterMetaDataLog) <<
"Unable to parse version from parameter meta data file name:" << metaDataFile;
#define QGC_LOGGING_CATEGORY(name, categoryStr)