MAVLinkXMLParser.cc 38.1 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9
/*=====================================================================
======================================================================*/

/**
 * @file
 *   @brief Implementation of class MAVLinkXMLParser
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
 */

pixhawk's avatar
pixhawk committed
10 11 12 13
#include <QFile>
#include <QDir>
#include <QPair>
#include <QList>
14
#include <QMap>
pixhawk's avatar
pixhawk committed
15
#include <QDateTime>
16
#include <QLocale>
pixhawk's avatar
pixhawk committed
17 18 19 20 21 22
#include "MAVLinkXMLParser.h"

#include <QDebug>

MAVLinkXMLParser::MAVLinkXMLParser(QDomDocument* document, QString outputDirectory, QObject* parent) : QObject(parent),
doc(document),
23 24
outputDirName(outputDirectory),
fileName("")
pixhawk's avatar
pixhawk committed
25 26 27 28 29 30 31 32 33 34 35 36
{
}

MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QObject* parent) : QObject(parent)
{
    doc = new QDomDocument();
    QFile file(document);
    if (file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        const QString instanceText(QString::fromUtf8(file.readAll()));
        doc->setContent(instanceText);
    }
37
    fileName = document;
pixhawk's avatar
pixhawk committed
38 39 40 41 42 43 44
    outputDirName = outputDirectory;
}

MAVLinkXMLParser::~MAVLinkXMLParser()
{
}

pixhawk's avatar
pixhawk committed
45 46 47
/**
 * Generate C-code (C-89 compliant) out of the XML protocol specs.
 */
pixhawk's avatar
pixhawk committed
48 49 50 51 52 53
bool MAVLinkXMLParser::generate()
{
    // Process result
    bool success = true;

    // Only generate if output dir is correctly set
54 55 56 57 58 59 60
    if (outputDirName == "")
    {
        emit parseState(tr("<font color=\"red\">ERROR: No output directory given.\nAbort.</font>"));
        return false;
    }

    QString topLevelOutputDirName = outputDirName;
pixhawk's avatar
pixhawk committed
61 62 63 64

    // print out the element names of all elements that are direct children
    // of the outermost element.
    QDomElement docElem = doc->documentElement();
65 66 67 68 69 70
    QDomNode n = docElem;//.firstChild();
    QDomNode p = docElem;

    // Sanity check variables
    QList<int>* usedMessageIDs = new QList<int>();
    QMap<QString, QString>* usedMessageNames = new QMap<QString, QString>();
71
    QMap<QString, QString>* usedEnumNames = new QMap<QString, QString>();
pixhawk's avatar
pixhawk committed
72 73 74 75

    QList< QPair<QString, QString> > cFiles;
    QString lcmStructDefs = "";

lm's avatar
lm committed
76
    QString pureFileName;
77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
    QString pureIncludeFileName;

    QFileInfo fInfo(this->fileName);
    pureFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first();

    // XML parsed and converted to C code. Now generating the files
    outputDirName += QDir::separator() + pureFileName;
    QDateTime now = QDateTime::currentDateTime().toUTC();
    QLocale loc(QLocale::English);
    QString dateFormat = "dddd, MMMM d yyyy, hh:mm UTC";
    QString date = loc.toString(now, dateFormat);
    QString includeLine = "#include \"%1\"\n";
    QString mainHeaderName = pureFileName + ".h";
    QString messagesDirName = ".";//"generated";
    QDir dir(outputDirName + "/" + messagesDirName);



    // Start main header
    QString mainHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://pixhawk.ethz.ch/software/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#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n";

102 103
    QString enums;

lm's avatar
lm committed
104

105 106 107
    // Run through root children
    while(!n.isNull())
    {
pixhawk's avatar
pixhawk committed
108 109
        // Each child is a message
        QDomElement e = n.toElement(); // try to convert the node to an element.
110 111 112
        if(!e.isNull())
        {
            if (e.tagName() == "mavlink")
pixhawk's avatar
pixhawk committed
113
            {
114 115 116
                p = n;
                n = n.firstChild();
                while (!n.isNull())
pixhawk's avatar
pixhawk committed
117
                {
118 119
                    e = n.toElement();
                    if (!e.isNull())
pixhawk's avatar
pixhawk committed
120
                    {
121 122
                        // Handle all include tags
                        if (e.tagName() == "include")
pixhawk's avatar
pixhawk committed
123
                        {
124
                            QString incFileName = e.text();
125
                            // Load file
126
                            //QDomDocument includeDoc = QDomDocument();
pixhawk's avatar
pixhawk committed
127

128 129
                            // Prepend file path if it is a relative path and
                            // make it relative to opened file
130 131 132
                            QFileInfo fInfo(incFileName);

                            QString incFilePath;
133
                            if (fInfo.isRelative())
134
                            {
135
                                QFileInfo rInfo(this->fileName);
136 137
                                incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName;
                                pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first();
138
                            }
pixhawk's avatar
pixhawk committed
139

140
                            QFile file(incFilePath);
141
                            if (file.open(QIODevice::ReadOnly | QIODevice::Text))
pixhawk's avatar
pixhawk committed
142
                            {
143 144 145 146 147 148 149 150 151 152 153
                                emit parseState(QString("<font color=\"green\">Included messages from file: %1</font>").arg(incFileName));
                                // NEW MODE: CREATE INDIVIDUAL FOLDERS
                                // Create new output directory, parse included XML and generate C-code
                                MAVLinkXMLParser includeParser(incFilePath, topLevelOutputDirName, this);
                                connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString)));
                                // Generate and write
                                includeParser.generate();
                                mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n";


                                // OLD MODE: MERGE BOTH FILES
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170
                                //                                        const QString instanceText(QString::fromUtf8(file.readAll()));
                                //                                        includeDoc.setContent(instanceText);
                                //                                // Get all messages
                                //                                QDomNode in = includeDoc.documentElement().firstChild();
                                //                                QDomElement ie = in.toElement();
                                //                                if (!ie.isNull())
                                //                                {
                                //                                    if (ie.tagName() == "messages" || ie.tagName() == "include")
                                //                                    {
                                //                                        QDomNode ref = n.parentNode().insertAfter(in, n);
                                //                                        if (ref.isNull())
                                //                                        {
                                //                                            emit parseState(QString("<font color=\"red\">ERROR: Inclusion failed: XML syntax error in file %1. Wrong/misspelled XML?\nAbort.</font>").arg(fileName));
                                //                                            return false;
                                //                                        }
                                //                                    }
                                //                                }
171 172

                                emit parseState(QString("<font color=\"green\">End of inclusion from file: %1</font>").arg(incFileName));
lm's avatar
lm committed
173
                            }
174 175
                            else
                            {
176 177 178
                                // Include file could not be opened
                                emit parseState(QString("<font color=\"red\">ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.</font>").arg(fileName));
                                return false;
179
                            }
180

pixhawk's avatar
pixhawk committed
181
                        }
182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257
                        // Handle all enum tags
                        else if (e.tagName() == "enums")
                        {
                            // One down into the enums list
                            p = n;
                            n = n.firstChild();
                            while (!n.isNull())
                            {
                                e = n.toElement();

                                QString currEnum;
                                QString currEnumEnd;
                                // Comment
                                QString comment;

                                if(!e.isNull() && e.tagName() == "enum")
                                {
                                    // Get enum name
                                    QString enumName = e.attribute("name", "").toLower();
                                    if (enumName.size() == 0)
                                    {
                                        emit parseState(tr("<font color=\"red\">ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.</font>").arg(QString::number(e.lineNumber()), e.tagName()));
                                        return false;
                                    }
                                    else
                                    {
                                        // Sanity check: Accept only enum names not used previously
                                        if (usedEnumNames->contains(enumName))
                                        {
                                            emit parseState(tr("<font color=\"red\">ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.</font>").arg(enumName, QString::number(e.lineNumber()), fileName));
                                            return false;
                                        }
                                        else
                                        {
                                            usedEnumNames->insert(enumName, QString::number(e.lineNumber()));
                                        }

                                        // Everything sane, starting with enum content
                                        currEnum = "enum " + enumName.toUpper() + "\n{\n";
                                        currEnumEnd = "};\n\n";

                                        int nextEnumValue = 0;

                                        // Get the message fields
                                        QDomNode f = e.firstChild();
                                        while (!f.isNull())
                                        {
                                            QDomElement e2 = f.toElement();
                                            if (!e2.isNull() && e2.tagName() == "entry")
                                            {
                                                QString fieldValue = e2.attribute("value", "");

                                                // If value was given, use it, if not, use the enum iterator
                                                // value. The iterator value gets reset by manual values

                                                QString fieldName = e2.attribute("name", "");
                                                if (fieldValue.length() == 0)
                                                {
                                                    fieldValue = QString::number(nextEnumValue);
                                                    nextEnumValue++;
                                                }
                                                else
                                                {
                                                    bool ok;
                                                    nextEnumValue = fieldValue.toInt(&ok) + 1;
                                                    if (!ok)
                                                    {
                                                        emit parseState(tr("<font color=\"red\">ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.</font>").arg(fieldName, fieldValue));
                                                        return false;
                                                    }
                                                }

                                                // Add comment of field if there is one
                                                QString fieldComment;
                                                if (e2.text().length() > 0)
                                                {
258
                                                     fieldComment = " /* " + e2.text() + "*/";
259 260 261
                                                }
                                                currEnum += "\t" + fieldName.toUpper() + "=" + fieldValue + "," + fieldComment + "\n";
                                            }
262
                                            else if(!e2.isNull() && e2.tagName() == "description")
263
                                            {
264
                                                comment = e2.text() + comment;
265 266 267 268 269 270 271 272 273
                                            }
                                            f = f.nextSibling();
                                        }
                                    }
                                    // Add the last parsed enum
                                    // Remove the last comma, as the last value has none
                                    int commaPosition = currEnum.lastIndexOf(",");
                                    currEnum.remove(commaPosition, 1);

274
                                    enums += "/** @brief " + comment  + " */\n" + currEnum + currEnumEnd;
275 276 277 278 279 280 281
                                } // Element is non-zero and element name is <enum>
                                n = n.nextSibling();
                            } // While through <enums>
                            // One up, back into the <mavlink> structure
                            n = p;
                        }

282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328
                        // Handle all message tags
                        else if (e.tagName() == "messages")
                        {
                            p = n;
                            n = n.firstChild();
                            while (!n.isNull())
                            {
                                e = n.toElement();
                                if(!e.isNull())
                                {
                                    //if (e.isNull()) continue;
                                    // Get message name
                                    QString messageName = e.attribute("name", "").toLower();
                                    if (messageName.size() == 0)
                                    {
                                        emit parseState(tr("<font color=\"red\">ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.</font>").arg(QString::number(e.lineNumber()), e.tagName()));
                                        return false;
                                    }
                                    else
                                    {
                                        // Get message id
                                        bool ok;
                                        int messageId = e.attribute("id", "-1").toInt(&ok, 10);
                                        emit parseState(tr("Compiling message <strong>%1 \t(#%3)</strong> \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId)));

                                        // Sanity check: Accept only message IDs not used previously
                                        if (usedMessageIDs->contains(messageId))
                                        {
                                            emit parseState(tr("<font color=\"red\">ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.</font>").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName));
                                            return false;
                                        }
                                        else
                                        {
                                            usedMessageIDs->append(messageId);
                                        }

                                        // Sanity check: Accept only message names not used previously
                                        if (usedMessageNames->contains(messageName))
                                        {
                                            emit parseState(tr("<font color=\"red\">ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.</font>").arg(messageName, QString::number(e.lineNumber()), fileName));
                                            return false;
                                        }
                                        else
                                        {
                                            usedMessageNames->insert(messageName, QString::number(e.lineNumber()));
                                        }

329 330
                                        QString channelType("mavlink_channel_t");
                                        QString messageType("mavlink_message_t");
331 332

                                        // Build up function call
333 334
                                        QString commentContainer("/**\n * @brief Send a %1 message\n *\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n");
                                        QString commentEntry(" * @param %1 %2\n");
335
                                        QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2").arg(messageName.toUpper(), QString::number(messageId));
336
                                        QString arrayDefines;
337
                                        QString cStructName = QString("mavlink_%1_t").arg(messageName);
338 339 340
                                        QString cStruct("typedef struct __%1 \n{\n%2\n} %1;");
                                        QString cStructLines;
                                        QString encode("static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n");
341

342 343
                                        QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n");
                                        QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n");
344 345
                                        QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);\n}\n\n");
                                        QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif");
346
                                        //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif";
347 348 349 350 351 352 353 354 355 356
                                        QString unpacking;
                                        QString prepends;
                                        QString packParameters;
                                        QString packArguments("system_id, component_id, msg");
                                        QString packLines;
                                        QString decodeLines;
                                        QString sendArguments;
                                        QString commentLines;


357 358 359 360 361 362

                                        // Get the message fields
                                        QDomNode f = e.firstChild();
                                        while (!f.isNull())
                                        {
                                            QDomElement e2 = f.toElement();
363
                                            if (!e2.isNull() && e2.tagName() == "field")
364 365 366 367 368
                                            {
                                                QString fieldType = e2.attribute("type", "");
                                                QString fieldName = e2.attribute("name", "");
                                                QString fieldText = e2.text();

369 370 371
                                                QString unpackingCode;
                                                QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText);

372 373 374 375 376 377 378 379 380 381 382 383 384 385
                                                // Send arguments are the same for integral types and arrays
                                                sendArguments += ", " + fieldName;

                                                // Array handling is different from simple types
                                                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;
                                                    packArguments += ", " + messageName + "->" + fieldName;

                                                    // Add field to C structure
                                                    cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("int8_t", fieldName, QString::number(arrayLength), fieldText);
                                                    // Add pack line to message_xx_pack function
386
                                                    packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); //%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText);
387 388
                                                    // Add decode function for this type
                                                    decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName);
389
                                                    arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength));
390 391 392 393 394 395 396 397 398 399 400 401 402 403
                                                }
                                                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;
                                                    packArguments += ", " + messageName + "->" + fieldName;

                                                    // Add field to C structure
                                                    cStructLines += QString("\t%1 %2[%3]; ///< %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());
                                                    // Add decode function for this type
                                                    decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName);
404
                                                    arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength));
405
                                                }
406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424
                                                // Expand array handling to all valid mavlink data types
                                                else if(fieldType.contains('[') && fieldType.contains(']'))
                                                {
                                                    int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt();
                                                    QString arrayType = fieldType.split("[").first();
                                                    packParameters += QString(", const ") + arrayType + "* " + fieldName;
                                                    packArguments += ", " + messageName + "->" + fieldName;

                                                    // Add field to C structure
                                                    cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText);
                                                    // Add pack line to message_xx_pack function
                                                    packLines += QString("\ti += put_array_by_index((int8_t*)%1, sizeof(%2)*%3, i, msg->payload); //%4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText);
                                                    // 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));

                                                    unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, sizeof(%2)*%3);\n\treturn sizeof(%2)*%3;").arg(prepends, arrayType, QString::number(arrayLength));

                                                    unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode);
425
                                                    //                                                    decodeLines += "";
426 427 428
                                                    prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength));

                                                }
429 430 431 432 433 434 435 436 437 438 439 440 441 442
                                                else
                                                    // Handle simple types like integers and floats
                                                {
                                                    packParameters += ", " + fieldType + " " + fieldName;
                                                    packArguments += ", " + messageName + "->" + fieldName;

                                                    // Add field to C structure
                                                    cStructLines += QString("\t%1 %2; ///< %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());
                                                    // Add decode function for this type
                                                    decodeLines += QString("\t%1->%2 = mavlink_msg_%1_get_%2(msg);\n").arg(messageName, fieldName);
                                                }
                                                commentLines += commentEntry.arg(fieldName, fieldText);
443

444
                                                //
445
                                                //                                                QString unpackingCode;
446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475

                                                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")
                                                {
                                                    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")
                                                {
                                                    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")
                                                {
                                                    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")
                                                {
                                                    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"))
                                                {  // 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"))
                                                {  // 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());
                                                }

476

477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492
                                                // Generate the message decoding function
                                                // Array handling is different from simple types
                                                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"))
                                                {
                                                    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;
                                                }
493 494 495 496
                                                else if(fieldType.contains('[') && fieldType.contains(']'))
                                                {
                                                    // prevent this case from being caught in the following else
                                                }
497 498 499 500 501 502 503 504 505 506 507 508 509
                                                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") + ")";
                                                }
                                            }
                                            f = f.nextSibling();
                                        }

                                        cStruct = cStruct.arg(cStructName, cStructLines);
                                        lcmStructDefs.append("\n").append(cStruct).append("\n");
                                        pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines);
510
                                        packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines);
511 512 513
                                        encode = encode.arg(messageName).arg(cStructName).arg(packArguments);
                                        decode = decode.arg(messageName).arg(cStructName).arg(decodeLines);
                                        compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters);
514
                                        QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + arrayDefines + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + packChan + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode;
515 516 517 518 519 520 521 522 523 524 525
                                        cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile));
                                    } // Check if tag = message
                                } // Check if e = NULL
                                n = n.nextSibling();
                            } // While through <message>
                            n = p;

                        } // Check if tag = messages
                    } // Check if e = NULL
                    n = n.nextSibling();
                } // While through include and messages
526
                // One up - current node = parent
527 528 529 530
                n = p;

            } // Check if tag = mavlink
        } // Check if e = NULL
pixhawk's avatar
pixhawk committed
531
        n = n.nextSibling();
532
    } // While through root children
pixhawk's avatar
pixhawk committed
533

534 535 536 537 538
    // Add enums to main header

    mainHeader += "// ENUM DEFINITIONS\n\n";
    mainHeader += enums;
    mainHeader += "\n";
539

540
    mainHeader += "// MESSAGE DEFINITIONS\n\n";
pixhawk's avatar
pixhawk committed
541
    // Create directory if it doesn't exist, report result in success
542
    if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName);
pixhawk's avatar
pixhawk committed
543 544 545 546 547 548
    for (int i = 0; i < cFiles.size(); i++)
    {
        QFile rawFile(dir.filePath(cFiles.at(i).first));
        bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text);
        success = success && ok;
        rawFile.write(cFiles.at(i).second.toLatin1());
549
        rawFile.close();
pixhawk's avatar
pixhawk committed
550 551 552 553 554
        mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first);
    }

    mainHeader += "#ifdef __cplusplus\n}\n#endif\n";
    mainHeader += "#endif";
lm's avatar
lm committed
555 556
    // Newline to make compiler happy
    mainHeader += "\n";
pixhawk's avatar
pixhawk committed
557 558 559 560 561 562

    // Write main header
    QFile rawHeader(outputDirName + "/" + mainHeaderName);
    bool ok = rawHeader.open(QIODevice::WriteOnly | QIODevice::Text);
    success = success && ok;
    rawHeader.write(mainHeader.toLatin1());
563 564 565 566 567 568 569 570 571 572 573 574
    rawHeader.close();

    // Write alias mavlink header
    QFile mavlinkHeader(outputDirName + "/mavlink.h");
    ok = mavlinkHeader.open(QIODevice::WriteOnly | QIODevice::Text);
    success = success && ok;
    QString mHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://pixhawk.ethz.ch/software/mavlink\n *\t Generated on %1\n */\n#ifndef MAVLINK_H\n#define MAVLINK_H\n\n").arg(date); // The main header includes all messages
    // Mark all code as C code
    mHeader += "#include \"" + mainHeaderName + "\"\n\n";
    mHeader += "#endif\n";
    mavlinkHeader.write(mHeader.toLatin1());
    mavlinkHeader.close();
pixhawk's avatar
pixhawk committed
575 576

    // Write C structs / lcm definitions
577 578 579 580
    //    QFile lcmStructs(outputDirName + "/mavlink.lcm");
    //    ok = lcmStructs.open(QIODevice::WriteOnly | QIODevice::Text);
    //    success = success && ok;
    //    lcmStructs.write(lcmStructDefs.toLatin1());
pixhawk's avatar
pixhawk committed
581 582 583

    return success;
}