From ce4926c4046306334385a0120c3dc3e4239960a3 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 24 May 2010 16:34:42 +0200 Subject: [PATCH] Added support for MAVLink XML inclusion, added support for multiple Autopilots, improved protocol generation --- mavlinkgen.pro | 48 +++ qgroundcontrol.pro | 8 +- src/comm/MAVLinkProtocol.cc | 135 +++++--- src/comm/MAVLinkSyntaxHighlighter.cc | 23 ++ src/comm/MAVLinkSyntaxHighlighter.h | 19 + src/comm/MAVLinkXMLParser.cc | 439 +++++++++++++++--------- src/comm/MAVLinkXMLParser.h | 5 + src/standalone/mavlinkgen/MAVLinkGen.cc | 93 +++++ src/standalone/mavlinkgen/MAVLinkGen.h | 58 ++++ src/standalone/mavlinkgen/main.cc | 46 +++ src/uas/ArduPilotMAV.cc | 27 ++ src/uas/ArduPilotMAV.h | 3 + src/uas/PxQuadMAV.cc | 26 ++ src/uas/PxQuadMAV.h | 3 + src/uas/SlugsMAV.cc | 11 +- src/uas/SlugsMAV.h | 1 + src/uas/UAS.cc | 15 +- src/ui/DebugConsole.cc | 2 +- src/ui/HDDisplay.cc | 6 +- src/ui/HUD.cc | 6 +- src/ui/XMLCommProtocolWidget.cc | 18 +- src/ui/XMLCommProtocolWidget.h | 2 + src/ui/XMLCommProtocolWidget.ui | 57 ++- 23 files changed, 810 insertions(+), 241 deletions(-) create mode 100644 mavlinkgen.pro create mode 100644 src/comm/MAVLinkSyntaxHighlighter.cc create mode 100644 src/comm/MAVLinkSyntaxHighlighter.h create mode 100644 src/standalone/mavlinkgen/MAVLinkGen.cc create mode 100644 src/standalone/mavlinkgen/MAVLinkGen.h create mode 100644 src/standalone/mavlinkgen/main.cc diff --git a/mavlinkgen.pro b/mavlinkgen.pro new file mode 100644 index 000000000..fb019cde6 --- /dev/null +++ b/mavlinkgen.pro @@ -0,0 +1,48 @@ + +# } +# Include general settings for MAVGround +# necessary as last include to override any non-acceptable settings +# done by the plugins above +QT += svg xml + +TEMPLATE = app +TARGET = mavlinkgen + +BASEDIR = . +BUILDDIR = build/mavlinkgen +LANGUAGE = C++ + +CONFIG += release +CONFIG -= debug + +OBJECTS_DIR = $$BUILDDIR/obj +MOC_DIR = $$BUILDDIR/moc +UI_HEADERS_DIR = src/ui/generated + +macx:DESTDIR = $$BASEDIR/bin/mac + +INCLUDEPATH += . \ + src \ + src/ui \ + src/comm \ + include/ui \ + src/ui/mavlink \ + src/standalone/mavlinkgen + +# Input +FORMS += src/ui/XMLCommProtocolWidget.ui + +HEADERS += src/standalone/mavlinkgen/MAVLinkGen.h \ + src/ui/XMLCommProtocolWidget.h \ + src/comm/MAVLinkXMLParser.h \ + src/ui/mavlink/DomItem.h \ + src/ui/mavlink/DomModel.h \ + src/comm/MAVLinkSyntaxHighlighter.h +SOURCES += src/standalone/mavlinkgen/main.cc \ + src/standalone/mavlinkgen/MAVLinkGen.cc \ + src/ui/XMLCommProtocolWidget.cc \ + src/ui/mavlink/DomItem.cc \ + src/ui/mavlink/DomModel.cc \ + src/comm/MAVLinkXMLParser.cc \ + src/comm/MAVLinkSyntaxHighlighter.cc +RESOURCES = mavground.qrc diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3b0ed5563..477d0cdfb 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -99,7 +99,6 @@ HEADERS += src/MG.h \ src/input/JoystickInput.h \ src/ui/JoystickWidget.h \ src/ui/PFD.h \ - src/ui/GaugePanel.h \ src/ui/DebugConsole.h \ src/ui/MapWidget.h \ src/ui/XMLCommProtocolWidget.h \ @@ -116,7 +115,8 @@ HEADERS += src/MG.h \ src/ui/linechart/Linecharts.h \ src/uas/SlugsMAV.h \ src/uas/PxQuadMAV.h \ - src/uas/ArduPilotMAV.h + src/uas/ArduPilotMAV.h \ + src/comm/MAVLinkSyntaxHighlighter.h SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -149,7 +149,6 @@ SOURCES += src/main.cc \ src/input/JoystickInput.cc \ src/ui/JoystickWidget.cc \ src/ui/PFD.cc \ - src/ui/GaugePanel.cc \ src/ui/DebugConsole.cc \ src/ui/MapWidget.cc \ src/ui/XMLCommProtocolWidget.cc \ @@ -166,5 +165,6 @@ SOURCES += src/main.cc \ src/ui/linechart/Linecharts.cc \ src/uas/SlugsMAV.cc \ src/uas/PxQuadMAV.cc \ - src/uas/ArduPilotMAV.cc + src/uas/ArduPilotMAV.cc \ + src/comm/MAVLinkSyntaxHighlighter.cc RESOURCES = mavground.qrc diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index bc056168e..889942480 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -42,7 +42,9 @@ This file is part of the PIXHAWK project #include "UASManager.h" #include "UASInterface.h" #include "UAS.h" -#include "SlugsMAV.h" /* FIXME REMOVE */ +#include "SlugsMAV.h" +#include "PxQuadMAV.h" +#include "ArduPilotMAV.h" #include "configuration.h" #include "LinkManager.h" #include @@ -115,7 +117,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) UASInterface* uas = UASManager::instance()->getUASForId(message.sysid); // Check and (if necessary) create UAS object - if (uas == NULL) + if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { // ORDER MATTERS HERE! // The UAS object has first to be created and connected, @@ -130,9 +132,33 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) qDebug() << "WARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\nWARNING\n\n RECEIVED MESSAGE FROM THIS SYSTEM WITH ID" << message.msgid << "FROM COMPONENT" << message.compid; } + // Create a new UAS based on the heartbeat received + // Todo dynamically load plugin at run-time for MAV + // WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION + // First create new UAS object - uas = new UAS(this, message.sysid); - //uas = new SlugsMAV(this, message.sysid); + // Decode heartbeat message + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(&message, &heartbeat); + switch (heartbeat.autopilot) + { + case MAV_AUTOPILOT_GENERIC: + uas = new UAS(this, message.sysid); + break; + case MAV_AUTOPILOT_PIXHAWK: + // Fixme differentiate between quadrotor and coaxial here + uas = new PxQuadMAV(this, message.sysid); + break; + case MAV_AUTOPILOT_SLUGS: + uas = new SlugsMAV(this, message.sysid); + break; + case MAV_AUTOPILOT_ARDUPILOT: + uas = new ArduPilotMAV(this, message.sysid); + break; + default: + uas = new UAS(this, message.sysid); + break; + } // Make UAS aware that this link can be used to communicate with the actual robot @@ -142,29 +168,20 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); } - // Increase receive counter - totalReceiveCounter++; - currReceiveCounter++; - qint64 lastLoss = totalLossCounter; - // Update last packet index - if (lastIndex[message.sysid][message.compid] == -1) - { - lastIndex[message.sysid][message.compid] = message.seq; - } - else + + // Only count message if UAS exists for this message + if (uas != NULL) { - if (lastIndex[message.sysid][message.compid] == 255) + // Increase receive counter + totalReceiveCounter++; + currReceiveCounter++; + qint64 lastLoss = totalLossCounter; + // Update last packet index + if (lastIndex[message.sysid][message.compid] == -1) { - lastIndex[message.sysid][message.compid] = 0; + lastIndex[message.sysid][message.compid] = message.seq; } else - { - lastIndex[message.sysid][message.compid]++; - } - - int safeguard = 0; - //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq; - while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255) { if (lastIndex[message.sysid][message.compid] == 255) { @@ -174,36 +191,50 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link) { lastIndex[message.sysid][message.compid]++; } - totalLossCounter++; - currLossCounter++; - safeguard++; + + int safeguard = 0; + //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq; + while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255) + { + if (lastIndex[message.sysid][message.compid] == 255) + { + lastIndex[message.sysid][message.compid] = 0; + } + else + { + lastIndex[message.sysid][message.compid]++; + } + totalLossCounter++; + currLossCounter++; + safeguard++; + } + } + // if (lastIndex.contains(message.sysid)) + // { + // QMap* lastCompIndex = lastIndex.value(message.sysid); + // if (lastCompIndex->contains(message.compid)) + // while (lastCompIndex->value(message.compid, 0)+1 ) + // } + //if () + + // If a new loss was detected or we just hit one 128th packet step + if (lastLoss != totalLossCounter || (totalReceiveCounter & 0x7F) == 0) + { + // Calculate new loss ratio + // Receive loss + float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter); + receiveLoss *= 100.0f; + // qDebug() << "LOSSCHANGED" << receiveLoss; + currLossCounter = 0; + currReceiveCounter = 0; + emit receiveLossChanged(receiveLoss); } - } - // if (lastIndex.contains(message.sysid)) - // { - // QMap* lastCompIndex = lastIndex.value(message.sysid); - // if (lastCompIndex->contains(message.compid)) - // while (lastCompIndex->value(message.compid, 0)+1 ) - // } - //if () - - // If a new loss was detected or we just hit one 128th packet step - if (lastLoss != totalLossCounter || (totalReceiveCounter & 0x7F) == 0) - { - // Calculate new loss ratio - // Receive loss - float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter); - receiveLoss *= 100.0f; - // qDebug() << "LOSSCHANGED" << receiveLoss; - currLossCounter = 0; - currReceiveCounter = 0; - emit receiveLossChanged(receiveLoss); - } - // The packet is emitted as a whole, as it is only 255 - 261 bytes short - // kind of inefficient, but no issue for a groundstation pc. - // It buys as reentrancy for the whole code over all threads - emit messageReceived(link, message); + // The packet is emitted as a whole, as it is only 255 - 261 bytes short + // kind of inefficient, but no issue for a groundstation pc. + // It buys as reentrancy for the whole code over all threads + emit messageReceived(link, message); + } } } receiveMutex.unlock(); @@ -273,7 +304,7 @@ void MAVLinkProtocol::sendHeartbeat() if (m_heartbeatsEnabled) { mavlink_message_t beat; - mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU); + mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU, MAV_AUTOPILOT_GENERIC); sendMessage(beat); } } diff --git a/src/comm/MAVLinkSyntaxHighlighter.cc b/src/comm/MAVLinkSyntaxHighlighter.cc new file mode 100644 index 000000000..170ac2cd2 --- /dev/null +++ b/src/comm/MAVLinkSyntaxHighlighter.cc @@ -0,0 +1,23 @@ +#include "MAVLinkSyntaxHighlighter.h" + +MAVLinkSyntaxHighlighter::MAVLinkSyntaxHighlighter(QObject *parent) : + QSyntaxHighlighter(parent) +{ +} + + +void MAVLinkSyntaxHighlighter::highlightBlock(const QString &text) + { + QTextCharFormat myClassFormat; + myClassFormat.setFontWeight(QFont::Bold); + myClassFormat.setForeground(Qt::darkMagenta); + QString pattern = "\"[A-Za-z0-9]+\""; + + QRegExp expression(pattern); + int index = text.indexOf(expression); + while (index >= 0) { + int length = expression.matchedLength(); + setFormat(index, length, myClassFormat); + index = text.indexOf(expression, index + length); + } + } diff --git a/src/comm/MAVLinkSyntaxHighlighter.h b/src/comm/MAVLinkSyntaxHighlighter.h new file mode 100644 index 000000000..23deacd33 --- /dev/null +++ b/src/comm/MAVLinkSyntaxHighlighter.h @@ -0,0 +1,19 @@ +#ifndef MAVLINKSYNTAXHIGHLIGHTER_H +#define MAVLINKSYNTAXHIGHLIGHTER_H + +#include + +class MAVLinkSyntaxHighlighter : public QSyntaxHighlighter +{ +Q_OBJECT +public: + explicit MAVLinkSyntaxHighlighter(QObject *parent = 0); + +signals: + +public slots: + void highlightBlock(const QString &text); + +}; + +#endif // MAVLINKSYNTAXHIGHLIGHTER_H diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index f2f208ed5..c1afa25fd 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include #include "MAVLinkXMLParser.h" @@ -9,7 +10,8 @@ MAVLinkXMLParser::MAVLinkXMLParser(QDomDocument* document, QString outputDirectory, QObject* parent) : QObject(parent), doc(document), -outputDirName(outputDirectory) +outputDirName(outputDirectory), +fileName("") { } @@ -22,6 +24,7 @@ MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QO const QString instanceText(QString::fromUtf8(file.readAll())); doc->setContent(instanceText); } + fileName = document; outputDirName = outputDirectory; } @@ -40,9 +43,13 @@ bool MAVLinkXMLParser::generate() // print out the element names of all elements that are direct children // of the outermost element. QDomElement docElem = doc->documentElement(); + QDomNode n = docElem;//.firstChild(); + QDomNode p = docElem; + + // Sanity check variables + QList* usedMessageIDs = new QList(); + QMap* usedMessageNames = new QMap(); - QList parseErrors(); - QDomNode n = docElem.firstChild(); /* // Seek for element "messages" until end of document @@ -61,186 +68,294 @@ bool MAVLinkXMLParser::generate() } qDebug() << "WORKING ON" << n.toElement().tagName(); -*/ + */ + QList< QPair > cFiles; QString lcmStructDefs = ""; - while(!n.isNull()) { + // Run through root children + while(!n.isNull()) + { // Each child is a message QDomElement e = n.toElement(); // try to convert the node to an element. - if(!e.isNull()) { - // Handle all message tags - if (e.tagName() == "message") + if(!e.isNull()) + { + if (e.tagName() == "mavlink") { - // Get message name - QString messageName = e.attribute("name", "").toLower(); - if (messageName.size() == 0) - { - //parseErrors.append(tr("Missing name attribute at line ")); - } - else + p = n; + n = n.firstChild(); + while (!n.isNull()) { - // 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 arrayDefines = ""; - QString cStructName = QString("mavlink_%1_t").arg(messageName); - QString cStruct = "typedef struct __%1 \n{\n%2\n} %1;"; - QString cStructLines = ""; - QString encode = "static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"; - - QString decode = "static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"; - QString pack = "static inline uint16_t mavlink_msg_%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 mavlink_finalize_message(msg, system_id, component_id, 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(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; - //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; - QString unpacking = ""; - QString prepends = ""; - QString packParameters = ""; - QString packArguments = "system_id, component_id, msg"; - QString packLines = ""; - QString decodeLines = ""; - QString sendArguments = ""; - QString commentLines = ""; - - // Get the message fields - QDomNode f = e.firstChild(); - while (!f.isNull()) + e = n.toElement(); + if (!e.isNull()) { - QDomElement e2 = f.toElement(); - if (!e2.isNull()) + // Handle all include tags + if (e.tagName() == "include") { - QString fieldType = e2.attribute("type", ""); - QString fieldName = e2.attribute("name", ""); - QString fieldText = e2.text(); + QString fileName = e.text(); + // Load file + QDomDocument includeDoc = QDomDocument(); - // 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 - 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); - arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - } - 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); - arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - } - else - // Handle simple types like integers and floats + // Prepend file path if it is a relative path and + // make it relative to opened file + QFileInfo fInfo(fileName); + if (fInfo.isRelative()) { - 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); + QFileInfo rInfo(this->fileName); + fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName; } - 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") + QFile file(fileName); + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { - 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()); - } + const QString instanceText(QString::fromUtf8(file.readAll())); + includeDoc.setContent(instanceText); - // 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; + // Get all messages + QDomNode in = includeDoc.documentElement().firstChild(); + //while (!in.isNull()) + //{ + 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("ERROR: Inclusion failed: XML syntax error in file %1. Wrong/misspelled XML?\nAbort.").arg(fileName)); + return false; + } + } + } + //in = in.nextSibling(); + //} + + emit parseState(QString("Included messages from file: %1").arg(fileName)); } 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") + ")"; + // 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; } + } - 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" + arrayDefines + "\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_msg_%1.h").arg(messageName), cFile)); - } - } - } + // Handle all message tags + else if (e.tagName() == "messages") + { + p = n; + n = n.firstChild(); + while (!n.isNull()) + { + e = n.toElement(); + if(!e.isNull()) + { + //if (e.isNull()) continue; + // Get message name + QString messageName = e.attribute("name", "").toLower(); + if (messageName.size() == 0) + { + emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); + return false; + } + else + { + // Get message id + bool ok; + int messageId = e.attribute("id", "-1").toInt(&ok, 10); + emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); + + // Sanity check: Accept only message IDs not used previously + if (usedMessageIDs->contains(messageId)) + { + emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); + return false; + } + else + { + usedMessageIDs->append(messageId); + } + + // Sanity check: Accept only message names not used previously + if (usedMessageNames->contains(messageName)) + { + emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); + return false; + } + else + { + usedMessageNames->insert(messageName, QString::number(e.lineNumber())); + } + + QString channelType = "mavlink_channel_t"; + QString messageType = "mavlink_message_t"; + + // 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 arrayDefines = ""; + QString cStructName = QString("mavlink_%1_t").arg(messageName); + QString cStruct = "typedef struct __%1 \n{\n%2\n} %1;"; + QString cStructLines = ""; + QString encode = "static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"; + + QString decode = "static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"; + QString pack = "static inline uint16_t mavlink_msg_%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 mavlink_finalize_message(msg, system_id, component_id, 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(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; + //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; + QString unpacking = ""; + QString prepends = ""; + QString packParameters = ""; + QString packArguments = "system_id, component_id, msg"; + QString packLines = ""; + QString decodeLines = ""; + QString sendArguments = ""; + QString commentLines = ""; + + // 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(); + + // 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 + 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); + arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + } + 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); + arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + } + else + // Handle simple types like integers and floats + { + packParameters += ", " + fieldType + " " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2; ///< %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); + 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_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()); + } + + // 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; + } + 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); + 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" + arrayDefines + "\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_msg_%1.h").arg(messageName), cFile)); + } // Check if tag = message + } // Check if e = NULL + n = n.nextSibling(); + } // While through + n = p; + + } // Check if tag = messages + } // Check if e = NULL + n = n.nextSibling(); + } // While through include and messages + n = p; + + } // Check if tag = mavlink + } // Check if e = NULL n = n.nextSibling(); - } + } // While through root children // XML parsed and converted to C code. Now generating the files QDateTime now = QDateTime::currentDateTime().toUTC(); diff --git a/src/comm/MAVLinkXMLParser.h b/src/comm/MAVLinkXMLParser.h index a0f10b730..c8c96c145 100644 --- a/src/comm/MAVLinkXMLParser.h +++ b/src/comm/MAVLinkXMLParser.h @@ -17,9 +17,14 @@ public slots: /** @brief Parse XML and generate C files */ bool generate(); +signals: + /** @brief Status message on the parsing */ + void parseState(QString message); + protected: QDomDocument* doc; QString outputDirName; + QString fileName; }; #endif // MAVLINKXMLPARSER_H diff --git a/src/standalone/mavlinkgen/MAVLinkGen.cc b/src/standalone/mavlinkgen/MAVLinkGen.cc new file mode 100644 index 000000000..86c06d47f --- /dev/null +++ b/src/standalone/mavlinkgen/MAVLinkGen.cc @@ -0,0 +1,93 @@ +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK 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. + + PIXHAWK 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 PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class MAVLinkGen + * + * @author Lorenz Meier + * + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MAVLinkGen.h" +#include "XMLCommProtocolWidget.h" + + +/** + * @brief Constructor for the main application. + * + * This constructor initializes and starts the whole application. It takes standard + * command-line parameters + * + * @param argc The number of command-line parameters + * @param argv The string array of parameters + **/ + +MAVLinkGen::MAVLinkGen(int &argc, char* argv[]) : QApplication(argc, argv) +{ + this->setApplicationName("MAVLink Generator"); + this->setApplicationVersion("v. 0.1.0 (Beta)"); + this->setOrganizationName(QLatin1String("OpenMAV Association")); + this->setOrganizationDomain("http://qgroundcontrol.org"); + + QSettings::setDefaultFormat(QSettings::IniFormat); + // Exit main application when last window is closed + connect(this, SIGNAL(lastWindowClosed()), this, SLOT(quit())); + + // Set application font + QFontDatabase fontDatabase = QFontDatabase(); + const QString fontFileName = ":/general/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app + const QString fontFamilyName = "Bitstream Vera Sans"; + if(!QFile::exists(fontFileName)) printf("ERROR! font file: %s DOES NOT EXIST!\n", fontFileName.toStdString().c_str()); + fontDatabase.addApplicationFont(fontFileName); + setFont(fontDatabase.font(fontFamilyName, "Roman", 12)); + + // Create main window + QMainWindow* window = new QMainWindow(); + window->setCentralWidget(new XMLCommProtocolWidget(window)); + window->setWindowTitle(applicationName() + " " + applicationVersion()); + window->show(); +} + +/** + * @brief Destructor for the groundstation. It destroys all loaded instances. + * + **/ +MAVLinkGen::~MAVLinkGen() +{ +} + diff --git a/src/standalone/mavlinkgen/MAVLinkGen.h b/src/standalone/mavlinkgen/MAVLinkGen.h new file mode 100644 index 000000000..518fed03d --- /dev/null +++ b/src/standalone/mavlinkgen/MAVLinkGen.h @@ -0,0 +1,58 @@ +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK 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. + + PIXHAWK 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 PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of class MAVLinkGen + * + * @author Lorenz Meier + * + */ + + +#ifndef MAVLINKGEN_H +#define MAVLINKGEN_H + +#include + +/** + * @brief The main application and management class. + * + * This class is started by the main method and provides + * the central management unit of the groundstation application. + * + **/ +class MAVLinkGen : public QApplication +{ + Q_OBJECT + +public: + MAVLinkGen(int &argc, char* argv[]); + ~MAVLinkGen(); + +protected: + +private: +}; + +#endif /* MAVLINKGEN_H */ diff --git a/src/standalone/mavlinkgen/main.cc b/src/standalone/mavlinkgen/main.cc new file mode 100644 index 000000000..4a57b3690 --- /dev/null +++ b/src/standalone/mavlinkgen/main.cc @@ -0,0 +1,46 @@ +/*===================================================================== + +PIXHAWK Micro Air Vehicle Flying Robotics Toolkit + +(c) 2009, 2010 PIXHAWK PROJECT + +This file is part of the PIXHAWK project + + PIXHAWK 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. + + PIXHAWK 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 PIXHAWK. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Main executable + * @author Lorenz Meier + * + */ + +#include +#include "MAVLinkGen.h" + +/** + * @brief Starts the application + * + * @param argc Number of commandline arguments + * @param argv Commandline arguments + * @return exit code, 0 for normal exit and !=0 for error cases + */ +int main(int argc, char *argv[]) +{ + + MAVLinkGen gen(argc, argv); + return gen.exec(); +} diff --git a/src/uas/ArduPilotMAV.cc b/src/uas/ArduPilotMAV.cc index 4fb529382..d752e87e9 100644 --- a/src/uas/ArduPilotMAV.cc +++ b/src/uas/ArduPilotMAV.cc @@ -5,3 +5,30 @@ ArduPilotMAV::ArduPilotMAV(MAVLinkProtocol* mavlink, int id) : // place other initializers here { } + +/** + * This function is called by MAVLink once a complete, uncorrupted (CRC check valid) + * mavlink packet is received. + * + * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port). + * messages can be sent back to the system via this link + * @param message MAVLink message, as received from the MAVLink protocol stack + */ +void ArduPilotMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) +{ + // Let UAS handle the default message set + UAS::receiveMessage(link, message); + + // Handle your special messages + switch (message.msgid) + { + case MAVLINK_MSG_ID_HEARTBEAT: + { + qDebug() << "ARDUPILOT RECEIVED HEARTBEAT"; + break; + } + default: + qDebug() << "\nARDUPILOT RECEIVED MESSAGE WITH ID" << message.msgid; + break; + } +} diff --git a/src/uas/ArduPilotMAV.h b/src/uas/ArduPilotMAV.h index 75d17cb7e..15e9d3071 100644 --- a/src/uas/ArduPilotMAV.h +++ b/src/uas/ArduPilotMAV.h @@ -8,6 +8,9 @@ class ArduPilotMAV : public UAS Q_OBJECT public: ArduPilotMAV(MAVLinkProtocol* mavlink, int id = 0); +public slots: + /** @brief Receive a MAVLink message from this MAV */ + void receiveMessage(LinkInterface* link, mavlink_message_t message); }; #endif // ARDUPILOTMAV_H diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 65c2eee31..1a6aeae8a 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -4,3 +4,29 @@ PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) : UAS(mavlink, id) { } + +/** + * This function is called by MAVLink once a complete, uncorrupted (CRC check valid) + * mavlink packet is received. + * + * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port). + * messages can be sent back to the system via this link + * @param message MAVLink message, as received from the MAVLink protocol stack + */ +void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) +{ + // Let UAS handle the default message set + UAS::receiveMessage(link, message); + + // Handle your special messages + switch (message.msgid) + { + case MAVLINK_MSG_ID_HEARTBEAT: + { + break; + } + default: + // Do nothing + break; + } +} diff --git a/src/uas/PxQuadMAV.h b/src/uas/PxQuadMAV.h index 5426e1bde..05f7688fd 100644 --- a/src/uas/PxQuadMAV.h +++ b/src/uas/PxQuadMAV.h @@ -8,6 +8,9 @@ class PxQuadMAV : public UAS Q_OBJECT public: PxQuadMAV(MAVLinkProtocol* mavlink, int id); +public slots: + /** @brief Receive a MAVLink message from this MAV */ + void receiveMessage(LinkInterface* link, mavlink_message_t message); }; #endif // PXQUADMAV_H diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 9f557af19..d5bf9f346 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -8,7 +8,14 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : { } - +/** + * This function is called by MAVLink once a complete, uncorrupted (CRC check valid) + * mavlink packet is received. + * + * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port). + * messages can be sent back to the system via this link + * @param message MAVLink message, as received from the MAVLink protocol stack + */ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { // Let UAS handle the default message set @@ -19,7 +26,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { case MAVLINK_MSG_ID_HEARTBEAT: { - qDebug() << "RECEIVED HEARTBEAT"; + qDebug() << "SLUGS RECEIVED HEARTBEAT"; break; } default: diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index 2e952066d..d983f86bc 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -10,6 +10,7 @@ public: SlugsMAV(MAVLinkProtocol* mavlink, int id = 0); public slots: + /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); }; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index bffe71c46..c01e62c8b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1053,7 +1053,20 @@ int UAS::calculateTimeRemaining() */ double UAS::getChargeLevel() { - return 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + float chargeLevel; + if (lpVoltage < emptyVoltage) + { + chargeLevel = 0.0f; + } + else if (lpVoltage > fullVoltage) + { + chargeLevel = 100.0f; + } + else + { + chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + } + return chargeLevel; } void UAS::startLowBattAlarm() diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index 85b5b3228..598f7b63f 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -163,7 +163,7 @@ void DebugConsole::setAutoHold(bool hold) void DebugConsole::receiveTextMessage(int id, int severity, QString text) { - m_ui->receiveText->appendHtml(QString("(MAV" + QString::number(id) + QString(":") + QString::number(severity) + QString(") ") + text + QString(""))); + m_ui->receiveText->appendHtml(QString("(MAV" + QString::number(id) + QString(":") + QString::number(severity) + QString(") ") + text + QString(""))); } void DebugConsole::updateTrafficMeasurements() diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index ad197acb3..58481b9e8 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -516,8 +516,10 @@ void HDDisplay::paintText(QString text, QColor color, float fontSize, float refX float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f); float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f); - //QFont font("Bitstream Vera Sans"); - font.setPixelSize((int)(fontSize*scalingFactor*1.26f)); + QFont font("Bitstream Vera Sans"); + // Enforce minimum font size of 5 pixels + int fSize = qMax(5, (int)(fontSize*scalingFactor*1.26f)); + font.setPixelSize(fSize); QFontMetrics metrics = QFontMetrics(font); int border = qMax(4, metrics.leading()); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 772cde8fc..15ca6b737 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -444,8 +444,10 @@ void HUD::paintText(QString text, QColor color, float fontSize, float refX, floa float pPositionX = refToScreenX(refX) - (fontSize*scalingFactor*0.072f); float pPositionY = refToScreenY(refY) - (fontSize*scalingFactor*0.212f); - //QFont font("Bitstream Vera Sans"); - font.setPixelSize((int)(fontSize*scalingFactor*1.26f)); + QFont font("Bitstream Vera Sans"); + // Enforce minimum font size of 5 pixels + int fSize = qMax(1, (int)(fontSize*scalingFactor*1.26f)); + font.setPixelSize(fSize); QFontMetrics metrics = QFontMetrics(font); int border = qMax(4, metrics.leading()); diff --git a/src/ui/XMLCommProtocolWidget.cc b/src/ui/XMLCommProtocolWidget.cc index be3004e79..387b69b18 100644 --- a/src/ui/XMLCommProtocolWidget.cc +++ b/src/ui/XMLCommProtocolWidget.cc @@ -6,6 +6,7 @@ #include "XMLCommProtocolWidget.h" #include "ui_XMLCommProtocolWidget.h" #include "MAVLinkXMLParser.h" +#include "MAVLinkSyntaxHighlighter.h" #include #include @@ -16,6 +17,9 @@ XMLCommProtocolWidget::XMLCommProtocolWidget(QWidget *parent) : { m_ui->setupUi(this); + // Now set syntax highlighter + highlighter = new MAVLinkSyntaxHighlighter(m_ui->xmlTextView->document()); + connect(m_ui->selectFileButton, SIGNAL(clicked()), this, SLOT(selectXMLFile())); connect(m_ui->selectOutputButton, SIGNAL(clicked()), this, SLOT(selectOutputDirectory())); connect(m_ui->generateButton, SIGNAL(clicked()), this, SLOT(generate())); @@ -66,11 +70,11 @@ void XMLCommProtocolWidget::setXML(const QString& xml) if (doc.setContent(xml)) { - m_ui->validXMLLabel->setText(tr("Valid XML file")); + m_ui->validXMLLabel->setText(tr("Valid XML file")); } else { - m_ui->validXMLLabel->setText(tr("File is NOT valid XML, please fix in editor")); + m_ui->validXMLLabel->setText(tr("File is NOT valid XML, please fix in editor")); } if (model != NULL) @@ -112,8 +116,11 @@ void XMLCommProtocolWidget::generate() { // First save file save(); - MAVLinkXMLParser parser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); - bool result = parser.generate(); + // Clean log + m_ui->compileLog->clear(); + 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) { QMessageBox msgBox; @@ -122,8 +129,9 @@ void XMLCommProtocolWidget::generate() } else { - QMessageBox::critical(this, tr("Could not write files"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok); + 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; } void XMLCommProtocolWidget::save() diff --git a/src/ui/XMLCommProtocolWidget.h b/src/ui/XMLCommProtocolWidget.h index 42915597c..ecb5e2521 100644 --- a/src/ui/XMLCommProtocolWidget.h +++ b/src/ui/XMLCommProtocolWidget.h @@ -3,6 +3,7 @@ #include #include "DomModel.h" +#include "MAVLinkSyntaxHighlighter.h" namespace Ui { class XMLCommProtocolWidget; @@ -31,6 +32,7 @@ protected slots: void save(); protected: + MAVLinkSyntaxHighlighter* highlighter; DomModel* model; void changeEvent(QEvent *e); diff --git a/src/ui/XMLCommProtocolWidget.ui b/src/ui/XMLCommProtocolWidget.ui index 0b387685a..c9b7fd39c 100644 --- a/src/ui/XMLCommProtocolWidget.ui +++ b/src/ui/XMLCommProtocolWidget.ui @@ -6,22 +6,43 @@ 0 0 - 402 + 846 480 Form - - + + + 6 + + + 6 + + + 6 + + + 12 + + + + + 300 + 16777215 + + Select input file true + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + @@ -35,8 +56,21 @@ + + + + false + + + + + + 400 + 16777215 + + Select output directory @@ -59,28 +93,31 @@ - - - - false + + + + Compile Output - + + + + No file loaded - + Save file - + Save and generate -- 2.22.0