MAVLinkXMLParser.cc 11.1 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 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 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 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
#include <QFile>
#include <QDir>
#include <QPair>
#include <QList>
#include <QDateTime>
#include "MAVLinkXMLParser.h"

#include <QDebug>

MAVLinkXMLParser::MAVLinkXMLParser(QDomDocument* document, QString outputDirectory, QObject* parent) : QObject(parent),
doc(document),
outputDirName(outputDirectory)
{
}

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);
    }
    outputDirName = outputDirectory;
}

MAVLinkXMLParser::~MAVLinkXMLParser()
{
}

bool MAVLinkXMLParser::generate()
{
    // Process result
    bool success = true;

    // Only generate if output dir is correctly set
    if (outputDirName == "") return false;

    // print out the element names of all elements that are direct children
    // of the outermost element.
    QDomElement docElem = doc->documentElement();

    QList<QString> parseErrors();
    QDomNode n = docElem.firstChild();

    /*
    // Seek for element "messages" until end of document
    // ignoring all other tags
    while(!n.isNull())
    {
        if (n.toElement().tagName() == "messages")
        {
            break;
        }
        else
        {
            qDebug() << "IGNORED TAG" << n.toElement().tagName();
            n = n.nextSibling();
        }
    }

    qDebug() << "WORKING ON" << n.toElement().tagName();
*/
    QList< QPair<QString, QString> > cFiles;
    QString lcmStructDefs = "";

    while(!n.isNull()) {
        // Each child is a message
        QDomElement e = n.toElement(); // try to convert the node to an element.
        if(!e.isNull()) {
            // Handle all message tags
            if (e.tagName() == "message")
            {
                // Get message name
                QString messageName = e.attribute("name", "").toLower();
                if (messageName.size() == 0)
                {
                    //parseErrors.append(tr("Missing name attribute at line "));
                }
                else
                {
                    // Get message id
                    bool ok;
                    int messageId = e.attribute("id", "-1").toInt(&ok, 10);

                    QString channelType = "mavlink_channel_t";
                    QString messageType = "mavlink_message_t";

                    // Build up function call
                    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";
                    QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2").arg(messageName.toUpper(), QString::number(messageId));
                    QString cStructName = QString("%1_t").arg(messageName);
                    QString cStruct = "typedef struct __%1 \n{\n%2\n} %1;";
                    QString cStructLines = "";
                    QString encode = "static inline uint16_t message_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn message_%1_pack(%3);\n}\n";

                    QString decode = "static inline void message_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n";
                    QString pack = "static inline uint16_t message_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\tuint16_t i = 0;\n\n%4\n\treturn finalize_message(msg, system_id, component_id, i);\n}\n\n";
                    QString compactSend = "#if !defined(_WIN32) && !defined(__linux) && !defined(__APPLE__)\n\n#include \"global_data.h\"\n\nstatic inline void message_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmessage_%3_pack(global_data.param[PARAM_SYSTEM_ID], global_data.param[PARAM_COMPONENT_ID], &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif";
                    QString unpacking = "";
                    QString prepends = "";
                    QString packParameters = "";
                    QString packArguments = "system_id, component_id, msg";
                    QString packLines = "";
                    QString decodeLines = "";
                    QString sendArguments = "";
                    QString commentLines = "";

                    // Get the message fields
                    QDomNode f = e.firstChild();
                    while (!f.isNull())
                    {
                        QDomElement e2 = f.toElement();
                        if (!e2.isNull())
                        {
                            QString fieldType = e2.attribute("type", "");
                            QString fieldName = e2.attribute("name", "");
                            QString fieldText = e2.text();

                            packParameters += ", " + fieldType + " " + fieldName;
                            packArguments += ", " + messageName + "->" + fieldName;
                            sendArguments += ", " + fieldName;
                            cStructLines += QString("\t%1 %2; ///< %3\n").arg(fieldType, fieldName, fieldText);
                            packLines += QString("\ti += put_%1_by_index(%2, i, msg->payload); //%3\n").arg(fieldType, fieldName, e2.text());
                            decodeLines += QString("\t%1->%2 = message_%1_get_%2(msg);\n").arg(messageName, fieldName);
                            commentLines += commentEntry.arg(fieldName, fieldText);
                            QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText);
                            //
                            QString unpackingCode;

                            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_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[1] = (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 == "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.f;").arg(prepends).arg(fieldType);
                            }
                            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\tmemcpy(r, msg->payload%1, %2);").arg(prepends, fieldType.split("[").at(1).split("]").first());
                            }

                            unpacking += unpackingComment + QString("static inline %1 message_%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);
                    encode = encode.arg(messageName).arg(cStructName).arg(packArguments);
                    decode = decode.arg(messageName).arg(cStructName).arg(decodeLines);
                    compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters);
                    QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode;
                    cFiles.append(qMakePair(QString("mavlink_message_%1.h").arg(messageName), cFile));
                }
            }
        }
        n = n.nextSibling();
    }

    // XML parsed and converted to C code. Now generating the files
    QDateTime now = QDateTime::currentDateTime().toUTC();
    QString dateFormat = "dddd, MMMM d yyyy, hh:mm UTC";
    QString date = now.toString(dateFormat);
    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 MAVLINK_H\n#define MAVLINK_H\n\n#include \"protocol.h\"\n\n\n").arg(date); // The main header includes all messages
    // Mark all code as C code
    mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n";
    QString includeLine = "#include \"%1\"\n";
    QString mainHeaderName = "mavlink.h";
    QString messagesDirName = "generated";
    QDir dir(outputDirName + "/" + messagesDirName);
    // Create directory if it doesn't exist, report result in success
    if (!dir.exists()) success = success && dir.mkdir(outputDirName);
    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());
        mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first);
    }

    mainHeader += "#ifdef __cplusplus\n}\n#endif\n";
    mainHeader += "#endif";

    // Write main header
    QFile rawHeader(outputDirName + "/" + mainHeaderName);
    bool ok = rawHeader.open(QIODevice::WriteOnly | QIODevice::Text);
    success = success && ok;
    rawHeader.write(mainHeader.toLatin1());

    // Write C structs / lcm definitions
    QFile lcmStructs(outputDirName + "/mavlink.lcm");
    ok = lcmStructs.open(QIODevice::WriteOnly | QIODevice::Text);
    success = success && ok;
    lcmStructs.write(lcmStructDefs.toLatin1());

    return success;
}