diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc index 028069b..2b3add2 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc @@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include "MAVLinkXMLParserV10.h" @@ -63,88 +64,30 @@ MAVLinkXMLParserV10::~MAVLinkXMLParserV10() { } -/** - * - * CALCULATE THE CHECKSUM - * - */ - -#define X25_INIT_CRC 0xffff -#define X25_VALIDATE_CRC 0xf0b8 - -/** - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new char to hash - * @param crcAccum the already accumulated checksum - **/ -void MAVLinkXMLParserV10::crcAccumulate(uint8_t data, uint16_t *crcAccum) +void MAVLinkXMLParserV10::processError(QProcess::ProcessError err) { - /*Accumulate one byte of data into the CRC*/ - uint8_t tmp; - - tmp=data ^ (uint8_t)(*crcAccum &0xff); - tmp^= (tmp<<4); - *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -} - -/** - * @param crcAccum the 16 bit X.25 CRC - */ -void MAVLinkXMLParserV10::crcInit(uint16_t* crcAccum) -{ - *crcAccum = X25_INIT_CRC; -} - - -const struct { - const char *prefix; - unsigned length; -} length_map[] = { - { "array", 1 }, - { "char", 1 }, - { "uint8", 1 }, - { "int8", 1 }, - { "uint16", 2 }, - { "int16", 2 }, - { "uint32", 4 }, - { "int32", 4 }, - { "uint64", 8 }, - { "int64", 8 }, - { "float", 4 }, - { "double", 8 }, -}; - -unsigned itemLength( const QString &s1 ) -{ - unsigned el1, i1, i2; - QString Ss1 = s1; - - Ss1 = Ss1.replace("_"," "); - Ss1 = Ss1.simplified(); - Ss1 = Ss1.section(" ",0,0); - - el1 = i1 = 0; - i2 = sizeof(length_map)/sizeof(length_map[0]); - - do { - if (Ss1.startsWith(length_map[i1].prefix)) - { - el1 = length_map[i1].length; - } - i1++; - } while ( (el1 == 0) && (i1 < i2)); - return el1; -} - -bool structSort(const QString &s1, const QString &s2) -{ - unsigned el1, el2; - - el1 = itemLength( s1 ); - el2 = itemLength( s2 ); - return el2 < el1; + switch(err) + { + case QProcess::FailedToStart: + emit parseState(tr("Generator failed to start. Please check if the path and command is correct.")); + break; + case QProcess::Crashed: + emit parseState("Generator crashed, This is a generator-related problem. Please upgrade MAVLink generator."); + break; + case QProcess::Timedout: + emit parseState(tr("Generator start timed out, please check if the path and command are correct")); + break; + case QProcess::WriteError: + emit parseState(tr("Could not communicate with generator. Please check if the path and command are correct")); + break; + case QProcess::ReadError: + emit parseState(tr("Could not communicate with generator. Please check if the path and command are correct")); + break; + case QProcess::UnknownError: + default: + emit parseState(tr("Generator error. Please check if the path and command is correct.")); + break; + } } /** @@ -152,882 +95,992 @@ bool structSort(const QString &s1, const QString &s2) */ bool MAVLinkXMLParserV10::generate() { - uint16_t crc_key = X25_INIT_CRC; - // Process result - bool success = true; - - // Only generate if output dir is correctly set - if (outputDirName == "") - { - emit parseState(tr("ERROR: No output directory given.\nAbort.")); - return false; - } - - QString topLevelOutputDirName = outputDirName; - - // print out the element names of all elements that are direct children - // of the outermost element. - QDomElement docElem = doc->documentElement(); - QDomNode n = docElem;//.firstChild(); - QDomNode p = docElem; - - // Sanity check variables - QList* usedMessageIDs = new QList(); - QMap* usedMessageNames = new QMap(); - QMap* usedEnumNames = new QMap(); - - QList< QPair > cFiles; - QString lcmStructDefs = ""; - - QString pureFileName; - QString pureIncludeFileName; - - QFileInfo fInfo(this->fileName); - pureFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); - - // XML parsed and converted to C code. Now generating the files - outputDirName += QDir::separator() + pureFileName; - QDateTime now = QDateTime::currentDateTime().toUTC(); - QLocale loc(QLocale::English); - QString dateFormat = "dddd, MMMM d yyyy, hh:mm UTC"; - QString date = loc.toString(now, dateFormat); - QString includeLine = "#include \"%1\"\n"; - QString mainHeaderName = pureFileName + ".h"; - QString messagesDirName = ".";//"generated"; - QDir dir(outputDirName + "/" + messagesDirName); - - int mavlinkVersion = 0; - static unsigned message_lengths[256]; - static unsigned message_key[256]; - static int highest_message_id; - static int recursion_level; - - if (recursion_level == 0) { - highest_message_id = 0; - memset(message_lengths, 0, sizeof(message_lengths)); - } - - - // Start main header - QString mainHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://qgroundcontrol.org/mavlink/\n *\t Generated on %1\n */\n#ifndef " + pureFileName.toUpper() + "_H\n#define " + pureFileName.toUpper() + "_H\n\n").arg(date); // The main header includes all messages - // Mark all code as C code - mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n"; - mainHeader += "\n#include \"../mavlink_protocol.h\"\n"; - mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n"; - - QString enums; - - - // Run through root children - while(!n.isNull()) - { - // Each child is a message - QDomElement e = n.toElement(); // try to convert the node to an element. - if(!e.isNull()) - { - if (e.tagName() == "mavlink") - { - p = n; - n = n.firstChild(); - while (!n.isNull()) - { - e = n.toElement(); - if (!e.isNull()) - { - // Handle all include tags - if (e.tagName() == "include") - { - QString incFileName = e.text(); - // Load file - //QDomDocument includeDoc = QDomDocument(); - - // Prepend file path if it is a relative path and - // make it relative to opened file - QFileInfo fInfo(incFileName); - - QString incFilePath; - if (fInfo.isRelative()) - { - QFileInfo rInfo(this->fileName); - incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName; - pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); - } - - QFile file(incFilePath); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) - { - emit parseState(QString("Included messages from file: %1").arg(incFileName)); - // NEW MODE: CREATE INDIVIDUAL FOLDERS - // Create new output directory, parse included XML and generate C-code - MAVLinkXMLParserV10 includeParser(incFilePath, topLevelOutputDirName, this); - connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString))); - // Generate and write - includeParser.generate(); - mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n"; - - emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); - } - else - { - // Include file could not be opened - emit parseState(QString("ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.").arg(fileName)); - return false; - } - - } - // Handle all enum tags - else if (e.tagName() == "version") - { - //QString fieldType = e.attribute("type", ""); - //QString fieldName = e.attribute("name", ""); - QString fieldText = e.text(); - - // Check if version has been previously set - if (mavlinkVersion != 0) - { - emit parseState(QString("ERROR: Protocol version tag set twice, please use it only once. First version was %1, second version is %2.\nAbort.").arg(mavlinkVersion).arg(fieldText)); - return false; - } - - bool ok; - int version = fieldText.toInt(&ok); - if (ok && (version > 0) && (version < 256)) - { - // Set MAVLink version - mavlinkVersion = version; - } - else - { - emit parseState(QString("ERROR: Reading version string failed: %1, string is not an integer number between 1 and 255.\nAbort.").arg(fieldText)); - return false; - } - } - // Handle all enum tags - else if (e.tagName() == "enums") - { - // One down into the enums list - p = n; - n = n.firstChild(); - while (!n.isNull()) - { - e = n.toElement(); - - QString currEnum; - QString currEnumEnd; - // Comment - QString comment; - - if(!e.isNull() && e.tagName() == "enum") - { - // Get enum name - QString enumName = e.attribute("name", "").toLower(); - if (enumName.size() == 0) - { - emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); - return false; - } - else - { - // Sanity check: Accept only enum names not used previously - if (usedEnumNames->contains(enumName)) - { - emit parseState(tr("ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(enumName, QString::number(e.lineNumber()), fileName)); - return false; - } - else - { - usedEnumNames->insert(enumName, QString::number(e.lineNumber())); - } - - // Everything sane, starting with enum content - currEnum = "enum " + enumName.toUpper() + "\n{\n"; - currEnumEnd = QString("\t%1_ENUM_END\n};\n\n").arg(enumName.toUpper()); - - int nextEnumValue = 0; - - // Get the enum fields - QDomNode f = e.firstChild(); - while (!f.isNull()) - { - QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "entry") - { - QString fieldValue = e2.attribute("value", ""); - - // If value was given, use it, if not, use the enum iterator - // value. The iterator value gets reset by manual values - - QString fieldName = e2.attribute("name", ""); - if (fieldValue.length() == 0) - { - fieldValue = QString::number(nextEnumValue); - nextEnumValue++; - } - else - { - bool ok; - nextEnumValue = fieldValue.toInt(&ok) + 1; - if (!ok) - { - emit parseState(tr("ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.").arg(fieldName, fieldValue)); - return false; - } - } - - // Add comment of field if there is one - QString fieldComment; - if (e2.text().length() > 0) - { - QString sep(" | "); - QDomNode pp = e2.firstChild(); - while (!pp.isNull()) - { - QDomElement pp2 = pp.toElement(); - if (pp2.isText() || pp2.isCDATASection()) - { - // If this is the only field, don't add the separator - if (pp.nextSibling().isNull()) - { - fieldComment += pp2.nodeValue(); - } - else - { - fieldComment += pp2.nodeValue() + sep; - } - } - else if (pp2.isElement()) - { - // If this is the only field, don't add the separator - if (pp.nextSibling().isNull()) - { - fieldComment += pp2.text(); - } - else - { - fieldComment += pp2.text() + sep; - } - } - pp = pp.nextSibling(); - } - fieldComment = fieldComment.replace("\n", " "); - fieldComment = " /* " + fieldComment.simplified() + " */"; - } - currEnum += "\t" + fieldName.toUpper() + "=" + fieldValue + "," + fieldComment + "\n"; - } - else if(!e2.isNull() && e2.tagName() == "description") - { - comment = " " + e2.text().replace("\n", " ") + comment; - } - f = f.nextSibling(); - } - } - // Add the last parsed enum - // Remove the last comma, as the last value has none - // ENUM END MARKER IS LAST ENTRY, COMMA REMOVAL NOT NEEDED - //int commaPosition = currEnum.lastIndexOf(","); - //currEnum.remove(commaPosition, 1); - - enums += "/** @brief " + comment.simplified() + " */\n" + currEnum + currEnumEnd; - } // Element is non-zero and element name is - n = n.nextSibling(); - } // While through - // One up, back into the structure - n = p; - } - - // Handle all message tags - else if (e.tagName() == "messages") - { - p = n; - n = n.firstChild(); - while (!n.isNull()) - { - e = n.toElement(); - if(!e.isNull()) - { - //if (e.isNull()) continue; - // Get message name - QString messageName = e.attribute("name", "").toLower(); - if (messageName.size() == 0) - { - emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); - return false; - } - else - { - // Get message id - bool ok; - int messageId = e.attribute("id", "-1").toInt(&ok, 10); -// emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); - - // Sanity check: Accept only message IDs not used previously - if (usedMessageIDs->contains(messageId)) - { - emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); - return false; - } - else - { - usedMessageIDs->append(messageId); - } - - // Sanity check: Accept only message names not used previously - if (usedMessageNames->contains(messageName)) - { - emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); - return false; - } - else - { - usedMessageNames->insert(messageName, QString::number(e.lineNumber())); - } - - QString channelType("mavlink_channel_t"); - QString messageType("mavlink_message_t"); - QString headerType("mavlink_header_t"); - - // Build up function call - QString commentContainer("/**\n * @brief Pack a %1 message\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n *\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); - QString commentPackChanContainer("/**\n * @brief Pack a %1 message\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param chan The MAVLink channel this message was sent over\n * @param msg The MAVLink message to compress the data into\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); - QString commentSendContainer("/**\n * @brief Send a %1 message\n * @param chan MAVLink channel to send the message\n *\n%2 */\n"); - QString commentEncodeContainer("/**\n * @brief Encode a %1 struct into a message\n *\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n * @param %1 C-struct to read the message contents from\n */\n"); - QString commentDecodeContainer("/**\n * @brief Decode a %1 message into a struct\n *\n * @param msg The message to decode\n * @param %1 C-struct to decode the message contents into\n */\n"); - QString commentEntry(" * @param %1 %2\n"); - QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2\n#define MAVLINK_MSG_ID_%1_LEN %3\n#define MAVLINK_MSG_%2_LEN %3\n#define MAVLINK_MSG_ID_%1_KEY 0x%4\n#define MAVLINK_MSG_%2_KEY 0x%4"); - QString arrayDefines; - QString cStructName = QString("mavlink_%1_t").arg(messageName); - QString cStruct("typedef struct __%1 \n{\n%2\n} %1;"); - QString cStructLines; - QString encode("static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"); - - // QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"); - QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n\tmemcpy( %1, msg->payload, sizeof(%2));\n}\n"); - // QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"); - QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_%3_LEN);\n}\n\n"); - // QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);\n}\n\n"); - QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_%3_LEN);\n}\n\n"); - //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; - QString unpacking; - QString prepends; - QString packParameters; - QString packArguments("system_id, component_id, msg"); - QString packLines; - QString decodeLines; - QString sendArguments; - QString commentLines; - int calculatedLength = 0; - unsigned message_length = 0; - - - // Get the message fields - QDomNode f = e.firstChild(); - - // The field types and order are hashed with a checksum - - // Initialize CRC - uint16_t fieldHash; - crcInit(&fieldHash); - - while (!f.isNull()) - { - QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "field") - { - QString fieldType = e2.attribute("type", ""); - QString fieldName = e2.attribute("name", ""); - QString fieldOffset = e2.attribute("offset", ""); - QString fieldText = e2.text(); - - QString unpackingCode; - QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); - - // Send arguments do not work for the version field - if (!fieldType.contains("uint8_t_mavlink_version")) - { - // Send arguments are the same for integral types and arrays - sendArguments += ", " + fieldName; - commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); - } - - // MAVLink version field - // this is a special field always containing the version define - if (fieldType.contains("uint8_t_mavlink_version")) - { - // Add field to C structure - cStructLines += QString("\t%1 %2;\t///< %3\n").arg("uint8_t", fieldName, fieldText); - calculatedLength += 1; - // Add pack line to message_xx_pack function - // packLines += QString("\ti += put_uint8_t_by_index(%1, i, msg->payload); // %2\n").arg(mavlinkVersion).arg(fieldText); - packLines += QString("\n\tp->%2 = MAVLINK_VERSION;\t// %1:%3").arg(fieldType, fieldName, e2.text()); - // Add decode function for this type - decodeLines += QString("\n\t%1->%2 = mavlink_msg_%1_get_%2(msg);").arg(messageName, fieldName); - - if (fieldOffset != "") { // does not use the number - always moves up one slot - QStringList itemList; - // Swap field in C structure - itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - cStructLines = itemList.join("\n") + "\n"; - - // Swap line in message_xx_pack function - itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - packLines = itemList.join("\n") + "\n"; - - // Swap line in decode function for this type - itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - decodeLines = itemList.join("\n") + "\n"; - } - } - - // ARRAYS are not longer supported - leave error message in here to inform users! - else if (fieldType.startsWith("array")) - { - emit parseState(tr("ERROR: In message %1 deprecated type used near line %2 of file %3. Please change from array[size] to uint8_t[size] to get the same behaviour.\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); - return false; - } - else if (fieldType.startsWith("string")) - { - int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); - // String array is always unsigned char, so bytes - calculatedLength += arrayLength; - packParameters += QString(", const ") + QString("char*") + " " + fieldName; - packArguments += ", " + messageName + "->" + fieldName; - - // Add field to C structure - cStructLines += QString("\t%1 %2[%3];\t///< %4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); - // Add pack line to message_xx_pack function - // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); - packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2));\t// %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); - // Add decode function for this type - decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); - arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - - if (fieldOffset != "") - { // does not use the number - always moves up one slot - QStringList itemList; - // Swap field in C structure - itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - cStructLines = itemList.join("\n") + "\n"; - - // Swap line in message_xx_pack function - itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - packLines = itemList.join("\n") + "\n"; - - // Swap line in decode function for this type - itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - decodeLines = itemList.join("\n") + "\n"; - } - } - // Expand array handling to all valid mavlink data types - else if(fieldType.contains('[') && fieldType.contains(']')) - { - int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); - QString arrayType = fieldType.split("[").first(); - if (arrayType.contains("array")) - { - calculatedLength += arrayLength; - } - else if (arrayType.contains("char")) - { - calculatedLength += arrayLength; - } - else if (arrayType.contains("int8")) - { - calculatedLength += arrayLength; - } - else if (arrayType.contains("int16")) - { - calculatedLength += arrayLength*2; - } - else if (arrayType.contains("int32")) - { - calculatedLength += arrayLength*4; - } - else if (arrayType.contains("int64")) - { - calculatedLength += arrayLength*8; - } - else if (arrayType == "float") - { - calculatedLength += arrayLength*4; - } - else - { - emit parseState(tr("ERROR: In message %1 invalid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); - return false; - } - packParameters += QString(", const ") + arrayType + "* " + fieldName; - packArguments += ", " + messageName + "->" + fieldName; - - // Add field to C structure - cStructLines += QString("\t%1 %2[%3];\t///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); - // Add pack line to message_xx_pack function - // packLines += QString("\ti += put_array_by_index((const int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); - packLines += QString("\tmemcpy(p->%2, %2, sizeof(p->%2));\t// %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); - // Add decode function for this type - decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); - - if (fieldOffset != "") - { // does not use the number - always moves up one slot - QStringList itemList; - // Swap field in C structure - itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - cStructLines = itemList.join("\n") + "\n"; - - // Swap line in message_xx_pack function - itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - packLines = itemList.join("\n") + "\n"; - - // Swap line in decode function for this type - itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - decodeLines = itemList.join("\n") + "\n"; - } - - arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - - // unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, sizeof(%2)*%3);\n\treturn sizeof(%2)*%3;").arg(prepends, arrayType, QString::number(arrayLength)); - unpackingCode = QString("\n\tmemcpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); - - // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); - unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* %2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); - // decodeLines += ""; - prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); - - } - else - // Handle simple types like integers and floats - { - packParameters += ", " + fieldType + " " + fieldName; - packArguments += ", " + messageName + "->" + fieldName; - - // Add field to C structure - cStructLines += QString("\t%1 %2;\t///< %3\n").arg(fieldType, fieldName, fieldText); - // Add pack line to message_xx_pack function - // packLines += QString("\ti += put_%1_by_index(%2, i, msg->payload); // %3\n").arg(fieldType, fieldName, e2.text()); - packLines += QString("\tp->%2 = %2;\t// %1:%3\n").arg(fieldType, fieldName, e2.text()); - // Add decode function for this type - // decodeLines += QString("\t%1->%2 = mavlink_msg_%1_get_%2(msg);\n").arg(messageName, fieldName); - decodeLines += QString("\n\t%1 = p->%1;").arg(fieldName); - - if (fieldOffset != "") { // does not use the number - always moves up one slot - QStringList itemList; - // Swap field in C structure - itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - cStructLines = itemList.join("\n") + "\n"; - - // Swap line in message_xx_pack function - itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - packLines = itemList.join("\n") + "\n"; - - // Swap line in decode function for this type - itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); - decodeLines = itemList.join("\n") + "\n"; - } - if (fieldType.contains("char")) - { - calculatedLength += 1; - } - else if (fieldType.contains("int8")) - { - calculatedLength += 1; - } - else if (fieldType.contains("int16")) - { - calculatedLength += 2; - } - else if (fieldType.contains("int32")) - { - calculatedLength += 4; - } - else if (fieldType.contains("int64")) - { - calculatedLength += 8; - } - else if (fieldType == "float") - { - calculatedLength += 4; - } - else - { - emit parseState(tr("ERROR: In message %1 inavlid type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, fieldType)); - return false; - } - } - - // message length calculation - unsigned element_multiplier = 1; - unsigned element_length = 0; - - if (fieldType.contains("[")) { - element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); - } - for (unsigned i=0; iERROR: Unable to calculate length for %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), fieldType)); - return false; - } - message_length += element_length; - - - // - // QString unpackingCode; - - if (fieldType == "uint8_t_mavlink_version") - { - // unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg("uint8_t", prepends); - unpackingCode = QString("\treturn (%1)(p->%2);").arg("uint8_t", fieldName); - } - else if (fieldType == "uint8_t" || fieldType == "int8_t") - { - // unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); - unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); - } - else if (fieldType == "uint16_t" || fieldType == "int16_t") - { - // unpackingCode = QString("\tgeneric_16bit r;\n\tr.b[1] = (msg->payload%1)[0];\n\tr.b[0] = (msg->payload%1)[1];\n\treturn (%2)r.s;").arg(prepends).arg(fieldType); - unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); - } - else if (fieldType == "uint32_t" || fieldType == "int32_t") - { - // unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.i;").arg(prepends).arg(fieldType); - unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); - } - else if (fieldType == "float") - { - // unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.f;").arg(prepends).arg(fieldType); - unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); - } - else if (fieldType == "uint64_t" || fieldType == "int64_t") - { - // unpackingCode = QString("\tgeneric_64bit r;\n\tr.b[7] = (msg->payload%1)[0];\n\tr.b[6] = (msg->payload%1)[1];\n\tr.b[5] = (msg->payload%1)[2];\n\tr.b[4] = (msg->payload%1)[3];\n\tr.b[3] = (msg->payload%1)[4];\n\tr.b[2] = (msg->payload%1)[5];\n\tr.b[1] = (msg->payload%1)[6];\n\tr.b[0] = (msg->payload%1)[7];\n\treturn (%2)r.ll;").arg(prepends).arg(fieldType); - unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); - } - else if (fieldType.startsWith("array")) - { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string - // unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(filedName, prepends, fieldType.split("[").at(1).split("]").first()); - unpackingCode = QString("\n\tmemcpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); - } - else if (fieldType.startsWith("string")) - { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string - // unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); - unpackingCode = QString("\n\tstrncpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); - } - - - // Generate the message decoding function - if (fieldType.contains("uint8_t_mavlink_version")) - { - // unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); - unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n\tmavlink_%2_t *p = (mavlink_%2_t *)&msg->payload[0];\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); - decodeLines += ""; - prepends += "+sizeof(uint8_t)"; - } - // Array handling is different from simple types - else if (fieldType.startsWith("array")) - { - // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); - unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* %2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n%3\n}\n\n").arg(messageName, fieldName, unpackingCode); - decodeLines += ""; - QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); - prepends += "+" + arrayLength; - } - else if (fieldType.startsWith("string")) - { - // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); - unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* %2)\n{\n\tmavlink_%1 *p = (mavlink_%1 *)&msg->payload[0];\n%3\n}\n\n").arg(messageName, fieldName, unpackingCode); - decodeLines += ""; - QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); - prepends += "+" + arrayLength; - } - else if(fieldType.contains('[') && fieldType.contains(']')) - { - // prevent this case from being caught in the following else - } - else - { - // unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); - unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n\tmavlink_%2_t *p = (mavlink_%2_t *)&msg->payload[0];\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); - decodeLines += ""; - prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; - } - } - f = f.nextSibling(); - } - - if (messageId > highest_message_id) { - highest_message_id = messageId; - } - message_lengths[messageId] = message_length; - - - // Sort fields to ensure 16bit-boundary aligned data - QStringList fieldList; - // Stable sort fields in C structure - fieldList = cStructLines.split("\n", QString::SkipEmptyParts); - if (fieldList.size() > 1) - { - qStableSort(fieldList.begin(), fieldList.end(), structSort); - } - - // struct now sorted, do crc calc for each field - QString fieldCRCstring; - QByteArray fieldText; - crc_key = X25_INIT_CRC; - - for (int i =0; i < fieldList.size(); i++) - { - fieldCRCstring = fieldList.at(i).simplified(); - fieldCRCstring = fieldCRCstring.section(" ",0,1); // note: this has one space bewteen type and name - fieldText = fieldCRCstring.toAscii(); - for (int i = 0; i < fieldText.size(); ++i) - { - crcAccumulate((uint8_t) fieldText.at(i), &crc_key); - } - } - - // generate the key byte value - QString stringCRC; - message_key[messageId] = (crc_key&0xff)^((crc_key>>8)&0xff); - stringCRC = stringCRC.number( message_key[messageId], 16); - - // create structure - cStructLines = fieldList.join("\n") + "\n"; - - // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); - cStruct = cStruct.arg(cStructName, cStructLines ); - lcmStructDefs.append("\n").append(cStruct).append("\n"); - pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); - packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines); - encode = encode.arg(messageName).arg(cStructName).arg(packArguments); -// decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); - decode = decode.arg(messageName).arg(cStructName); -// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); -// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); -// QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); -// QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); -// compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); -// compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; -// QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); - QString compact2Send0( "\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n" ); - QString compact2Send1("static inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\n\tMAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_%4_LEN )\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); - QString compact2Send2("\thdr.sysid = mavlink_system.sysid;\n\thdr.compid = mavlink_system.compid;\n\thdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );\n\tmavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) );\n"); - QString compact2Send3("\n\tcrc_init(&hdr.ck);\n\tcrc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN);\n\tcrc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len );\n\tcrc_accumulate( 0x%1, &hdr.ck); /// include key in X25 checksum\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES);\n\tMAVLINK_BUFFER_CHECK_END\n}\n\n#endif"); - QString compact2Send = compact2Send0 + commentSendContainer.arg(messageName.toLower(), commentLines) + compact2Send1.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines.replace(QString("p->"),QString("payload.")) ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); -// QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; - QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; - cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); - - emit parseState(tr("Compiled message %1 \t(#%3) \tend near line %2, length %4, crc key 0x%5(%6)").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId), QString::number(message_lengths[messageId]), stringCRC.toUpper(), QString::number(message_key[messageId]))); - } // Check if tag = message - } // Check if e = NULL - n = n.nextSibling(); - } // While through - n = p; - - } // Check if tag = messages - } // Check if e = NULL - n = n.nextSibling(); - } // While through include and messages - // One up - current node = parent - n = p; - - } // Check if tag = mavlink - } // Check if e = NULL - n = n.nextSibling(); - } // While through root children - - // Add version to main header - - mainHeader += "// MAVLINK VERSION\n\n"; - mainHeader += QString("#ifndef MAVLINK_VERSION\n#define MAVLINK_VERSION %1\n#endif\n\n").arg(mavlinkVersion); - mainHeader += QString("#if (MAVLINK_VERSION == 0)\n#undef MAVLINK_VERSION\n#define MAVLINK_VERSION %1\n#endif\n\n").arg(mavlinkVersion); - - // Add enums to main header - - mainHeader += "// ENUM DEFINITIONS\n\n"; - mainHeader += enums; - mainHeader += "\n"; - - mainHeader += "// MESSAGE DEFINITIONS\n\n"; - // Create directory if it doesn't exist, report result in success - if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName); - for (int i = 0; i < cFiles.size(); i++) - { - QFile rawFile(dir.filePath(cFiles.at(i).first)); - bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text); - success = success && ok; - rawFile.write(cFiles.at(i).second.toLatin1()); - rawFile.close(); - mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first); - } - - // CRC seeds - mainHeader += "\n\n// MESSAGE CRC KEYS\n\n"; - mainHeader += "#undef MAVLINK_MESSAGE_KEYS\n"; - mainHeader += "#define MAVLINK_MESSAGE_KEYS { "; - for (int i=0; iexecute(generatorCall, arguments) == 0); } + +///** +// * Generate C-code (C-89 compliant) out of the XML protocol specs. +// */ +//bool MAVLinkXMLParserV10::generate() +//{ +// uint16_t crc_key = X25_INIT_CRC; +// // Process result +// bool success = true; + +// // Only generate if output dir is correctly set +// if (outputDirName == "") +// { +// emit parseState(tr("ERROR: No output directory given.\nAbort.")); +// return false; +// } + +// QString topLevelOutputDirName = outputDirName; + +// // print out the element names of all elements that are direct children +// // of the outermost element. +// QDomElement docElem = doc->documentElement(); +// QDomNode n = docElem;//.firstChild(); +// QDomNode p = docElem; + +// // Sanity check variables +// QList* usedMessageIDs = new QList(); +// QMap* usedMessageNames = new QMap(); +// QMap* usedEnumNames = new QMap(); + +// QList< QPair > cFiles; +// QString lcmStructDefs = ""; + +// QString pureFileName; +// QString pureIncludeFileName; + +// QFileInfo fInfo(this->fileName); +// pureFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); + +// // XML parsed and converted to C code. Now generating the files +// outputDirName += QDir::separator() + pureFileName; +// QDateTime now = QDateTime::currentDateTime().toUTC(); +// QLocale loc(QLocale::English); +// QString dateFormat = "dddd, MMMM d yyyy, hh:mm UTC"; +// QString date = loc.toString(now, dateFormat); +// QString includeLine = "#include \"%1\"\n"; +// QString mainHeaderName = pureFileName + ".h"; +// QString messagesDirName = ".";//"generated"; +// QDir dir(outputDirName + "/" + messagesDirName); + +// int mavlinkVersion = 0; +// static unsigned message_lengths[256]; +// static unsigned message_key[256]; +// static int highest_message_id; +// static int recursion_level; + +// if (recursion_level == 0) { +// highest_message_id = 0; +// memset(message_lengths, 0, sizeof(message_lengths)); +// } + + +// // Start main header +// QString mainHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://qgroundcontrol.org/mavlink/\n *\t Generated on %1\n */\n#ifndef " + pureFileName.toUpper() + "_H\n#define " + pureFileName.toUpper() + "_H\n\n").arg(date); // The main header includes all messages +// // Mark all code as C code +// mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n"; +// mainHeader += "\n#include \"../mavlink_protocol.h\"\n"; +// mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n"; + +// QString enums; + + +// // Run through root children +// while(!n.isNull()) +// { +// // Each child is a message +// QDomElement e = n.toElement(); // try to convert the node to an element. +// if(!e.isNull()) +// { +// if (e.tagName() == "mavlink") +// { +// p = n; +// n = n.firstChild(); +// while (!n.isNull()) +// { +// e = n.toElement(); +// if (!e.isNull()) +// { +// // Handle all include tags +// if (e.tagName() == "include") +// { +// QString incFileName = e.text(); +// // Load file +// //QDomDocument includeDoc = QDomDocument(); + +// // Prepend file path if it is a relative path and +// // make it relative to opened file +// QFileInfo fInfo(incFileName); + +// QString incFilePath; +// if (fInfo.isRelative()) +// { +// QFileInfo rInfo(this->fileName); +// incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName; +// pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); +// } + +// QFile file(incFilePath); +// if (file.open(QIODevice::ReadOnly | QIODevice::Text)) +// { +// emit parseState(QString("Included messages from file: %1").arg(incFileName)); +// // NEW MODE: CREATE INDIVIDUAL FOLDERS +// // Create new output directory, parse included XML and generate C-code +// MAVLinkXMLParserV10 includeParser(incFilePath, topLevelOutputDirName, this); +// connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString))); +// // Generate and write +// includeParser.generate(); +// mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n"; + +// emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); +// } +// else +// { +// // Include file could not be opened +// emit parseState(QString("ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.").arg(fileName)); +// return false; +// } + +// } +// // Handle all enum tags +// else if (e.tagName() == "version") +// { +// //QString fieldType = e.attribute("type", ""); +// //QString fieldName = e.attribute("name", ""); +// QString fieldText = e.text(); + +// // Check if version has been previously set +// if (mavlinkVersion != 0) +// { +// emit parseState(QString("ERROR: Protocol version tag set twice, please use it only once. First version was %1, second version is %2.\nAbort.").arg(mavlinkVersion).arg(fieldText)); +// return false; +// } + +// bool ok; +// int version = fieldText.toInt(&ok); +// if (ok && (version > 0) && (version < 256)) +// { +// // Set MAVLink version +// mavlinkVersion = version; +// } +// else +// { +// emit parseState(QString("ERROR: Reading version string failed: %1, string is not an integer number between 1 and 255.\nAbort.").arg(fieldText)); +// return false; +// } +// } +// // Handle all enum tags +// else if (e.tagName() == "enums") +// { +// // One down into the enums list +// p = n; +// n = n.firstChild(); +// while (!n.isNull()) +// { +// e = n.toElement(); + +// QString currEnum; +// QString currEnumEnd; +// // Comment +// QString comment; + +// if(!e.isNull() && e.tagName() == "enum") +// { +// // Get enum name +// QString enumName = e.attribute("name", "").toLower(); +// if (enumName.size() == 0) +// { +// emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); +// return false; +// } +// else +// { +// // Sanity check: Accept only enum names not used previously +// if (usedEnumNames->contains(enumName)) +// { +// emit parseState(tr("ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(enumName, QString::number(e.lineNumber()), fileName)); +// return false; +// } +// else +// { +// usedEnumNames->insert(enumName, QString::number(e.lineNumber())); +// } + +// // Everything sane, starting with enum content +// currEnum = "enum " + enumName.toUpper() + "\n{\n"; +// currEnumEnd = QString("\t%1_ENUM_END\n};\n\n").arg(enumName.toUpper()); + +// int nextEnumValue = 0; + +// // Get the enum fields +// QDomNode f = e.firstChild(); +// while (!f.isNull()) +// { +// QDomElement e2 = f.toElement(); +// if (!e2.isNull() && e2.tagName() == "entry") +// { +// QString fieldValue = e2.attribute("value", ""); + +// // If value was given, use it, if not, use the enum iterator +// // value. The iterator value gets reset by manual values + +// QString fieldName = e2.attribute("name", ""); +// if (fieldValue.length() == 0) +// { +// fieldValue = QString::number(nextEnumValue); +// nextEnumValue++; +// } +// else +// { +// bool ok; +// nextEnumValue = fieldValue.toInt(&ok) + 1; +// if (!ok) +// { +// emit parseState(tr("ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.").arg(fieldName, fieldValue)); +// return false; +// } +// } + +// // Add comment of field if there is one +// QString fieldComment; +// if (e2.text().length() > 0) +// { +// QString sep(" | "); +// QDomNode pp = e2.firstChild(); +// while (!pp.isNull()) +// { +// QDomElement pp2 = pp.toElement(); +// if (pp2.isText() || pp2.isCDATASection()) +// { +// // If this is the only field, don't add the separator +// if (pp.nextSibling().isNull()) +// { +// fieldComment += pp2.nodeValue(); +// } +// else +// { +// fieldComment += pp2.nodeValue() + sep; +// } +// } +// else if (pp2.isElement()) +// { +// // If this is the only field, don't add the separator +// if (pp.nextSibling().isNull()) +// { +// fieldComment += pp2.text(); +// } +// else +// { +// fieldComment += pp2.text() + sep; +// } +// } +// pp = pp.nextSibling(); +// } +// fieldComment = fieldComment.replace("\n", " "); +// fieldComment = " /* " + fieldComment.simplified() + " */"; +// } +// currEnum += "\t" + fieldName.toUpper() + "=" + fieldValue + "," + fieldComment + "\n"; +// } +// else if(!e2.isNull() && e2.tagName() == "description") +// { +// comment = " " + e2.text().replace("\n", " ") + comment; +// } +// f = f.nextSibling(); +// } +// } +// // Add the last parsed enum +// // Remove the last comma, as the last value has none +// // ENUM END MARKER IS LAST ENTRY, COMMA REMOVAL NOT NEEDED +// //int commaPosition = currEnum.lastIndexOf(","); +// //currEnum.remove(commaPosition, 1); + +// enums += "/** @brief " + comment.simplified() + " */\n" + currEnum + currEnumEnd; +// } // Element is non-zero and element name is +// n = n.nextSibling(); +// } // While through +// // One up, back into the structure +// n = p; +// } + +// // Handle all message tags +// else if (e.tagName() == "messages") +// { +// p = n; +// n = n.firstChild(); +// while (!n.isNull()) +// { +// e = n.toElement(); +// if(!e.isNull()) +// { +// //if (e.isNull()) continue; +// // Get message name +// QString messageName = e.attribute("name", "").toLower(); +// if (messageName.size() == 0) +// { +// emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); +// return false; +// } +// else +// { +// // Get message id +// bool ok; +// int messageId = e.attribute("id", "-1").toInt(&ok, 10); +//// emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); + +// // Sanity check: Accept only message IDs not used previously +// if (usedMessageIDs->contains(messageId)) +// { +// emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); +// return false; +// } +// else +// { +// usedMessageIDs->append(messageId); +// } + +// // Sanity check: Accept only message names not used previously +// if (usedMessageNames->contains(messageName)) +// { +// emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); +// return false; +// } +// else +// { +// usedMessageNames->insert(messageName, QString::number(e.lineNumber())); +// } + +// QString channelType("mavlink_channel_t"); +// QString messageType("mavlink_message_t"); +// QString headerType("mavlink_header_t"); + +// // Build up function call +// QString commentContainer("/**\n * @brief Pack a %1 message\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n *\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); +// QString commentPackChanContainer("/**\n * @brief Pack a %1 message\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param chan The MAVLink channel this message was sent over\n * @param msg The MAVLink message to compress the data into\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); +// QString commentSendContainer("/**\n * @brief Send a %1 message\n * @param chan MAVLink channel to send the message\n *\n%2 */\n"); +// QString commentEncodeContainer("/**\n * @brief Encode a %1 struct into a message\n *\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n * @param %1 C-struct to read the message contents from\n */\n"); +// QString commentDecodeContainer("/**\n * @brief Decode a %1 message into a struct\n *\n * @param msg The message to decode\n * @param %1 C-struct to decode the message contents into\n */\n"); +// QString commentEntry(" * @param %1 %2\n"); +// QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2\n#define MAVLINK_MSG_ID_%1_LEN %3\n#define MAVLINK_MSG_%2_LEN %3\n#define MAVLINK_MSG_ID_%1_KEY 0x%4\n#define MAVLINK_MSG_%2_KEY 0x%4"); +// QString arrayDefines; +// QString cStructName = QString("mavlink_%1_t").arg(messageName); +// QString cStruct("typedef struct __%1 \n{\n%2\n} %1;"); +// QString cStructLines; +// QString encode("static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"); + +// // QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"); +// QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n\tmemcpy( %1, msg->payload, sizeof(%2));\n}\n"); +// // QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"); +// QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_%3_LEN);\n}\n\n"); +// // QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);\n}\n\n"); +// QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_%3_LEN);\n}\n\n"); +// //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; +// QString unpacking; +// QString prepends; +// QString packParameters; +// QString packArguments("system_id, component_id, msg"); +// QString packLines; +// QString decodeLines; +// QString sendArguments; +// QString commentLines; +// int calculatedLength = 0; +// unsigned message_length = 0; + + +// // Get the message fields +// QDomNode f = e.firstChild(); + +// // The field types and order are hashed with a checksum + +// // Initialize CRC +// uint16_t fieldHash; +// crcInit(&fieldHash); + +// while (!f.isNull()) +// { +// QDomElement e2 = f.toElement(); +// if (!e2.isNull() && e2.tagName() == "field") +// { +// QString fieldType = e2.attribute("type", ""); +// QString fieldName = e2.attribute("name", ""); +// QString fieldOffset = e2.attribute("offset", ""); +// QString fieldText = e2.text(); + +// QString unpackingCode; +// QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); + +// // Send arguments do not work for the version field +// if (!fieldType.contains("uint8_t_mavlink_version")) +// { +// // Send arguments are the same for integral types and arrays +// sendArguments += ", " + fieldName; +// commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); +// } + +// // MAVLink version field +// // this is a special field always containing the version define +// if (fieldType.contains("uint8_t_mavlink_version")) +// { +// // Add field to C structure +// cStructLines += QString("\t%1 %2;\t///< %3\n").arg("uint8_t", fieldName, fieldText); +// calculatedLength += 1; +// // Add pack line to message_xx_pack function +// // packLines += QString("\ti += put_uint8_t_by_index(%1, i, msg->payload); // %2\n").arg(mavlinkVersion).arg(fieldText); +// packLines += QString("\n\tp->%2 = MAVLINK_VERSION;\t// %1:%3").arg(fieldType, fieldName, e2.text()); +// // Add decode function for this type +// decodeLines += QString("\n\t%1->%2 = mavlink_msg_%1_get_%2(msg);").arg(messageName, fieldName); + +// if (fieldOffset != "") { // does not use the number - always moves up one slot +// QStringList itemList; +// // Swap field in C structure +// itemList = cStructLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// cStructLines = itemList.join("\n") + "\n"; + +// // Swap line in message_xx_pack function +// itemList = packLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// packLines = itemList.join("\n") + "\n"; + +// // Swap line in decode function for this type +// itemList = decodeLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// decodeLines = itemList.join("\n") + "\n"; +// } +// } + +// // ARRAYS are not longer supported - leave error message in here to inform users! +// else if (fieldType.startsWith("array")) +// { +// emit parseState(tr("ERROR: In message %1 deprecated type used near line %2 of file %3. Please change from array[size] to uint8_t[size] to get the same behaviour.\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); +// return false; +// } +// else if (fieldType.startsWith("string")) +// { +// int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); +// // String array is always unsigned char, so bytes +// calculatedLength += arrayLength; +// packParameters += QString(", const ") + QString("char*") + " " + fieldName; +// packArguments += ", " + messageName + "->" + fieldName; + +// // Add field to C structure +// cStructLines += QString("\t%1 %2[%3];\t///< %4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); +// // Add pack line to message_xx_pack function +// // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); +// packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2));\t// %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); +// // Add decode function for this type +// decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); +// arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + +// if (fieldOffset != "") +// { // does not use the number - always moves up one slot +// QStringList itemList; +// // Swap field in C structure +// itemList = cStructLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// cStructLines = itemList.join("\n") + "\n"; + +// // Swap line in message_xx_pack function +// itemList = packLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// packLines = itemList.join("\n") + "\n"; + +// // Swap line in decode function for this type +// itemList = decodeLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// decodeLines = itemList.join("\n") + "\n"; +// } +// } +// // Expand array handling to all valid mavlink data types +// else if(fieldType.contains('[') && fieldType.contains(']')) +// { +// int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); +// QString arrayType = fieldType.split("[").first(); +// if (arrayType.contains("array")) +// { +// calculatedLength += arrayLength; +// } +// else if (arrayType.contains("char")) +// { +// calculatedLength += arrayLength; +// } +// else if (arrayType.contains("int8")) +// { +// calculatedLength += arrayLength; +// } +// else if (arrayType.contains("int16")) +// { +// calculatedLength += arrayLength*2; +// } +// else if (arrayType.contains("int32")) +// { +// calculatedLength += arrayLength*4; +// } +// else if (arrayType.contains("int64")) +// { +// calculatedLength += arrayLength*8; +// } +// else if (arrayType == "float") +// { +// calculatedLength += arrayLength*4; +// } +// else +// { +// emit parseState(tr("ERROR: In message %1 invalid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); +// return false; +// } +// packParameters += QString(", const ") + arrayType + "* " + fieldName; +// packArguments += ", " + messageName + "->" + fieldName; + +// // Add field to C structure +// cStructLines += QString("\t%1 %2[%3];\t///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); +// // Add pack line to message_xx_pack function +// // packLines += QString("\ti += put_array_by_index((const int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); +// packLines += QString("\tmemcpy(p->%2, %2, sizeof(p->%2));\t// %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); +// // Add decode function for this type +// decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); + +// if (fieldOffset != "") +// { // does not use the number - always moves up one slot +// QStringList itemList; +// // Swap field in C structure +// itemList = cStructLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// cStructLines = itemList.join("\n") + "\n"; + +// // Swap line in message_xx_pack function +// itemList = packLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// packLines = itemList.join("\n") + "\n"; + +// // Swap line in decode function for this type +// itemList = decodeLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// decodeLines = itemList.join("\n") + "\n"; +// } + +// arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + +// // unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, sizeof(%2)*%3);\n\treturn sizeof(%2)*%3;").arg(prepends, arrayType, QString::number(arrayLength)); +// unpackingCode = QString("\n\tmemcpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); + +// // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); +// unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* %2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); +// // decodeLines += ""; +// prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); + +// } +// else +// // Handle simple types like integers and floats +// { +// packParameters += ", " + fieldType + " " + fieldName; +// packArguments += ", " + messageName + "->" + fieldName; + +// // Add field to C structure +// cStructLines += QString("\t%1 %2;\t///< %3\n").arg(fieldType, fieldName, fieldText); +// // Add pack line to message_xx_pack function +// // packLines += QString("\ti += put_%1_by_index(%2, i, msg->payload); // %3\n").arg(fieldType, fieldName, e2.text()); +// packLines += QString("\tp->%2 = %2;\t// %1:%3\n").arg(fieldType, fieldName, e2.text()); +// // Add decode function for this type +// // decodeLines += QString("\t%1->%2 = mavlink_msg_%1_get_%2(msg);\n").arg(messageName, fieldName); +// decodeLines += QString("\n\t%1 = p->%1;").arg(fieldName); + +// if (fieldOffset != "") { // does not use the number - always moves up one slot +// QStringList itemList; +// // Swap field in C structure +// itemList = cStructLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// cStructLines = itemList.join("\n") + "\n"; + +// // Swap line in message_xx_pack function +// itemList = packLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// packLines = itemList.join("\n") + "\n"; + +// // Swap line in decode function for this type +// itemList = decodeLines.split("\n", QString::SkipEmptyParts); +// if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); +// decodeLines = itemList.join("\n") + "\n"; +// } +// if (fieldType.contains("char")) +// { +// calculatedLength += 1; +// } +// else if (fieldType.contains("int8")) +// { +// calculatedLength += 1; +// } +// else if (fieldType.contains("int16")) +// { +// calculatedLength += 2; +// } +// else if (fieldType.contains("int32")) +// { +// calculatedLength += 4; +// } +// else if (fieldType.contains("int64")) +// { +// calculatedLength += 8; +// } +// else if (fieldType == "float") +// { +// calculatedLength += 4; +// } +// else +// { +// emit parseState(tr("ERROR: In message %1 inavlid type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, fieldType)); +// return false; +// } +// } + +// // message length calculation +// unsigned element_multiplier = 1; +// unsigned element_length = 0; + +// if (fieldType.contains("[")) { +// element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); +// } +// for (unsigned i=0; iERROR: Unable to calculate length for %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), fieldType)); +// return false; +// } +// message_length += element_length; + + +// // +// // QString unpackingCode; + +// if (fieldType == "uint8_t_mavlink_version") +// { +// // unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg("uint8_t", prepends); +// unpackingCode = QString("\treturn (%1)(p->%2);").arg("uint8_t", fieldName); +// } +// else if (fieldType == "uint8_t" || fieldType == "int8_t") +// { +// // unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); +// unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); +// } +// else if (fieldType == "uint16_t" || fieldType == "int16_t") +// { +// // unpackingCode = QString("\tgeneric_16bit r;\n\tr.b[1] = (msg->payload%1)[0];\n\tr.b[0] = (msg->payload%1)[1];\n\treturn (%2)r.s;").arg(prepends).arg(fieldType); +// unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); +// } +// else if (fieldType == "uint32_t" || fieldType == "int32_t") +// { +// // unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.i;").arg(prepends).arg(fieldType); +// unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); +// } +// else if (fieldType == "float") +// { +// // unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.f;").arg(prepends).arg(fieldType); +// unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); +// } +// else if (fieldType == "uint64_t" || fieldType == "int64_t") +// { +// // unpackingCode = QString("\tgeneric_64bit r;\n\tr.b[7] = (msg->payload%1)[0];\n\tr.b[6] = (msg->payload%1)[1];\n\tr.b[5] = (msg->payload%1)[2];\n\tr.b[4] = (msg->payload%1)[3];\n\tr.b[3] = (msg->payload%1)[4];\n\tr.b[2] = (msg->payload%1)[5];\n\tr.b[1] = (msg->payload%1)[6];\n\tr.b[0] = (msg->payload%1)[7];\n\treturn (%2)r.ll;").arg(prepends).arg(fieldType); +// unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); +// } +// else if (fieldType.startsWith("array")) +// { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string +// // unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(filedName, prepends, fieldType.split("[").at(1).split("]").first()); +// unpackingCode = QString("\n\tmemcpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); +// } +// else if (fieldType.startsWith("string")) +// { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string +// // unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); +// unpackingCode = QString("\n\tstrncpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); +// } + + +// // Generate the message decoding function +// if (fieldType.contains("uint8_t_mavlink_version")) +// { +// // unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); +// unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n\tmavlink_%2_t *p = (mavlink_%2_t *)&msg->payload[0];\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); +// decodeLines += ""; +// prepends += "+sizeof(uint8_t)"; +// } +// // Array handling is different from simple types +// else if (fieldType.startsWith("array")) +// { +// // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); +// unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* %2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n%3\n}\n\n").arg(messageName, fieldName, unpackingCode); +// decodeLines += ""; +// QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); +// prepends += "+" + arrayLength; +// } +// else if (fieldType.startsWith("string")) +// { +// // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); +// unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* %2)\n{\n\tmavlink_%1 *p = (mavlink_%1 *)&msg->payload[0];\n%3\n}\n\n").arg(messageName, fieldName, unpackingCode); +// decodeLines += ""; +// QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); +// prepends += "+" + arrayLength; +// } +// else if(fieldType.contains('[') && fieldType.contains(']')) +// { +// // prevent this case from being caught in the following else +// } +// else +// { +// // unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); +// unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n\tmavlink_%2_t *p = (mavlink_%2_t *)&msg->payload[0];\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); +// decodeLines += ""; +// prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; +// } +// } +// f = f.nextSibling(); +// } + +// if (messageId > highest_message_id) { +// highest_message_id = messageId; +// } +// message_lengths[messageId] = message_length; + + +// // Sort fields to ensure 16bit-boundary aligned data +// QStringList fieldList; +// // Stable sort fields in C structure +// fieldList = cStructLines.split("\n", QString::SkipEmptyParts); +// if (fieldList.size() > 1) +// { +// qStableSort(fieldList.begin(), fieldList.end(), structSort); +// } + +// // struct now sorted, do crc calc for each field +// QString fieldCRCstring; +// QByteArray fieldText; +// crc_key = X25_INIT_CRC; + +// for (int i =0; i < fieldList.size(); i++) +// { +// fieldCRCstring = fieldList.at(i).simplified(); +// fieldCRCstring = fieldCRCstring.section(" ",0,1); // note: this has one space bewteen type and name +// fieldText = fieldCRCstring.toAscii(); +// for (int i = 0; i < fieldText.size(); ++i) +// { +// crcAccumulate((uint8_t) fieldText.at(i), &crc_key); +// } +// } + +// // generate the key byte value +// QString stringCRC; +// message_key[messageId] = (crc_key&0xff)^((crc_key>>8)&0xff); +// stringCRC = stringCRC.number( message_key[messageId], 16); + +// // create structure +// cStructLines = fieldList.join("\n") + "\n"; + +// // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); +// cStruct = cStruct.arg(cStructName, cStructLines ); +// lcmStructDefs.append("\n").append(cStruct).append("\n"); +// pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); +// packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines); +// encode = encode.arg(messageName).arg(cStructName).arg(packArguments); +//// decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); +// decode = decode.arg(messageName).arg(cStructName); +//// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); +//// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); +//// QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); +//// QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); +//// compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); +//// compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; +//// QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); +// QString compact2Send0( "\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n" ); +// QString compact2Send1("static inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\n\tMAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_%4_LEN )\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); +// QString compact2Send2("\thdr.sysid = mavlink_system.sysid;\n\thdr.compid = mavlink_system.compid;\n\thdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );\n\tmavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) );\n"); +// QString compact2Send3("\n\tcrc_init(&hdr.ck);\n\tcrc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN);\n\tcrc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len );\n\tcrc_accumulate( 0x%1, &hdr.ck); /// include key in X25 checksum\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES);\n\tMAVLINK_BUFFER_CHECK_END\n}\n\n#endif"); +// QString compact2Send = compact2Send0 + commentSendContainer.arg(messageName.toLower(), commentLines) + compact2Send1.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines.replace(QString("p->"),QString("payload.")) ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); +//// QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; +// QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; +// cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); + +// emit parseState(tr("Compiled message %1 \t(#%3) \tend near line %2, length %4, crc key 0x%5(%6)").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId), QString::number(message_lengths[messageId]), stringCRC.toUpper(), QString::number(message_key[messageId]))); +// } // Check if tag = message +// } // Check if e = NULL +// n = n.nextSibling(); +// } // While through +// n = p; + +// } // Check if tag = messages +// } // Check if e = NULL +// n = n.nextSibling(); +// } // While through include and messages +// // One up - current node = parent +// n = p; + +// } // Check if tag = mavlink +// } // Check if e = NULL +// n = n.nextSibling(); +// } // While through root children + +// // Add version to main header + +// mainHeader += "// MAVLINK VERSION\n\n"; +// mainHeader += QString("#ifndef MAVLINK_VERSION\n#define MAVLINK_VERSION %1\n#endif\n\n").arg(mavlinkVersion); +// mainHeader += QString("#if (MAVLINK_VERSION == 0)\n#undef MAVLINK_VERSION\n#define MAVLINK_VERSION %1\n#endif\n\n").arg(mavlinkVersion); + +// // Add enums to main header + +// mainHeader += "// ENUM DEFINITIONS\n\n"; +// mainHeader += enums; +// mainHeader += "\n"; + +// mainHeader += "// MESSAGE DEFINITIONS\n\n"; +// // Create directory if it doesn't exist, report result in success +// if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName); +// for (int i = 0; i < cFiles.size(); i++) +// { +// QFile rawFile(dir.filePath(cFiles.at(i).first)); +// bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text); +// success = success && ok; +// rawFile.write(cFiles.at(i).second.toLatin1()); +// rawFile.close(); +// mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first); +// } + +// // CRC seeds +// mainHeader += "\n\n// MESSAGE CRC KEYS\n\n"; +// mainHeader += "#undef MAVLINK_MESSAGE_KEYS\n"; +// mainHeader += "#define MAVLINK_MESSAGE_KEYS { "; +// for (int i=0; i>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +//} + +///** +// * @param crcAccum the 16 bit X.25 CRC +// */ +//void MAVLinkXMLParserV10::crcInit(uint16_t* crcAccum) +//{ +// *crcAccum = X25_INIT_CRC; +//} + + +//const struct { +// const char *prefix; +// unsigned length; +//} length_map[] = { +// { "array", 1 }, +// { "char", 1 }, +// { "uint8", 1 }, +// { "int8", 1 }, +// { "uint16", 2 }, +// { "int16", 2 }, +// { "uint32", 4 }, +// { "int32", 4 }, +// { "uint64", 8 }, +// { "int64", 8 }, +// { "float", 4 }, +// { "double", 8 }, +//}; + +//unsigned itemLength( const QString &s1 ) +//{ +// unsigned el1, i1, i2; +// QString Ss1 = s1; + +// Ss1 = Ss1.replace("_"," "); +// Ss1 = Ss1.simplified(); +// Ss1 = Ss1.section(" ",0,0); + +// el1 = i1 = 0; +// i2 = sizeof(length_map)/sizeof(length_map[0]); + +// do { +// if (Ss1.startsWith(length_map[i1].prefix)) +// { +// el1 = length_map[i1].length; +// } +// i1++; +// } while ( (el1 == 0) && (i1 < i2)); +// return el1; +//} + +//bool structSort(const QString &s1, const QString &s2) +//{ +// unsigned el1, el2; + +// el1 = itemLength( s1 ); +// el2 = itemLength( s2 ); +// return el2 < el1; +//} diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h index c52c546..7e1e38a 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h @@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include @@ -55,20 +56,24 @@ public slots: /** @brief Parse XML and generate C files */ bool generate(); + /** @brief Handle process errors */ + void processError(QProcess::ProcessError err); + signals: /** @brief Status message on the parsing */ void parseState(QString message); protected: - /** @brief Accumulate the X.25 CRC by adding one char at a time. */ - void crcAccumulate(uint8_t data, uint16_t *crcAccum); +// /** @brief Accumulate the X.25 CRC by adding one char at a time. */ +// void crcAccumulate(uint8_t data, uint16_t *crcAccum); - /** @brief Initialize the buffer for the X.25 CRC */ - void crcInit(uint16_t* crcAccum); +// /** @brief Initialize the buffer for the X.25 CRC */ +// void crcInit(uint16_t* crcAccum); QDomDocument* doc; QString outputDirName; QString fileName; + QProcess* process; }; #endif // MAVLINKXMLPARSERV10_H diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index 85e582f..1c57209 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -8,6 +8,18 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256); memset(receivedMessages, 0, sizeof(mavlink_message_t)*256); + // Fill filter + messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false); + messageFilter.insert(MAVLINK_MSG_ID_SYS_STATUS, false); + messageFilter.insert(MAVLINK_MSG_ID_STATUSTEXT, false); + messageFilter.insert(MAVLINK_MSG_ID_COMMAND, false); + messageFilter.insert(MAVLINK_MSG_ID_COMMAND_ACK, false); + messageFilter.insert(MAVLINK_MSG_ID_PARAM_SET, false); + messageFilter.insert(MAVLINK_MSG_ID_PARAM_VALUE, false); + messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false); + messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false); + messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false); + connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); } @@ -24,20 +36,21 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag quint64 time = 0; uint8_t fieldid = 0; uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8; - if (messageInfo[msgid].fields[fieldid].name == "time_boot_ms" && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT32_T) + if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_boot_ms") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT32_T) { time = *((quint32*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); } - else if (messageInfo[msgid].fields[fieldid].name == "time_usec" && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT64_T) + else if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_usec") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT64_T) { time = *((quint64*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); } else { + // First value is not time, send out value 0 emitFieldValue(&message, fieldid, time); } - // Send out field values + // Send out field values from 1..n for (unsigned int i = 1; i < messageInfo[msgid].num_fields; ++i) { emitFieldValue(&message, i, time); @@ -51,6 +64,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 { // Add field tree widget item uint8_t msgid = msg->msgid; + if (messageFilter.contains(msgid)) return; QString fieldName(messageInfo[msgid].fields[fieldid].name); QString fieldType; uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8; diff --git a/src/ui/MAVLinkDecoder.h b/src/ui/MAVLinkDecoder.h index 6f1c271..4e55eb4 100644 --- a/src/ui/MAVLinkDecoder.h +++ b/src/ui/MAVLinkDecoder.h @@ -27,7 +27,8 @@ protected: void emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time); mavlink_message_t receivedMessages[256]; ///< Available / known messages - mavlink_message_info_t messageInfo[256]; + mavlink_message_info_t messageInfo[256]; ///< Message information + QMap messageFilter; ///< Message/field names not to emit }; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 3060c0f..53e0d31 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -91,8 +91,8 @@ MainWindow::MainWindow(QWidget *parent): styleFileName(QCoreApplication::applicationDirPath() + "/style-indoor.css"), autoReconnect(false), currentStyle(QGC_MAINWINDOW_STYLE_INDOOR), - lowPowerMode(false), - centerStackActionGroup(this) + centerStackActionGroup(this), + lowPowerMode(false) { loadSettings(); if (!settings.contains("CURRENT_VIEW")) @@ -115,12 +115,20 @@ MainWindow::MainWindow(QWidget *parent): settings.sync(); - // Setup UI state machines - centerStackActionGroup.setExclusive(true); + loadStyle(currentStyle); // Setup user interface ui.setupUi(this); + // Set dock options + setDockOptions(AnimatedDocks | AllowTabbedDocks | AllowNestedDocks); + statusBar()->setSizeGripEnabled(true); + + configureWindowName(); + + // Setup UI state machines + centerStackActionGroup.setExclusive(true); + centerStack = new QStackedWidget(this); setCentralWidget(centerStack); @@ -131,22 +139,14 @@ MainWindow::MainWindow(QWidget *parent): toolBar->addPerspectiveChangeAction(ui.actionOperatorsView); toolBar->addPerspectiveChangeAction(ui.actionEngineersView); toolBar->addPerspectiveChangeAction(ui.actionPilotsView); -// toolBar->addPerspectiveChangeAction(ui.actionUnconnectedView); buildCommonWidgets(); connectCommonWidgets(); - configureWindowName(); - - loadStyle(currentStyle); - // Create actions connectCommonActions(); - // Set dock options - setDockOptions(AnimatedDocks | AllowTabbedDocks | AllowNestedDocks); - statusBar()->setSizeGripEnabled(true); // Restore the window setup if (settings.contains(getWindowStateKey())) @@ -179,14 +179,6 @@ MainWindow::MainWindow(QWidget *parent): joystickWidget = 0; joystick = new JoystickInput(); - // Load Toolbar - toolBar = new QGCToolBar(this); - this->addToolBar(toolBar); - // Add actions - toolBar->addPerspectiveChangeAction(ui.actionOperatorsView); - toolBar->addPerspectiveChangeAction(ui.actionEngineersView); - toolBar->addPerspectiveChangeAction(ui.actionPilotsView); - // Connect link if (autoReconnect) { @@ -204,6 +196,9 @@ MainWindow::MainWindow(QWidget *parent): windowStateVal = windowState(); show(); + + connect(&windowNameUpdateTimer, SIGNAL(timeout()), this, SLOT(configureWindowName())); + windowNameUpdateTimer.start(15000); } MainWindow::~MainWindow() diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 3bb0bf4..f222620 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -213,6 +213,9 @@ public slots: */ void showCentralWidget(); + /** @brief Update the window name */ + void configureWindowName(); + public: QGCMAVLinkLogPlayer* getLogPlayer() { @@ -283,7 +286,6 @@ protected: void connectCommonWidgets(); void connectCommonActions(); - void configureWindowName(); void loadSettings(); void storeSettings(); @@ -369,6 +371,7 @@ protected: Qt::WindowStates windowStateVal; bool lowPowerMode; ///< If enabled, QGC reduces the update rates of all widgets QGCFlightGearLink* fgLink; + QTimer windowNameUpdateTimer; private: Ui::MainWindow ui; diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 14a00b6..0739b55 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -641,7 +641,7 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit) curvesWidgetLayout->addWidget(label, labelRow, 2); //checkBox->setText(QString()); - label->setText(curve); + label->setText(getCurveName(curve+unit, ui.shortNameCheckBox->isChecked())); QColor color(Qt::gray);// = plot->getColorForCurve(curve+unit); QString colorstyle; colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue()); @@ -770,54 +770,70 @@ void LinechartWidget::recolor() } } -void LinechartWidget::setShortNames(bool enable) +QString LinechartWidget::getCurveName(const QString& key, bool shortEnabled) { - foreach (QString key, curveNames.keys()) + if (shortEnabled) { QString name; - if (enable) + QStringList parts = curveNames.value(key).split("."); + if (parts.length() > 1) { - QStringList parts = curveNames.value(key).split("."); - if (parts.length() > 1) - { - name = parts.at(1); - } - else - { - name = parts.at(0); - } - - const unsigned int sizeLimit = 10; + name = parts.at(1); + } + else + { + name = parts.at(0); + } - // Replace known words with abbreviations - if (name.length() > sizeLimit) - { - name.replace("gyroscope", "gyro"); - name.replace("accelerometer", "acc"); - name.replace("magnetometer", "mag"); - name.replace("distance", "dist"); - name.replace("altitude", "alt"); - name.replace("waypoint", "wp"); - name.replace("error", "err"); - name.replace("message", "msg"); - name.replace("source", "src"); - } + const int sizeLimit = 20; - // Check if sub-part is still exceeding N chars - if (name.length() > sizeLimit) - { - name.replace("a", ""); - name.replace("e", ""); - name.replace("i", ""); - name.replace("o", ""); - name.replace("u", ""); - } + // Replace known words with abbreviations + if (name.length() > sizeLimit) + { + name.replace("gyroscope", "gyro"); + name.replace("accelerometer", "acc"); + name.replace("magnetometer", "mag"); + name.replace("distance", "dist"); + name.replace("ailerons", "ail"); + name.replace("altitude", "alt"); + name.replace("waypoint", "wp"); + name.replace("throttle", "thr"); + name.replace("elevator", "elev"); + name.replace("rudder", "rud"); + name.replace("error", "err"); + name.replace("version", "ver"); + name.replace("message", "msg"); + name.replace("count", "cnt"); + name.replace("value", "val"); + name.replace("source", "src"); + name.replace("index", "idx"); + name.replace("type", "typ"); + name.replace("mode", "mod"); } - else + + // Check if sub-part is still exceeding N chars + if (name.length() > sizeLimit) { - name = curveNames.value(key); + name.replace("a", ""); + name.replace("e", ""); + name.replace("i", ""); + name.replace("o", ""); + name.replace("u", ""); } - curveNameLabels.value(key)->setText(name); + + return name; + } + else + { + return curveNames.value(key); + } +} + +void LinechartWidget::setShortNames(bool enable) +{ + foreach (QString key, curveNames.keys()) + { + curveNameLabels.value(key)->setText(getCurveName(key, enable)); } } diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index f094eb1..ff429bc 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -124,6 +124,8 @@ protected: QToolButton* createButton(QWidget* parent); void createCurveItem(QString curve); void createLayout(); + /** @brief Get the name for a curve key */ + QString getCurveName(const QString& key, bool shortEnabled); int sysid; ///< ID of the unmanned system this plot belongs to LinechartPlot* activePlot; ///< Plot for this system