From 4cc06ffcb292e051249f7b504f6a2569623f2fc1 Mon Sep 17 00:00:00 2001 From: lm Date: Sat, 13 Aug 2011 09:24:57 +0200 Subject: [PATCH] Updated MAVLink code generator --- .../mavlinkgen/generator/MAVLinkXMLParser.cc | 233 +++++++--- .../generator/MAVLinkXMLParserV10.cc | 51 +- .../generator/MAVLinkXMLParserV10.h | 1 + src/apps/mavlinkgen/mavlinkgen.pri | 2 + src/apps/mavlinkgen/mavlinkgen.pro | 7 +- src/apps/mavlinkgen/msinttypes/inttypes.h | 305 ++++++++++++ src/apps/mavlinkgen/msinttypes/stdint.h | 247 ++++++++++ src/apps/mavlinkgen/template/checksum.h | 139 ++++++ .../mavlinkgen/template/documentation.dox | 41 ++ .../mavlinkgen/template/mavlink_checksum.h | 183 ++++++++ src/apps/mavlinkgen/template/mavlink_data.h | 21 + .../mavlinkgen/template/mavlink_options.h | 135 ++++++ .../mavlinkgen/template/mavlink_protocol.h | 423 +++++++++++++++++ src/apps/mavlinkgen/template/mavlink_types.h | 120 +++++ src/apps/mavlinkgen/template/protocol.h | 439 ++++++++++++++++++ .../mavlinkgen/ui/XMLCommProtocolWidget.cc | 34 +- .../mavlinkgen/ui/XMLCommProtocolWidget.ui | 45 +- 17 files changed, 2321 insertions(+), 105 deletions(-) create mode 100644 src/apps/mavlinkgen/msinttypes/inttypes.h create mode 100644 src/apps/mavlinkgen/msinttypes/stdint.h create mode 100644 src/apps/mavlinkgen/template/checksum.h create mode 100644 src/apps/mavlinkgen/template/documentation.dox create mode 100644 src/apps/mavlinkgen/template/mavlink_checksum.h create mode 100644 src/apps/mavlinkgen/template/mavlink_data.h create mode 100644 src/apps/mavlinkgen/template/mavlink_options.h create mode 100644 src/apps/mavlinkgen/template/mavlink_protocol.h create mode 100644 src/apps/mavlinkgen/template/mavlink_types.h create mode 100644 src/apps/mavlinkgen/template/protocol.h diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc index 34cc9ec9b..66769ae15 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc @@ -1,4 +1,24 @@ /*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009 - 2011 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + ======================================================================*/ /** @@ -29,7 +49,8 @@ MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QO { doc = new QDomDocument(); QFile file(document); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { const QString instanceText(QString::fromUtf8(file.readAll())); doc->setContent(instanceText); } @@ -50,7 +71,8 @@ bool MAVLinkXMLParser::generate() bool success = true; // Only generate if output dir is correctly set - if (outputDirName == "") { + if (outputDirName == "") + { emit parseState(tr("ERROR: No output directory given.\nAbort.")); return false; } @@ -96,7 +118,7 @@ bool MAVLinkXMLParser::generate() static int highest_message_id; static int recursion_level; - if (recursion_level == 0) { + if (recursion_level == 0){ highest_message_id = 0; memset(message_lengths, 0, sizeof(message_lengths)); } @@ -113,18 +135,24 @@ bool MAVLinkXMLParser::generate() // Run through root children - while(!n.isNull()) { + 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") { + if(!e.isNull()) + { + if (e.tagName() == "mavlink") + { p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); - if (!e.isNull()) { + if (!e.isNull()) + { // Handle all include tags - if (e.tagName() == "include") { + if (e.tagName() == "include") + { QString incFileName = e.text(); // Load file //QDomDocument includeDoc = QDomDocument(); @@ -134,14 +162,16 @@ bool MAVLinkXMLParser::generate() QFileInfo fInfo(incFileName); QString incFilePath; - if (fInfo.isRelative()) { + 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)) { + 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 @@ -175,7 +205,9 @@ bool MAVLinkXMLParser::generate() // } emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); - } else { + } + 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; @@ -183,33 +215,40 @@ bool MAVLinkXMLParser::generate() } // Handle all enum tags - else if (e.tagName() == "version") { + 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) { + 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)) { + if (ok && (version > 0) && (version < 256)) + { // Set MAVLink version mavlinkVersion = version; - } else { + } + 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") { + else if (e.tagName() == "enums") + { // One down into the enums list p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); QString currEnum; @@ -217,18 +256,25 @@ bool MAVLinkXMLParser::generate() // Comment QString comment; - if(!e.isNull() && e.tagName() == "enum") { + if(!e.isNull() && e.tagName() == "enum") + { // Get enum name QString enumName = e.attribute("name", "").toLower(); - if (enumName.size() == 0) { + 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 { + } + else + { // Sanity check: Accept only enum names not used previously - if (usedEnumNames->contains(enumName)) { + 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 { + } + else + { usedEnumNames->insert(enumName, QString::number(e.lineNumber())); } @@ -240,22 +286,28 @@ bool MAVLinkXMLParser::generate() // Get the enum fields QDomNode f = e.firstChild(); - while (!f.isNull()) { + while (!f.isNull()) + { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "entry") { + 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) { + if (fieldValue.length() == 0) + { fieldValue = QString::number(nextEnumValue); nextEnumValue++; - } else { + } + else + { bool ok; nextEnumValue = fieldValue.toInt(&ok) + 1; - if (!ok) { + 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; } @@ -267,7 +319,8 @@ bool MAVLinkXMLParser::generate() { QString sep(" | "); QDomNode pp = e2.firstChild(); - while (!pp.isNull()) { + while (!pp.isNull()) + { QDomElement pp2 = pp.toElement(); if (pp2.isText() || pp2.isCDATASection()) { @@ -306,37 +359,49 @@ bool MAVLinkXMLParser::generate() } // Handle all message tags - else if (e.tagName() == "messages") { + else if (e.tagName() == "messages") + { p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); - if(!e.isNull()) { + if(!e.isNull()) + { //if (e.isNull()) continue; // Get message name QString messageName = e.attribute("name", "").toLower(); - if (messageName.size() == 0) { + 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 { + } + 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)) { + 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 { + } + else + { usedMessageIDs->append(messageId); } // Sanity check: Accept only message names not used previously - if (usedMessageNames->contains(messageName)) { + 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 { + } + else + { usedMessageNames->insert(messageName, QString::number(e.lineNumber())); } @@ -375,9 +440,11 @@ bool MAVLinkXMLParser::generate() // Get the message fields QDomNode f = e.firstChild(); - while (!f.isNull()) { + while (!f.isNull()) + { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "field") { + if (!e2.isNull() && e2.tagName() == "field") + { QString fieldType = e2.attribute("type", ""); QString fieldName = e2.attribute("name", ""); QString fieldText = e2.text(); @@ -386,7 +453,8 @@ bool MAVLinkXMLParser::generate() 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")) { + 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", " ")); @@ -394,7 +462,8 @@ bool MAVLinkXMLParser::generate() // MAVLink version field // this is a special field always containing the version define - if (fieldType.contains("uint8_t_mavlink_version")) { + if (fieldType.contains("uint8_t_mavlink_version")) + { // Add field to C structure cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); // Add pack line to message_xx_pack function @@ -404,7 +473,8 @@ bool MAVLinkXMLParser::generate() } // Array handling is different from simple types - else if (fieldType.startsWith("array")) { + else if (fieldType.startsWith("array")) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("int8_t*") + " " + fieldName; @@ -417,7 +487,9 @@ bool MAVLinkXMLParser::generate() // Add decode function for this type decodeLines += QString("\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)); - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("char*") + " " + fieldName; @@ -432,7 +504,8 @@ bool MAVLinkXMLParser::generate() arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); } // Expand array handling to all valid mavlink data types - else if(fieldType.contains('[') && fieldType.contains(']')) { + else if(fieldType.contains('[') && fieldType.contains(']')) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + arrayType + "* " + fieldName; @@ -452,7 +525,8 @@ bool MAVLinkXMLParser::generate() // decodeLines += ""; prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); - } else + } + else // Handle simple types like integers and floats { packParameters += ", " + fieldType + " " + fieldName; @@ -470,7 +544,8 @@ bool MAVLinkXMLParser::generate() // message length calculation unsigned element_multiplier = 1; unsigned element_length = 0; - const struct { + const struct + { const char *prefix; unsigned length; } length_map[] = { @@ -487,10 +562,12 @@ bool MAVLinkXMLParser::generate() { "float", 4 }, { "double", 8 }, }; - if (fieldType.contains("[")) { + if (fieldType.contains("[")) + { element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); } - for (unsigned i=0; ipayload%2)[0];").arg("uint8_t", prepends); - } else if (fieldType == "uint8_t" || fieldType == "int8_t") { + } + else if (fieldType == "uint8_t" || fieldType == "int8_t") + { unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); - } else if (fieldType == "uint16_t" || fieldType == "int16_t") { + } + 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); - } else if (fieldType == "uint32_t" || fieldType == "int32_t") { + } + 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); - } else if (fieldType == "float") { + } + 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); - } else if (fieldType == "uint64_t" || fieldType == "int64_t") { + } + 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); - } else if (fieldType.startsWith("array")) { + } + 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(prepends, fieldType.split("[").at(1).split("]").first()); - } else if (fieldType.startsWith("string")) { + } + 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()); } // Generate the message decoding function - if (fieldType.contains("uint8_t_mavlink_version")) { + 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); decodeLines += ""; prepends += "+sizeof(uint8_t)"; } // Array handling is different from simple types - else if (fieldType.startsWith("array")) { + 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); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } else if (fieldType.startsWith("string")) { + } + 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); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } else if(fieldType.contains('[') && fieldType.contains(']')) { + } + else if(fieldType.contains('[') && fieldType.contains(']')) + { // prevent this case from being caught in the following else - } 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); decodeLines += ""; prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; @@ -553,7 +653,8 @@ bool MAVLinkXMLParser::generate() f = f.nextSibling(); } - if (messageId > highest_message_id) { + if (messageId > highest_message_id) + { highest_message_id = messageId; } message_lengths[messageId] = message_length; @@ -600,7 +701,8 @@ bool MAVLinkXMLParser::generate() 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++) { + 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; @@ -612,7 +714,8 @@ bool MAVLinkXMLParser::generate() mainHeader += "\n\n// MESSAGE LENGTHS\n\n"; mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n"; mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { "; - for (int i=0; i #include #include + #include "MAVLinkXMLParserV10.h" #include @@ -129,8 +130,9 @@ unsigned itemLength( const QString &s1 ) do { if (Ss1.startsWith(length_map[i1].prefix)) + { el1 = length_map[i1].length; - else ; + } i1++; } while ( (el1 == 0) && (i1 < i2)); return el1; @@ -210,7 +212,7 @@ bool MAVLinkXMLParserV10::generate() 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 \"../protocol.h\"\n"; + mainHeader += "\n#include \"../mavlink_protocol.h\"\n"; mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n"; QString enums; @@ -456,7 +458,7 @@ bool MAVLinkXMLParserV10::generate() // 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))); +// 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)) @@ -491,7 +493,7 @@ bool MAVLinkXMLParserV10::generate() 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"); + 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;"); @@ -552,11 +554,11 @@ bool MAVLinkXMLParserV10::generate() if (fieldType.contains("uint8_t_mavlink_version")) { // Add field to C structure - cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); + 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; // %1:%3").arg(fieldType, fieldName, e2.text()); + 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); @@ -594,10 +596,10 @@ bool MAVLinkXMLParserV10::generate() packArguments += ", " + messageName + "->" + fieldName; // Add field to C structure - cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); + 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)); // %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); + 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)); @@ -663,10 +665,10 @@ bool MAVLinkXMLParserV10::generate() packArguments += ", " + messageName + "->" + fieldName; // Add field to C structure - cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + 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)); // %1[%3]:%4\n").arg(arrayType, fieldName, 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); @@ -707,10 +709,10 @@ bool MAVLinkXMLParserV10::generate() packArguments += ", " + messageName + "->" + fieldName; // Add field to C structure - cStructLines += QString("\t%1 %2; ///< %3\n").arg(fieldType, fieldName, fieldText); + 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; // %1:%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); @@ -882,7 +884,7 @@ bool MAVLinkXMLParserV10::generate() if (fieldList.size() > 1) { qStableSort(fieldList.begin(), fieldList.end(), structSort); - } else ; + } // struct now sorted, do crc calc for each field QString fieldCRCstring; @@ -908,9 +910,7 @@ bool MAVLinkXMLParserV10::generate() // create structure cStructLines = fieldList.join("\n") + "\n"; - - - // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); + // 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); @@ -925,12 +925,13 @@ bool MAVLinkXMLParserV10::generate() // 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 compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\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"); - QString compact2Send3("\n\tcrc_init(&checksum);\n\tchecksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);\n\tchecksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );\n\thdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\thdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);\n}\n\n#endif"); - compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); + 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" + commentSendContainer.arg(messageName.toLower(), commentLines) + 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]))); @@ -985,6 +986,16 @@ bool MAVLinkXMLParserV10::generate() mainHeader += QString::number(message_key[i]); if (i < highest_message_id-1) mainHeader += ", "; } + mainHeader += " }"; + + // Message lengths + mainHeader += "\n\n// MESSAGE LENGTHS\n\n"; + mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n"; + mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { "; + for (int i=0; i #include #include + #include /** diff --git a/src/apps/mavlinkgen/mavlinkgen.pri b/src/apps/mavlinkgen/mavlinkgen.pri index 8270518b4..a5ef920b5 100644 --- a/src/apps/mavlinkgen/mavlinkgen.pri +++ b/src/apps/mavlinkgen/mavlinkgen.pri @@ -29,6 +29,7 @@ FORMS += ui/XMLCommProtocolWidget.ui HEADERS += \ ui/XMLCommProtocolWidget.h \ generator/MAVLinkXMLParser.h \ + generator/MAVLinkXMLParserV10.h \ ui/DomItem.h \ ui/DomModel.h \ ui/QGCMAVLinkTextEdit.h @@ -37,6 +38,7 @@ SOURCES += \ ui/DomItem.cc \ ui/DomModel.cc \ generator/MAVLinkXMLParser.cc \ + generator/MAVLinkXMLParserV10.cc \ ui/QGCMAVLinkTextEdit.cc RESOURCES += mavlinkgen.qrc diff --git a/src/apps/mavlinkgen/mavlinkgen.pro b/src/apps/mavlinkgen/mavlinkgen.pro index 48e3bba52..278695604 100644 --- a/src/apps/mavlinkgen/mavlinkgen.pro +++ b/src/apps/mavlinkgen/mavlinkgen.pro @@ -13,5 +13,10 @@ include(mavlinkgen.pri) # Standalone files HEADERS += MAVLinkGen.h +win32-msvc2008|win32-msvc2010 { +HEADERS += msinttypes/inttypes.h \ + msinttypes/stdint.h +INCLUDEPATH += msinttypes +} SOURCES += main.cc \ - MAVLinkGen.cc \ No newline at end of file + MAVLinkGen.cc diff --git a/src/apps/mavlinkgen/msinttypes/inttypes.h b/src/apps/mavlinkgen/msinttypes/inttypes.h new file mode 100644 index 000000000..4b3828a21 --- /dev/null +++ b/src/apps/mavlinkgen/msinttypes/inttypes.h @@ -0,0 +1,305 @@ +// ISO C9x compliant inttypes.h for Microsoft Visual Studio +// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 +// +// Copyright (c) 2006 Alexander Chemeris +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// 3. The name of the author may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED +// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO +// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef _MSC_VER // [ +#error "Use this header only with Microsoft Visual C++ compilers!" +#endif // _MSC_VER ] + +#ifndef _MSC_INTTYPES_H_ // [ +#define _MSC_INTTYPES_H_ + +#if _MSC_VER > 1000 +#pragma once +#endif + +#include "stdint.h" + +// 7.8 Format conversion of integer types + +typedef struct { + intmax_t quot; + intmax_t rem; +} imaxdiv_t; + +// 7.8.1 Macros for format specifiers + +#if !defined(__cplusplus) || defined(__STDC_FORMAT_MACROS) // [ See footnote 185 at page 198 + +// The fprintf macros for signed integers are: +#define PRId8 "d" +#define PRIi8 "i" +#define PRIdLEAST8 "d" +#define PRIiLEAST8 "i" +#define PRIdFAST8 "d" +#define PRIiFAST8 "i" + +#define PRId16 "hd" +#define PRIi16 "hi" +#define PRIdLEAST16 "hd" +#define PRIiLEAST16 "hi" +#define PRIdFAST16 "hd" +#define PRIiFAST16 "hi" + +#define PRId32 "I32d" +#define PRIi32 "I32i" +#define PRIdLEAST32 "I32d" +#define PRIiLEAST32 "I32i" +#define PRIdFAST32 "I32d" +#define PRIiFAST32 "I32i" + +#define PRId64 "I64d" +#define PRIi64 "I64i" +#define PRIdLEAST64 "I64d" +#define PRIiLEAST64 "I64i" +#define PRIdFAST64 "I64d" +#define PRIiFAST64 "I64i" + +#define PRIdMAX "I64d" +#define PRIiMAX "I64i" + +#define PRIdPTR "Id" +#define PRIiPTR "Ii" + +// The fprintf macros for unsigned integers are: +#define PRIo8 "o" +#define PRIu8 "u" +#define PRIx8 "x" +#define PRIX8 "X" +#define PRIoLEAST8 "o" +#define PRIuLEAST8 "u" +#define PRIxLEAST8 "x" +#define PRIXLEAST8 "X" +#define PRIoFAST8 "o" +#define PRIuFAST8 "u" +#define PRIxFAST8 "x" +#define PRIXFAST8 "X" + +#define PRIo16 "ho" +#define PRIu16 "hu" +#define PRIx16 "hx" +#define PRIX16 "hX" +#define PRIoLEAST16 "ho" +#define PRIuLEAST16 "hu" +#define PRIxLEAST16 "hx" +#define PRIXLEAST16 "hX" +#define PRIoFAST16 "ho" +#define PRIuFAST16 "hu" +#define PRIxFAST16 "hx" +#define PRIXFAST16 "hX" + +#define PRIo32 "I32o" +#define PRIu32 "I32u" +#define PRIx32 "I32x" +#define PRIX32 "I32X" +#define PRIoLEAST32 "I32o" +#define PRIuLEAST32 "I32u" +#define PRIxLEAST32 "I32x" +#define PRIXLEAST32 "I32X" +#define PRIoFAST32 "I32o" +#define PRIuFAST32 "I32u" +#define PRIxFAST32 "I32x" +#define PRIXFAST32 "I32X" + +#define PRIo64 "I64o" +#define PRIu64 "I64u" +#define PRIx64 "I64x" +#define PRIX64 "I64X" +#define PRIoLEAST64 "I64o" +#define PRIuLEAST64 "I64u" +#define PRIxLEAST64 "I64x" +#define PRIXLEAST64 "I64X" +#define PRIoFAST64 "I64o" +#define PRIuFAST64 "I64u" +#define PRIxFAST64 "I64x" +#define PRIXFAST64 "I64X" + +#define PRIoMAX "I64o" +#define PRIuMAX "I64u" +#define PRIxMAX "I64x" +#define PRIXMAX "I64X" + +#define PRIoPTR "Io" +#define PRIuPTR "Iu" +#define PRIxPTR "Ix" +#define PRIXPTR "IX" + +// The fscanf macros for signed integers are: +#define SCNd8 "d" +#define SCNi8 "i" +#define SCNdLEAST8 "d" +#define SCNiLEAST8 "i" +#define SCNdFAST8 "d" +#define SCNiFAST8 "i" + +#define SCNd16 "hd" +#define SCNi16 "hi" +#define SCNdLEAST16 "hd" +#define SCNiLEAST16 "hi" +#define SCNdFAST16 "hd" +#define SCNiFAST16 "hi" + +#define SCNd32 "ld" +#define SCNi32 "li" +#define SCNdLEAST32 "ld" +#define SCNiLEAST32 "li" +#define SCNdFAST32 "ld" +#define SCNiFAST32 "li" + +#define SCNd64 "I64d" +#define SCNi64 "I64i" +#define SCNdLEAST64 "I64d" +#define SCNiLEAST64 "I64i" +#define SCNdFAST64 "I64d" +#define SCNiFAST64 "I64i" + +#define SCNdMAX "I64d" +#define SCNiMAX "I64i" + +#ifdef _WIN64 // [ +# define SCNdPTR "I64d" +# define SCNiPTR "I64i" +#else // _WIN64 ][ +# define SCNdPTR "ld" +# define SCNiPTR "li" +#endif // _WIN64 ] + +// The fscanf macros for unsigned integers are: +#define SCNo8 "o" +#define SCNu8 "u" +#define SCNx8 "x" +#define SCNX8 "X" +#define SCNoLEAST8 "o" +#define SCNuLEAST8 "u" +#define SCNxLEAST8 "x" +#define SCNXLEAST8 "X" +#define SCNoFAST8 "o" +#define SCNuFAST8 "u" +#define SCNxFAST8 "x" +#define SCNXFAST8 "X" + +#define SCNo16 "ho" +#define SCNu16 "hu" +#define SCNx16 "hx" +#define SCNX16 "hX" +#define SCNoLEAST16 "ho" +#define SCNuLEAST16 "hu" +#define SCNxLEAST16 "hx" +#define SCNXLEAST16 "hX" +#define SCNoFAST16 "ho" +#define SCNuFAST16 "hu" +#define SCNxFAST16 "hx" +#define SCNXFAST16 "hX" + +#define SCNo32 "lo" +#define SCNu32 "lu" +#define SCNx32 "lx" +#define SCNX32 "lX" +#define SCNoLEAST32 "lo" +#define SCNuLEAST32 "lu" +#define SCNxLEAST32 "lx" +#define SCNXLEAST32 "lX" +#define SCNoFAST32 "lo" +#define SCNuFAST32 "lu" +#define SCNxFAST32 "lx" +#define SCNXFAST32 "lX" + +#define SCNo64 "I64o" +#define SCNu64 "I64u" +#define SCNx64 "I64x" +#define SCNX64 "I64X" +#define SCNoLEAST64 "I64o" +#define SCNuLEAST64 "I64u" +#define SCNxLEAST64 "I64x" +#define SCNXLEAST64 "I64X" +#define SCNoFAST64 "I64o" +#define SCNuFAST64 "I64u" +#define SCNxFAST64 "I64x" +#define SCNXFAST64 "I64X" + +#define SCNoMAX "I64o" +#define SCNuMAX "I64u" +#define SCNxMAX "I64x" +#define SCNXMAX "I64X" + +#ifdef _WIN64 // [ +# define SCNoPTR "I64o" +# define SCNuPTR "I64u" +# define SCNxPTR "I64x" +# define SCNXPTR "I64X" +#else // _WIN64 ][ +# define SCNoPTR "lo" +# define SCNuPTR "lu" +# define SCNxPTR "lx" +# define SCNXPTR "lX" +#endif // _WIN64 ] + +#endif // __STDC_FORMAT_MACROS ] + +// 7.8.2 Functions for greatest-width integer types + +// 7.8.2.1 The imaxabs function +#define imaxabs _abs64 + +// 7.8.2.2 The imaxdiv function + +// This is modified version of div() function from Microsoft's div.c found +// in %MSVC.NET%\crt\src\div.c +#ifdef STATIC_IMAXDIV // [ +static +#else // STATIC_IMAXDIV ][ +_inline +#endif // STATIC_IMAXDIV ] +imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) +{ + imaxdiv_t result; + + result.quot = numer / denom; + result.rem = numer % denom; + + if (numer < 0 && result.rem > 0) { + // did division wrong; must fix up + ++result.quot; + result.rem -= denom; + } + + return result; +} + +// 7.8.2.3 The strtoimax and strtoumax functions +#define strtoimax _strtoi64 +#define strtoumax _strtoui64 + +// 7.8.2.4 The wcstoimax and wcstoumax functions +#define wcstoimax _wcstoi64 +#define wcstoumax _wcstoui64 + + +#endif // _MSC_INTTYPES_H_ ] diff --git a/src/apps/mavlinkgen/msinttypes/stdint.h b/src/apps/mavlinkgen/msinttypes/stdint.h new file mode 100644 index 000000000..d02608a59 --- /dev/null +++ b/src/apps/mavlinkgen/msinttypes/stdint.h @@ -0,0 +1,247 @@ +// ISO C9x compliant stdint.h for Microsoft Visual Studio +// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 +// +// Copyright (c) 2006-2008 Alexander Chemeris +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// 3. The name of the author may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED +// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO +// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef _MSC_VER // [ +#error "Use this header only with Microsoft Visual C++ compilers!" +#endif // _MSC_VER ] + +#ifndef _MSC_STDINT_H_ // [ +#define _MSC_STDINT_H_ + +#if _MSC_VER > 1000 +#pragma once +#endif + +#include + +// For Visual Studio 6 in C++ mode and for many Visual Studio versions when +// compiling for ARM we should wrap include with 'extern "C++" {}' +// or compiler give many errors like this: +// error C2733: second C linkage of overloaded function 'wmemchr' not allowed +#ifdef __cplusplus +extern "C" { +#endif +# include +#ifdef __cplusplus +} +#endif + +// Define _W64 macros to mark types changing their size, like intptr_t. +#ifndef _W64 +# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300 +# define _W64 __w64 +# else +# define _W64 +# endif +#endif + + +// 7.18.1 Integer types + +// 7.18.1.1 Exact-width integer types + +// Visual Studio 6 and Embedded Visual C++ 4 doesn't +// realize that, e.g. char has the same size as __int8 +// so we give up on __intX for them. +#if (_MSC_VER < 1300) + typedef signed char int8_t; + typedef signed short int16_t; + typedef signed int int32_t; + typedef unsigned char uint8_t; + typedef unsigned short uint16_t; + typedef unsigned int uint32_t; +#else + typedef signed __int8 int8_t; + typedef signed __int16 int16_t; + typedef signed __int32 int32_t; + typedef unsigned __int8 uint8_t; + typedef unsigned __int16 uint16_t; + typedef unsigned __int32 uint32_t; +#endif +typedef signed __int64 int64_t; +typedef unsigned __int64 uint64_t; + + +// 7.18.1.2 Minimum-width integer types +typedef int8_t int_least8_t; +typedef int16_t int_least16_t; +typedef int32_t int_least32_t; +typedef int64_t int_least64_t; +typedef uint8_t uint_least8_t; +typedef uint16_t uint_least16_t; +typedef uint32_t uint_least32_t; +typedef uint64_t uint_least64_t; + +// 7.18.1.3 Fastest minimum-width integer types +typedef int8_t int_fast8_t; +typedef int16_t int_fast16_t; +typedef int32_t int_fast32_t; +typedef int64_t int_fast64_t; +typedef uint8_t uint_fast8_t; +typedef uint16_t uint_fast16_t; +typedef uint32_t uint_fast32_t; +typedef uint64_t uint_fast64_t; + +// 7.18.1.4 Integer types capable of holding object pointers +#ifdef _WIN64 // [ + typedef signed __int64 intptr_t; + typedef unsigned __int64 uintptr_t; +#else // _WIN64 ][ + typedef _W64 signed int intptr_t; + typedef _W64 unsigned int uintptr_t; +#endif // _WIN64 ] + +// 7.18.1.5 Greatest-width integer types +typedef int64_t intmax_t; +typedef uint64_t uintmax_t; + + +// 7.18.2 Limits of specified-width integer types + +#if !defined(__cplusplus) || defined(__STDC_LIMIT_MACROS) // [ See footnote 220 at page 257 and footnote 221 at page 259 + +// 7.18.2.1 Limits of exact-width integer types +#define INT8_MIN ((int8_t)_I8_MIN) +#define INT8_MAX _I8_MAX +#define INT16_MIN ((int16_t)_I16_MIN) +#define INT16_MAX _I16_MAX +#define INT32_MIN ((int32_t)_I32_MIN) +#define INT32_MAX _I32_MAX +#define INT64_MIN ((int64_t)_I64_MIN) +#define INT64_MAX _I64_MAX +#define UINT8_MAX _UI8_MAX +#define UINT16_MAX _UI16_MAX +#define UINT32_MAX _UI32_MAX +#define UINT64_MAX _UI64_MAX + +// 7.18.2.2 Limits of minimum-width integer types +#define INT_LEAST8_MIN INT8_MIN +#define INT_LEAST8_MAX INT8_MAX +#define INT_LEAST16_MIN INT16_MIN +#define INT_LEAST16_MAX INT16_MAX +#define INT_LEAST32_MIN INT32_MIN +#define INT_LEAST32_MAX INT32_MAX +#define INT_LEAST64_MIN INT64_MIN +#define INT_LEAST64_MAX INT64_MAX +#define UINT_LEAST8_MAX UINT8_MAX +#define UINT_LEAST16_MAX UINT16_MAX +#define UINT_LEAST32_MAX UINT32_MAX +#define UINT_LEAST64_MAX UINT64_MAX + +// 7.18.2.3 Limits of fastest minimum-width integer types +#define INT_FAST8_MIN INT8_MIN +#define INT_FAST8_MAX INT8_MAX +#define INT_FAST16_MIN INT16_MIN +#define INT_FAST16_MAX INT16_MAX +#define INT_FAST32_MIN INT32_MIN +#define INT_FAST32_MAX INT32_MAX +#define INT_FAST64_MIN INT64_MIN +#define INT_FAST64_MAX INT64_MAX +#define UINT_FAST8_MAX UINT8_MAX +#define UINT_FAST16_MAX UINT16_MAX +#define UINT_FAST32_MAX UINT32_MAX +#define UINT_FAST64_MAX UINT64_MAX + +// 7.18.2.4 Limits of integer types capable of holding object pointers +#ifdef _WIN64 // [ +# define INTPTR_MIN INT64_MIN +# define INTPTR_MAX INT64_MAX +# define UINTPTR_MAX UINT64_MAX +#else // _WIN64 ][ +# define INTPTR_MIN INT32_MIN +# define INTPTR_MAX INT32_MAX +# define UINTPTR_MAX UINT32_MAX +#endif // _WIN64 ] + +// 7.18.2.5 Limits of greatest-width integer types +#define INTMAX_MIN INT64_MIN +#define INTMAX_MAX INT64_MAX +#define UINTMAX_MAX UINT64_MAX + +// 7.18.3 Limits of other integer types + +#ifdef _WIN64 // [ +# define PTRDIFF_MIN _I64_MIN +# define PTRDIFF_MAX _I64_MAX +#else // _WIN64 ][ +# define PTRDIFF_MIN _I32_MIN +# define PTRDIFF_MAX _I32_MAX +#endif // _WIN64 ] + +#define SIG_ATOMIC_MIN INT_MIN +#define SIG_ATOMIC_MAX INT_MAX + +#ifndef SIZE_MAX // [ +# ifdef _WIN64 // [ +# define SIZE_MAX _UI64_MAX +# else // _WIN64 ][ +# define SIZE_MAX _UI32_MAX +# endif // _WIN64 ] +#endif // SIZE_MAX ] + +// WCHAR_MIN and WCHAR_MAX are also defined in +#ifndef WCHAR_MIN // [ +# define WCHAR_MIN 0 +#endif // WCHAR_MIN ] +#ifndef WCHAR_MAX // [ +# define WCHAR_MAX _UI16_MAX +#endif // WCHAR_MAX ] + +#define WINT_MIN 0 +#define WINT_MAX _UI16_MAX + +#endif // __STDC_LIMIT_MACROS ] + + +// 7.18.4 Limits of other integer types + +#if !defined(__cplusplus) || defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260 + +// 7.18.4.1 Macros for minimum-width integer constants + +#define INT8_C(val) val##i8 +#define INT16_C(val) val##i16 +#define INT32_C(val) val##i32 +#define INT64_C(val) val##i64 + +#define UINT8_C(val) val##ui8 +#define UINT16_C(val) val##ui16 +#define UINT32_C(val) val##ui32 +#define UINT64_C(val) val##ui64 + +// 7.18.4.2 Macros for greatest-width integer constants +#define INTMAX_C INT64_C +#define UINTMAX_C UINT64_C + +#endif // __STDC_CONSTANT_MACROS ] + + +#endif // _MSC_STDINT_H_ ] diff --git a/src/apps/mavlinkgen/template/checksum.h b/src/apps/mavlinkgen/template/checksum.h new file mode 100644 index 000000000..07bab9102 --- /dev/null +++ b/src/apps/mavlinkgen/template/checksum.h @@ -0,0 +1,139 @@ +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * 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 + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*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); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/apps/mavlinkgen/template/documentation.dox b/src/apps/mavlinkgen/template/documentation.dox new file mode 100644 index 000000000..49de0050e --- /dev/null +++ b/src/apps/mavlinkgen/template/documentation.dox @@ -0,0 +1,41 @@ +/** + * @file + * @brief MAVLink communication protocol + * + * @author Lorenz Meier + * + */ + + +/** + * @mainpage MAVLink API Documentation + * + * @section intro_sec Introduction + * + * This API documentation covers the MAVLink + * protocol developed PIXHAWK project. + * In case you have generated this documentation locally, the most recent version (generated on every commit) + * is also publicly available on the internet. + * + * @sa http://pixhawk.ethz.ch/api/qgroundcontrol/ - Groundstation code base + * @sa http://pixhawk.ethz.ch/api/mavlink - (this) MAVLink communication protocol + * @sa http://pixhawk.ethz.ch/api/imu_autopilot/ - Flight board (ARM MCU) code base + * @sa http://pixhawk.ethz.ch/api/ai_vision - Computer Vision / AI API docs + * + * @section further_sec Further Information + * + * How to run our software and a general overview of the software architecture is documented in the project + * wiki pages. + * + * @sa http://pixhawk.ethz.ch/software/mavlink/ - MAVLink main documentation + * + * See the PIXHAWK website for more information. + * + * @section usage_sec Doxygen Usage + * + * You can exclude files from being parsed into this Doxygen documentation + * by adding them to the EXCLUDE list in the file in embedded/cmake/doc/api/doxy.config.in. + * + * + * + **/ diff --git a/src/apps/mavlinkgen/template/mavlink_checksum.h b/src/apps/mavlinkgen/template/mavlink_checksum.h new file mode 100644 index 000000000..fdcee99d1 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_checksum.h @@ -0,0 +1,183 @@ +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * 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 + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*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); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_mem(uint8_t *pBuffer, uint16_t *crcTmp, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + //uint16_t tmp; + //uint8_t* pTmp; + int i; + +// pTmp=pBuffer; + + for (i = 0; i < length; i++){ + crc_accumulate(*pBuffer++, crcTmp); + } + + return(*crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/apps/mavlinkgen/template/mavlink_data.h b/src/apps/mavlinkgen/template/mavlink_data.h new file mode 100644 index 000000000..17c88cc2a --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_data.h @@ -0,0 +1,21 @@ +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +mavlink_system_t mavlink_system; +#endif \ No newline at end of file diff --git a/src/apps/mavlinkgen/template/mavlink_options.h b/src/apps/mavlinkgen/template/mavlink_options.h new file mode 100644 index 000000000..550a85ae5 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_options.h @@ -0,0 +1,135 @@ +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +//#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STATIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +/** + * + * Convience functions. These are all in one send functions that are very + * easy to use. Just define the symbol MAVLINK_USE_CONVENIENCE_FUNCTIONS. + * These functions also support a buffer check, to ensure there is enough + * space in your comm buffer that the function would not block - it could + * also be used as the basis of a MUTEX. This is implemented in the send + * function as a macro with two arguments, first the comm chan number and + * the message length in the form + * MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LEN ) + * followed by the function code and then + * MAVLINK_BUFFER_CHECK_START + * Note that there are no terminators on these statements to allow for + * code nesting or other constructs. Default value for both is empty. + * A sugested implementation is shown below and the symbols will be defined + * only if they are not allready. + * + * if ( serial_space( chan ) > len ) { // serial_space returns available space + * ..... code that creates message + * } + * + * #define MAVLINK_BUFFER_CHECK_START( c, l ) if ( serial_space( c ) > l ) { + * #define MAVLINK_BUFFER_CHECK_END } + * + */ +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +#ifndef MAVLINK_BUFFER_CHECK_START +#define MAVLINK_BUFFER_CHECK_START( c, l ) ; +#endif +#ifndef MAVLINK_BUFFER_CHECK_END +#define MAVLINK_BUFFER_CHECK_END ; +#endif + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/apps/mavlinkgen/template/mavlink_protocol.h b/src/apps/mavlinkgen/template/mavlink_protocol.h new file mode 100644 index 000000000..8cf62f115 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_protocol.h @@ -0,0 +1,423 @@ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +extern mavlink_system_t mavlink_system; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->ck); +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->ck); +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STATIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + { + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + break; + } else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; +#ifdef MAVLINK_STATIC_BUFFER + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; +#else + if (status->msg_received == 1) + return 1; + else return 0; +#endif +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, uint8_t *mem, uint16_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/src/apps/mavlinkgen/template/mavlink_types.h b/src/apps/mavlinkgen/template/mavlink_types.h new file mode 100644 index 000000000..77b2e2409 --- /dev/null +++ b/src/apps/mavlinkgen/template/mavlink_types.h @@ -0,0 +1,120 @@ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system +{ + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode +} mavlink_system_t; + +typedef struct __mavlink_message +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload +} mavlink_header_t; + +typedef enum +{ + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 +} mavlink_channel_t; + +typedef enum +{ + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/src/apps/mavlinkgen/template/protocol.h b/src/apps/mavlinkgen/template/protocol.h new file mode 100644 index 000000000..a45993e69 --- /dev/null +++ b/src/apps/mavlinkgen/template/protocol.h @@ -0,0 +1,439 @@ +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "string.h" +#include "mavlink_types.h" + +#include "checksum.h" + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +#endif + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + union checksum_ ck; + crc_init(&(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + union checksum_ ck; + ck.c[0] = msg->ck_a; + ck.c[1] = msg->ck_b; + crc_accumulate(c, &(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +#elif defined(NB_MAVLINK_COMM) + static mavlink_message_t m_mavlink_message[NB_MAVLINK_COMM]; +#else + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +#endif + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != mavlink_msg_lengths[c] ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; + + // For future use + +// if (status->msg_received == 1) +// { +// if ( r_message != NULL ) +// { +// return r_message; +// } +// else +// { +// return rxmsg; +// } +// } +// else +// { +// return NULL; +// } + return status->msg_received; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc index 337a01958..37cbd19cf 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc @@ -6,6 +6,7 @@ #include "XMLCommProtocolWidget.h" #include "ui_XMLCommProtocolWidget.h" #include "MAVLinkXMLParser.h" +#include "MAVLinkXMLParserV10.h" #include #include @@ -131,18 +132,37 @@ void XMLCommProtocolWidget::generate() // Syntax check already gives output return; } - - MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); - connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); - bool result = parser->generate(); - if (result) { + + MAVLinkXMLParser* parser = NULL; + MAVLinkXMLParserV10* parserV10 = NULL; + + bool result = false; + + if (m_ui->versionComboBox->currentIndex() == 1) + { + MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + result = parser->generate(); + } + else if (m_ui->versionComboBox->currentIndex() == 0) + { + MAVLinkXMLParserV10* parserV10 = new MAVLinkXMLParserV10(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parserV10, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + result = parserV10->generate(); + } + + if (result) + { QMessageBox msgBox; msgBox.setText(QString("The C code / headers have been generated in folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed())); msgBox.exec(); - } else { + } + else + { QMessageBox::critical(this, tr("C code generation failed, please see the compile log for further information"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok); } - delete parser; + if (parser) delete parser; + if (parserV10) delete parserV10; } void XMLCommProtocolWidget::save() diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui index 089c8afa7..41af4832f 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui @@ -13,7 +13,7 @@ Form - + 6 @@ -51,12 +51,12 @@ Select input file - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + @@ -97,49 +97,70 @@ Select directory - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + - + Compile Output - + - + No file loaded - + Save file - + Save and generate - + :/images/categories/applications-system.svg:/images/categories/applications-system.svg + + + + Select MAVLink Version + + + + + + + + MAVLink v1.0 (Sept'10+) + + + + + MAVLink v0.9 (-Aug'10) + + + + @@ -150,7 +171,7 @@ - + -- 2.22.0